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());
}
public void setDesiredConfiguration(FrameOrientation orientation, FramePoint position) {
desiredConfiguration.reshape(7, 1);
MatrixTools.insertQuat4dIntoEJMLVector(desiredConfiguration, orientation.getQuaternion(), 0);
MatrixTools.insertTuple3dIntoEJMLVector(position.getPoint(), desiredConfiguration, 4);
}
@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);
}
