@Override
public void onEntry() {
// initialize body orientation
bodyOrientation.setToZero(bodyZUpFrame);
bodyOrientation.changeFrame(worldFrame);
bodyYaw = bodyOrientation.getYaw();
// initialize step queue
updateXGaitSettings();
supportCentroid.setToZero(supportFrame);
double initialTime = timestamp.getDoubleValue() + initialStepDelayParameter.get();
Vector3d initialVelocity = planarVelocityProvider.get();
RobotQuadrant initialQuadrant =
(xGaitSettings.getEndPhaseShift() < 90)
? RobotQuadrant.HIND_LEFT
: RobotQuadrant.FRONT_LEFT;
xGaitStepPlanner.computeInitialPlan(
xGaitPreviewSteps,
initialVelocity,
initialQuadrant,
supportCentroid,
initialTime,
bodyYaw,
xGaitSettings);
for (int i = 0; i < 2; i++) {
RobotEnd robotEnd = xGaitPreviewSteps.get(i).getRobotQuadrant().getEnd();
xGaitCurrentSteps.get(robotEnd).set(xGaitPreviewSteps.get(i));
}
this.process();
}
public void setInitialPoseToMatchReferenceFrame(ReferenceFrame referenceFrame) {
initialPosition.setToZero(referenceFrame);
initialOrientation.setToZero(referenceFrame);
initialPosition.changeFrame(trajectoryFrame);
initialOrientation.changeFrame(trajectoryFrame);
yoInitialPosition.set(initialPosition);
yoInitialOrientation.set(initialOrientation);
}
private void rotateInitialOrientation(
FrameOrientation orientationToPack, double angleFromInitialOrientation) {
initialOrientation.changeFrame(circleFrame);
orientationToPack.setIncludingFrame(initialOrientation);
axisRotationTransform.rotZ(angleFromInitialOrientation);
orientationToPack.applyTransform(axisRotationTransform);
}
@ContinuousIntegrationTest(estimatedDuration = 0.2)
@Test(timeout = 30000)
public void testChangeFrame() throws Exception {
double epsilon = 1.0e-10;
Random random = new Random(21651016L);
ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();
ReferenceFrame expectedFrame = worldFrame;
double expectedTime = RandomTools.generateRandomDouble(random, 0.0, 1000.0);
FrameOrientation expectedOrientation =
FrameOrientation.generateRandomFrameOrientation(random, expectedFrame);
FrameVector expectedAngularVelocity =
FrameVector.generateRandomFrameVector(random, expectedFrame);
String expectedNamePrefix = "test";
String expectedNameSuffix = "blop";
YoFrameSO3TrajectoryPoint testedYoFrameSO3TrajectoryPoint =
new YoFrameSO3TrajectoryPoint(
expectedNamePrefix,
expectedNameSuffix,
new YoVariableRegistry("schnoop"),
expectedFrame);
testedYoFrameSO3TrajectoryPoint.set(expectedTime, expectedOrientation, expectedAngularVelocity);
ReferenceFrame[] randomFrames = new ReferenceFrame[10];
randomFrames[0] = worldFrame;
for (int i = 1; i < 10; i++)
randomFrames[i] =
ReferenceFrame.generateRandomReferenceFrame(
"randomFrame" + i,
random,
random.nextBoolean() ? worldFrame : randomFrames[random.nextInt(i)]);
for (int i = 0; i < 10000; i++) {
expectedFrame = randomFrames[random.nextInt(10)];
testedYoFrameSO3TrajectoryPoint.registerReferenceFrame(expectedFrame);
expectedOrientation.changeFrame(expectedFrame);
expectedAngularVelocity.changeFrame(expectedFrame);
testedYoFrameSO3TrajectoryPoint.changeFrame(expectedFrame);
assertWaypointContainsExpectedData(
expectedNamePrefix,
expectedNameSuffix,
expectedFrame,
expectedTime,
expectedOrientation,
expectedAngularVelocity,
testedYoFrameSO3TrajectoryPoint,
epsilon);
}
}
public void initialize() {
currentTime.set(0.0);
desiredTrajectoryTime.set(desiredTrajectoryTimeProvider.getValue());
yoInitialPosition.getFrameTupleIncludingFrame(initialPosition);
initialPosition.changeFrame(circleFrame);
if (rotateHandAngleAboutAxis)
rotateInitialOrientation(finalOrientation, desiredRotationAngle.getDoubleValue());
else finalOrientation.setIncludingFrame(initialOrientation);
finalOrientation.changeFrame(trajectoryFrame);
yoFinalOrientation.set(finalOrientation);
double x = initialPosition.getX();
double y = initialPosition.getY();
initialZ.set(initialPosition.getZ());
initialRadius.set(Math.sqrt(x * x + y * y));
initialAngle.set(Math.atan2(y, x));
finalAngle.set(initialAngle.getDoubleValue() + desiredRotationAngle.getDoubleValue());
// anglePolynomial.setLinear(0.0, desiredTrajectoryTime.getDoubleValue(),
// initialAngle.getDoubleValue(), finalAngle.getDoubleValue());
anglePolynomial.setCubic(
0.0,
desiredTrajectoryTime.getDoubleValue(),
initialAngle.getDoubleValue(),
0.0,
finalAngle.getDoubleValue(),
0.0);
// anglePolynomial.setQuintic(0.0, desiredTrajectoryTime.getDoubleValue(),
// initialAngle.getDoubleValue(), 0.0, 0.0, finalAngle.getDoubleValue(), 0.0, 0.0);
double xFinal = initialRadius.getDoubleValue() * Math.cos(finalAngle.getDoubleValue());
double yFinal = initialRadius.getDoubleValue() * Math.sin(finalAngle.getDoubleValue());
double zFinal = initialZ.getDoubleValue();
finalPosition.setIncludingFrame(circleFrame, xFinal, yFinal, zFinal);
yoFinalPosition.setAndMatchFrame(finalPosition);
rotateInitialOrientation(finalOrientation, finalAngle.getDoubleValue());
currentAngleTrackingError.set(0.0);
currentControlledFrameRelativeAngle.set(initialAngle.getDoubleValue());
updateTangentialCircleFrame();
if (visualize) visualizeTrajectory();
}
private void computeProportionalTerm(FrameOrientation desiredOrientation) {
desiredOrientation.changeFrame(bodyFrame);
desiredOrientation.getQuaternion(errorQuaternion);
errorAngleAxis.set(errorQuaternion);
errorAngleAxis.setAngle(AngleTools.trimAngleMinusPiToPi(errorAngleAxis.getAngle()));
// Limit the maximum position error considered for control action
double maximumError = gains.getMaximumProportionalError();
if (errorAngleAxis.getAngle() > maximumError) {
errorAngleAxis.setAngle(Math.signum(errorAngleAxis.getAngle()) * maximumError);
}
proportionalTerm.set(errorAngleAxis.getX(), errorAngleAxis.getY(), errorAngleAxis.getZ());
proportionalTerm.scale(errorAngleAxis.getAngle());
rotationErrorInBody.set(proportionalTerm);
proportionalGainMatrix.transform(proportionalTerm.getVector());
}
@Override
public void process() {
double currentTime = timestamp.getDoubleValue();
// update body orientation
bodyYaw += planarVelocityProvider.get().getZ() * controlDT;
bodyOrientation.changeFrame(worldFrame);
bodyOrientation.setYawPitchRoll(bodyYaw, 0.0, 0.0);
// update xgait current steps
for (int i = 0; i < xGaitPreviewSteps.size(); i++) {
QuadrupedTimedStep xGaitPreviewStep = xGaitPreviewSteps.get(i);
if (xGaitPreviewStep.getTimeInterval().getStartTime() <= currentTime) {
xGaitCurrentSteps.get(xGaitPreviewStep.getRobotEnd()).set(xGaitPreviewStep);
}
}
// update xgait preview steps
updateXGaitSettings();
Vector3d inputVelocity = planarVelocityProvider.get();
xGaitStepPlanner.computeOnlinePlan(
xGaitPreviewSteps, xGaitCurrentSteps, inputVelocity, currentTime, bodyYaw, xGaitSettings);
// update step sequence
stepSequence.clear();
for (RobotEnd robotEnd : RobotEnd.values) {
if (xGaitCurrentSteps.get(robotEnd).getTimeInterval().getEndTime() >= currentTime) {
stepSequence.add();
stepSequence.get(stepSequence.size() - 1).set(xGaitCurrentSteps.get(robotEnd));
}
}
for (int i = 0; i < xGaitPreviewSteps.size(); i++) {
if (xGaitPreviewSteps.get(i).getTimeInterval().getEndTime() >= currentTime) {
stepSequence.add();
stepSequence.get(stepSequence.size() - 1).set(xGaitPreviewSteps.get(i));
}
}
}
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();
}
@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testSomeMoreSettersAndGetters() {
String namePrefix = "point";
String nameSuffix = "toTest";
YoVariableRegistry registry = new YoVariableRegistry("myRegistry");
ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();
YoFrameSO3TrajectoryPoint yoFrameSO3TrajectoryPoint =
new YoFrameSO3TrajectoryPoint(namePrefix, nameSuffix, registry, worldFrame);
yoFrameSO3TrajectoryPoint.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame());
double time = 3.4;
FrameOrientation orientation =
new FrameOrientation(worldFrame, new Quat4d(0.1, 0.22, 0.34, 0.56));
FrameVector angularVelocity = new FrameVector(worldFrame, 1.7, 8.4, 2.2);
yoFrameSO3TrajectoryPoint.setTime(time);
yoFrameSO3TrajectoryPoint.setOrientation(orientation);
yoFrameSO3TrajectoryPoint.setAngularVelocity(angularVelocity);
PoseReferenceFrame poseFrame = new PoseReferenceFrame("poseFrame", new FramePose(worldFrame));
FramePoint poseFramePosition = new FramePoint(worldFrame, new Point3d(0.5, 7.7, 9.2));
poseFrame.setPositionAndUpdate(poseFramePosition);
FrameOrientation poseOrientation =
new FrameOrientation(worldFrame, new AxisAngle4d(1.2, 3.9, 4.7, 2.2));
poseFrame.setOrientationAndUpdate(poseOrientation);
yoFrameSO3TrajectoryPoint.registerReferenceFrame(poseFrame);
yoFrameSO3TrajectoryPoint.changeFrame(poseFrame);
assertFalse(
orientation.epsilonEquals(
yoFrameSO3TrajectoryPoint.getOrientation().getFrameOrientationCopy(), 1e-10));
assertFalse(
angularVelocity.epsilonEquals(
yoFrameSO3TrajectoryPoint.getAngularVelocity().getFrameVectorCopy(), 1e-10));
orientation.changeFrame(poseFrame);
angularVelocity.changeFrame(poseFrame);
assertTrue(
orientation.epsilonEquals(
yoFrameSO3TrajectoryPoint.getOrientation().getFrameOrientationCopy(), 1e-10));
assertTrue(
angularVelocity.epsilonEquals(
yoFrameSO3TrajectoryPoint.getAngularVelocity().getFrameVectorCopy(), 1e-10));
YoFrameSO3TrajectoryPoint yoFrameSO3TrajectoryPointTwo =
new YoFrameSO3TrajectoryPoint(namePrefix, nameSuffix + "Two", registry, poseFrame);
yoFrameSO3TrajectoryPointTwo.setTime(time);
yoFrameSO3TrajectoryPointTwo.setOrientation(orientation);
yoFrameSO3TrajectoryPointTwo.setAngularVelocity(angularVelocity);
assertTrue(yoFrameSO3TrajectoryPointTwo.epsilonEquals(yoFrameSO3TrajectoryPointTwo, 1e-10));
}