public void setRobotTorquesToMatchFullRobotModel() { FloatingJointBasics rootJoint = fullRobotModel.getRootJoint(); ReferenceFrame bodyFixedFrame = fullRobotModel.getPelvis().getBodyFixedFrame(); Wrench rootJointWrench = new Wrench(bodyFixedFrame, bodyFixedFrame); rootJointWrench.setIncludingFrame(rootJoint.getJointWrench()); rootJointWrench.changeFrame(bodyFixedFrame); FrameVector3D rootJointForce = new FrameVector3D(rootJointWrench.getLinearPart()); FrameVector3D rootJointTorque = new FrameVector3D(rootJointWrench.getAngularPart()); rootJointForce.changeFrame(ReferenceFrame.getWorldFrame()); rootJointTorque.changeFrame(ReferenceFrame.getWorldFrame()); computedRootJointForces.set(rootJointForce); computedRootJointTorques.set(rootJointTorque); rootJointExternalForcePoint.setForce(new Vector3D(rootJointForce)); rootJointExternalForcePoint.setMoment(new Vector3D(rootJointTorque)); FramePoint3D rootJointPosition = new FramePoint3D(bodyFixedFrame); rootJointPosition.changeFrame(ReferenceFrame.getWorldFrame()); rootJointExternalForcePoint.setOffsetWorld(rootJointPosition); ArrayList<OneDegreeOfFreedomJoint> oneDegreeOfFreedomJoints = new ArrayList<OneDegreeOfFreedomJoint>(); robot.getAllOneDegreeOfFreedomJoints(oneDegreeOfFreedomJoints); for (OneDegreeOfFreedomJoint oneDegreeOfFreedomJoint : oneDegreeOfFreedomJoints) { OneDoFJointBasics oneDoFJoint = fullRobotModel.getOneDoFJointByName(oneDegreeOfFreedomJoint.getName()); double inverseDynamicsTorque = oneDoFJoint.getTau(); oneDegreeOfFreedomJoint.setTau(inverseDynamicsTorque); YoDouble computedJointTorque = computedJointTorques.get(oneDoFJoint); computedJointTorque.set(inverseDynamicsTorque); } }
public void setRobotsExternalForcesToMatchFullRobotModel(InverseDynamicsCalculator inverseDynamicsCalculator) { for (RobotSide robotSide : RobotSide.values) { RigidBodyBasics foot = fullRobotModel.getFoot(robotSide); Wrench wrench = new Wrench(inverseDynamicsCalculator.getExternalWrench(foot)); ReferenceFrame bodyFixedFrame = foot.getBodyFixedFrame(); FramePoint3D pointOfWrenchApplication = new FramePoint3D(bodyFixedFrame); pointOfWrenchApplication.changeFrame(ReferenceFrame.getWorldFrame()); ExternalForcePoint footExternalForcePoint = feetExternalForcePoints.get(robotSide); footExternalForcePoint.setOffsetWorld(pointOfWrenchApplication); FrameVector3D wrenchForce = new FrameVector3D(wrench.getLinearPart()); wrenchForce.changeFrame(ReferenceFrame.getWorldFrame()); FrameVector3D wrenchTorque = new FrameVector3D(wrench.getAngularPart()); wrenchTorque.changeFrame(ReferenceFrame.getWorldFrame()); footExternalForcePoint.setForce(new Vector3D(wrenchForce)); footExternalForcePoint.setMoment(new Vector3D(wrenchTorque)); } }