public void getTwistAngularPart(Vector3d tempVector) { twist.getAngularPart(tempVector); }
public Vector3d getAngularVelocityForReading() { return jointTwist.getAngularPart(); }
public void getAngularVelocity(Vector3d angularVelocityToPack) { jointTwist.getAngularPart(angularVelocityToPack); }
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"); }
public void get(FrameVector frameVectorToPack) { twistCalculator.getTwistOfBody(twist, rigidBody); twist.changeFrame(referenceFrame); twist.getAngularPart(frameVectorToPack); } }
public Vector3d getAngularVelocityForReading() { Vector3d angularVelocity = jointTwist.getAngularPart(); angularVelocity.setX(0.0); angularVelocity.setZ(0.0); return angularVelocity; }
private void updateRootJointTwistLinearPart(FrameVector centerOfMassVelocityWorld, FloatingInverseDynamicsJoint rootJoint) { rootJoint.getJointTwist(tempRootJointTwist); tempRootJointTwist.getAngularPart(tempRootJointAngularVelocity); computeRootJointLinearVelocity(centerOfMassVelocityWorld, tempRootJointLinearVelocity, tempRootJointAngularVelocity, rootJoint); computeRootJointTwistLinearPart(rootJoint, tempRootJointTwist, tempRootJointLinearVelocity); rootJoint.setJointTwist(tempRootJointTwist); }
/** * Computes using Twists, ignores linear part */ public void compute(Twist twistToPack, FramePose desiredPose, Twist desiredTwist) { checkBodyFrames(desiredTwist, twistToPack); checkBaseFrames(desiredTwist, twistToPack); checkExpressedInFrames(desiredTwist, twistToPack); twistToPack.setToZero(bodyFrame, desiredTwist.getBaseFrame(), bodyFrame); desiredPose.getOrientationIncludingFrame(desiredOrientation); desiredTwist.getAngularPart(desiredAngularVelocity); desiredTwist.getAngularPart(feedForwardAngularAction); compute(angularActionFromOrientationController, desiredOrientation, desiredAngularVelocity, null, feedForwardAngularAction); twistToPack.setAngularPart(angularActionFromOrientationController.getVector()); }
public void get(FloatingJoint joint) { rotationMatrix.set(rotation); joint.setRotation(rotationMatrix); joint.setPosition(translation); twist.getAngularPart(tempVector); joint.setAngularVelocityInBody(tempVector); twist.getLinearPart(tempVector); joint.setVelocity(tempVector); }
public void startComputation() { twistCalculator.getTwistOfBody(twist, rigidBody); twist.changeFrame(measurementFrame); twist.getAngularPart(angularVelocity); yoFrameVectorPerfect.set(angularVelocity); corrupt(angularVelocity); yoFrameVectorNoisy.set(angularVelocity); angularVelocityOutputPort.setData(angularVelocity); }
private void getCorrectionVelocityForMeasurementFrameOffset(FrameVector correctionTermToPack) { rootJoint.getJointTwist(tempRootJointTwist); tempRootJointTwist.getAngularPart(tempRootJointAngularVelocity); measurementOffset.setToZero(measurementFrame); measurementOffset.changeFrame(rootJoint.getFrameAfterJoint()); correctionTermToPack.setToZero(tempRootJointAngularVelocity.getReferenceFrame()); correctionTermToPack.cross(tempRootJointAngularVelocity, measurementOffset); } }
public void compute(FrameVector outputToPack, FrameOrientation desiredOrientation, FrameVector desiredAngularVelocity, FrameVector feedForwardAngularAcceleration, RigidBody base) { // using twists is a bit overkill; optimize if needed. twistCalculator.getRelativeTwist(endEffectorTwist, base, endEffector); currentAngularVelocity.setToZero(endEffectorTwist.getExpressedInFrame()); endEffectorTwist.getAngularPart(currentAngularVelocity); desiredAngularVelocity.changeFrame(currentAngularVelocity.getReferenceFrame()); feedForwardAngularAcceleration.changeFrame(endEffectorTwist.getExpressedInFrame()); axisAngleOrientationController.compute(outputToPack, desiredOrientation, desiredAngularVelocity, currentAngularVelocity, feedForwardAngularAcceleration); }
private void computeRpd(FrameVector rPdToPack, TwistCalculator twistCalculator, FramePoint rP, FrameVector rd) { // T_{p}^{p,w} twistCalculator.getTwistOfBody(twistOfEstimationLink, estimationLink); twistOfEstimationLink.changeFrame(estimationFrame); // \dot{r}^{p} = R_{w}^{p} \dot{r} - \tilde{\omega}r^{p} - v_{p}^{p,w} rPdToPack.setIncludingFrame(rd); rPdToPack.changeFrame(estimationFrame); twistOfEstimationLink.getAngularPart(tempFrameVector); tempFrameVector.cross(tempFrameVector, rP); rPdToPack.sub(tempFrameVector); twistOfEstimationLink.getLinearPart(tempFrameVector); rPdToPack.sub(tempFrameVector); }
@Override public void calculateJointStateChecksum(GenericCRC32 checksum) { checksum.update(jointTranslation); checksum.update(jointRotation); checksum.update(jointTwist.getAngularPart()); checksum.update(jointTwist.getLinearPart()); checksum.update(jointAcceleration.getAngularPart()); checksum.update(jointAcceleration.getLinearPart()); }
@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 void getLinearAccelerationFromOriginAcceleration(Twist twistOfBodyWithRespectToBase, FrameVector linearAccelerationToPack) { bodyFrame.checkReferenceFrameMatch(expressedInFrame); twistOfBodyWithRespectToBase.getBodyFrame().checkReferenceFrameMatch(bodyFrame); twistOfBodyWithRespectToBase.getBaseFrame().checkReferenceFrameMatch(baseFrame); twistOfBodyWithRespectToBase.changeFrame(bodyFrame); linearAccelerationToPack.setToZero(bodyFrame); linearAccelerationToPack.cross(twistOfBodyWithRespectToBase.getAngularPart(), twistOfBodyWithRespectToBase.getLinearPart()); linearAccelerationToPack.add(linearPart); }
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 void setBasedOnOriginAcceleration(FrameVector angularAcceleration, FrameVector originAcceleration, Twist twistOfBodyWithRespectToBase) { bodyFrame.checkReferenceFrameMatch(expressedInFrame); twistOfBodyWithRespectToBase.getBodyFrame().checkReferenceFrameMatch(bodyFrame); twistOfBodyWithRespectToBase.getBaseFrame().checkReferenceFrameMatch(baseFrame); angularAcceleration.changeFrame(bodyFrame); angularPart.set(angularAcceleration.getVector()); originAcceleration.changeFrame(bodyFrame); twistOfBodyWithRespectToBase.changeFrame(bodyFrame); linearPart.cross(twistOfBodyWithRespectToBase.getAngularPart(), twistOfBodyWithRespectToBase.getLinearPart()); linearPart.sub(originAcceleration.getVector(), linearPart); }
private void setColumn(Twist twist, FramePoint comPositionScaledByMass, double subTreeMass, int column) { tempVector.set(comPositionScaledByMass.getPoint()); tempVector.cross(twist.getAngularPart(), tempVector); tempJacobianColumn.set(tempVector); tempVector.set(twist.getLinearPart()); tempVector.scale(subTreeMass); tempJacobianColumn.add(tempVector); jacobianMatrix.set(0, column, tempJacobianColumn.getX()); jacobianMatrix.set(1, column, tempJacobianColumn.getY()); jacobianMatrix.set(2, column, tempJacobianColumn.getZ()); } }
private void convertAngularVelocityAndSetOnOutputPort() { Vector3d measuredAngularVelocityVector3d = angularVelocityInputPort.getData(); TwistCalculator twistCalculator = inverseDynamicsStructureInputPort.getData().getTwistCalculator(); twistCalculator.getRelativeTwist(tempRelativeTwistOrientationMeasFrameToEstFrame, angularVelocityMeasurementLink, estimationLink); tempRelativeTwistOrientationMeasFrameToEstFrame.getAngularPart(relativeAngularVelocity); relativeAngularVelocity.changeFrame(estimationFrame); tempAngularVelocityMeasurementLink.setIncludingFrame(angularVelocityMeasurementFrame, measuredAngularVelocityVector3d); tempAngularVelocityMeasurementLink.changeFrame(estimationFrame); relativeAngularVelocity.add(tempAngularVelocityMeasurementLink); tempAngularVelocityEstimationLink.setIncludingFrame(relativeAngularVelocity); // just a copy for clarity // Introduce simulated IMU drift tempAngularVelocityEstimationLink.setZ(tempAngularVelocityEstimationLink.getZ() + imuSimulatedDriftYawVelocity.getDoubleValue()); angularVelocityOutputPort.setData(tempAngularVelocityEstimationLink); }