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); }
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; }
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 DesiredCoMAccelerationsFromRobotStealerController(ReferenceFrame estimationFrame, double comAccelerationProcessNoiseStandardDeviation, double angularAccelerationProcessNoiseStandardDeviation, InverseDynamicsJointsFromSCSRobotGenerator generator, Joint estimationJoint, double controlDT) { this.estimationFrame = estimationFrame; this.comAccelerationProcessNoiseStandardDeviation = comAccelerationProcessNoiseStandardDeviation; this.angularAccelerationProcessNoiseStandardDeviation = angularAccelerationProcessNoiseStandardDeviation; this.controlDT = controlDT; this.generator = generator; RigidBody elevator = generator.getElevator(); perfectTwistCalculator = new TwistCalculator(ReferenceFrame.getWorldFrame(), elevator); perfectSpatialAccelerationCalculator = new SpatialAccelerationCalculator(elevator, perfectTwistCalculator, 0.0, false); perfectCenterOfMassCalculator = new CenterOfMassCalculator(elevator, ReferenceFrame.getWorldFrame()); perfectCenterOfMassJacobian = new CenterOfMassJacobian(elevator); perfectCenterOfMassAccelerationCalculator = new CenterOfMassAccelerationCalculator(elevator, perfectSpatialAccelerationCalculator); centerOfMassAccelerationFromFullRobotModelStealer = new CenterOfMassAccelerationFromFullRobotModelStealer(elevator, perfectSpatialAccelerationCalculator); angularAccelerationFromRobotStealer = new AngularAccelerationFromRobotStealer(estimationJoint); }
public CentroidalMomentumRateADotVTerm(RigidBody rootBody, ReferenceFrame centerOfMassFrame, CentroidalMomentumMatrix centroidalMomentumMatrix, double robotMass, DenseMatrix64F v) { this.jointsInOrder = ScrewTools.computeSupportAndSubtreeJoints(rootBody); this.robotMass = robotMass; this.centerOfMassFrame = centerOfMassFrame; this.rootBody = rootBody; this.v = v; this.twistCalculator = new TwistCalculator(ReferenceFrame.getWorldFrame(), rootBody); this.centroidalMomentumMatrix = centroidalMomentumMatrix; this.rootAcceleration = new SpatialAccelerationVector(rootBody.getBodyFixedFrame(), ReferenceFrame.getWorldFrame(), rootBody.getBodyFixedFrame()); this.spatialAccelerationCalculator = new SpatialAccelerationCalculator(rootBody, ReferenceFrame.getWorldFrame(), this.rootAcceleration, this.twistCalculator, true, false, false); this.comTwist = new Twist(centerOfMassFrame, rootBody.getBodyFixedFrame(), centerOfMassFrame); comTwist.setToZero(); this.comVelocityVector = new Vector3d(); this.aDotV = new DenseMatrix64F(6, 1); this.tempSpatialAcceleration = new SpatialAccelerationVector(); this.tempMomentum = new Momentum(); this.tempTwist = new Twist(); this.tempCoMTwist = new Twist(); this.tempVector = new Vector3d(0, 0, 0); this.leftSide = new Momentum(centerOfMassFrame); }
this.spatialAccelerationCalculator = new SpatialAccelerationCalculator(rootBody, ReferenceFrame.getWorldFrame(), this.rootAcceleration, this.twistCalculator, true, false, false);