/** * Computes and packs the linear acceleration of the point {@code bodyFixedPoint} that * is attached to {@code body} with respect to {@code rootBody}. * <p> * WARNING: This method assumes that the internal memory of this {@code TwistCalculator} * is up-to-date. * The update of the internal memory is done through the method {@link #compute()}. * </p> * <p> * See {@link #getLinearAccelerationOfBodyFixedPoint(FrameVector, RigidBody, RigidBody, FramePoint)}. * </p> * * @param linearAccelerationToPack the linear acceleration of the body fixed point. Modified. * @param body the rigid-body to which {@code bodyFixedPoint} belongs. * @param bodyFixedPoint the point to compute the linear acceleration of. Not modified. */ // FIXME Change the method signature to have linearAccelerationToPack as the last argument. public void getLinearAccelerationOfBodyFixedPoint(FrameVector linearAccelerationToPack, RigidBody body, FramePoint bodyFixedPoint) { getLinearAccelerationOfBodyFixedPoint(linearAccelerationToPack, getRootBody(), body, bodyFixedPoint); }
@Override public void computeAchievedAcceleration() { accelerationControlModule.getBodyFixedPoint(tempPosition); spatialAccelerationCalculator.getLinearAccelerationOfBodyFixedPoint(achievedLinearAcceleration, base, endEffector, tempPosition); yoAchievedLinearAcceleration.setAndMatchFrame(achievedLinearAcceleration); }
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); } }
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 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); }