public ConstrainedCenterOfMassJacobianCalculator(FloatingInverseDynamicsJoint rootJoint) { this.dynamicallyConsistentNullspaceCalculator = new OriginalDynamicallyConsistentNullspaceCalculator(rootJoint, true); this.centerOfMassJacobian = new CenterOfMassJacobian(rootJoint.getSuccessor()); }
public PositionStateRobotModelUpdater(ControlFlowInputPort<FullInverseDynamicsStructure> inverseDynamicsStructureInputPort, ControlFlowOutputPort<FramePoint> centerOfMassPositionOutputPort, ControlFlowOutputPort<FrameVector> centerOfMassVelocityOutputPort) { this.inverseDynamicsStructureInputPort = inverseDynamicsStructureInputPort; this.centerOfMassPositionOutputPort = centerOfMassPositionOutputPort; this.centerOfMassVelocityOutputPort = centerOfMassVelocityOutputPort; FullInverseDynamicsStructure inverseDynamicsStructure = inverseDynamicsStructureInputPort.getData(); RigidBody elevator = inverseDynamicsStructure.getElevator(); FloatingInverseDynamicsJoint rootJoint = inverseDynamicsStructure.getRootJoint(); this.centerOfMassCalculator = new CenterOfMassCalculator(elevator, rootJoint.getFrameAfterJoint()); this.centerOfMassJacobianBody = new CenterOfMassJacobian(ScrewTools.computeSupportAndSubtreeSuccessors(elevator), ScrewTools.computeSubtreeJoints(rootJoint.getSuccessor()), rootJoint.getFrameAfterJoint()); }
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 OrientationAndPositionFullRobotModelUpdater(ControlFlowInputPort<FullInverseDynamicsStructure> inverseDynamicsStructureInputPort, ControlFlowOutputPort<FramePoint> centerOfMassPositionPort, ControlFlowOutputPort<FrameVector> centerOfMassVelocityPort, ControlFlowOutputPort<FrameVector> centerOfMassAccelerationPort, ControlFlowOutputPort<FrameOrientation> orientationPort, ControlFlowOutputPort<FrameVector> angularVelocityPort, ControlFlowOutputPort<FrameVector> angularAccelerationPort) { this.inverseDynamicsStructureInputPort = inverseDynamicsStructureInputPort; this.centerOfMassPositionPort = centerOfMassPositionPort; this.centerOfMassVelocityPort = centerOfMassVelocityPort; this.centerOfMassAccelerationPort = centerOfMassAccelerationPort; this.orientationPort = orientationPort; this.angularVelocityPort = angularVelocityPort; this.angularAccelerationPort = angularAccelerationPort; FullInverseDynamicsStructure inverseDynamicsStructure = inverseDynamicsStructureInputPort.getData(); RigidBody elevator = inverseDynamicsStructure.getElevator(); FloatingInverseDynamicsJoint rootJoint = inverseDynamicsStructure.getRootJoint(); this.centerOfMassCalculator = new CenterOfMassCalculator(elevator, rootJoint.getFrameAfterJoint()); this.centerOfMassJacobianBody = new CenterOfMassJacobian(ScrewTools.computeSupportAndSubtreeSuccessors(elevator), ScrewTools.computeSubtreeJoints(rootJoint.getSuccessor()), rootJoint.getFrameAfterJoint()); // TODO: Should pass the input port for the spatial acceleration calculator here too... this.centerOfMassAccelerationCalculator = new CenterOfMassAccelerationCalculator(rootJoint.getSuccessor(), ScrewTools.computeSupportAndSubtreeSuccessors(elevator), inverseDynamicsStructure.getSpatialAccelerationCalculator()); }
this.omega0.set(omega0); this.centerOfMassJacobian = new CenterOfMassJacobian(fullRobotModel.getElevator()); if (yoGraphicsListRegistry != null)
this.centerOfMassJacobianWorld = new CenterOfMassJacobian(elevator);