/** * Computes and packs the angular velocity of the given {@code body} with respect to the given * {@code base}. * <p> * The result will be the angular velocity of {@code body.getBodyFixedFrame()} with respect to * {@code base.getBodyFixedFrame()} expressed in {@code body.getBodyFixedFrame()}. * </p> * * @param base the rigid-body with respect to which the angular velocity is to be computed. * @param body the rigid-body to compute the angular velocity of. * @param angularVelocityToPack the angular velocity of {@code body} relative to the * {@code base}. Modified. */ public void getRelativeAngularVelocity(RigidBodyBasics base, RigidBodyBasics body, FrameVector3D angularVelocityToPack) { getRelativeTwist(base, body, twistForGetAngularVelocityOfBody); angularVelocityToPack.setIncludingFrame(twistForGetAngularVelocityOfBody.getAngularPart()); }
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); }
twistCalculator.getRelativeTwist(twist, trustedFoot, measurementLink); feetToIMUAngularVelocityMagnitude += twist.getAngularPartMagnitude(); feetToIMULinearVelocityMagnitude += twist.getLinearPartMagnitude();
public void convertToSpatialAcceleration(FrameVector linearAccelerationOfOrigin, FrameVector angularAcceleration, RigidBody base, SpatialAccelerationVector toPack) { angularAcceleration.changeFrame(endEffectorFrame); linearAccelerationOfOrigin.changeFrame(endEffectorFrame); twistCalculator.getRelativeTwist(twistOfEndEffectorWithRespectToElevator, base, endEffector); twistOfEndEffectorWithRespectToElevator.changeBodyFrameNoRelativeTwist(endEffectorFrame); ReferenceFrame baseFrame = base.getBodyFixedFrame(); toPack.setBasedOnOriginAcceleration(endEffectorFrame, baseFrame, endEffectorFrame, angularAcceleration, linearAccelerationOfOrigin, twistOfEndEffectorWithRespectToElevator); }
/** * Computes and packs the linear velocity of a point defined by {@code bodyFixedPoint} that is * fixed to the given {@code body} with respect to the given {@code base}. * <p> * The result will be the linear velocity of the {@code bodyFixedPoint} with respect to the * {@code base.getBodyFixedFrame()}. The vector is expressed in {@code base.getBodyFixedFrame()}. * </p> * * @param base the rigid-body with respect to which the linear velocity is to be computed. * @param body the rigid-body to which {@code bodyFixedPoint} is attached to. * @param bodyFixedPoint the coordinates of the point attached to {@code body} that linear * velocity is to be computed. Not modified. * @param linearVelocityToPack the linear velocity of the body fixed point with respect to the * inertial frame. Modified. */ public void getLinearVelocityOfBodyFixedPoint(RigidBodyBasics base, RigidBodyBasics body, FramePoint3D bodyFixedPoint, FrameVector3D linearVelocityToPack) { FramePoint3D localPoint = pointForGetLinearVelocityOfBodyFixedPoint; Twist localTwist = twistForGetLinearVelocityOfBodyFixedPoint; getRelativeTwist(base, body, localTwist); ReferenceFrame baseFrame = localTwist.getBaseFrame(); localPoint.setIncludingFrame(bodyFixedPoint); localTwist.changeFrame(baseFrame); localPoint.changeFrame(baseFrame); localTwist.getLinearVelocityAt(localPoint, linearVelocityToPack); }
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); }
twistCalculator.getRelativeTwist(twistOfCurrentWithRespectToNew, body, base); twistOfCurrentWithRespectToNew.changeFrame(base.getBodyFixedFrame());
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 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); }
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); } }
twistCalculator.getRelativeTwist(twistOfCurrentWithRespectToNew, base, body); twistCalculator.getTwistOfBody(twistOfBodyWithRespectToBase, body);
public DenseMatrix64F computeResidual() { Vector3d measuredAngularVelocityVector3d = angularVelocityMeasurementInputPort.getData(); TwistCalculator twistCalculator = inverseDynamicsStructureInputPort.getData().getTwistCalculator(); twistCalculator.getRelativeTwist(tempTwist, orientationEstimationLink, measurementLink); tempTwist.getAngularPart(relativeAngularVelocity); relativeAngularVelocity.changeFrame(measurementFrame); predictedAngularVelocityMeasurementTemp.setIncludingFrame(angularVelocityStatePort.getData()); predictedAngularVelocityMeasurementTemp.changeFrame(measurementFrame); predictedAngularVelocityMeasurementTemp.add(relativeAngularVelocity); predictedAngularVelocityMeasurementTemp.add(biasStatePort.getData()); angularVelocityResidual.setIncludingFrame(measurementFrame, measuredAngularVelocityVector3d); angularVelocityResidual.sub(predictedAngularVelocityMeasurementTemp); MatrixTools.insertTuple3dIntoEJMLVector(angularVelocityResidual.getVector(), residual, 0); return residual; }
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); }
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); }
public void computeMatrixBlocks() { FullInverseDynamicsStructure inverseDynamicsStructure = inverseDynamicsStructureInputPort.getData(); TwistCalculator twistCalculator = inverseDynamicsStructure.getTwistCalculator(); SpatialAccelerationCalculator spatialAccelerationCalculator = inverseDynamicsStructure.getSpatialAccelerationCalculator(); computeUnbiasedEstimatedMeasurement(spatialAccelerationCalculator, estimatedMeasurement); // R_{w}^{p} estimationFrame.getTransformToDesiredFrame(tempTransform, ReferenceFrame.getWorldFrame()); tempTransform.getRotation(rotationFromEstimationToWorld); // R_{p}^{m} estimationFrame.getTransformToDesiredFrame(tempTransform, measurementFrame); tempTransform.getRotation(rotationFromEstimationToMeasurement); // T_{i}^{p,p} twistCalculator.getRelativeTwist(twistOfMeasurementFrameWithRespectToEstimation, estimationLink, measurementLink); // r^{p} rPTemp.setIncludingFrame(centerOfMassPositionPort.getData()); rPTemp.changeFrame(estimationFrame); rdTemp.setIncludingFrame(centerOfMassVelocityPort.getData()); computeRpd(rPdTemp, twistCalculator, rPTemp, rdTemp); jacobianAssembler.preCompute(estimatedMeasurement.getVector()); computeCenterOfMassVelocityBlock(); computeCenterOfMassAccelerationBlock(); computeOrientationBlock(twistCalculator, spatialAccelerationCalculator, rotationFromEstimationToWorld, twistOfMeasurementFrameWithRespectToEstimation, rPTemp, rPdTemp); computeAngularVelocityBlock(rotationFromEstimationToWorld, twistOfMeasurementFrameWithRespectToEstimation, rPTemp, rdTemp, rPdTemp); computeAngularAccelerationBlock(rotationFromEstimationToMeasurement); }
twistCalculator.getRelativeTwist(twistOfMeasurementLink, elevator, measurementLink); spatialAccelerationCalculator.getRelativeAcceleration(spatialAccelerationOfMeasurementLink, elevator, measurementLink); spatialAccelerationOfMeasurementLink.changeFrame(elevatorFrame, twistOfMeasurementLink, twistOfMeasurementLink);
@Override public void doSpecificAction() { feedbackControlCommandList.clear(); desiredOrientation.setToZero(contactableFoot.getFrameAfterParentJoint()); desiredOrientation.changeFrame(worldFrame); desiredOrientation.getYawPitchRoll(tempYawPitchRoll); twistCalculator.getRelativeTwist(footTwist, rootBody, contactableFoot.getRigidBody()); footTwist.changeFrame(contactableFoot.getFrameAfterParentJoint()); toeOffCurrentPitchAngle.set(tempYawPitchRoll[1]); toeOffCurrentPitchVelocity.set(footTwist.getAngularPartY()); desiredPosition.setToZero(contactableFoot.getFrameAfterParentJoint()); desiredPosition.changeFrame(worldFrame); computeDesiredsForFreeMotion(); desiredOrientation.setYawPitchRoll(desiredYawToHold, toeOffDesiredPitchAngle.getDoubleValue(), desiredRollToHold); desiredLinearVelocity.setToZero(worldFrame); desiredAngularVelocity.setIncludingFrame(contactableFoot.getFrameAfterParentJoint(), 0.0, toeOffDesiredPitchVelocity.getDoubleValue(), 0.0); desiredAngularVelocity.changeFrame(worldFrame); desiredLinearAcceleration.setToZero(worldFrame); desiredAngularAcceleration.setIncludingFrame(contactableFoot.getFrameAfterParentJoint(), 0.0, toeOffDesiredPitchAcceleration.getDoubleValue(), 0.0); desiredAngularAcceleration.changeFrame(worldFrame); orientationFeedbackControlCommand.set(desiredOrientation, desiredAngularVelocity, desiredAngularAcceleration); pointFeedbackControlCommand.set(desiredContactPointPosition, desiredLinearVelocity, desiredLinearAcceleration); setupSingleContactPoint(); feedbackControlCommandList.addCommand(orientationFeedbackControlCommand); feedbackControlCommandList.addCommand(pointFeedbackControlCommand); }
twistCalculator.getRelativeTwist(tempRootToEstimationTwist, estimationLink, rootJoint.getSuccessor()); tempRootToEstimationTwist.changeFrame(rootJoint.getFrameAfterJoint());
twistCalculator.getRelativeTwist(bodyTwists[j], rootBody, rigidBodies[j]);
twistCalculator.getRelativeTwist(tempTwist, rootBody, rigidBody);