public void get(FrameVector frameVectorToPack) { twistCalculator.getTwistOfBody(twist, rigidBody); twist.changeFrame(referenceFrame); twist.getAngularPart(frameVectorToPack); } }
public void get(FrameVector frameVectorToPack) { twistCalculator.getTwistOfBody(twist, rigidBody); twist.changeFrame(referenceFrame); twist.getLinearPart(frameVectorToPack); } }
public void doPositionControl(FramePose desiredEndEffectorPose, Twist desiredEndEffectorTwist, SpatialAccelerationVector feedForwardEndEffectorSpatialAcceleration, RigidBody base) { twistCalculator.getRelativeTwist(currentTwist, base, endEffector); currentTwist.changeBodyFrameNoRelativeTwist(endEffectorFrame); currentTwist.changeFrame(endEffectorFrame); se3pdController.compute(acceleration, desiredEndEffectorPose, desiredEndEffectorTwist, feedForwardEndEffectorSpatialAcceleration, currentTwist); }
/** * 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); } }
private void setMassMatrixPart(int i, int j, GeometricJacobian motionSubspace) { int rowStart = massMatrixIndices[i]; int colStart = massMatrixIndices[j]; for (int m = 0; m < nMomentaInUse; m++) { Momentum unitMomentum = unitMomenta[m]; int massMatrixRow = rowStart + m; for (int n = 0; n < motionSubspace.getNumberOfColumns(); n++) { Twist unitRelativeTwist = motionSubspace.getAllUnitTwists().get(n); unitRelativeTwist.changeFrame(unitMomentum.getExpressedInFrame()); double entry = unitMomentum.computeKineticCoEnergy(unitRelativeTwist); int massMatrixCol = colStart + n; setMassMatrixSymmetrically(massMatrixRow, massMatrixCol, entry); } } }
public void setLinearVelocity(ReferenceFrame bodyFrame, ReferenceFrame baseFrame, FrameVector desiredLinearVelocity) { spatialVelocity.setToZero(bodyFrame, baseFrame, desiredLinearVelocity.getReferenceFrame()); spatialVelocity.setLinearPart(desiredLinearVelocity.getVector()); spatialVelocity.changeFrame(bodyFrame); setSelectionMatrixForLinearControl(); }
@Override public void getSuccessorTwist(Twist twistToPack) { getJointTwist(twistToPack); ReferenceFrame predecessorFrame = predecessor.getBodyFixedFrame(); ReferenceFrame successorFrame = successor.getBodyFixedFrame(); twistToPack.changeBaseFrameNoRelativeTwist(predecessorFrame); twistToPack.changeBodyFrameNoRelativeTwist(successorFrame); twistToPack.changeFrame(successorFrame); }
public void startComputation() { twistCalculator.getTwistOfBody(twist, rigidBody); twist.changeFrame(measurementFrame); twist.getAngularPart(angularVelocity); yoFrameVectorPerfect.set(angularVelocity); corrupt(angularVelocity); yoFrameVectorNoisy.set(angularVelocity); angularVelocityOutputPort.setData(angularVelocity); }
@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); }
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 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); }
/** * 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 startComputation() { twistCalculator.getTwistOfBody(twist, rigidBody); twist.changeFrame(twist.getBaseFrame()); tempPointToMeasureVelocityOf.setIncludingFrame(pointToMeasureVelocityOf); tempPointToMeasureVelocityOf.changeFrame(twist.getBaseFrame()); twist.getLinearVelocityOfPointFixedInBodyFrame(pointVelocityFrameVector, tempPointToMeasureVelocityOf); pointVelocityFrameVector.changeFrame(worldFrame); pointVelocityFrameVector.get(pointVelocity); yoFrameVectorPerfect.set(pointVelocity); corrupt(pointVelocity); yoFrameVectorNoisy.set(pointVelocity); pointVelocityOutputPort.setData(pointVelocity); }
private void computeDerivativeTerm(FrameVector desiredVelocity, RigidBody base) { ReferenceFrame baseFrame = base.getBodyFixedFrame(); twistCalculator.getRelativeTwist(endEffectorTwist, base, endEffector); endEffectorTwist.changeFrame(baseFrame); bodyFixedPoint.setToZero(frameAtBodyFixedPoint); bodyFixedPoint.changeFrame(baseFrame); endEffectorTwist.getLinearVelocityOfPointFixedInBodyFrame(currentVelocity, bodyFixedPoint); desiredVelocity.changeFrame(baseFrame); currentVelocity.changeFrame(baseFrame); derivativeTerm.setToZero(baseFrame); derivativeTerm.sub(desiredVelocity, currentVelocity); velocityError.setAndMatchFrame(derivativeTerm); derivativeGainMatrix.transform(derivativeTerm.getVector()); }
private void computeVelocityOfStationaryPoint(FrameVector stationaryPointVelocityToPack) { FullInverseDynamicsStructure inverseDynamicsStructure = inverseDynamicsStructureInputPort.getData(); RigidBody stationaryPointLink = estimatorRigidBodyToIndexMap.getRigidBodyByName(pointVelocityMeasurementInputPort.getData().getRigidBodyName()); TwistCalculator twistCalculator = inverseDynamicsStructure.getTwistCalculator(); try { twistCalculator.getTwistOfBody(tempTwist, stationaryPointLink); } catch(Exception e) { e.printStackTrace(); } tempTwist.changeFrame(tempTwist.getBaseFrame()); ReferenceFrame referenceFrame = referenceFrameNameMap.getFrameByName(pointVelocityMeasurementInputPort.getData().getBodyFixedReferenceFrameName()); tempFramePoint.setIncludingFrame(referenceFrame, pointVelocityMeasurementInputPort.getData().getMeasurementPointInBodyFrame()); tempFramePoint.changeFrame(tempTwist.getBaseFrame()); tempTwist.getLinearVelocityOfPointFixedInBodyFrame(stationaryPointVelocityToPack, tempFramePoint); }
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); }
@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); }
public void compute(PointJacobian pointJacobian, FrameVector pPointVelocity) { GeometricJacobian jacobian = pointJacobian.getGeometricJacobian(); bodyFixedPoint.setIncludingFrame(pointJacobian.getPoint()); bodyFixedPoint.changeFrame(jacobian.getBaseFrame()); twistCalculator.getRelativeTwist(twist, jacobian.getBase(), jacobian.getEndEffector()); convectiveTermCalculator.computeJacobianDerivativeTerm(jacobian, convectiveTerm); convectiveTerm.changeFrame(jacobian.getBaseFrame(), twist, twist); twist.changeFrame(jacobian.getBaseFrame()); convectiveTerm.getAccelerationOfPointFixedInBodyFrame(twist, bodyFixedPoint, pPointVelocity); // bodyFixedPointVelocity.setToZero(jacobian.getBaseFrame()); // twist.packVelocityOfPointFixedInBodyFrame(bodyFixedPointVelocity, bodyFixedPoint); // // twist.packAngularPart(tempVector); // tempVector.cross(tempVector, bodyFixedPointVelocity); // pPointVelocity.add(tempVector); } }
private void computeRootJointAngularVelocity(TwistCalculator twistCalculator, FrameVector rootJointAngularVelocityToPack, FrameVector angularVelocityEstimationLink) { FullInverseDynamicsStructure inverseDynamicsStructure = inverseDynamicsStructureInputPort.getData(); FloatingInverseDynamicsJoint rootJoint = inverseDynamicsStructure.getRootJoint(); RigidBody estimationLink = inverseDynamicsStructure.getEstimationLink(); tempEstimationLinkAngularVelocity.setIncludingFrame(angularVelocityEstimationLink); // T_{root}^{root, estimation} twistCalculator.getRelativeTwist(tempRootToEstimationTwist, estimationLink, rootJoint.getSuccessor()); tempRootToEstimationTwist.changeFrame(rootJoint.getFrameAfterJoint()); // omega_{root}^{root, estimation} tempRootToEstimationAngularVelocity.setToZero(rootJoint.getFrameAfterJoint()); tempRootToEstimationTwist.getAngularPart(tempRootToEstimationAngularVelocity); // omega_{estimation}^{root, world} tempEstimationLinkAngularVelocity.changeFrame(rootJoint.getFrameAfterJoint()); // omega_{root}^{root, world} = omega_{estimation}^{root, world} + omega_{root}^{root, estimation} rootJointAngularVelocityToPack.setToZero(rootJoint.getFrameAfterJoint()); rootJointAngularVelocityToPack.add(tempEstimationLinkAngularVelocity, tempRootToEstimationAngularVelocity); }