private void computeBiasedEstimatedMeasurement(SpatialAccelerationCalculator spatialAccelerationCalculator, FrameVector estimatedMeasurement) { computeUnbiasedEstimatedMeasurement(spatialAccelerationCalculator, estimatedMeasurement); estimatedMeasurement.add(biasPort.getData()); }
private void updateCenterOfMassVelocity(FrameVector centerOfMassVelocityDelta) { centerOfMassVelocity.set(centerOfMassVelocityPort.getData()); centerOfMassVelocity.add(centerOfMassVelocityDelta); centerOfMassVelocityPort.setData(centerOfMassVelocity); } }
private void updateCenterOfMassVelocity(FrameVector centerOfMassVelocityDelta) { centerOfMassVelocity.set(centerOfMassVelocityPort.getData()); centerOfMassVelocity.add(centerOfMassVelocityDelta); centerOfMassVelocityPort.setData(centerOfMassVelocity); } }
private void updateBias(FrameVector biasDelta) { bias.set(biasPort.getData()); bias.add(biasDelta); biasPort.setData(bias); } }
private void updateAngularVelocity(FrameVector angularVelocityDelta) { angularVelocity.set(angularVelocityPort.getData()); angularVelocity.add(angularVelocityDelta); angularVelocityPort.setData(angularVelocity); } }
public void correctState(DenseMatrix64F correction) { MatrixTools.extractTuple3dFromEJMLVector(angularAccelerationDelta.getVector(), correction, 0); angularAcceleration.set(angularAccelerationStatePort.getData()); angularAcceleration.add(angularAccelerationDelta); angularAccelerationStatePort.setData(angularAcceleration); } }
/** * Packs the linear velocity of a point that is fixed in bodyFrame but is expressed in baseFrame, * with respect to baseFrame, expressed in expressedInFrame */ public void getLinearVelocityOfPointFixedInBodyFrame(FrameVector linearVelocityToPack, FramePoint pointFixedInBodyFrame) { baseFrame.checkReferenceFrameMatch(expressedInFrame); pointFixedInBodyFrame.checkReferenceFrameMatch(baseFrame); pointFixedInBodyFrame.get(freeVector); linearVelocityToPack.setToZero(expressedInFrame); linearVelocityToPack.cross(angularPart, freeVector); linearVelocityToPack.add(linearPart); }
public void getVelocity(FrameVector velocityToPack, double parameter) { double q = parameter; MathTools.checkIfInRange(q, 0.0, 1.0); velocityToPack.setToZero(referenceFrame); // 2 * c2 * q c2.getFrameTuple(velocityToPack); velocityToPack.scale(2.0 * q); // c1 c1.getFrameTuple(tempPackVelocity); velocityToPack.add(tempPackVelocity); }
public void correctState(DenseMatrix64F correction) { MatrixTools.extractTuple3dFromEJMLVector(comAccelerationDelta.getVector(), correction, 0); comAcceleration.set(centerOfMassAccelerationStatePort.getData()); comAcceleration.add(comAccelerationDelta); centerOfMassAccelerationStatePort.setData(comAcceleration); } }
/** * 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 computeLinearVelocityFromMergingMeasurements() { // TODO Check out AlphaFusedYoVariable to that imuBasedLinearStateCalculator.updateIMUAndRootJointLinearVelocity(pelvisVelocityIMUPart); kinematicsBasedLinearStateCalculator.getPelvisVelocity(pelvisVelocityKinPart); pelvisVelocityIMUPart.scale(alphaIMUAgainstKinematicsForVelocity.getDoubleValue()); pelvisVelocityKinPart.scale(1.0 - alphaIMUAgainstKinematicsForVelocity.getDoubleValue()); rootJointVelocity.add(pelvisVelocityIMUPart, pelvisVelocityKinPart); yoRootJointVelocity.set(rootJointVelocity); imuBasedLinearStateCalculator.correctIMULinearVelocity(rootJointVelocity); }
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; }
public void getAcceleration(FrameVector accelerationToPack) { double parameter = minimumJerkTrajectory.getPosition(); parameter = MathTools.clipToMinMax(parameter, 0.0, 1.0); parabolicTrajectoryGenerator.getAcceleration(accelerationToPack); accelerationToPack.scale(minimumJerkTrajectory.getVelocity() * minimumJerkTrajectory.getVelocity()); parabolicTrajectoryGenerator.getVelocity(tempVector, parameter); tempVector.scale(minimumJerkTrajectory.getAcceleration()); accelerationToPack.add(tempVector); }
public void getCoMAcceleration(FrameVector comAccelerationToPack) { boolean firstIteration = true; double totalMass = 0.0; for (RigidBody rigidBody : rigidBodies) { double mass = rigidBody.getInertia().getMass(); rigidBody.getCoMOffset(comLocation); spatialAccelerationCalculator.getLinearAccelerationOfBodyFixedPoint(linkLinearMomentumDot, base, rigidBody, comLocation); linkLinearMomentumDot.scale(mass); if (firstIteration) comAccelerationToPack.setIncludingFrame(linkLinearMomentumDot); else comAccelerationToPack.add(linkLinearMomentumDot); totalMass += mass; firstIteration = false; } comAccelerationToPack.scale(1.0 / totalMass); } }
public void getAcceleration(FrameVector accelerationToPack) { double parameter = minimumJerkTrajectory.getPosition(); parameter = MathTools.clipToMinMax(parameter, 0.0, 1.0); parabolicTrajectoryGenerator.getAcceleration(accelerationToPack); accelerationToPack.scale(minimumJerkTrajectory.getVelocity() * minimumJerkTrajectory.getVelocity()); parabolicTrajectoryGenerator.getVelocity(tempVector, parameter); tempVector.scale(minimumJerkTrajectory.getAcceleration()); accelerationToPack.add(tempVector); }
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); }
public void startComputation() { imuFramePoint.setToZero(measurementFrame); spatialAccelerationCalculator.getLinearAccelerationOfBodyFixedPoint(linearAccelerationFrameVector, rigidBody, imuFramePoint); linearAccelerationFrameVector.changeFrame(gravitationalAcceleration.getReferenceFrame()); linearAccelerationFrameVector.add(gravitationalAcceleration); linearAccelerationFrameVector.changeFrame(measurementFrame); linearAccelerationFrameVector.get(linearAcceleration); yoFrameVectorPerfect.set(linearAcceleration); corrupt(linearAcceleration); yoFrameVectorNoisy.set(linearAcceleration); linearAccelerationOutputPort.setData(linearAcceleration); }
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); }
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 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); }