@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 300000)
public void testTwoIntersectingBoxes() {
CombinedTerrainObject3D combinedTerrainObject =
new CombinedTerrainObject3D("Combined Terrain Object to Test");
Point3d pointToCheck = new Point3d();
Point3d expectedIntersection = new Point3d();
Vector3d expectedNormal = new Vector3d();
Point3d resultIntersection = new Point3d();
Vector3d resultNormal = new Vector3d();
setupTwoIntersectingBoxesMadeFromPolygons(combinedTerrainObject);
pointToCheck.set(0.4, 0.45, 0.25);
expectedIntersection.set(0.4, 0.5, 0.25);
expectedNormal.set(0.0, 1.0, 0.0);
combinedTerrainObject.checkIfInside(
pointToCheck.getX(),
pointToCheck.getY(),
pointToCheck.getZ(),
resultIntersection,
resultNormal);
JUnitTools.assertTuple3dEquals(expectedIntersection, resultIntersection, 1e-4);
JUnitTools.assertTuple3dEquals(expectedNormal, resultNormal, 1e-4);
}
@DeployableTestMethod(estimatedDuration = 0.1)
@Test(timeout = 30000)
public void testGetLegPairs() {
final QuadrupedSupportPolygon quadrupedSupportPolygon = createSimplePolygon();
RobotQuadrant[][] legPairs = quadrupedSupportPolygon.getLegPairs();
assertEquals("not 4", 4, legPairs.length);
quadrupedSupportPolygon.removeFootstep(RobotQuadrant.FRONT_LEFT);
legPairs = quadrupedSupportPolygon.getLegPairs();
assertEquals("not 3", 3, legPairs.length);
quadrupedSupportPolygon.removeFootstep(RobotQuadrant.FRONT_RIGHT);
legPairs = quadrupedSupportPolygon.getLegPairs();
assertEquals("not 1", 1, legPairs.length);
quadrupedSupportPolygon.removeFootstep(RobotQuadrant.HIND_LEFT);
JUnitTools.assertExceptionThrown(
UndefinedOperationException.class,
new RunnableThatThrows() {
@Override
public void run() throws Throwable {
quadrupedSupportPolygon.getLegPairs();
}
});
}
@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);
}
@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testChainAgainstCentroidalMomentumMatrix() {
Random random = new Random(17679L);
ArrayList<RevoluteJoint> joints = new ArrayList<RevoluteJoint>();
RigidBody elevator = new RigidBody("elevator", world);
int nJoints = 10;
Vector3d[] jointAxes = new Vector3d[nJoints];
for (int i = 0; i < nJoints; i++) {
Vector3d jointAxis = RandomTools.generateRandomVector(random);
jointAxis.normalize();
jointAxes[i] = jointAxis;
}
ScrewTestTools.createRandomChainRobot("randomChain", joints, elevator, jointAxes, random);
InverseDynamicsJoint[] jointsArray = new RevoluteJoint[joints.size()];
joints.toArray(jointsArray);
ReferenceFrame centerOfMassFrame = new CenterOfMassReferenceFrame("comFrame", world, elevator);
for (RevoluteJoint joint : joints) {
joint.setQ(random.nextDouble());
joint.setQd(random.nextDouble());
}
Momentum momentum = computeMomentum(elevator, centerOfMassFrame);
DenseMatrix64F momentumMatrix = new DenseMatrix64F(Momentum.SIZE, 1);
momentum.getMatrix(momentumMatrix);
CentroidalMomentumMatrix centroidalMomentumMatrix =
new CentroidalMomentumMatrix(elevator, centerOfMassFrame);
centroidalMomentumMatrix.compute();
DenseMatrix64F centroidalMomentumMatrixMatrix = centroidalMomentumMatrix.getMatrix();
DenseMatrix64F jointVelocitiesMatrix =
new DenseMatrix64F(ScrewTools.computeDegreesOfFreedom(jointsArray), 1);
ScrewTools.getJointVelocitiesMatrix(jointsArray, jointVelocitiesMatrix);
DenseMatrix64F momentumFromCentroidalMomentumMatrix = new DenseMatrix64F(Momentum.SIZE, 1);
CommonOps.mult(
centroidalMomentumMatrixMatrix,
jointVelocitiesMatrix,
momentumFromCentroidalMomentumMatrix);
double epsilon = 1e-9;
assertEquals(momentum.getExpressedInFrame(), centerOfMassFrame);
JUnitTools.assertMatrixEquals(momentumFromCentroidalMomentumMatrix, momentumMatrix, epsilon);
double norm = NormOps.normP2(momentumMatrix);
assertTrue(norm > epsilon);
}
@DeployableTestMethod(estimatedDuration = 0.3)
@Test(timeout = 30000)
public void test() {
Random random = new Random(176L);
int matrixSize = 5;
YoVariableRegistry parentRegistry = new YoVariableRegistry("test");
DenseMatrix64F jacobianMatrix = RandomMatrices.createRandom(matrixSize, matrixSize, random);
DenseMatrix64F taskVelocity = RandomMatrices.createRandom(matrixSize, 1, random);
DampedLeastSquaresJacobianSolver solver =
new DampedLeastSquaresJacobianSolver("testSolver", matrixSize, parentRegistry);
solver.setAlpha(1.0);
DenseMatrix64F jointVelocities = new DenseMatrix64F(matrixSize, 1);
solver.setJacobian(jacobianMatrix);
solver.solve(jointVelocities, taskVelocity);
DenseMatrix64F taskVelocityBack = new DenseMatrix64F(matrixSize, 1);
solver.inverseSolve(taskVelocityBack, jointVelocities);
double delta = 1e-9;
JUnitTools.assertMatrixEquals(taskVelocity, taskVelocityBack, delta);
}