/** * 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 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); }
/** * 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++); } }
@Override public void getJointTwist(Twist twistToPack) { twistToPack.setToZero(jointTwist.getBodyFrame(), jointTwist.getBaseFrame(), jointTwist.getExpressedInFrame()); twistToPack.setAngularPartY(jointTwist.getAngularPartY()); twistToPack.setLinearPartX(jointTwist.getLinearPartX()); twistToPack.setLinearPartZ(jointTwist.getLinearPartZ()); }
@Override public void setVelocity(DenseMatrix64F matrix, int rowStart) { jointTwist.set(jointTwist.getBodyFrame(), jointTwist.getBaseFrame(), jointTwist.getExpressedInFrame(), matrix, rowStart); }
/** * Computes the kinetic co-energy of the rigid body to which this inertia belongs * twistTranspose * Inertia * twist * * See Duindam, Port-Based Modeling and Control for Efficient Bipedal Walking Robots, page 40, eq. (2.56) * http://sites.google.com/site/vincentduindam/publications * * @param twist * @return */ public double computeKineticCoEnergy(Twist twist) { this.expressedInframe.checkReferenceFrameMatch(twist.getExpressedInFrame()); this.bodyFrame.checkReferenceFrameMatch(twist.getBodyFrame()); twist.getBaseFrame().checkIsWorldFrame(); double ret = 0.0; tempVector.set(twist.getAngularPart()); // omega massMomentOfInertiaPart.transform(tempVector); // J * omega ret += twist.getAngularPart().dot(tempVector); // omega . J * omega tempVector.set(twist.getAngularPart()); // omega tempVector.cross(crossPart, tempVector); // c x omega ret += 2.0 * twist.getLinearPart().dot(tempVector); // omega . J * omega + 2 * v . (c x omega) ret += mass * twist.getLinearPart().dot(twist.getLinearPart()); // omega . J * omega + 2 * v . (c x omega) + m * v . v return ret; }
/** * 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()); }
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; }
@Override public void getPredecessorTwist(Twist twistToPack) { getJointTwist(twistToPack); ReferenceFrame predecessorFrame = predecessor.getBodyFixedFrame(); ReferenceFrame successorFrame = successor.getBodyFixedFrame(); twistToPack.changeBaseFrameNoRelativeTwist(predecessorFrame); twistToPack.changeBodyFrameNoRelativeTwist(successorFrame); twistToPack.invert(); twistToPack.changeFrame(predecessorFrame); }
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); }
@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()); }
private void updateKinematicsNewTwist() { rootJoint.getJointTwist(tempRootBodyTwist); rootJointLinearVelocityNewTwist.getFrameTupleIncludingFrame(tempFrameVector); tempFrameVector.changeFrame(tempRootBodyTwist.getExpressedInFrame()); tempRootBodyTwist.setLinearPart(tempFrameVector); rootJoint.setJointTwist(tempRootBodyTwist); twistCalculator.compute(); for (int i = 0; i < feetRigidBodies.size(); i++) { RigidBody foot = feetRigidBodies.get(i); Twist footTwistInWorld = footTwistsInWorld.get(foot); YoFrameVector footVelocityInWorld = footVelocitiesInWorld.get(foot); twistCalculator.getTwistOfBody(footTwistInWorld, foot); footTwistInWorld.changeBodyFrameNoRelativeTwist(soleFrames.get(foot)); footTwistInWorld.changeFrame(soleFrames.get(foot)); this.copsFilteredInFootFrame.get(foot).getFrameTuple2dIncludingFrame(tempCoP2d); tempCoP.setXYIncludingFrame(tempCoP2d); footTwistInWorld.changeFrame(footTwistInWorld.getBaseFrame()); tempCoP.changeFrame(footTwistInWorld.getExpressedInFrame()); footTwistInWorld.getLinearVelocityOfPointFixedInBodyFrame(tempFrameVector, tempCoP); tempFrameVector.changeFrame(worldFrame); footVelocityInWorld.set(tempFrameVector); } }
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); } }
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); }
public void getTwistAngularPart(Vector3d tempVector) { twist.getAngularPart(tempVector); }
/** * 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(FrameVector frameVectorToPack) { twistCalculator.getTwistOfBody(twist, rigidBody); twist.changeFrame(referenceFrame); twist.getAngularPart(frameVectorToPack); } }
private void updateRootJointTwistAngularPart() { // T_{rootBody}^{rootBody, measurementLink} twistCalculator.getRelativeTwist(twistRootJointFrameRelativeToMeasurementLink, measurementLink, rootJoint.getSuccessor()); // T_{rootBody}^{rootJointFrame, measurementLink} twistRootJointFrameRelativeToMeasurementLink.changeFrame(rootJointFrame); // T_{rootJointFrame}^{rootJointFrame, measurementLink} twistRootJointFrameRelativeToMeasurementLink.changeBodyFrameNoRelativeTwist(rootJointFrame); // omega_{rootJointFrame}^{rootJointFrame, measurementLink} twistRootJointFrameRelativeToMeasurementLink.getAngularPart(angularVelocityRootJointFrameRelativeToMeasurementLink); // omega_{measurementLink}^{measurementFrame, world} imuProcessedOutput.getAngularVelocityMeasurement(angularVelocityMeasurement); if (imuBiasProvider != null) { imuBiasProvider.getAngularVelocityBiasInIMUFrame(imuProcessedOutput, angularVelocityMeasurementBias); angularVelocityMeasurement.sub(angularVelocityMeasurementBias); } angularVelocityMeasurementLinkRelativeToWorld.setIncludingFrame(measurementFrame, angularVelocityMeasurement); // omega_{measurementLink}^{rootJointFrame, world} angularVelocityMeasurementLinkRelativeToWorld.changeFrame(rootJointFrame); // omega_{rootJointFrame}^{rootJointFrame, world} = omega_{rootJointFrame}^{rootJointFrame, measurementLink} + omega_{measurementLink}^{rootJointFrame, world} angularVelocityRootJointFrameRelativeToWorld.add(angularVelocityRootJointFrameRelativeToMeasurementLink, angularVelocityMeasurementLinkRelativeToWorld); rootJoint.getJointTwist(twistRootBodyRelativeToWorld); twistRootBodyRelativeToWorld.setAngularPart(angularVelocityRootJointFrameRelativeToWorld); rootJoint.setJointTwist(twistRootBodyRelativeToWorld); twistCalculator.compute(); yoRootJointAngularVelocity.setAndMatchFrame(angularVelocityMeasurementLinkRelativeToWorld); yoRootJointAngularVelocityMeasFrame.setAndMatchFrame(angularVelocityMeasurementLinkRelativeToWorld); yoRootJointAngularVelocityInWorld.setAndMatchFrame(angularVelocityRootJointFrameRelativeToWorld); }
/** * 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()); }