public void getTwistLinearPart(Vector3d tempVector) { twist.getLinearPart(tempVector); } }
public Vector3d getLinearVelocityForReading() { return jointTwist.getLinearPart(); }
public void getLinearVelocity(Vector3d linearVelocityToPack) { jointTwist.getLinearPart(linearVelocityToPack); }
public Vector3d getLinearVelocityForReading() { Vector3d linearVelocity = jointTwist.getLinearPart(); linearVelocity.setY(0.0); return linearVelocity; }
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"); }
public void get(FrameVector frameVectorToPack) { twistCalculator.getTwistOfBody(twist, rigidBody); twist.changeFrame(referenceFrame); twist.getLinearPart(frameVectorToPack); } }
private void updateFootLinearVelocities() { for (int i = 0; i < numberOfFeet; i++) { RigidBody foot = allFeet.get(i); twistCalculator.getTwistOfBody(footTwist, foot); footTwist.getLinearPart(footLinearVelocity); currentFootLinearVelocities.get(foot).set(footLinearVelocity.length()); } }
/** * Computes linear portion of twist to pack */ 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.getPositionIncludingFrame(desiredPosition); desiredTwist.getLinearPart(desiredVelocity); desiredTwist.getLinearPart(feedForwardLinearAction); compute(actionFromPositionController, desiredPosition, desiredVelocity, null, feedForwardLinearAction); twistToPack.setLinearPart(actionFromPositionController.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); }
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); }
/** * This method provides a twist feedback controller, intended for spatial velocity control. * @param twistToPack twist to return * @param desiredPose desired pose that we want to achieve. * @param desiredTwist feed forward twist from a reference trajectory */ 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); orientationController.compute(angularActionFromOrientationController, desiredOrientation, desiredAngularVelocity, null, feedForwardAngularAction); twistToPack.setAngularPart(angularActionFromOrientationController.getVector()); desiredPose.getPositionIncludingFrame(desiredPosition); desiredTwist.getLinearPart(desiredVelocity); desiredTwist.getLinearPart(feedForwardLinearAction); positionController.compute(actionFromPositionController, desiredPosition, desiredVelocity, null, feedForwardLinearAction); twistToPack.setLinearPart(actionFromPositionController.getVector()); }
/** * Sets this momentum to inertia * twist */ public void compute(GeneralizedRigidBodyInertia inertia, Twist twist) { ReferenceFrame frame = inertia.getExpressedInFrame(); frame.checkReferenceFrameMatch(twist.getExpressedInFrame()); tempVector.set(twist.getAngularPart()); inertia.massMomentOfInertiaPart.transform(tempVector); // J * omegad setAngularPart(tempVector); if (!inertia.isCrossPartZero()) { tempVector.cross(twist.getLinearPart(), inertia.crossPart); addAngularPart(tempVector); // J * omegad - c x vd } tempVector.set(twist.getLinearPart()); tempVector.scale(inertia.getMass()); setLinearPart(tempVector); // m * vd if (!inertia.isCrossPartZero()) { tempVector.cross(inertia.crossPart, twist.getAngularPart()); addLinearPart(tempVector); // m * vd + c x omegad } this.expressedInFrame = frame; }
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()); } }
public static void integrate(Matrix3d rotationToPack, Tuple3d positionToPack, double dt, Twist twist) { twist.changeFrame(twist.getBodyFrame()); Vector3d dPosition = new Vector3d(); twist.getLinearPart(dPosition); // velocity in body frame rotationToPack.transform(dPosition); // velocity in base frame dPosition.scale(dt); // translation in base frame positionToPack.add(dPosition); Vector3d axis = new Vector3d(); twist.getAngularPart(axis); axis.scale(dt); double angle = axis.length(); if (angle > 0.0) axis.normalize(); else axis.set(1.0, 0.0, 0.0); AxisAngle4d axisAngle = new AxisAngle4d(axis, angle); Matrix3d dRotation = new Matrix3d(); dRotation.set(axisAngle); rotationToPack.mul(dRotation); }
private void computeADotVLeftSide() { for(int i = 0; i < bodyTwists.length; i++) { tempTwist.set(comTwist); tempTwist.sub(bodyTwists[i]); // Left multiply by ad^{T} : this is separated out rather than creating matrix objects to save computation time. // See method below for creating adjoint from twist for reference. // ad^{T} * Ad^{T} * I * J * v // The order of these cross products are backwards because I need -a x b and cross products are anticommutative, // so -a x b = b x a. leftSide.setToZero(); tempVector.cross(bodyMomenta[i].getLinearPart(), tempTwist.getLinearPart()); leftSide.addAngularPart(tempVector); tempVector.cross(bodyMomenta[i].getAngularPart(), tempTwist.getAngularPart()); leftSide.addAngularPart(tempVector); tempVector.cross(bodyMomenta[i].getLinearPart(), tempTwist.getAngularPart()); leftSide.addLinearPart(tempVector); // leftSide.getMatrix(tempSpatialForceMatrix); CommonOps.add(aDotV, tempSpatialForceMatrix, aDotV); } }
public void computeDynamicWrenchInBodyCoordinates(SpatialAccelerationVector acceleration, Twist twist, Wrench dynamicWrenchToPack) // TODO: write test { checkExpressedInBodyFixedFrame(); checkIsCrossPartZero(); // otherwise this operation would be a lot less efficient acceleration.getBodyFrame().checkReferenceFrameMatch(this.bodyFrame); acceleration.getBaseFrame().checkIsWorldFrame(); acceleration.getExpressedInFrame().checkReferenceFrameMatch(this.expressedInframe); twist.getBodyFrame().checkReferenceFrameMatch(this.bodyFrame); twist.getBaseFrame().checkIsWorldFrame(); twist.getExpressedInFrame().checkReferenceFrameMatch(this.expressedInframe); dynamicWrenchToPack.getBodyFrame().checkReferenceFrameMatch(this.bodyFrame); dynamicWrenchToPack.getExpressedInFrame().checkReferenceFrameMatch(this.expressedInframe); massMomentOfInertiaPart.transform(acceleration.getAngularPart(), tempVector); // J * omegad dynamicWrenchToPack.setAngularPart(tempVector); // [J * omegad; 0] massMomentOfInertiaPart.transform(twist.getAngularPart(), tempVector); // J * omega tempVector.cross(twist.getAngularPart(), tempVector); // omega x J * omega dynamicWrenchToPack.addAngularPart(tempVector); // [J * omegad + omega x J * omega; 0] tempVector.set(acceleration.getLinearPart()); // vd tempVector.scale(mass); // m * vd dynamicWrenchToPack.setLinearPart(tempVector); // [J * omegad + omega x J * omega; m * vd] tempVector.set(twist.getLinearPart()); // v tempVector.scale(mass); // m * v tempVector.cross(twist.getAngularPart(), tempVector); // omega x m * v dynamicWrenchToPack.addLinearPart(tempVector); // [J * omegad + omega x J * omega; m * vd + omega x m * v] }