public CenterOfMassReferenceFrame(String frameName, ReferenceFrame parentFrame, RigidBody rootBody) { super(frameName, parentFrame); centerOfMassCalculator = new CenterOfMassCalculator(rootBody, parentFrame); centerOfMass = new FramePoint(parentFrame); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { centerOfMassCalculator.compute(); centerOfMassCalculator.getCenterOfMass(centerOfMass); centerOfMassVector3d.set(centerOfMass.getPoint()); transformToParent.setIdentity(); transformToParent.setTranslation(centerOfMassVector3d); }
spinePitchZUpFrameViz.setToReferenceFrame(spinePitchCenterOfMassCalculator.getDesiredFrame()); leftHipPitchZUpFrameViz.setToReferenceFrame(leftHipPitchCenterOfMassCalculator.getDesiredFrame()); leftHipPitchFrameViz.setToReferenceFrame(fullRobotModel.getLegJoint(RobotSide.LEFT, LegJointName.HIP_PITCH).getFrameAfterJoint()); spinePitchCenterOfMassCalculator.getDesiredFrame().update(); leftHipPitchCenterOfMassCalculator.getDesiredFrame().update(); rightHipPitchCenterOfMassCalculator.getDesiredFrame().update(); leftKneeCenterOfMassCalculator.getDesiredFrame().update(); rightKneeCenterOfMassCalculator.getDesiredFrame().update(); spinePitchCenterOfMassCalculator.compute(); spinePitchCenterOfMassCalculator.getCenterOfMass(tempFramePoint); spinePitchCoMInZUpFrame.set(tempFramePoint); leftHipPitchCenterOfMassCalculator.compute(); leftHipPitchCenterOfMassCalculator.getCenterOfMass(tempFramePoint); leftHipPitchCoMInZUpFrame.set(tempFramePoint); rightHipPitchCenterOfMassCalculator.compute(); rightHipPitchCenterOfMassCalculator.getCenterOfMass(tempFramePoint); rightHipPitchCoMInZUpFrame.set(tempFramePoint); leftKneeCenterOfMassCalculator.compute(); leftKneeCenterOfMassCalculator.getCenterOfMass(tempFramePoint); leftKneeCoMInZUpFrame.set(tempFramePoint); rightKneeCenterOfMassCalculator.compute(); rightKneeCenterOfMassCalculator.getCenterOfMass(tempFramePoint); rightKneeCoMInZUpFrame.set(tempFramePoint);
private void updateCenterOfMass() { distalMassCalc.compute(); distalMass.set(distalMassCalc.getTotalMass()); distalMassForceInWorld.set(0.0, 0.0, Math.abs(GRAVITY) * distalMass.getDoubleValue()); FramePoint distalCoMinWorld = distalMassCalc.getCenterOfMass(); distalCoMInWorld.set(distalCoMinWorld); lowPassSensorForceZ.update(yoSensorForce.getZ()); } }
public void update() { centerOfMassCalculator.getDesiredFrame().update(); centerOfMassCalculator.compute(); centerOfMassCalculator.getCenterOfMass(centerOfMassPosition); belowJointCoMInZUpFrame.set(centerOfMassPosition); jointAxis.setIncludingFrame(parentJoint.getJointAxis()); jointAxis.changeFrame(parentJoint.getFrameAfterJoint()); jointAxisInWorld.setIncludingFrame(jointAxis); jointAxisInWorld.changeFrame(ReferenceFrame.getWorldFrame()); yoJointAxis.set(jointAxisInWorld); centerOfMassPosition.changeFrame(jointAxis.getReferenceFrame()); jointToCenterOfMass.setIncludingFrame(centerOfMassPosition); jointToCenterOfMassInWorld.setIncludingFrame(jointToCenterOfMass); jointToCenterOfMassInWorld.changeFrame(ReferenceFrame.getWorldFrame()); yoJointToCenterOfMass.set(jointToCenterOfMassInWorld); totalMass.set(centerOfMassCalculator.getTotalMass()); forceVector.setIncludingFrame(ReferenceFrame.getWorldFrame(), 0.0, 0.0, -9.81 * totalMass.getDoubleValue()); forceVector.changeFrame(jointAxis.getReferenceFrame()); FrameVector forceVectorInWorld = new FrameVector(forceVector); forceVectorInWorld.changeFrame(ReferenceFrame.getWorldFrame()); yoForceVector.set(forceVectorInWorld); rCrossFVector.setToZero(jointAxis.getReferenceFrame()); rCrossFVector.cross(forceVector, jointToCenterOfMass); estimatedTorque.set(rCrossFVector.dot(jointAxis)); if (isSpineJoint) estimatedTorque.mul(-1.0); }
private void computeEstimationLinkToWorldTransform(ReferenceFrame estimationFrame, RigidBodyTransform estimationLinkToWorldToPack, FramePoint centerOfMassWorld, FrameOrientation estimationLinkOrientation) { // r^{estimation} tempCenterOfMassBody.setToZero(estimationFrame); centerOfMassCalculator.getCenterOfMass(tempCenterOfMassBody); tempCenterOfMassBody.changeFrame(estimationFrame); // R_{estimation}^{w} estimationLinkOrientation.changeFrame(worldFrame); estimationLinkOrientation.getTransform3D(estimationLinkToWorldToPack); // R_{estimation}^{w} * r^{estimation} tempCenterOfMassBody.get(tempCenterOfMassBodyVector3d); estimationLinkToWorldToPack.transform(tempCenterOfMassBodyVector3d); // p_{estimation}^{w} = r^{w} - R_{estimation}^{w} r^{estimation} centerOfMassWorld.get(tempEstimationLinkPosition); tempEstimationLinkPosition.sub(tempCenterOfMassBodyVector3d); // H_{estimation}^{w} tempEstimationLinkPositionVector3d.set(tempEstimationLinkPosition); estimationLinkToWorldToPack.setTranslation(tempEstimationLinkPositionVector3d); }
public void run() { FullInverseDynamicsStructure inverseDynamicsStructure = inverseDynamicsStructureInputPort.getData(); FloatingInverseDynamicsJoint rootJoint = inverseDynamicsStructure.getRootJoint(); ReferenceFrame estimationFrame = inverseDynamicsStructure.getEstimationFrame(); centerOfMassCalculator.compute(); updateRootJointPosition(rootJoint, estimationFrame, centerOfMassPositionOutputPort.getData()); rootJoint.getFrameAfterJoint().update(); TwistCalculator twistCalculator = inverseDynamicsStructure.getTwistCalculator(); updateRootJointTwistLinearPart(centerOfMassVelocityOutputPort.getData(), rootJoint); twistCalculator.compute(); }
spinePitchCoMInZUpFrame = new YoFramePoint("spinePitchCoMInZUpFrame", spinePitchCenterOfMassCalculator.getDesiredFrame(), registry); leftHipPitchCoMInZUpFrame = new YoFramePoint("leftHipPitchCoMInZUpFrame", leftHipPitchCenterOfMassCalculator.getDesiredFrame(), registry); rightHipPitchCoMInZUpFrame = new YoFramePoint("rightHipPitchCoMInZUpFrame", rightHipPitchCenterOfMassCalculator.getDesiredFrame(), registry); leftKneeCoMInZUpFrame = new YoFramePoint("leftKneeCoMInZUpFrame", leftKneeCenterOfMassCalculator.getDesiredFrame(), registry); rightKneeCoMInZUpFrame = new YoFramePoint("rightKneeCoMInZUpFrame", rightKneeCenterOfMassCalculator.getDesiredFrame(), registry);
private void computeEstimationLinkTransform(ReferenceFrame estimationFrame, RigidBodyTransform estimationLinkToWorldToPack, FramePoint centerOfMassWorld, FrameOrientation estimationLinkOrientation) { // r^{estimation} tempCenterOfMassBody.setToZero(estimationFrame); centerOfMassCalculator.getCenterOfMass(tempCenterOfMassBody); tempCenterOfMassBody.changeFrame(estimationFrame); // R_{estimation}^{w} estimationLinkOrientation.changeFrame(worldFrame); estimationLinkOrientation.getTransform3D(estimationLinkToWorldToPack); // R_{estimation}^{w} * r^{estimation} tempCenterOfMassBody.get(tempCenterOfMassBodyVector3d); estimationLinkToWorldToPack.transform(tempCenterOfMassBodyVector3d); // p_{estimation}^{w} = r^{w} - R_{estimation}^{w} r^{estimation} centerOfMassWorld.get(tempEstimationLinkPosition); tempEstimationLinkPosition.sub(tempCenterOfMassBodyVector3d); // H_{estimation}^{w} tempEstimationLinkPositionVector3d.set(tempEstimationLinkPosition); estimationLinkToWorldToPack.setTranslation(tempEstimationLinkPositionVector3d); }
public void run() { FullInverseDynamicsStructure inverseDynamicsStructure = inverseDynamicsStructureInputPort.getData(); FloatingInverseDynamicsJoint rootJoint = inverseDynamicsStructure.getRootJoint(); ReferenceFrame estimationFrame = inverseDynamicsStructure.getEstimationFrame(); centerOfMassCalculator.compute(); updateRootJointConfiguration(rootJoint, estimationFrame); rootJoint.getFrameAfterJoint().update(); TwistCalculator twistCalculator = inverseDynamicsStructure.getTwistCalculator(); SpatialAccelerationCalculator spatialAccelerationCalculator = inverseDynamicsStructure.getSpatialAccelerationCalculator(); updateRootJointTwistAndSpatialAcceleration(twistCalculator, spatialAccelerationCalculator); twistCalculator.compute(); spatialAccelerationCalculator.compute(); }
public DiagnosticsWhenHangingHelper(OneDoFJoint parentJoint, boolean preserveY, boolean isSpineJoint, SideDependentList<InverseDynamicsJoint> topLegJointsIfSpine, YoVariableRegistry registry) { this.parentJoint = parentJoint; this.isSpineJoint = isSpineJoint; centerOfMassCalculator = createCenterOfMassCalculatorInJointZUpFrame(parentJoint, preserveY, isSpineJoint, topLegJointsIfSpine); belowJointCoMInZUpFrame = new YoFramePoint(parentJoint.getName() + "CoMInZUpFrame", centerOfMassCalculator.getDesiredFrame(), registry); centerOfMassPosition = new FramePoint(centerOfMassCalculator.getDesiredFrame()); yoJointAxis = new YoFrameVector(parentJoint.getName() + "JointAxis", ReferenceFrame.getWorldFrame(), registry); yoJointToCenterOfMass = new YoFrameVector(parentJoint.getName() + "JointToCoM", ReferenceFrame.getWorldFrame(), registry); yoForceVector = new YoFrameVector(parentJoint.getName() + "ForceVector", ReferenceFrame.getWorldFrame(), registry); estimatedTorque = new DoubleYoVariable("tau_est_" + parentJoint.getName(), registry); torqueOffset = new DoubleYoVariable("tau_off_" + parentJoint.getName(), registry); appliedTorque = new DoubleYoVariable("tau_app_" + parentJoint.getName(), registry); totalMass = new DoubleYoVariable("totalMass_" + parentJoint.getName(), registry); torqueCorrectionAlpha = new DoubleYoVariable("torqueCorrectionAlpha_" + parentJoint.getName(), registry); torqueCorrectionAlpha.set(0.001); }
@Override public void update(double time) { comCalculator.compute(); centerOfMass.set(comCalculator.getCenterOfMass()); momentumCalculator.computeAndPack(momentum); momentum.getLinearPart(frameVector); linearMomentum.set(frameVector); } }
public CenterOfMassReferenceFrame(String frameName, ReferenceFrame parentFrame, RigidBody[] rigidBodies) { super(frameName, parentFrame); centerOfMassCalculator = new CenterOfMassCalculator(rigidBodies, parentFrame); centerOfMass = new FramePoint(parentFrame); }
private void computeRootJointLinearVelocity(FrameVector centerOfMassVelocityWorld, FrameVector rootJointVelocityToPack, FrameVector rootJointAngularVelocity, FloatingInverseDynamicsJoint rootJoint) { tempCenterOfMassVelocityWorld.setIncludingFrame(centerOfMassVelocityWorld); ReferenceFrame rootJointFrame = rootJoint.getFrameAfterJoint(); // \dot{r}^{root} centerOfMassJacobianBody.compute(); tempComVelocityBody.setToZero(rootJointFrame); centerOfMassJacobianBody.getCenterOfMassVelocity(tempComVelocityBody); tempComVelocityBody.changeFrame(rootJointFrame); // \tilde{\omega} r^{root} tempComBody.setToZero(rootJointFrame); centerOfMassCalculator.getCenterOfMass(tempComBody); tempComBody.changeFrame(rootJointFrame); tempCrossPart.setToZero(rootJointFrame); tempCrossPart.cross(rootJointAngularVelocity, tempComBody); // v_{r/p}= \tilde{\omega} r^{root} + \dot{r}^{root} tempCenterOfMassVelocityOffset.setToZero(rootJointFrame); tempCenterOfMassVelocityOffset.add(tempCrossPart, tempComVelocityBody); // v_{root}^{p,w} = R_{w}^{root} \dot{r} - v_{r/p} tempCenterOfMassVelocityWorld.changeFrame(rootJointFrame); rootJointVelocityToPack.setIncludingFrame(tempCenterOfMassVelocityWorld); rootJointVelocityToPack.sub(tempCenterOfMassVelocityOffset); }
private void updateGroundTruth() { FramePoint com = new FramePoint(); perfectCenterOfMassCalculator.compute(); perfectCenterOfMassCalculator.getCenterOfMass(com); perfectCoM.set(com); FrameVector comd = new FrameVector(); perfectCenterOfMassJacobian.compute(); perfectCenterOfMassJacobian.getCenterOfMassVelocity(comd); comd.changeFrame(ReferenceFrame.getWorldFrame()); perfectCoMd.set(comd); FrameVector comdd = new FrameVector(); perfectCenterOfMassAccelerationCalculator.getCoMAcceleration(comdd); comdd.changeFrame(ReferenceFrame.getWorldFrame()); perfectCoMdd.set(comdd); }
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.getCenterOfMass(tempComBody); tempComBody.changeFrame(rootJointFrame); tempCrossPart.setToZero(rootJointFrame);
private void initializeRobotState() { reinitialize.set(false); centerOfMassCalculator.compute(); centerOfMassCalculator.getCenterOfMass(tempCenterOfMassPosition); tempCenterOfMassPosition.changeFrame(worldFrame); yoInitialCenterOfMassPosition.set(tempCenterOfMassPosition); if (!initializeToActual && DRCKinematicsBasedStateEstimator.INITIALIZE_HEIGHT_WITH_FOOT) { RigidBody foot = feet.get(0); footPositionInWorld.setToZero(footFrames.get(foot)); footPositionInWorld.changeFrame(worldFrame); yoInitialFootPosition.set(footPositionInWorld); double footToCoMZ = tempCenterOfMassPosition.getZ() - footPositionInWorld.getZ(); yoInitialComHeight.set(footToCoMZ); tempCenterOfMassPosition.setZ(tempCenterOfMassPosition.getZ() - footToCoMZ); } tempFrameVector.setIncludingFrame(tempCenterOfMassPosition); rootJointPosition.set(centerOfMassPosition); rootJointPosition.sub(tempFrameVector); yoRootJointPosition.set(rootJointPosition); rootJointVelocity.setToZero(worldFrame); yoRootJointVelocity.setToZero(); kinematicsBasedLinearStateCalculator.initialize(rootJointPosition); }
CenterOfMassCalculator centerOfMassCalculator = new CenterOfMassCalculator(rigidBodies, jointZUpFrame);
private void updateCoMState() centerOfMassCalculator.compute(); centerOfMassCalculator.getCenterOfMass(centerOfMassPosition); centerOfMassPosition.changeFrame(worldFrame); yoCenterOfMassPosition.set(centerOfMassPosition);