public InverseDynamicsCalculator(ReferenceFrame inertialFrame, SpatialAccelerationVector rootAcceleration, HashMap<RigidBody, Wrench> externalWrenches, List<InverseDynamicsJoint> jointsToIgnore, boolean doVelocityTerms, boolean doAccelerationTerms, TwistCalculator twistCalculator) { this(inertialFrame, externalWrenches, jointsToIgnore, new SpatialAccelerationCalculator(twistCalculator.getRootBody(), inertialFrame, rootAcceleration, twistCalculator, doVelocityTerms, doAccelerationTerms, true), twistCalculator, doVelocityTerms); }
private void computeTwistsAndSpatialAccelerations() { spatialAccelerationCalculator.compute(); }
/** * 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); }
TwistCalculator twistCalculator, boolean doVelocityTerms, boolean doAccelerationTerms, boolean useDesireds) this.rootBody = addAllPrecedingJoints(body); this.rootAcceleration = new SpatialAccelerationVector(rootAcceleration); this.twistCalculator = twistCalculator; this.useDesireds = useDesireds; addAllSuccedingJoints(body); populateMapsAndLists();
@Override public void computeAchievedAcceleration() { accelerationControlModule.getBodyFixedPoint(tempPosition); spatialAccelerationCalculator.getLinearAccelerationOfBodyFixedPoint(achievedLinearAcceleration, base, endEffector, tempPosition); yoAchievedLinearAcceleration.setAndMatchFrame(achievedLinearAcceleration); }
SpatialAccelerationCalculator spatialAccelerationCalculator = inverseDynamicsStructure.getSpatialAccelerationCalculator(); RigidBody elevator = spatialAccelerationCalculator.getRootBody(); ReferenceFrame elevatorFrame = elevator.getBodyFixedFrame(); spatialAccelerationCalculator.getRelativeAcceleration(spatialAccelerationOfMeasurementLink, elevator, measurementLink); spatialAccelerationOfMeasurementLink.changeFrame(elevatorFrame, twistOfMeasurementLink, twistOfMeasurementLink); twistOfMeasurementLink.changeFrame(elevatorFrame);
spatialAccelerationCalculator.compute(); computeCoMVelocity(centroidalMomentumMatrixDense); spatialAccelerationCalculator.getAccelerationOfBody(tempSpatialAcceleration, rigidBody);
@Override public void computeAchievedAcceleration() { spatialAccelerationCalculator.getRelativeAcceleration(endEffectorAchievedAcceleration, base, endEffector); endEffectorAchievedAcceleration.getAngularPart(achievedAngularAcceleration); yoAchievedAngularAcceleration.setAndMatchFrame(achievedAngularAcceleration); }
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); }
private void addAllSuccedingJoints(RigidBody body) { for (InverseDynamicsJoint inverseDynamicsJoint : body.getChildrenJoints()) { if (inverseDynamicsJoint.getSuccessor() != null) { allJoints.add(inverseDynamicsJoint); addAllSuccedingJoints(inverseDynamicsJoint.getSuccessor()); } } }
private void computeUnbiasedEstimatedMeasurement(SpatialAccelerationCalculator spatialAccelerationCalculator, FrameVector estimatedMeasurement) { tempFramePoint.setToZero(measurementFrame); spatialAccelerationCalculator.getLinearAccelerationOfBodyFixedPoint(estimatedMeasurement, measurementLink, tempFramePoint); estimatedMeasurement.changeFrame(gravitationalAcceleration.getReferenceFrame()); estimatedMeasurement.sub(gravitationalAcceleration); estimatedMeasurement.changeFrame(measurementFrame); } }
public void computeAchievedAcceleration() { spatialAccelerationCalculator.getRelativeAcceleration(endEffectorAchievedAcceleration, base, endEffector); endEffectorAchievedAcceleration.getAngularPart(achievedAngularAcceleration); endEffectorAchievedAcceleration.getLinearPart(achievedLinearAcceleration); yoAchievedAngularAcceleration.setAndMatchFrame(achievedAngularAcceleration); yoAchievedLinearAcceleration.setAndMatchFrame(achievedLinearAcceleration); }
public void updateInternalState() { twistCalculator.compute(); spatialAccelerationCalculator.compute(); }
public FullInverseDynamicsStructure(RigidBody elevator, RigidBody estimationLink, FloatingInverseDynamicsJoint rootInverseDynamicsJoint) { this.elevator = elevator; this.rootJoint = rootInverseDynamicsJoint; twistCalculator = new TwistCalculator(ReferenceFrame.getWorldFrame(), elevator); spatialAccelerationCalculator = new SpatialAccelerationCalculator(elevator, twistCalculator, 0.0, false); this.estimationLink = estimationLink; }
public void getCoMAcceleration(FrameVector comAccelerationToPack) { boolean firstIteration = true; double totalMass = 0.0; for (RigidBody rigidBody : rigidBodies) { double mass = rigidBody.getInertia().getMass(); rigidBody.getCoMOffset(comLocation); spatialAccelerationCalculator.getLinearAccelerationOfBodyFixedPoint(linkLinearMomentumDot, base, rigidBody, comLocation); linkLinearMomentumDot.scale(mass); if (firstIteration) comAccelerationToPack.setIncludingFrame(linkLinearMomentumDot); else comAccelerationToPack.add(linkLinearMomentumDot); totalMass += mass; firstIteration = false; } comAccelerationToPack.scale(1.0 / totalMass); } }
spatialAccelerationCalculator.getRelativeAcceleration(tempRootToEstimationAcceleration, estimationLink, rootJoint.getSuccessor()); tempRootToEstimationAcceleration.changeFrameNoRelativeMotion(rootJoint.getFrameAfterJoint()); tempRootToEstimationAcceleration.getAngularPart(tempRootToEstimationAngularAcceleration);
public void doControl() { boolean updateRootJoints = true; generator.updateInverseDynamicsRobotModelFromRobot(updateRootJoints, false); perfectTwistCalculator.compute(); perfectSpatialAccelerationCalculator.compute(); updateGroundTruth(); centerOfMassAccelerationFromFullRobotModelStealer.run(); angularAccelerationFromRobotStealer.run(); }
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 void startComputation() { imuFramePoint.setToZero(measurementFrame); spatialAccelerationCalculator.getLinearAccelerationOfBodyFixedPoint(linearAccelerationFrameVector, rigidBody, imuFramePoint); linearAccelerationFrameVector.changeFrame(gravitationalAcceleration.getReferenceFrame()); linearAccelerationFrameVector.add(gravitationalAcceleration); linearAccelerationFrameVector.changeFrame(measurementFrame); linearAccelerationFrameVector.get(linearAcceleration); yoFrameVectorPerfect.set(linearAcceleration); corrupt(linearAcceleration); yoFrameVectorNoisy.set(linearAcceleration); linearAccelerationOutputPort.setData(linearAcceleration); }
ReferenceFrame elevatorFrame = elevator.getBodyFixedFrame(); twistCalculator.getRelativeTwist(twistOfMeasurementLink, elevator, measurementLink); spatialAccelerationCalculator.getRelativeAcceleration(spatialAccelerationOfMeasurementLink, elevator, measurementLink); spatialAccelerationOfMeasurementLink.changeFrame(elevatorFrame, twistOfMeasurementLink, twistOfMeasurementLink); spatialAccelerationCalculator.getRelativeAcceleration(spatialAccelerationOfMeasurementLink, elevator, measurementLink); twistOfMeasurementWithRespectToEstimation.changeFrame(measurementLink.getBodyFixedFrame()); twistOfMeasurementLink.changeFrame(measurementLink.getBodyFixedFrame());