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);
@Override public void update(double time) 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();
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); }
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); }