public void setDesiredAcceleration(
FrameVector angularAcceleration, FrameVector linearAcceleration) {
desiredAcceleration.reshape(6, 1);
MatrixTools.insertTuple3dIntoEJMLVector(
angularAcceleration.getVector(), desiredAcceleration, 0);
MatrixTools.insertTuple3dIntoEJMLVector(linearAcceleration.getVector(), desiredAcceleration, 3);
}
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);
}
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));
}
@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);
}
}
protected void updateClosedJoint() {
super.updateClosedJoint();
integralForce.scale(
integralStiffness.getDoubleValue(), yoConnectionPositionIntegratedError.getFrameTuple());
connectionPointA.getForce(tempForce);
tempForce.add(integralForce.getVector());
connectionPointA.setForce(tempForce);
tempForce.scale(-1.0);
connectionPointB.setForce(tempForce);
}
public void getVelocity(FrameVector linearVelocityToPack) {
linearVelocityToPack.setIncludingFrame(
ReferenceFrame.getWorldFrame(),
qd_x.getDoubleValue(),
qd_y.getDoubleValue(),
qd_z.getDoubleValue());
}
public void getLinearAcceleration(FrameVector linearAccelerationToPack) {
linearAccelerationToPack.setIncludingFrame(
ReferenceFrame.getWorldFrame(),
qdd_x.getDoubleValue(),
qdd_y.getDoubleValue(),
qdd_z.getDoubleValue());
}
@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);
}
}
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());
}
@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testSetToNaN() throws Exception {
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);
testedYoFrameSO3TrajectoryPoint.setToNaN();
assertTrue(Double.isNaN(testedYoFrameSO3TrajectoryPoint.getTime()));
assertTrue(testedYoFrameSO3TrajectoryPoint.getOrientation().containsNaN());
assertTrue(testedYoFrameSO3TrajectoryPoint.getAngularVelocity().containsNaN());
expectedFrame = ReferenceFrame.generateRandomReferenceFrame("blop", random, worldFrame);
testedYoFrameSO3TrajectoryPoint.registerReferenceFrame(expectedFrame);
expectedTime = RandomTools.generateRandomDouble(random, 0.0, 1000.0);
expectedOrientation = FrameOrientation.generateRandomFrameOrientation(random, worldFrame);
expectedAngularVelocity = FrameVector.generateRandomFrameVector(random, worldFrame);
testedYoFrameSO3TrajectoryPoint.switchCurrentReferenceFrame(worldFrame);
testedYoFrameSO3TrajectoryPoint.registerReferenceFrame(worldFrame);
testedYoFrameSO3TrajectoryPoint.set(expectedTime, expectedOrientation, expectedAngularVelocity);
testedYoFrameSO3TrajectoryPoint.setToNaN(expectedFrame);
assertTrue(expectedFrame == testedYoFrameSO3TrajectoryPoint.getReferenceFrame());
assertTrue(Double.isNaN(testedYoFrameSO3TrajectoryPoint.getTime()));
assertTrue(testedYoFrameSO3TrajectoryPoint.getOrientation().containsNaN());
assertTrue(testedYoFrameSO3TrajectoryPoint.getAngularVelocity().containsNaN());
}
private void computeIntegralTerm() {
if (gains.getMaximumIntegralError() < 1e-5) {
integralTerm.setToZero(bodyFrame);
return;
}
double integratedErrorAngle = errorAngleAxis.getAngle() * dt;
double errorIntegratedX = errorAngleAxis.getX() * integratedErrorAngle;
double errorIntegratedY = errorAngleAxis.getY() * integratedErrorAngle;
double errorIntegratedZ = errorAngleAxis.getZ() * integratedErrorAngle;
rotationErrorCumulated.add(errorIntegratedX, errorIntegratedY, errorIntegratedZ);
double errorMagnitude = rotationErrorCumulated.length();
if (errorMagnitude > gains.getMaximumIntegralError()) {
rotationErrorCumulated.scale(gains.getMaximumIntegralError() / errorMagnitude);
}
rotationErrorCumulated.getFrameTuple(integralTerm);
integralGainMatrix.transform(integralTerm.getVector());
}
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);
}
/** Computes using Twists, ignores linear part */
public void compute(Twist twistToPack, FramePose desiredPose, Twist desiredTwist) {
checkBodyFrames(desiredTwist, twistToPack);
checkBaseFrames(desiredTwist, twistToPack);
checkExpressedInFrames(desiredTwist, twistToPack);
twistToPack.setToZero(bodyFrame, desiredTwist.getBaseFrame(), bodyFrame);
desiredPose.getOrientationIncludingFrame(desiredOrientation);
desiredTwist.getAngularPart(desiredAngularVelocity);
desiredTwist.getAngularPart(feedForwardAngularAction);
compute(
angularActionFromOrientationController,
desiredOrientation,
desiredAngularVelocity,
null,
feedForwardAngularAction);
twistToPack.setAngularPart(angularActionFromOrientationController.getVector());
}
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.0)
@Test(timeout = 30000)
public void testSetters() {
double epsilon = 1.0e-20;
Random random = new Random(21651016L);
ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();
ReferenceFrame expectedFrame = worldFrame;
double expectedTime = 0.0;
FrameOrientation expectedOrientation = new FrameOrientation(expectedFrame);
FrameVector expectedAngularVelocity = new FrameVector(expectedFrame);
String expectedNamePrefix = "test";
String expectedNameSuffix = "blop";
YoFrameSO3TrajectoryPoint testedYoFrameSO3TrajectoryPoint =
new YoFrameSO3TrajectoryPoint(
expectedNamePrefix,
expectedNameSuffix,
new YoVariableRegistry("schnoop"),
expectedFrame);
assertWaypointContainsExpectedData(
expectedNamePrefix,
expectedNameSuffix,
expectedFrame,
expectedTime,
expectedOrientation,
expectedAngularVelocity,
testedYoFrameSO3TrajectoryPoint,
epsilon);
expectedFrame = worldFrame;
expectedTime = RandomTools.generateRandomDouble(random, 0.0, 1000.0);
expectedOrientation = FrameOrientation.generateRandomFrameOrientation(random, expectedFrame);
expectedAngularVelocity = FrameVector.generateRandomFrameVector(random, expectedFrame);
testedYoFrameSO3TrajectoryPoint.set(expectedTime, expectedOrientation, expectedAngularVelocity);
assertWaypointContainsExpectedData(
expectedNamePrefix,
expectedNameSuffix,
expectedFrame,
expectedTime,
expectedOrientation,
expectedAngularVelocity,
testedYoFrameSO3TrajectoryPoint,
epsilon);
expectedFrame = worldFrame;
expectedTime = RandomTools.generateRandomDouble(random, 0.0, 1000.0);
expectedOrientation = FrameOrientation.generateRandomFrameOrientation(random, expectedFrame);
expectedAngularVelocity = FrameVector.generateRandomFrameVector(random, expectedFrame);
testedYoFrameSO3TrajectoryPoint.set(
expectedTime, expectedOrientation.getQuaternion(), expectedAngularVelocity.getVector());
assertWaypointContainsExpectedData(
expectedNamePrefix,
expectedNameSuffix,
expectedFrame,
expectedTime,
expectedOrientation,
expectedAngularVelocity,
testedYoFrameSO3TrajectoryPoint,
epsilon);
expectedFrame = worldFrame;
expectedTime = RandomTools.generateRandomDouble(random, 0.0, 1000.0);
expectedOrientation = FrameOrientation.generateRandomFrameOrientation(random, expectedFrame);
expectedAngularVelocity = FrameVector.generateRandomFrameVector(random, expectedFrame);
YoFrameSO3TrajectoryPoint expectedYoFrameSO3TrajectoryPoint =
new YoFrameSO3TrajectoryPoint(
"sdfsd", "asd", new YoVariableRegistry("asawe"), expectedFrame);
testedYoFrameSO3TrajectoryPoint.set(expectedYoFrameSO3TrajectoryPoint);
assertTrue(
expectedYoFrameSO3TrajectoryPoint.epsilonEquals(testedYoFrameSO3TrajectoryPoint, epsilon));
assertWaypointContainsExpectedData(
expectedNamePrefix,
expectedNameSuffix,
testedYoFrameSO3TrajectoryPoint.getReferenceFrame(),
testedYoFrameSO3TrajectoryPoint.getTime(),
testedYoFrameSO3TrajectoryPoint.getOrientation().getFrameOrientationCopy(),
testedYoFrameSO3TrajectoryPoint.getAngularVelocity().getFrameVectorCopy(),
testedYoFrameSO3TrajectoryPoint,
epsilon);
}
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());
}
private void assertWaypointContainsExpectedData(
String expectedNamePrefix,
String expectedNameSuffix,
ReferenceFrame expectedFrame,
double expectedTime,
FrameOrientation expectedOrientation,
FrameVector expectedAngularVelocity,
YoFrameSO3TrajectoryPoint testedYoFrameSO3TrajectoryPoint,
double epsilon) {
assertTrue(expectedFrame == testedYoFrameSO3TrajectoryPoint.getReferenceFrame());
assertEquals(expectedTime, testedYoFrameSO3TrajectoryPoint.getTime(), epsilon);
assertEquals(expectedNamePrefix, testedYoFrameSO3TrajectoryPoint.getNamePrefix());
assertEquals(expectedNameSuffix, testedYoFrameSO3TrajectoryPoint.getNameSuffix());
assertTrue(
expectedOrientation.epsilonEquals(
testedYoFrameSO3TrajectoryPoint.getOrientation().getFrameOrientationCopy(), epsilon));
assertTrue(
expectedAngularVelocity.epsilonEquals(
testedYoFrameSO3TrajectoryPoint.getAngularVelocity().getFrameVectorCopy(), epsilon));
FrameSO3TrajectoryPoint actualFrameSO3TrajectoryPoint = new FrameSO3TrajectoryPoint();
testedYoFrameSO3TrajectoryPoint.getIncludingFrame(actualFrameSO3TrajectoryPoint);
FrameSO3TrajectoryPointTest.assertTrajectoryPointContainsExpectedData(
expectedFrame,
expectedTime,
expectedOrientation,
expectedAngularVelocity,
actualFrameSO3TrajectoryPoint,
epsilon);
actualFrameSO3TrajectoryPoint = new FrameSO3TrajectoryPoint(expectedFrame);
testedYoFrameSO3TrajectoryPoint.get(actualFrameSO3TrajectoryPoint);
FrameSO3TrajectoryPointTest.assertTrajectoryPointContainsExpectedData(
expectedFrame,
expectedTime,
expectedOrientation,
expectedAngularVelocity,
actualFrameSO3TrajectoryPoint,
epsilon);
Quat4d actualOrientation = new Quat4d();
Vector3d actualAngularVelocity = new Vector3d();
testedYoFrameSO3TrajectoryPoint.getOrientation(actualOrientation);
testedYoFrameSO3TrajectoryPoint.getAngularVelocity(actualAngularVelocity);
assertTrue(expectedOrientation.epsilonEquals(actualOrientation, epsilon));
assertTrue(expectedAngularVelocity.epsilonEquals(actualAngularVelocity, epsilon));
FrameOrientation actualFrameOrientation = new FrameOrientation(expectedFrame);
FrameVector actualFrameAngularVelocity = new FrameVector(expectedFrame);
testedYoFrameSO3TrajectoryPoint.getOrientation(actualFrameOrientation);
testedYoFrameSO3TrajectoryPoint.getAngularVelocity(actualFrameAngularVelocity);
assertTrue(expectedOrientation.epsilonEquals(actualFrameOrientation, epsilon));
assertTrue(expectedAngularVelocity.epsilonEquals(actualFrameAngularVelocity, epsilon));
actualFrameOrientation = new FrameOrientation();
actualFrameAngularVelocity = new FrameVector();
testedYoFrameSO3TrajectoryPoint.getOrientationIncludingFrame(actualFrameOrientation);
testedYoFrameSO3TrajectoryPoint.getAngularVelocityIncludingFrame(actualFrameAngularVelocity);
assertTrue(expectedOrientation.epsilonEquals(actualFrameOrientation, epsilon));
assertTrue(expectedAngularVelocity.epsilonEquals(actualFrameAngularVelocity, epsilon));
}
@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));
}
public void setDesiredVelocity(FrameVector angularVelocity, FrameVector linearVelocity) {
desiredVelocity.reshape(6, 1);
MatrixTools.insertTuple3dIntoEJMLVector(angularVelocity.getVector(), desiredVelocity, 0);
MatrixTools.insertTuple3dIntoEJMLVector(linearVelocity.getVector(), desiredVelocity, 3);
}