public CenterOfMassReferenceFrame(String frameName, ReferenceFrame parentFrame, RigidBody rootBody) { super(frameName, parentFrame); centerOfMassCalculator = new CenterOfMassCalculator(rootBody, parentFrame); centerOfMass = new FramePoint(parentFrame); }
public CenterOfMassReferenceFrame(String frameName, ReferenceFrame parentFrame, RigidBody[] rigidBodies) { super(frameName, parentFrame); centerOfMassCalculator = new CenterOfMassCalculator(rigidBodies, parentFrame); centerOfMass = new FramePoint(parentFrame); }
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()); }
CenterOfMassCalculator centerOfMassCalculator = new CenterOfMassCalculator(rigidBodies, jointZUpFrame);
public MomentumVisualizer(String name, TwistCalculator twistCalculator, YoVariableRegistry registry, YoGraphicsListRegistry graphicsRegistry, RigidBody... rigidBodies) { comCalculator = new CenterOfMassCalculator(rigidBodies, ReferenceFrame.getWorldFrame()); momentumCalculator = new MomentumCalculator(twistCalculator, rigidBodies); centerOfMass = new YoFramePoint(name + "CoM", ReferenceFrame.getWorldFrame(), registry); linearMomentum = new YoFrameVector(name + "Momentum", ReferenceFrame.getWorldFrame(), registry); YoGraphicPosition yoCoMGraphics = new YoGraphicPosition(name + "CoM", centerOfMass, 0.05, YoAppearance.Brown()); YoGraphicVector yoMomentumGraphics = new YoGraphicVector(name + "Momentum", centerOfMass, linearMomentum, 0.05, YoAppearance.Brown()); graphicsRegistry.registerYoGraphic(name, yoCoMGraphics); graphicsRegistry.registerYoGraphic(name, yoMomentumGraphics); }
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); }
private static CenterOfMassCalculator createCenterOfMassCalculatorInJointZUpFrame(RigidBody rootBody, boolean preserveY) { if (DEBUG) System.out.println("\nCenterOfMassCalibrationTool: rootBody = " + rootBody); InverseDynamicsJoint parentJoint = rootBody.getParentJoint(); if (DEBUG) System.out.println("parentJoint = " + parentJoint); ReferenceFrame jointFrame = parentJoint.getFrameAfterJoint(); if (DEBUG) System.out.println("jointFrame = " + jointFrame); String jointName = parentJoint.getName(); if (DEBUG) System.out.println("jointName = " + jointName); ReferenceFrame jointZUpFrame; if (preserveY) { jointZUpFrame = new ZUpPreserveYReferenceFrame(ReferenceFrame.getWorldFrame(), jointFrame, jointName + "ZUp"); } else { jointZUpFrame = new ZUpFrame(ReferenceFrame.getWorldFrame(), jointFrame, jointName + "ZUp"); } RigidBody[] rigidBodies = ScrewTools.computeSubtreeSuccessors(rootBody.getParentJoint()); CenterOfMassCalculator centerOfMassCalculator = new CenterOfMassCalculator(rigidBodies, jointZUpFrame); return centerOfMassCalculator; } }
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()); }
public ForceSensorDistalMassCompensator(ForceSensorDefinition forceSensorDefinition, double dtForLowpassFilter, YoVariableRegistry registry) { String sensorName = forceSensorDefinition.getSensorName(); InverseDynamicsJoint parentJointOfSensorBody = forceSensorDefinition.getRigidBody().getParentJoint(); sensorFrame = forceSensorDefinition.getSensorFrame(); sensorPose = new FramePose(world); yoSensorPositionInWorld = new YoFramePoint(sensorName + "Position", world, registry); distalMassCalc = new CenterOfMassCalculator(ScrewTools.computeRigidBodiesAfterThisJoint(parentJointOfSensorBody), world); distalMass = new DoubleYoVariable(sensorName + "DistalMass", registry); lowPassSensorForceZ = new AlphaFilteredYoVariable(sensorName + "LowPassFz", registry, AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(0.0001, dtForLowpassFilter)); distalMassForceInWorld = new YoFrameVector(sensorName + "DistalWeight", world, registry); distalCoMInWorld = new YoFramePoint(sensorName + "DistalCoM", world, registry); yoSensorToDistalCoMvectorInWorld = new YoFrameVector(sensorName + "ToDistalCoM", world, registry); distalMassWrench = new Wrench(sensorFrame, world); // Put sensor values in world frame since it's easy to interpret from looking at GUI yoSensorForce = new YoFrameVector(sensorName + "Force", world, registry); yoSensorTorque = new YoFrameVector(sensorName + "Torque", world, registry); yoSensorForceFromDistalMass = new YoFrameVector(sensorName + "ForceDueToDistalMass", world, registry); yoSensorTorqueFromDistalMass = new YoFrameVector(sensorName + "TorqueDueToDistalMass", world, registry); yoSensorForceMassCompensated = new YoFrameVector(sensorName + "ForceMassCompensated", world, registry); yoSensorTorqueMassCompensated = new YoFrameVector(sensorName + "TorqueMassCompensated", world, registry); addSimulatedSensorNoise = new BooleanYoVariable(sensorName + "AddSimulatedNoise", registry); addSimulatedSensorNoise.set(false); }
this.centerOfMassCalculator = new CenterOfMassCalculator(elevator, rootJointFrame); this.centerOfMassJacobianWorld = new CenterOfMassJacobian(elevator);