public void setRobotRootJointExternalForcesRandomly(Random random, double maxRootJointForceAndTorque) { rootJointExternalForcePoint.setForce(RandomGeometry.nextVector3D(random, maxRootJointForceAndTorque)); rootJointExternalForcePoint.setMoment(RandomGeometry.nextVector3D(random, maxRootJointForceAndTorque)); }
public void setInitialForce(Vector3D initialForce, Vector3D initialTorque) { forcePoint.setForce(initialForce); forcePoint.setMoment(initialTorque); this.initialForce.set(initialForce); this.initialTorque.set(initialTorque); hasInitialForce = true; }
public void setRobotExternalForcesRandomly(Random random, double maxGroundContactPointForce, double maxFootExternalForce, double maxFootExternalTorque) { for (RobotSide robotSide : RobotSide.values) { List<GroundContactPoint> footGroundContactPoints = robot.getFootGroundContactPoints(robotSide); for (GroundContactPoint groundContactPoint : footGroundContactPoints) { groundContactPoint.setForce(RandomGeometry.nextVector3D(random, maxGroundContactPointForce)); } ExternalForcePoint footExternalForcePoint = feetExternalForcePoints.get(robotSide); footExternalForcePoint.setForce(RandomGeometry.nextVector3D(random, maxFootExternalForce)); footExternalForcePoint.setMoment(RandomGeometry.nextVector3D(random, maxFootExternalTorque)); } }
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)); } }
forcePoint.setMoment(torqueX, torqueY, torqueZ);