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); }
/** * 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 twistCalculator a twist calculator for the same system and the same inertial frame * as this calculator is used to account for the centrifugal and Coriolis effects on the * rigid-body accelerations. * @param gravity the magnitude of the gravitational acceleration. * It is positive and the gravity field is assumed to be pulling the system down (toward z negative). * @param useDesireds whether the desired or actual joint accelerations are used to compute * the rigid-body accelerations. */ public SpatialAccelerationCalculator(RigidBody body, TwistCalculator twistCalculator, double gravity, boolean useDesireds) { this(body, ReferenceFrame.getWorldFrame(), ScrewTools.createGravitationalSpatialAcceleration(body, gravity), twistCalculator, true, useDesireds); }
public InverseDynamicsCalculator(TwistCalculator twistCalculator, double gravity, List<InverseDynamicsJoint> jointsToIgnore) { this(ReferenceFrame.getWorldFrame(), ScrewTools.createGravitationalSpatialAcceleration(twistCalculator.getRootBody(), gravity), new LinkedHashMap<RigidBody, Wrench>(), jointsToIgnore, true, true, twistCalculator); }
private void initialize() { hasBeenInitialized = true; TwistCalculator twistCalculator = new TwistCalculator(ReferenceFrame.getWorldFrame(), toolBody); boolean doVelocityTerms = true; boolean useDesireds = false; SpatialAccelerationCalculator spatialAccelerationCalculator = new SpatialAccelerationCalculator(toolBody, elevatorFrame, ScrewTools.createGravitationalSpatialAcceleration(fullRobotModel.getElevator(), gravity), twistCalculator, doVelocityTerms, useDesireds); ArrayList<InverseDynamicsJoint> jointsToIgnore = new ArrayList<InverseDynamicsJoint>(); jointsToIgnore.addAll(twistCalculator.getRootBody().getChildrenJoints()); jointsToIgnore.remove(toolJoint); inverseDynamicsCalculator = new InverseDynamicsCalculator(ReferenceFrame.getWorldFrame(), new LinkedHashMap<RigidBody, Wrench>(), jointsToIgnore, spatialAccelerationCalculator, twistCalculator, doVelocityTerms); }
public AntiGravityJointTorquesVisualizer(FullRobotModel fullRobotModel, TwistCalculator twistCalculator, SideDependentList<WrenchBasedFootSwitch> wrenchBasedFootSwitches, YoVariableRegistry parentRegistry, double gravity) { SpatialAccelerationVector rootAcceleration = ScrewTools.createGravitationalSpatialAcceleration(twistCalculator.getRootBody(), gravity); this.inverseDynamicsCalculator = new InverseDynamicsCalculator(ReferenceFrame.getWorldFrame(), rootAcceleration, new LinkedHashMap<RigidBody, Wrench>(), new ArrayList<InverseDynamicsJoint>(), false, false, twistCalculator); this.wrenchBasedFootSwitches = wrenchBasedFootSwitches; allJoints = ScrewTools.computeSubtreeJoints(inverseDynamicsCalculator.getSpatialAccelerationCalculator().getRootBody()); allOneDoFJoints = ScrewTools.filterJoints(allJoints, OneDoFJoint.class); antiGravityJointTorques = new LinkedHashMap<>(allOneDoFJoints.length); for (int i = 0; i < allOneDoFJoints.length; i++) { OneDoFJoint oneDoFJoint = allOneDoFJoints[i]; DoubleYoVariable antiGravityJointTorque = new DoubleYoVariable("antiGravity_tau_" + oneDoFJoint.getName(), registry); antiGravityJointTorques.put(oneDoFJoint, antiGravityJointTorque); } parentRegistry.addChild(registry); }
public DifferentialIDMassMatrixCalculator(ReferenceFrame inertialFrame, RigidBody rootBody) { twistCalculator = new TwistCalculator(inertialFrame, rootBody); LinkedHashMap<RigidBody, Wrench> zeroExternalWrench = new LinkedHashMap<RigidBody, Wrench>(); ArrayList<InverseDynamicsJoint> zeroJointToIgnore = new ArrayList<InverseDynamicsJoint>(); SpatialAccelerationVector zeroRootAcceleration = ScrewTools.createGravitationalSpatialAcceleration(rootBody, 0.0); idCalculator = new InverseDynamicsCalculator(inertialFrame, zeroRootAcceleration, zeroExternalWrench, zeroJointToIgnore, false, true, twistCalculator); jointsInOrder = ScrewTools.computeSubtreeJoints(rootBody); totalNumberOfDoFs = ScrewTools.computeDegreesOfFreedom(jointsInOrder); massMatrix = new DenseMatrix64F(totalNumberOfDoFs, totalNumberOfDoFs); storedJointDesiredAccelerations = new DenseMatrix64F(totalNumberOfDoFs, 1); storedJointVelocities = new DenseMatrix64F(totalNumberOfDoFs, 1); tmpDesiredJointAccelerationsMatrix = new DenseMatrix64F(totalNumberOfDoFs, 1); tmpTauMatrix = new DenseMatrix64F(totalNumberOfDoFs, 1); for (InverseDynamicsJoint joint : jointsInOrder) { ReferenceFrame bodyFixedFrame = joint.getSuccessor().getBodyFixedFrame(); Wrench jointWrench = new Wrench(bodyFixedFrame, bodyFixedFrame); storedJointWrenches.put(joint, jointWrench); } }
public DifferentialIDMassMatrixCalculator(ReferenceFrame inertialFrame, RigidBodyBasics rootBody) { SpatialAcceleration zeroRootAcceleration = ScrewTools.createGravitationalSpatialAcceleration(rootBody, 0.0); idCalculator = new InverseDynamicsCalculator(rootBody, false, true); idCalculator.setRootAcceleration(zeroRootAcceleration); jointsInOrder = MultiBodySystemTools.collectSubtreeJoints(rootBody); totalNumberOfDoFs = MultiBodySystemTools.computeDegreesOfFreedom(jointsInOrder); massMatrix = new DenseMatrix64F(totalNumberOfDoFs, totalNumberOfDoFs); storedJointDesiredAccelerations = new DenseMatrix64F(totalNumberOfDoFs, 1); storedJointVelocities = new DenseMatrix64F(totalNumberOfDoFs, 1); tmpDesiredJointAccelerationsMatrix = new DenseMatrix64F(totalNumberOfDoFs, 1); tmpTauMatrix = new DenseMatrix64F(totalNumberOfDoFs, 1); for (JointBasics joint : jointsInOrder) { ReferenceFrame bodyFixedFrame = joint.getSuccessor().getBodyFixedFrame(); Wrench jointWrench = new Wrench(bodyFixedFrame, bodyFixedFrame); storedJointWrenches.put(joint, jointWrench); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testCreateGravitationalSpatialAcceleration() { Vector3D[] jointAxes = {X, Y, Z, Y, X}; RandomFloatingRevoluteJointChain chain = new RandomFloatingRevoluteJointChain(random, jointAxes); chain.nextState(random, JointStateType.CONFIGURATION, JointStateType.VELOCITY); double gravity = RandomNumbers.nextDouble(random, 100.0); SpatialAcceleration result = ScrewTools. createGravitationalSpatialAcceleration(chain.getElevator(), gravity); Vector3DReadOnly angularPart = result.getAngularPart(); Vector3D zeroes = new Vector3D(0.0, 0.0, 0.0); assertTrue(angularPart.epsilonEquals(zeroes, epsilon)); Vector3DReadOnly linearPart = result.getLinearPart(); assertEquals(zeroes.getX(), linearPart.getX(), epsilon); assertEquals(zeroes.getY(), linearPart.getY(), epsilon); assertEquals(gravity, linearPart.getZ(), epsilon); }