public void compute(
FrameVector output,
FrameOrientation desiredOrientation,
FrameVector desiredAngularVelocity,
FrameVector currentAngularVelocity,
FrameVector feedForward) {
computeProportionalTerm(desiredOrientation);
if (currentAngularVelocity != null)
computeDerivativeTerm(desiredAngularVelocity, currentAngularVelocity);
computeIntegralTerm();
output.setToZero(proportionalTerm.getReferenceFrame());
output.add(proportionalTerm);
output.add(derivativeTerm);
output.add(integralTerm);
// Limit the max acceleration of the feedback, but not of the feedforward...
// JEP changed 150430 based on Atlas hitting limit stops.
double feedbackAngularActionMagnitude = output.length();
double maximumAction = gains.getMaximumFeedback();
if (feedbackAngularActionMagnitude > maximumAction) {
output.scale(maximumAction / feedbackAngularActionMagnitude);
}
feedbackAngularAction.set(output);
rateLimitedFeedbackAngularAction.update();
rateLimitedFeedbackAngularAction.getFrameTuple(output);
feedForward.changeFrame(bodyFrame);
output.add(feedForward);
}
private void computeDerivativeTerm(
FrameVector desiredAngularVelocity, FrameVector currentAngularVelocity) {
desiredAngularVelocity.changeFrame(bodyFrame);
currentAngularVelocity.changeFrame(bodyFrame);
derivativeTerm.sub(desiredAngularVelocity, currentAngularVelocity);
// Limit the maximum velocity error considered for control action
double maximumVelocityError = gains.getMaximumDerivativeError();
double velocityErrorMagnitude = derivativeTerm.length();
if (velocityErrorMagnitude > maximumVelocityError) {
derivativeTerm.scale(maximumVelocityError / velocityErrorMagnitude);
}
velocityError.set(derivativeTerm);
derivativeGainMatrix.transform(derivativeTerm.getVector());
}
@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);
}
}
@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testSingleRigidBodyRotation() {
Random random = new Random(1766L);
RigidBody elevator = new RigidBody("elevator", world);
Vector3d jointAxis = RandomTools.generateRandomVector(random);
jointAxis.normalize();
RigidBodyTransform transformToParent = new RigidBodyTransform();
transformToParent.setIdentity();
RevoluteJoint joint =
ScrewTools.addRevoluteJoint("joint", elevator, transformToParent, jointAxis);
RigidBody body =
ScrewTools.addRigidBody(
"body",
joint,
RandomTools.generateRandomDiagonalMatrix3d(random),
random.nextDouble(),
new Vector3d());
joint.setQ(random.nextDouble());
joint.setQd(random.nextDouble());
Momentum momentum = computeMomentum(elevator, world);
momentum.changeFrame(world);
FrameVector linearMomentum =
new FrameVector(momentum.getExpressedInFrame(), momentum.getLinearPartCopy());
FrameVector angularMomentum =
new FrameVector(momentum.getExpressedInFrame(), momentum.getAngularPartCopy());
FrameVector linearMomentumCheck = new FrameVector(world);
Matrix3d inertia = body.getInertia().getMassMomentOfInertiaPartCopy();
Vector3d angularMomentumCheckVector = new Vector3d(jointAxis);
angularMomentumCheckVector.scale(joint.getQd());
inertia.transform(angularMomentumCheckVector);
FrameVector angularMomentumCheck =
new FrameVector(body.getInertia().getExpressedInFrame(), angularMomentumCheckVector);
angularMomentumCheck.changeFrame(world);
double epsilon = 1e-9;
JUnitTools.assertTuple3dEquals(
linearMomentumCheck.getVector(), linearMomentum.getVector(), epsilon);
JUnitTools.assertTuple3dEquals(
angularMomentumCheck.getVector(), angularMomentum.getVector(), epsilon);
assertTrue(angularMomentum.length() > epsilon);
}
protected List<FramePoint> getContactPoints(RobotSide swingSide) {
double stepPitch = this.stepPitch.getDoubleValue();
List<FramePoint> allContactPoints = contactableBodies.get(swingSide).getContactPointsCopy();
if (stepPitch == 0.0) {
return allContactPoints;
} else {
FrameVector forwardInFootFrame =
new FrameVector(contactableBodies.get(swingSide).getFrameAfterParentJoint());
ReferenceFrame frame = allContactPoints.get(0).getReferenceFrame();
forwardInFootFrame.changeFrame(frame);
forwardInFootFrame.scale(Math.signum(stepPitch));
int nPoints = 2;
return DesiredFootstepCalculatorTools.computeMaximumPointsInDirection(
allContactPoints, forwardInFootFrame, nPoints);
}
}
@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));
}
