@Override public FrameVector getJointAxis() { return new FrameVector(jointAxis); }
private void computeOrientationStateOutputBlock(Matrix3d rotationFromPelvisToWorld) { computeVelocityOfStationaryPoint(tempFrameVector); tempFrameVector.changeFrame(estimationFrame); tempCenterOfMassVelocity.setIncludingFrame(centerOfMassVelocityPort.getData()); tempCenterOfMassVelocity.changeFrame(estimationFrame); tempFrameVector.sub(tempCenterOfMassVelocity); tempFrameVector.scale(-1.0); MatrixTools.toTildeForm(tempMatrix, tempFrameVector.getVector()); tempMatrix.mul(rotationFromPelvisToWorld, tempMatrix); MatrixTools.setDenseMatrixFromMatrix3d(0, 0, tempMatrix, getOutputMatrixBlock(orientationPort)); }
public void correctState(DenseMatrix64F correction) { MatrixTools.extractTuple3dFromEJMLVector(comAccelerationDelta.getVector(), correction, 0); comAcceleration.set(centerOfMassAccelerationStatePort.getData()); comAcceleration.add(comAccelerationDelta); centerOfMassAccelerationStatePort.setData(comAcceleration); } }
public void cross(FrameTuple<?, ?> frameTuple1, FrameTuple<?, ?> frameTuple2) { checkReferenceFrameMatch(frameTuple1); checkReferenceFrameMatch(frameTuple2); cross(this.tuple, frameTuple1.tuple, frameTuple2.tuple); }
@Override public void setDesiredAccelerationToZero() { jointAngularAccelerationDesired.setToZero(jointAngularAccelerationDesired.getReferenceFrame()); }
public int compare(FramePoint o1, FramePoint o2) { differenceVector.setIncludingFrame(o1); differenceVector.sub(o2); double dotProduct = searchDirection.dot(differenceVector); return Double.compare(dotProduct, 0.0); } }
private void computeRootJointLinearVelocity(FrameVector centerOfMassVelocityWorld, FrameVector rootJointVelocityToPack, FrameVector rootJointAngularVelocity, FloatingInverseDynamicsJoint rootJoint) { tempCenterOfMassVelocityWorld.setIncludingFrame(centerOfMassVelocityWorld); ReferenceFrame rootJointFrame = rootJoint.getFrameAfterJoint(); // \dot{r}^{root} centerOfMassJacobianBody.compute(); tempComVelocityBody.setToZero(rootJointFrame); centerOfMassJacobianBody.getCenterOfMassVelocity(tempComVelocityBody); tempComVelocityBody.changeFrame(rootJointFrame); // \tilde{\omega} r^{root} tempComBody.setToZero(rootJointFrame); centerOfMassCalculator.getCenterOfMass(tempComBody); tempComBody.changeFrame(rootJointFrame); tempCrossPart.setToZero(rootJointFrame); tempCrossPart.cross(rootJointAngularVelocity, tempComBody); // v_{r/p}= \tilde{\omega} r^{root} + \dot{r}^{root} tempCenterOfMassVelocityOffset.setToZero(rootJointFrame); tempCenterOfMassVelocityOffset.add(tempCrossPart, tempComVelocityBody); // v_{root}^{p,w} = R_{w}^{root} \dot{r} - v_{r/p} tempCenterOfMassVelocityWorld.changeFrame(rootJointFrame); rootJointVelocityToPack.setIncludingFrame(tempCenterOfMassVelocityWorld); rootJointVelocityToPack.sub(tempCenterOfMassVelocityOffset); }
private void computeDerivativeTerm(FrameVector desiredAngularVelocity, FrameVector currentAngularVelocity) { desiredAngularVelocity.changeFrame(bodyFrame); currentAngularVelocity.changeFrame(bodyFrame); derivativeTerm.sub(desiredAngularVelocity, currentAngularVelocity); // Limit the maximum velocity error considered for control action double maximumVelocityError = gains.getMaximumDerivativeError(); double velocityErrorMagnitude = derivativeTerm.length(); if (velocityErrorMagnitude > maximumVelocityError) { derivativeTerm.scale(maximumVelocityError / velocityErrorMagnitude); } velocityError.set(derivativeTerm); derivativeGainMatrix.transform(derivativeTerm.getVector()); }
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); }
/** * Packs the linear velocity of a point2d that is fixed in bodyFrame but is expressed in baseFrame, * with respect to baseFrame, expressed in expressedInFrame */ public void getLineaVelocityOfPoint2dFixedInBodyFrame(FrameVector linearVelocityToPack, FramePoint2d point2dFixedInBodyFrame) { baseFrame.checkReferenceFrameMatch(expressedInFrame); point2dFixedInBodyFrame.checkReferenceFrameMatch(baseFrame); point2dFixedInBodyFrame.get(freeVector); linearVelocityToPack.setToZero(expressedInFrame); linearVelocityToPack.cross(angularPart, freeVector); linearVelocityToPack.add(linearPart); }
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()); }
public static Vector3d getTransformedVector(Vector3d vector3d, RigidBodyTransform transform3D) { ReferenceFrame ending = ReferenceFrame.constructARootFrame("ending", false, true, true); ReferenceFrame starting = ReferenceFrame.constructFrameWithUnchangingTransformToParent("starting", ending, transform3D, false, true, true); FrameVector framePoint = new FrameVector(starting, vector3d); framePoint.changeFrame(ending); return framePoint.getVector(); }
public static void computeForce(FrameVector forceToPack, FramePoint centerOfMass, FramePoint cmp, double fZ) { cmp.changeFrame(centerOfMass.getReferenceFrame()); forceToPack.setIncludingFrame(centerOfMass); forceToPack.sub(cmp); forceToPack.scale(fZ / forceToPack.getZ()); }
/** * Packs a version of the linear velocity, rotated to the base frame. */ public void getBodyOriginLinearPartInBaseFrame(FrameVector linearVelocityAtBodyOriginToPack) { linearVelocityAtBodyOriginToPack.setToZero(baseFrame); getBodyOriginLinearPartInBaseFrame(linearVelocityAtBodyOriginToPack.getVector()); }
public void setAndMatchFrameLinearMomentumRate(FrameVector linearMomentumRate) { this.linearMomentumRate.setIncludingFrame(linearMomentumRate); this.linearMomentumRate.changeFrame(ReferenceFrame.getWorldFrame()); }
public RevoluteJoint(String name, RigidBody predecessor, ReferenceFrame beforeJointFrame, FrameVector jointAxis) { super(name, predecessor, beforeJointFrame, new RevoluteJointReferenceFrame(name, beforeJointFrame, jointAxis)); jointAxis.checkReferenceFrameMatch(beforeJointFrame); this.jointAxis = new FrameVector(jointAxis); unitJointTwist = new Twist(afterJointFrame, beforeJointFrame, afterJointFrame, new Vector3d(), jointAxis.getVector()); }
@Override public void changeFrame(ReferenceFrame desiredFrame) { get(vector); ReferenceFrame currentReferenceFrame = multipleFramesHelper.switchCurrentReferenceFrame(desiredFrame); frameVector.setIncludingFrame(currentReferenceFrame, vector); frameVector.changeFrame(desiredFrame); frameVector.get(vector); set(vector); }
private void computeUnbiasedEstimatedMeasurement(SpatialAccelerationCalculator spatialAccelerationCalculator, FrameVector estimatedMeasurement) { tempFramePoint.setToZero(measurementFrame); spatialAccelerationCalculator.getLinearAccelerationOfBodyFixedPoint(estimatedMeasurement, measurementLink, tempFramePoint); estimatedMeasurement.changeFrame(gravitationalAcceleration.getReferenceFrame()); estimatedMeasurement.sub(gravitationalAcceleration); estimatedMeasurement.changeFrame(measurementFrame); } }