public void getSteeringWheelPose(
RobotSide robotSide,
FramePoint centerToPack,
FrameVector rotationAxisToPack,
FrameVector zeroAxisToPack) {
centerToPack.setIncludingFrame(
worldFrame, steeringWheelCenterAtomic.get(robotSide).getAndSet(null));
rotationAxisToPack.setIncludingFrame(
worldFrame, steeringWheelRotationAxisAtomic.get(robotSide).getAndSet(null));
zeroAxisToPack.setIncludingFrame(
worldFrame, steeringWheelZeroAxisAtomic.get(robotSide).getAndSet(null));
}
public void getLinearAcceleration(FrameVector linearAccelerationToPack) {
linearAccelerationToPack.setIncludingFrame(
ReferenceFrame.getWorldFrame(),
qdd_x.getDoubleValue(),
qdd_y.getDoubleValue(),
qdd_z.getDoubleValue());
}
public void getVelocity(FrameVector linearVelocityToPack) {
linearVelocityToPack.setIncludingFrame(
ReferenceFrame.getWorldFrame(),
qd_x.getDoubleValue(),
qd_y.getDoubleValue(),
qd_z.getDoubleValue());
}
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 compute(double time, boolean adjustAngle) {
this.currentTime.set(time);
time = MathTools.clipToMinMax(time, 0.0, desiredTrajectoryTime.getDoubleValue());
anglePolynomial.compute(time);
double angle = anglePolynomial.getPosition();
double angleDot = anglePolynomial.getVelocity();
double angleDDot = anglePolynomial.getAcceleration();
double cos = Math.cos(angle);
double sin = Math.sin(angle);
double r = initialRadius.getDoubleValue();
double x = r * cos;
double y = r * sin;
double z = initialZ.getDoubleValue();
currentPosition.setIncludingFrame(circleFrame, x, y, z);
yoCurrentPositionWorld.setAndMatchFrame(currentPosition);
currentRelativeAngle.set(
computeAngleDifferenceMinusPiToPi(angle, initialAngle.getDoubleValue()));
if (adjustAngle)
currentAdjustedRelativeAngle.set(
adjustCurrentDesiredRelativeAngle(currentRelativeAngle.getDoubleValue()));
else currentAdjustedRelativeAngle.set(currentRelativeAngle.getDoubleValue());
angle =
trimAngleMinusPiToPi(
currentAdjustedRelativeAngle.getDoubleValue() + initialAngle.getDoubleValue());
if (isDone()) {
angle = finalAngle.getDoubleValue();
angleDot = 0.0;
angleDDot = 0.0;
}
cos = Math.cos(angle);
sin = Math.sin(angle);
x = r * cos;
y = r * sin;
double xDot = -r * sin * angleDot;
double yDot = x * angleDot;
double zDot = 0.0;
double xDDot = -r * cos * angleDot * angleDot - y * angleDDot;
double yDDot = xDot * angleDot + x * angleDDot;
double zDDot = 0.0;
currentPosition.setIncludingFrame(circleFrame, x, y, z);
currentVelocity.setIncludingFrame(circleFrame, xDot, yDot, zDot);
currentAcceleration.setIncludingFrame(circleFrame, xDDot, yDDot, zDDot);
if (rotateHandAngleAboutAxis) {
rotateInitialOrientation(currentOrientation, angle - initialAngle.getDoubleValue());
currentAngularVelocity.setIncludingFrame(circleFrame, 0.0, 0.0, angleDot);
currentAngularAcceleration.setIncludingFrame(circleFrame, 0.0, 0.0, angleDDot);
} else {
currentOrientation.setIncludingFrame(initialOrientation);
currentAngularVelocity.setIncludingFrame(circleFrame, 0.0, 0.0, 0.0);
currentAngularAcceleration.setIncludingFrame(circleFrame, 0.0, 0.0, 0.0);
}
yoCurrentPosition.setAndMatchFrame(currentPosition);
yoCurrentAdjustedPositionWorld.setAndMatchFrame(currentPosition);
yoCurrentVelocity.setAndMatchFrame(currentVelocity);
yoCurrentAcceleration.setAndMatchFrame(currentAcceleration);
currentOrientation.changeFrame(trajectoryFrame);
yoCurrentOrientation.set(currentOrientation);
yoCurrentAngularVelocity.setAndMatchFrame(currentAngularVelocity);
yoCurrentAngularAcceleration.setAndMatchFrame(currentAngularAcceleration);
updateTangentialCircleFrame();
}
public void getAngularAcceleration(
FrameVector angularAccelerationToPack, ReferenceFrame bodyFrame) {
angularAccelerationToPack.setIncludingFrame(
bodyFrame, qdd_wx.getDoubleValue(), qdd_wy.getDoubleValue(), qdd_wz.getDoubleValue());
}
public void getAngularVelocity(FrameVector angularVelocityToPack, ReferenceFrame bodyFrame) {
angularVelocityToPack.setIncludingFrame(
bodyFrame, qd_wx.getDoubleValue(), qd_wy.getDoubleValue(), qd_wz.getDoubleValue());
}