@Override public void getJointTwist(Twist twistToPack) { twistToPack.set(jointTwist); }
public void getUnitJointTwist(Twist twistToPack) { twistToPack.set(unitJointTwist); }
@Override public void getPredecessorTwist(Twist twistToPack) { twistToPack.set(unitPredecessorTwist); twistToPack.scale(qd); }
public void set(Twist spatialVelocity) { this.spatialVelocity.set(spatialVelocity); setSelectionMatrixToIdentity(); }
@Override public void getJointTwist(Twist twistToPack) { twistToPack.set(unitJointTwist); twistToPack.scale(qd); }
@Override public void getSuccessorTwist(Twist twistToPack) { twistToPack.set(unitSuccessorTwist); twistToPack.scale(qd); }
public void set(Twist spatialVelocity, DenseMatrix64F selectionMatrix) { this.spatialVelocity.set(spatialVelocity); setSelectionMatrix(selectionMatrix); }
public void compute(FramePoint desiredPosition, FrameOrientation desiredOrientation, FrameVector desiredLinearVelocity, FrameVector desiredAngularVelocity) { desiredControlFramePose.setPoseIncludingFrame(desiredPosition, desiredOrientation); desiredControlFrameTwist.set(originalEndEffectorFrame, originalBaseFrame, originalControlFrame, desiredLinearVelocity, desiredAngularVelocity); compute(desiredControlFramePose, desiredControlFrameTwist); }
/** * Copy constructor */ public Twist(Twist other) { this.angularPart = new Vector3d(); this.linearPart = new Vector3d(); set(other); }
public void compute(FramePose desiredPose, Twist desiredTwist) { jacobian.compute(); desiredControlFramePose.setPoseIncludingFrame(desiredPose); desiredControlFrameTwist.set(desiredTwist); computeJointAnglesAndVelocities(desiredControlFramePose, desiredControlFrameTwist); }
/** * Sets this twist so that it is the same as another twist */ public void checkAndSet(Twist other) { this.bodyFrame.checkReferenceFrameMatch(other.bodyFrame); this.baseFrame.checkReferenceFrameMatch(other.baseFrame); this.expressedInFrame.checkReferenceFrameMatch(other.expressedInFrame); set(other); }
@Override public void setVelocity(DenseMatrix64F matrix, int rowStart) { jointTwist.set(jointTwist.getBodyFrame(), jointTwist.getBaseFrame(), jointTwist.getExpressedInFrame(), matrix, rowStart); }
/** * Computes and packs the twist that corresponds to the given joint velocity vector. * * @param jointVelocities the joint velocity column vector. * The vector should have the same ordering as the unit twists of the ordered joint list of this Jacobian * that were passed in during construction. * @return the twist of the end effector with respect to the base, expressed in the jacobianFrame. */ public void getTwist(DenseMatrix64F jointVelocities, Twist twistToPack) { CommonOps.mult(jacobian, jointVelocities, tempMatrix); twistToPack.set(getEndEffectorFrame(), getBaseFrame(), jacobianFrame, tempMatrix, 0); }
/** * Computes the Jacobian. */ public void compute() { int column = 0; for (int i = 0; i < unitTwistList.size(); i++) { Twist twist = unitTwistList.get(i); tempTwist.set(twist); tempTwist.changeFrame(jacobianFrame); tempTwist.getMatrix(tempMatrix, 0); CommonOps.extract(tempMatrix, 0, tempMatrix.getNumRows(), 0, tempMatrix.getNumCols(), jacobian, 0, column++); } }
private void setUnitMomenta(CompositeRigidBodyInertia currentBodyInertia, GeometricJacobian motionSubspace) { nMomentaInUse = motionSubspace.getNumberOfColumns(); for (int i = 0; i < nMomentaInUse; i++) { tempTwist.set(motionSubspace.getAllUnitTwists().get(i)); tempTwist.changeFrame(currentBodyInertia.getExpressedInFrame()); unitMomenta[i].compute(currentBodyInertia, tempTwist); } }
public void convertToTwist(FrameVector linearVelocityOfOrigin, FrameVector angularVelocity, RigidBody base, Twist twistToPack) { angularVelocity.changeFrame(endEffectorFrame); linearVelocityOfOrigin.changeFrame(endEffectorFrame); twistToPack.set(endEffectorFrame, base.getBodyFixedFrame(), endEffectorFrame, linearVelocityOfOrigin.getVector(), angularVelocity.getVector()); }
@Override public void set(SpatialVelocityCommand other) { weight = other.weight; spatialVelocity.set(other.getSpatialVelocity()); selectionMatrix.set(other.getSelectionMatrix()); base = other.getBase(); endEffector = other.getEndEffector(); baseName = other.baseName; endEffectorName = other.endEffectorName; }
private void computeRootJointTwist(FloatingInverseDynamicsJoint rootJoint, Twist rootJointTwistToPack, FrameVector rootJointAngularVelocity, FrameVector rootJointLinearVelocity) { rootJointAngularVelocity.checkReferenceFrameMatch(rootJoint.getFrameAfterJoint()); rootJointLinearVelocity.checkReferenceFrameMatch(rootJoint.getFrameAfterJoint()); rootJointTwistToPack.set(rootJoint.getFrameAfterJoint(), rootJoint.getFrameBeforeJoint(), rootJoint.getFrameAfterJoint(), rootJointLinearVelocity.getVector(), rootJointAngularVelocity.getVector()); }
private void computeRootJointTwistLinearPart(FloatingInverseDynamicsJoint rootJoint, Twist rootJointTwistToPack, FrameVector rootJointLinearVelocity) { rootJoint.getJointTwist(tempRootJointTwistExisting); tempRootJointTwistExisting.checkReferenceFramesMatch(rootJoint.getFrameAfterJoint(), rootJoint.getFrameBeforeJoint(), rootJoint.getFrameAfterJoint()); tempRootJointTwistExisting.getAngularPart(tempRootJointTwistExistingAngularPart); rootJointLinearVelocity.checkReferenceFrameMatch(rootJoint.getFrameAfterJoint()); rootJointTwistToPack.set(rootJoint.getFrameAfterJoint(), rootJoint.getFrameBeforeJoint(), rootJoint.getFrameAfterJoint(), rootJointLinearVelocity.getVector(), tempRootJointTwistExistingAngularPart.getVector()); }
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; }