/** * Computes and packs the linear acceleration of the point {@code bodyFixedPoint} that * is attached to {@code body} with respect to {@code rootBody}. * <p> * WARNING: This method assumes that the internal memory of this {@code TwistCalculator} * is up-to-date. * The update of the internal memory is done through the method {@link #compute()}. * </p> * <p> * See {@link #getLinearAccelerationOfBodyFixedPoint(FrameVector, RigidBody, RigidBody, FramePoint)}. * </p> * * @param linearAccelerationToPack the linear acceleration of the body fixed point. Modified. * @param body the rigid-body to which {@code bodyFixedPoint} belongs. * @param bodyFixedPoint the point to compute the linear acceleration of. Not modified. */ // FIXME Change the method signature to have linearAccelerationToPack as the last argument. public void getLinearAccelerationOfBodyFixedPoint(FrameVector linearAccelerationToPack, RigidBody body, FramePoint bodyFixedPoint) { getLinearAccelerationOfBodyFixedPoint(linearAccelerationToPack, getRootBody(), body, bodyFixedPoint); }
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); }
SpatialAccelerationCalculator spatialAccelerationCalculator = inverseDynamicsStructure.getSpatialAccelerationCalculator(); RigidBody elevator = spatialAccelerationCalculator.getRootBody(); ReferenceFrame elevatorFrame = elevator.getBodyFixedFrame();