public ForceSensorToJointTorqueProjector(String namePrefix, ForceSensorData forceSensorData, RigidBody sensorLinkBody) { registry = new YoVariableRegistry(namePrefix+getClass().getSimpleName()); this.forceSensorData = forceSensorData; //ground reaction wrench on joints yoTorqueInJoints = new ArrayList<>(); RigidBody currentBody = sensorLinkBody; for(int i=0;i<numberOfJointFromSensor;i++) { FrameVector jAxis = ((OneDoFJoint)currentBody.getParentJoint()).getJointAxis(); yoTorqueInJoints.add(new ImmutablePair<>(jAxis,new DoubleYoVariable("NegGRFWrenchIn"+ currentBody.getParentJoint().getName(), registry))); currentBody=currentBody.getParentJoint().getPredecessor(); } } @Override
public ForceSensorToJointTorqueProjector(String namePrefix, ForceSensorData forceSensorData, RigidBody sensorLinkBody) { registry = new YoVariableRegistry(namePrefix+getClass().getSimpleName()); this.forceSensorData = forceSensorData; //ground reaction wrench on joints yoTorqueInJoints = new ArrayList<>(); RigidBody currentBody = sensorLinkBody; for(int i=0;i<numberOfJointFromSensor;i++) { FrameVector jAxis = ((OneDoFJoint)currentBody.getParentJoint()).getJointAxis(); yoTorqueInJoints.add(new ImmutablePair<>(jAxis,new DoubleYoVariable("NegGRFWrenchIn"+ currentBody.getParentJoint().getName(), registry))); currentBody=currentBody.getParentJoint().getPredecessor(); } } @Override
private static OneDoFJoint cloneOneDoFJoint(OneDoFJoint original, String cloneSuffix, RigidBody clonePredecessor) { String jointNameOriginal = original.getName(); RigidBodyTransform jointTransform = original.getOffsetTransform3D(); Vector3d jointAxisCopy = original.getJointAxis().getVectorCopy(); OneDoFJoint clone; if (original instanceof RevoluteJoint) clone = ScrewTools.addRevoluteJoint(jointNameOriginal + cloneSuffix, clonePredecessor, jointTransform, jointAxisCopy); else if (original instanceof PrismaticJoint) clone = ScrewTools.addPrismaticJoint(jointNameOriginal + cloneSuffix, clonePredecessor, jointTransform, jointAxisCopy); else throw new RuntimeException("Unhandled type of " + OneDoFJoint.class.getSimpleName() + ": " + original.getClass().getSimpleName()); clone.setJointLimitLower(original.getJointLimitLower()); clone.setJointLimitUpper(original.getJointLimitUpper()); return clone; }
if (anklePitchJoint != null) pitchJointAxis = anklePitchJoint.getJointAxis(); xSignsForCoPControl.put(robotSide, pitchJointAxis.getY()); rollJointAxis = ankleRollJoint.getJointAxis(); ySignsForCoPControl.put(robotSide, rollJointAxis.getY());
joint.getJointAxis(tempFrameVector); tempFrameVector.scale(torque);
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); }
FrameVector elbowJointAxis = elbowJoint.getJointAxis(); zRotationDueToAccountForElbowAxis.setRotationPitchAndZeroTranslation(-elbowJointAxis.angle(expectedElbowAxis)); armZeroJointAngleConfigurationOffset.multiply(zRotationDueToAccountForElbowAxis);
if (hipPitchJoint.getJointAxis().getY() > 0.0)