@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()); } }
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); }
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); }
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); }
spinePitchCenterOfMassCalculator.getCenterOfMass(tempFramePoint); spinePitchCoMInZUpFrame.set(tempFramePoint); leftHipPitchCenterOfMassCalculator.getCenterOfMass(tempFramePoint); leftHipPitchCoMInZUpFrame.set(tempFramePoint); rightHipPitchCenterOfMassCalculator.getCenterOfMass(tempFramePoint); rightHipPitchCoMInZUpFrame.set(tempFramePoint); leftKneeCenterOfMassCalculator.getCenterOfMass(tempFramePoint); leftKneeCoMInZUpFrame.set(tempFramePoint); rightKneeCenterOfMassCalculator.getCenterOfMass(tempFramePoint); rightKneeCoMInZUpFrame.set(tempFramePoint);
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 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 updateCoMState() centerOfMassCalculator.getCenterOfMass(centerOfMassPosition); centerOfMassPosition.changeFrame(worldFrame); yoCenterOfMassPosition.set(centerOfMassPosition);
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); }
centerOfMassCalculator.getCenterOfMass(tempComBody); tempComBody.changeFrame(rootJointFrame); tempCrossPart.setToZero(rootJointFrame);