@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);
}
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());
}
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());
}
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());
}
@Override
public void convertToAbsolutePosition(Vector3d point) {
M.transform(point);
point.add(origin);
}
