public GravityCoriolisExternalWrenchMatrixCalculator(RigidBodyBasics body, ArrayList<JointBasics> jointsToIgnore, double gravity) { this(body,ScrewTools.createGravitationalSpatialAcceleration(MultiBodySystemTools.getRootBody(body), gravity), new LinkedHashMap<>(), jointsToIgnore, DEFAULT_DO_VELOCITY_TERMS, DO_ACCELERATION_TERMS); }
/** * Collects and returns all the joints located between the given {@code rigidBody} and the root * body. * <p> * WARNING: This method generates garbage. * </p> * * @param rigidBody the rigid-body to collect the support joints of. * @return the array containing the support joints of the given rigid-body. */ public static JointReadOnly[] collectSupportJoints(RigidBodyReadOnly rigidBody) { return createJointPath(getRootBody(rigidBody), rigidBody); }
/** * Collects and returns all the joints located between the given {@code rigidBody} and the root * body. * <p> * WARNING: This method generates garbage. * </p> * * @param rigidBody the rigid-body to collect the support joints of. * @return the array containing the support joints of the given rigid-body. */ public static JointBasics[] collectSupportJoints(RigidBodyBasics rigidBody) { return createJointPath(getRootBody(rigidBody), rigidBody); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testGetRootBody() { RigidBodyBasics randomBody = MultiBodySystemTools.getRootBody(joints.get(joints.size() - 1).getPredecessor()); assertTrue(randomBody.isRootBody()); }
public KinematicsPlanningToolboxCommandConverter(FullHumanoidRobotModel fullRobotModel) { referenceFrameHashCodeResolver = new ReferenceFrameHashCodeResolver(fullRobotModel, new HumanoidReferenceFrames(fullRobotModel)); RigidBodyBasics rootBody = MultiBodySystemTools.getRootBody(fullRobotModel.getElevator()); for (RigidBodyBasics rigidBody : rootBody.subtreeIterable()) rigidBodyHashMap.put(rigidBody.hashCode(), rigidBody); }
public KinematicsToolboxCommandConverter(FullHumanoidRobotModel fullRobotModel) { referenceFrameHashCodeResolver = new ReferenceFrameHashCodeResolver(fullRobotModel, new HumanoidReferenceFrames(fullRobotModel)); RigidBodyBasics rootBody = MultiBodySystemTools.getRootBody(fullRobotModel.getElevator()); for (RigidBodyBasics rigidBody : rootBody.subtreeIterable()) rigidBodyHashMap.put(rigidBody.hashCode(), rigidBody); }
public WholeBodyTrajectoryToolboxCommandConverter(FullHumanoidRobotModel fullRobotModel) { referenceFrameHashCodeResolver = new ReferenceFrameHashCodeResolver(fullRobotModel, new HumanoidReferenceFrames(fullRobotModel)); RigidBodyBasics rootBody = MultiBodySystemTools.getRootBody(fullRobotModel.getElevator()); for (RigidBodyBasics rigidBody : rootBody.subtreeIterable()) rigidBodyHashMap.put(rigidBody.hashCode(), rigidBody); }
private RigidBodyBasics getRigidBodyHasSameName(FullHumanoidRobotModel fullRobotModel, RigidBodyBasics givenRigidBody) { RigidBodyBasics rootBody = MultiBodySystemTools.getRootBody(fullRobotModel.getElevator()); RigidBodyBasics[] allRigidBodies = rootBody.subtreeArray(); for (RigidBodyBasics rigidBody : allRigidBodies) if (givenRigidBody.getName().equals(rigidBody.getName())) return rigidBody; return null; }
/** * Creates a new {@code TwistCalculator} that will compute all the twists of all the rigid-bodies * of the system to which {@code body} belongs. * * @param inertialFrame non-moving frame with respect to which the twists are computed. Typically * {@link ReferenceFrame#getWorldFrame()} is used here. * @param body a body that belongs to the system this twist calculator will be available for. */ public TwistCalculator(ReferenceFrame inertialFrame, RigidBodyBasics body) { this.inertialFrame = inertialFrame; this.rootBody = MultiBodySystemTools.getRootBody(body); this.rootTwist = new Twist(rootBody.getBodyFixedFrame(), inertialFrame, rootBody.getBodyFixedFrame()); int numberOfRigidBodies = MultiBodySystemTools.collectSubtreeSuccessors(MultiBodySystemTools.collectSubtreeJoints(rootBody)).length; while (unnassignedTwists.size() < numberOfRigidBodies) unnassignedTwists.add(new Twist()); assignedTwists = new ArrayList<>(numberOfRigidBodies); rigidBodiesWithAssignedTwist = new ArrayList<>(numberOfRigidBodies); assignedTwists.add(rootTwist); rigidBodiesWithAssignedTwist.add(rootBody); rigidBodyToAssignedTwistIndex.put(rootBody, new MutableInt(0)); }
private void randomizeJointPositions(Random random, Pair<FloatingJointBasics, OneDoFJointBasics[]> randomizedFullRobotModel, double percentOfMotionRangeAllowed) { for (OneDoFJointBasics joint : randomizedFullRobotModel.getRight()) { double jointLimitLower = joint.getJointLimitLower(); if (Double.isInfinite(jointLimitLower)) jointLimitLower = -Math.PI; double jointLimitUpper = joint.getJointLimitUpper(); if (Double.isInfinite(jointLimitUpper)) jointLimitUpper = -Math.PI; double rangeReduction = (1.0 - percentOfMotionRangeAllowed) * (jointLimitUpper - jointLimitLower); jointLimitLower += 0.5 * rangeReduction; jointLimitUpper -= 0.5 * rangeReduction; joint.setQ(RandomNumbers.nextDouble(random, jointLimitLower, jointLimitUpper)); } MultiBodySystemTools.getRootBody(randomizedFullRobotModel.getRight()[0].getPredecessor()).updateFramesRecursively(); }
/** * Creates a new {@code SpatialAccelerationCalculator} that will compute all the spatial * accelerations of all the rigid-bodies of the system to which {@code body} belongs. * * @param body a body that belongs to the system this spatial acceleration calculator will be * available for. * @param inertialFrame each rigid-body acceleration is calculated with respect to the inertial * frame. * @param doVelocityTerms whether the centrifugal and Coriolis are considered or ignored in the * computation of the rigid-body accelerations. * @param doAccelerationTerms whether the joint accelerations are considered or ignored in the * computation of the rigid-body accelerations. */ public SpatialAccelerationCalculator(RigidBodyReadOnly body, ReferenceFrame inertialFrame, boolean doVelocityTerms, boolean doAccelerationTerms) { this.inertialFrame = inertialFrame; rootBody = MultiBodySystemTools.getRootBody(body); this.doVelocityTerms = doVelocityTerms; this.doAccelerationTerms = doAccelerationTerms; int numberOfRigidBodies = (int) rootBody.subtreeStream().count(); while (unnassignedAccelerations.size() < numberOfRigidBodies) unnassignedAccelerations.add(new SpatialAcceleration()); rigidBodiesWithAssignedAcceleration = new ArrayList<>(numberOfRigidBodies); assignedAccelerations = new ArrayList<>(numberOfRigidBodies); assignedAccelerations.add(new SpatialAcceleration(rootBody.getBodyFixedFrame(), inertialFrame, rootBody.getBodyFixedFrame())); rigidBodiesWithAssignedAcceleration.add(rootBody); rigidBodyToAssignedAccelerationIndex.put(rootBody, new AccelerationIndex(0)); accelerationProvider = RigidBodyAccelerationProvider.toRigidBodyAccelerationProvider(this::getAccelerationOfBody, inertialFrame, doVelocityTerms, doAccelerationTerms); }
/** * Finds amongst the given {@code joints} which is the closest to the root body. * * @param joints the list of joints to search. Not modified. * @return the closest joint to the root body. */ public static JointReadOnly getClosestJointToRoot(List<? extends JointReadOnly> joints) { JointReadOnly closest = joints.get(0); RigidBodyReadOnly rootBody = MultiBodySystemTools.getRootBody(closest.getPredecessor()); int distanceToRoot = MultiBodySystemTools.computeDistanceToAncestor(closest.getPredecessor(), rootBody); for (int i = 1; i < joints.size(); i++) { JointReadOnly candidate = joints.get(i); int candidateDistanceToRoot = MultiBodySystemTools.computeDistanceToAncestor(candidate.getPredecessor(), rootBody); if (candidateDistanceToRoot < distanceToRoot) { distanceToRoot = candidateDistanceToRoot; closest = candidate; } } return closest; } }
protected BodyVelocitySensor(String prefix, double dt, RigidBodyBasics body, ReferenceFrame measurementFrame, boolean estimateBias, DoubleProvider variance, YoVariableRegistry registry) { this.sqrtHz = 1.0 / Math.sqrt(dt); this.variance = variance; measurement = new FrameVector3D(measurementFrame); robotJacobian.setKinematicChain(MultiBodySystemTools.getRootBody(body), body); robotJacobian.setJacobianFrame(measurementFrame); List<OneDoFJointBasics> oneDofJoints = MultiBodySystemTools.filterJoints(robotJacobian.getJointsFromBaseToEndEffector(), OneDoFJointBasics.class); oneDofJoints.stream().forEach(joint -> oneDofJointNames.add(joint.getName())); if (estimateBias) { biasState = new BiasState(prefix, dt, registry); } else { biasState = null; } int degreesOfFreedom = robotJacobian.getNumberOfDegreesOfFreedom(); jacobianRelevantPart.reshape(getMeasurementSize(), degreesOfFreedom); }
private Pair<FloatingJointBasics, OneDoFJointBasics[]> createFullRobotModelAtInitialConfiguration() { RobotDescription robotDescription = new KinematicsToolboxControllerTestRobots.SevenDoFArm(); Pair<FloatingJointBasics, OneDoFJointBasics[]> fullRobotModel = KinematicsToolboxControllerTestRobots.createInverseDynamicsRobot(robotDescription); for (OneDoFJointBasics joint : fullRobotModel.getRight()) { if (Double.isFinite(joint.getJointLimitLower()) && Double.isFinite(joint.getJointLimitUpper())) joint.setQ(0.5 * (joint.getJointLimitLower() + joint.getJointLimitUpper())); } MultiBodySystemTools.getRootBody(fullRobotModel.getRight()[0].getPredecessor()).updateFramesRecursively(); return fullRobotModel; }
public KinematicsToolboxController(CommandInputManager commandInputManager, StatusMessageOutputManager statusOutputManager, FloatingJointBasics rootJoint, OneDoFJointBasics[] oneDoFJoints, Collection<RigidBodyBasics> controllableRigidBodies, YoGraphicsListRegistry yoGraphicsListRegistry, YoVariableRegistry parentRegistry) { super(statusOutputManager, parentRegistry); this.commandInputManager = commandInputManager; this.rootJoint = rootJoint; this.oneDoFJoints = oneDoFJoints; this.yoGraphicsListRegistry = yoGraphicsListRegistry; // This will find the root body without using rootJoint so it can be null. rootBody = MultiBodySystemTools.getRootBody(oneDoFJoints[0].getPredecessor()); centerOfMassFrame = new CenterOfMassReferenceFrame("centerOfMass", worldFrame, rootBody); Arrays.stream(oneDoFJoints).forEach(joint -> jointHashCodeMap.put(joint.hashCode(), joint)); controllerCore = createControllerCore(controllableRigidBodies); feedbackControllerDataHolder = controllerCore.getWholeBodyFeedbackControllerDataHolder(); inverseKinematicsSolution = MessageTools.createKinematicsToolboxOutputStatus(oneDoFJoints); inverseKinematicsSolution.setDestination(-1); gains.setProportionalGains(1200.0); // Gains used for everything. It is as high as possible to reduce the convergence time. gains.setMaxFeedbackAndFeedbackRate(1500.0, Double.POSITIVE_INFINITY); privilegedWeight.set(1.0); privilegedConfigurationGain.set(50.0); privilegedMaxVelocity.set(Double.POSITIVE_INFINITY); }
RigidBodyBasics rootBody = MultiBodySystemTools.getRootBody(joints.get(0).getPredecessor()); RigidBodyBasics rootBodyInFuture = MultiBodySystemFactories.cloneMultiBodySystem(rootBody, worldFrame, "Test"); List<OneDoFJointBasics> jointsInFuture = SubtreeStreams.fromChildren(OneDoFJointBasics.class, rootBodyInFuture).collect(Collectors.toList());
public LinearAccelerationSensor(String sensorName, double dt, RigidBodyBasics body, ReferenceFrame measurementFrame, boolean estimateBias, YoVariableRegistry registry) { this.dt = dt; this.sqrtHz = 1.0 / Math.sqrt(dt); this.measurementFrame = measurementFrame; robotJacobian.setKinematicChain(MultiBodySystemTools.getRootBody(body), body); robotJacobian.setJacobianFrame(measurementFrame); List<OneDoFJointBasics> oneDofJoints = MultiBodySystemTools.filterJoints(robotJacobian.getJointsFromBaseToEndEffector(), OneDoFJointBasics.class); oneDofJoints.stream().forEach(joint -> oneDofJointNames.add(joint.getName())); variance = FilterTools.findOrCreate(sensorName + "Variance", registry, 1.0); if (estimateBias) { biasState = new BiasState(sensorName, dt, registry); } else { biasState = null; } int degreesOfFreedom = robotJacobian.getNumberOfDegreesOfFreedom(); jacobianAngularPart.reshape(3, degreesOfFreedom); jacobianLinearPart.reshape(3, degreesOfFreedom); jacobianDotLinearPart.reshape(3, degreesOfFreedom); crossProductLinearization.reshape(3, degreesOfFreedom); AqdxL.reshape(3, degreesOfFreedom); LqdxA.reshape(3, degreesOfFreedom); }
public ReachabilityMapSolver(OneDoFJointBasics[] robotArmJoints, YoGraphicsListRegistry yoGraphicsListRegistry, YoVariableRegistry parentRegistry) { this.robotArmJoints = robotArmJoints; endEffector = robotArmJoints[robotArmJoints.length - 1].getSuccessor(); kinematicsToolboxController = new KinematicsToolboxController(commandInputManager, statusOutputManager, null, robotArmJoints, Collections.singleton(endEffector), yoGraphicsListRegistry, registry); commandInputManager.registerConversionHelper(new KinematicsToolboxCommandConverter(MultiBodySystemTools.getRootBody(endEffector))); defaultArmConfiguration = RobotConfigurationDataFactory.create(robotArmJoints, new ForceSensorDefinition[0], new IMUDefinition[0]); RobotConfigurationDataFactory.packJointState(defaultArmConfiguration, robotArmJoints); maximumNumberOfIterations.set(DEFAULT_MAX_NUMBER_OF_ITERATIONS); solutionQualityThreshold.set(DEFAULT_QUALITY_THRESHOLD); solutionStabilityThreshold.set(DEFAULT_STABILITY_THRESHOLD); solutionMinimumProgression.set(DEFAULT_MIN_PROGRESSION); parentRegistry.addChild(registry); }
RigidBodyBasics rootBody = MultiBodySystemTools.getRootBody(joints.get(0).getSuccessor());
Pair<FloatingJointBasics, OneDoFJointBasics[]> desiredFullRobotModel = KinematicsToolboxControllerTestRobots.createInverseDynamicsRobot(robotDescription); commandInputManager = new CommandInputManager(KinematicsToolboxModule.supportedCommands()); commandInputManager.registerConversionHelper(new KinematicsToolboxCommandConverter(MultiBodySystemTools.getRootBody(desiredFullRobotModel.getRight()[0].getSuccessor())));