private Twist createEmptyTwist() { return new Twist(bodyFrame, baseFrame, expressedInFrame); }
private void printPinJointInformation(RevoluteJoint revoluteJoint, StringBuffer buffer) { Twist twist = new Twist(); revoluteJoint.getUnitJointTwist(twist); Vector3d jointAxis = twist.getAngularPart(); buffer.append("Joint axis = " + jointAxis + "\n"); buffer.append("Joint is a Pin Joint.\n"); }
private void printSliderJointInformation(PrismaticJoint prismaticJoint, StringBuffer buffer) { Twist twist = new Twist(); prismaticJoint.getUnitJointTwist(twist); Vector3d jointAxis = twist.getLinearPart(); buffer.append("Joint axis = " + jointAxis + "\n"); buffer.append("Joint is a Slider Joint.\n"); }
newAssignedTwist = new Twist(); else newAssignedTwist = unnassignedTwists.remove(unnassignedTwists.size() - 1);
/** * Creates a new {@code TwistCalculator} that will compute all the twists of all the rigid-bodies * of the system to which {@code body} belongs. * * @param inertialFrame non-moving frame with respect to which the twists are computed. * Typically {@link ReferenceFrame#getWorldFrame()} is used here. * @param body a body that belongs to the system this twist calculator will be available for. */ public TwistCalculator(ReferenceFrame inertialFrame, RigidBody body) { this.rootBody = ScrewTools.getRootBody(body); this.rootTwist = new Twist(rootBody.getBodyFixedFrame(), inertialFrame, rootBody.getBodyFixedFrame()); int numberOfRigidBodies = ScrewTools.computeSubtreeSuccessors(ScrewTools.computeSubtreeJoints(rootBody)).length; while (unnassignedTwists.size() < numberOfRigidBodies) unnassignedTwists.add(new Twist()); assignedTwists = new ArrayList<>(numberOfRigidBodies); rigidBodiesWithAssignedTwist = new ArrayList<>(numberOfRigidBodies); assignedTwists.add(rootTwist); rigidBodiesWithAssignedTwist.add(rootBody); rigidBodyToAssignedTwistIndex.put(rootBody, new MutableInt(0)); }
public PlanarJoint(String name, RigidBody predecessor, ReferenceFrame beforeJointFrame) { super(name, predecessor, beforeJointFrame); this.afterJointFrame = new FloatingInverseDynamicsJointReferenceFrame(name, beforeJointFrame); this.jointTwist = new Twist(afterJointFrame, beforeJointFrame, afterJointFrame); this.jointAcceleration = new SpatialAccelerationVector(afterJointFrame, beforeJointFrame, afterJointFrame); this.jointAccelerationDesired = new SpatialAccelerationVector(afterJointFrame, beforeJointFrame, afterJointFrame); }
public SixDoFJoint(String name, RigidBody predecessor, ReferenceFrame beforeJointFrame) { super(name, predecessor, beforeJointFrame); this.afterJointFrame = new FloatingInverseDynamicsJointReferenceFrame(name, beforeJointFrame); this.jointTwist = new Twist(afterJointFrame, beforeJointFrame, afterJointFrame); this.jointAcceleration = new SpatialAccelerationVector(afterJointFrame, beforeJointFrame, afterJointFrame); this.jointAccelerationDesired = new SpatialAccelerationVector(afterJointFrame, beforeJointFrame, afterJointFrame); }
public static Twist generateRandomTwist(Random random, ReferenceFrame bodyFrame, ReferenceFrame baseFrame, ReferenceFrame expressedInFrame, double angularVelocityMagnitude, double linearVelocityMagnitude) { Twist randomTwist = new Twist(bodyFrame, baseFrame, expressedInFrame); randomTwist.setLinearPart(RandomTools.generateRandomVector(random, linearVelocityMagnitude)); randomTwist.setAngularPart(RandomTools.generateRandomVector(random, angularVelocityMagnitude)); return randomTwist; }
public PrismaticJoint(String name, RigidBody predecessor, ReferenceFrame beforeJointFrame, FrameVector jointAxis) { super(name, predecessor, beforeJointFrame, new PrismaticJointReferenceFrame(name, beforeJointFrame, jointAxis)); this.jointAxis = new FrameVector(jointAxis); this.unitJointTwist = new Twist(afterJointFrame, beforeJointFrame, afterJointFrame, jointAxis.getVector(), new Vector3d()); }
public RevoluteJoint(String name, RigidBody predecessor, ReferenceFrame beforeJointFrame, FrameVector jointAxis) { super(name, predecessor, beforeJointFrame, new RevoluteJointReferenceFrame(name, beforeJointFrame, jointAxis)); jointAxis.checkReferenceFrameMatch(beforeJointFrame); this.jointAxis = new FrameVector(jointAxis); unitJointTwist = new Twist(afterJointFrame, beforeJointFrame, afterJointFrame, new Vector3d(), jointAxis.getVector()); }
public static void setRandomVelocity(FloatingInverseDynamicsJoint rootJoint, Random random) { Twist jointTwist = new Twist(); rootJoint.getJointTwist(jointTwist); jointTwist.setAngularPart(RandomTools.generateRandomVector(random)); jointTwist.setLinearPart(RandomTools.generateRandomVector(random)); rootJoint.setJointTwist(jointTwist); }
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 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); }
private boolean exponentialCoordinatesOK() { Twist twist = new Twist(jacobian.getEndEffectorFrame(), jacobian.getBaseFrame(), jacobian.getJacobianFrame(), error); Matrix3d rotationCheck = new Matrix3d(); rotationCheck.setIdentity(); Vector3d positionCheck = new Vector3d(); ScrewTestTools.integrate(rotationCheck, positionCheck, 1.0, twist); RigidBodyTransform transformCheck = new RigidBodyTransform(rotationCheck, positionCheck); return transformCheck.epsilonEquals(errorTransform, 1e-5); }
private void readAndUpdateRootJointAngularAndLinearVelocity() { ReferenceFrame elevatorFrame = rootJoint.getFrameBeforeJoint(); ReferenceFrame pelvisFrame = rootJoint.getFrameAfterJoint(); FrameVector linearVelocity = robot.getRootJointVelocity(); linearVelocity.changeFrame(pelvisFrame); FrameVector angularVelocity = robot.getRootJointAngularVelocityInRootJointFrame(pelvisFrame); angularVelocity.changeFrame(pelvisFrame); Twist bodyTwist = new Twist(pelvisFrame, elevatorFrame, pelvisFrame, linearVelocity.getVector(), angularVelocity.getVector()); rootJoint.setJointTwist(bodyTwist); }
@Override public void setSuccessor(RigidBody successor) { this.successor = successor; ReferenceFrame predecessorFrame = getPredecessor().getBodyFixedFrame(); ReferenceFrame successorFrame = getSuccessor().getBodyFixedFrame(); unitJointTwist = new Twist(afterJointFrame, beforeJointFrame, afterJointFrame, jointAxis.getVector(), new Vector3d()); unitSuccessorTwist = new Twist(unitJointTwist); unitSuccessorTwist.changeBaseFrameNoRelativeTwist(predecessorFrame); unitSuccessorTwist.changeBodyFrameNoRelativeTwist(successorFrame); unitSuccessorTwist.changeFrame(successorFrame); unitPredecessorTwist = new Twist(unitSuccessorTwist); unitPredecessorTwist.invert(); unitPredecessorTwist.changeFrame(predecessorFrame); unitJointAcceleration = new SpatialAccelerationVector(afterJointFrame, beforeJointFrame, afterJointFrame, jointAxis.getVector(), new Vector3d()); unitSuccessorAcceleration = new SpatialAccelerationVector(unitJointAcceleration); unitSuccessorAcceleration.changeBaseFrameNoRelativeAcceleration(predecessorFrame); unitSuccessorAcceleration.changeBodyFrameNoRelativeAcceleration(successorFrame); unitSuccessorAcceleration.changeFrameNoRelativeMotion(successorFrame); unitPredecessorAcceleration = new SpatialAccelerationVector(unitSuccessorAcceleration); unitPredecessorAcceleration.invert(); unitPredecessorAcceleration.changeFrameNoRelativeMotion(predecessorFrame); // actually, there is relative motion, but not in the directions that matter setMotionSubspace(unitSuccessorTwist); }
/** * The implementation for this method generates garbage and is wrong. * Do not use it or fix it. * * The implementation should be something like this: * <p> {@code RigidBodyTransform inverseTransformToRoot = afterJointFrame.getInverseTransformToRoot();} * <p> {@code inverseTransformToRoot.transform(linearVelocityInWorld);} * <p> {@code jointTwist.setLinearPart(linearVelocityInWorld);} * * Sylvain * * @deprecated * @param linearVelocityInWorld */ public void setLinearVelocityInWorld(Vector3d linearVelocityInWorld) { Twist newTwist = new Twist(jointTwist.getBodyFrame(), jointTwist.getBaseFrame(), ReferenceFrame.getWorldFrame()); newTwist.setLinearPart(linearVelocityInWorld); newTwist.changeFrame(jointTwist.getExpressedInFrame()); newTwist.setAngularPart(jointTwist.getAngularPart()); jointTwist.set(newTwist); }
public Twist computeDesiredTwist(FramePose desiredPose, RigidBody endEffector, ReferenceFrame controlFrame, DenseMatrix64F selectionMatrix, MutableDouble errorMagnitude) { errorFramePose.setIncludingFrame(desiredPose); errorFramePose.changeFrame(controlFrame); errorFramePose.getPosition(errorPosition); errorFramePose.getOrientation(errorAxisAngle); errorRotation.set(errorAxisAngle.getX(), errorAxisAngle.getY(), errorAxisAngle.getZ()); errorRotation.scale(errorAxisAngle.getAngle()); ReferenceFrame endEffectorFrame = endEffector.getBodyFixedFrame(); Twist desiredTwist = new Twist(); desiredTwist.set(endEffectorFrame, elevatorFrame, controlFrame, errorPosition, errorRotation); desiredTwist.getMatrix(spatialError, 0); subspaceError.reshape(selectionMatrix.getNumRows(), 1); CommonOps.mult(selectionMatrix, spatialError, subspaceError); errorMagnitude.setValue(NormOps.normP2(subspaceError)); desiredTwist.scale(1.0 / updateDT); return desiredTwist; }
public Twist computeDesiredTwist(FrameOrientation desiredOrientation, RigidBody endEffector, ReferenceFrame controlFrame, DenseMatrix64F selectionMatrix, MutableDouble errorMagnitude) { errorFrameOrientation.setIncludingFrame(desiredOrientation); errorFrameOrientation.changeFrame(controlFrame); errorFrameOrientation.getAxisAngle(errorAxisAngle); errorRotation.set(errorAxisAngle.getX(), errorAxisAngle.getY(), errorAxisAngle.getZ()); errorRotation.scale(AngleTools.trimAngleMinusPiToPi(errorAxisAngle.getAngle())); ReferenceFrame endEffectorFrame = endEffector.getBodyFixedFrame(); Twist desiredTwist = new Twist(); desiredTwist.set(endEffectorFrame, elevatorFrame, controlFrame, new Vector3d(), errorRotation); desiredTwist.getMatrix(spatialError, 0); subspaceError.reshape(selectionMatrix.getNumRows(), 1); CommonOps.mult(selectionMatrix, spatialError, subspaceError); errorMagnitude.setValue(NormOps.normP2(subspaceError)); desiredTwist.scale(1.0 / updateDT); return desiredTwist; }
public Twist computeDesiredTwist(FramePose desiredPose, RigidBody endEffector, ReferenceFrame controlFrame, DenseMatrix64F selectionMatrix, MutableDouble errorMagnitude) { errorFramePose.setIncludingFrame(desiredPose); errorFramePose.changeFrame(controlFrame); errorFramePose.getPosition(errorPosition); errorFramePose.getOrientation(errorAxisAngle); errorRotation.set(errorAxisAngle.getX(), errorAxisAngle.getY(), errorAxisAngle.getZ()); errorRotation.scale(AngleTools.trimAngleMinusPiToPi(errorAxisAngle.getAngle())); ReferenceFrame endEffectorFrame = endEffector.getBodyFixedFrame(); Twist desiredTwist = new Twist(); desiredTwist.set(endEffectorFrame, elevatorFrame, controlFrame, errorPosition, errorRotation); desiredTwist.getMatrix(spatialError, 0); subspaceError.reshape(selectionMatrix.getNumRows(), 1); CommonOps.mult(selectionMatrix, spatialError, subspaceError); errorMagnitude.setValue(NormOps.normP2(subspaceError)); desiredTwist.scale(1.0 / updateDT); return desiredTwist; }