@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { centerOfMassCalculator.compute(); centerOfMassCalculator.getCenterOfMass(centerOfMass); centerOfMassVector3d.set(centerOfMass.getPoint()); transformToParent.setIdentity(); transformToParent.setTranslation(centerOfMassVector3d); }
@Override public void update(double time) { comCalculator.compute(); centerOfMass.set(comCalculator.getCenterOfMass()); momentumCalculator.computeAndPack(momentum); momentum.getLinearPart(frameVector); linearMomentum.set(frameVector); } }
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 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(); }
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(); }
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 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); }
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); }
private void updateCoMState() centerOfMassCalculator.compute(); centerOfMassCalculator.getCenterOfMass(centerOfMassPosition); centerOfMassPosition.changeFrame(worldFrame);
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); }