public static void integrateVelocities(SixDoFJoint sixDoFJoint, double dt) { Twist twist = new Twist(); sixDoFJoint.getJointTwist(twist); Matrix3d rotation = new Matrix3d(); sixDoFJoint.getRotation(rotation); Vector3d position = new Vector3d(); sixDoFJoint.getTranslation(position); integrate(rotation, position, dt, twist); sixDoFJoint.setRotation(rotation); sixDoFJoint.setPosition(position); }
@Override public void setJointPositionVelocityAndAcceleration(InverseDynamicsJoint originalJoint) { SixDoFJoint sixDoFOriginalJoint = checkAndGetAsSiXDoFJoint(originalJoint); setPosition(sixDoFOriginalJoint.jointTranslation); setRotation(sixDoFOriginalJoint.jointRotation); jointTwist.setAngularPart(sixDoFOriginalJoint.jointTwist.getAngularPart()); jointTwist.setLinearPart(sixDoFOriginalJoint.jointTwist.getLinearPart()); jointAcceleration.setAngularPart(sixDoFOriginalJoint.jointAcceleration.getAngularPart()); jointAcceleration.setLinearPart(sixDoFOriginalJoint.jointAcceleration.getLinearPart()); }
public static FramePose setupStanceFoot(SideDependentList<PoseReferenceFrame> soleFrames, SideDependentList<SixDoFJoint> sixDoFJoints) { FramePose startStanceFootPose = new FramePose(worldFrame, new Point3d(0.0, 0.2, 0.0), new Quat4d()); soleFrames.get(RobotSide.LEFT).setPoseAndUpdate(startStanceFootPose); soleFrames.get(RobotSide.LEFT).update(); Point3d stanceAnklePosition = new Point3d(); startStanceFootPose.getPosition(stanceAnklePosition); sixDoFJoints.get(RobotSide.LEFT).setPosition(stanceAnklePosition); return startStanceFootPose; }
public static FramePose setupSwingFoot(SideDependentList<PoseReferenceFrame> soleFrames, SideDependentList<SixDoFJoint> sixDoFJoints) { FramePose startSwingFootPose = new FramePose(worldFrame, new Point3d(0.0, -0.2, 0.0), new Quat4d()); soleFrames.get(RobotSide.RIGHT).setPoseAndUpdate(startSwingFootPose); soleFrames.get(RobotSide.RIGHT).update(); Point3d swingAnklePosition = new Point3d(); startSwingFootPose.getPosition(swingAnklePosition); sixDoFJoints.get(RobotSide.RIGHT).setPosition(swingAnklePosition); return startSwingFootPose; }