public RandomFloatingChain(Random random, Vector3d[] jointAxes) { ReferenceFrame elevatorFrame = ReferenceFrame.constructFrameWithUnchangingTransformToParent("elevator", ReferenceFrame.getWorldFrame(), new RigidBodyTransform()); elevator = new RigidBody("elevator", elevatorFrame); rootJoint = new SixDoFJoint("rootJoint", elevator, elevatorFrame); RigidBody rootBody = ScrewTestTools.addRandomRigidBody("rootBody", random, rootJoint); revoluteJoints = new ArrayList<RevoluteJoint>(); ScrewTestTools.createRandomChainRobot("test", revoluteJoints, rootBody, jointAxes, random); inverseDynamicsJoints.add(rootJoint); inverseDynamicsJoints.addAll(revoluteJoints); }
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 void integrateAccelerations(SixDoFJoint rootJoint, double dt) { SpatialAccelerationVector deltaTwist = new SpatialAccelerationVector(); rootJoint.getJointAcceleration(deltaTwist); deltaTwist.scale(dt); Twist rootJointTwist = new Twist(); rootJoint.getJointTwist(rootJointTwist); rootJointTwist.angularPart.add(deltaTwist.angularPart); rootJointTwist.linearPart.add(deltaTwist.linearPart); rootJoint.setJointTwist(rootJointTwist); }
public void get(double[] buffer, int offset) { inverseDynamicsJoint.getRotation(rotation); inverseDynamicsJoint.getTranslation(translation); inverseDynamicsJoint.getJointTwist(twist); buffer[offset + 0] = rotation.getW(); buffer[offset + 1] = rotation.getX(); buffer[offset + 2] = rotation.getY(); buffer[offset + 3] = rotation.getZ(); buffer[offset + 4] = translation.getX(); buffer[offset + 5] = translation.getY(); buffer[offset + 6] = translation.getZ(); twist.getArray(buffer, offset + 7); } }
joint.getRotation(previousOrientation); joint.getTranslation(previousTranslation); joint.getLinearVelocity(previousLinearVelocity); joint.getAngularVelocity(previousAngularVelocity);
joint.getRotation(previousOrientation); joint.getTranslation(previousTranslation);
RigidBody rootBody = jointOriginal.getPredecessor(); originalToClonedRigidBodies.put(rootBody, rootBodyCopy); String jointNameOriginal = jointOriginal.getName(); SixDoFJoint jointCopy = new SixDoFJoint(jointNameOriginal + suffix, rootBodyCopy, rootBodyFrame); cloned[i] = jointCopy;
public static void main(String[] args) SixDoFJoint rootJoint = new SixDoFJoint("sixdof", elevator, ReferenceFrame.getWorldFrame()); RigidBodyTransform inertiaPose = new RigidBodyTransform(); Matrix3d momentOfInertia = new Matrix3d(); System.out.println(momentOfInertia); RigidBody aBody = ScrewTools.addRigidBody("test", rootJoint, momentOfInertia, mass, inertiaPose); rootJoint.updateFramesRecursively();
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; }
@Override public void setQddDesired(InverseDynamicsJoint originalJoint) { SixDoFJoint sixDoFOriginalJoint = checkAndGetAsSiXDoFJoint(originalJoint); jointAccelerationDesired.setAngularPart(sixDoFOriginalJoint.jointAccelerationDesired.getAngularPart()); jointAccelerationDesired.setLinearPart(sixDoFOriginalJoint.jointAccelerationDesired.getLinearPart()); }
public void get(double[] buffer, int offset) { inverseDynamicsJoint.getRotation(rotation); inverseDynamicsJoint.getTranslation(translation); inverseDynamicsJoint.getJointTwist(twist); buffer[offset + 0] = rotation.getW(); buffer[offset + 1] = rotation.getX(); buffer[offset + 2] = rotation.getY(); buffer[offset + 3] = rotation.getZ(); buffer[offset + 4] = translation.getX(); buffer[offset + 5] = translation.getY(); buffer[offset + 6] = translation.getZ(); twist.getArray(buffer, offset + 7); } }
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; }
soleFrames.set(robotSide, soleFrame); SixDoFJoint sixDoFJoint = new SixDoFJoint("SixDofJoint" + robotSide, rootBody, rootBody.getBodyFixedFrame()); sixDoFJoints.set(robotSide, sixDoFJoint);
FloatingInverseDynamicsJoint currentIDJoint = new SixDoFJoint(currentJoint.getName(), elevator, ReferenceFrame.getWorldFrame()); ScrewTools.addRigidBody(currentJoint.getName(), currentIDJoint, momentOfInertia, mass, comOffset);
public ProvidedMassMatrixToolRigidBody(RobotSide robotSide, final FullHumanoidRobotModel fullRobotModel, double gravity, ArmControllerParameters armControllerParameters, YoVariableRegistry parentRegistry, YoGraphicsListRegistry yoGraphicsListRegistry) { String name = robotSide.getCamelCaseNameForStartOfExpression() + "Tool"; this.registry = new YoVariableRegistry(name); this.fullRobotModel = fullRobotModel; this.gravity = gravity; this.handFixedFrame = fullRobotModel.getHand(robotSide).getBodyFixedFrame(); this.handControlFrame = fullRobotModel.getHandControlFrame(robotSide); this.elevatorFrame = fullRobotModel.getElevatorFrame(); toolFrame = new PoseReferenceFrame(name + "Frame", elevatorFrame); RigidBodyInertia inertia = new RigidBodyInertia(toolFrame, new Matrix3d(), 0.0); this.toolJoint = new SixDoFJoint(name + "Joint", fullRobotModel.getElevator(), fullRobotModel.getElevator().getBodyFixedFrame()); this.toolBody = new RigidBody(name + "Body", inertia, toolJoint); objectCenterOfMass = new YoFramePoint(name + "CoMOffset", handControlFrame, registry); objectMass = new DoubleYoVariable(name + "ObjectMass", registry); objectForceInWorld = new YoFrameVector(name + "Force", ReferenceFrame.getWorldFrame(), registry); this.objectCenterOfMassInWorld = new YoFramePoint(name + "CoMInWorld", ReferenceFrame.getWorldFrame(), registry); if (yoGraphicsListRegistry != null) { YoGraphicsList yoGraphicsList = new YoGraphicsList(name); YoGraphic comViz = new YoGraphicPosition(name + "CenterOfMassViz", objectCenterOfMassInWorld, 0.05, YoAppearance.Red()); yoGraphicsList.add(comViz); YoGraphic vectorViz = new YoGraphicVector(name + "ForceViz", objectCenterOfMassInWorld, objectForceInWorld, YoAppearance.Yellow()); yoGraphicsList.add(vectorViz); yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList); } parentRegistry.addChild(registry); }