How to use

us.ihmc.mecano.tools.MultiBodySystemRandomTools$RandomFloatingRevoluteJointChain

constructor

private RandomFloatingRevoluteJointChain getRandomFloatingChain()
{
 Random random = new Random();
 Vector3D[] jointAxes = {new Vector3D(1.0, 0.0, 0.0)};
 RandomFloatingRevoluteJointChain randomFloatingChain = new RandomFloatingRevoluteJointChain(random, jointAxes);
 randomFloatingChain.nextState(random, JointStateType.CONFIGURATION, JointStateType.VELOCITY);
 return randomFloatingChain;
}

@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void floatingChainTest() throws UnreasonableAccelerationException
{
 Random random = new Random(12651L);
 ArrayList<RevoluteJoint> joints = new ArrayList<>();
 int numberOfJoints = 12; 
 Vector3D[] jointAxes = new Vector3D[numberOfJoints];
 for (int i = 0; i < numberOfJoints; i++)
   jointAxes[i] = RandomGeometry.nextVector3D(random, 1.0);
 RandomFloatingRevoluteJointChain idRobot = new RandomFloatingRevoluteJointChain(random, jointAxes);
 RigidBodyBasics elevator = idRobot.getElevator();
 joints.addAll(idRobot.getRevoluteJoints());
 SCSRobotFromInverseDynamicsRobotModel robot = new SCSRobotFromInverseDynamicsRobotModel("robot", idRobot.getRootJoint());
 assertAAndADotV(random, joints, elevator, robot, numberOfJoints + 1);
}

private RandomFloatingRevoluteJointChain getRandomFloatingChain(int minNumberOfAxes, int maxNumberOfAxes)
{
 Random random = new Random(519651L);
 int numberOfAxes = RandomNumbers.nextInt(random, minNumberOfAxes, maxNumberOfAxes);
 Vector3D[] jointAxes = new Vector3D[numberOfAxes];
 for (int i = 0; i < numberOfAxes; i++)
 {
   jointAxes[i] = EuclidCoreRandomTools.nextVector3DWithFixedLength(random, 1.0);
 }
 RandomFloatingRevoluteJointChain randomFloatingChain = new RandomFloatingRevoluteJointChain(random, jointAxes);
 randomFloatingChain.nextState(random, JointStateType.CONFIGURATION, JointStateType.VELOCITY);
 return randomFloatingChain;
}

RandomFloatingRevoluteJointChain randomFloatingChain = new RandomFloatingRevoluteJointChain(random, jointAxes);
randomFloatingChain.nextState(random, JointStateType.CONFIGURATION, JointStateType.VELOCITY);

@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testFilterJoints_dest()
{
 Vector3D[] jointAxes = {X, Y, Z, Y, X};
 RandomFloatingRevoluteJointChain chain = new RandomFloatingRevoluteJointChain(random, jointAxes);
 JointBasics[] jointsArr = MultiBodySystemTools.collectSubtreeJoints(chain.getElevator());
 RevoluteJoint[] justRevolutes = new RevoluteJoint[jointsArr.length - 1];
 MultiBodySystemTools.filterJoints(jointsArr, justRevolutes, RevoluteJoint.class);
 assertEquals(jointsArr.length - 1, justRevolutes.length);
 SixDoFJoint[] justSix = new SixDoFJoint[1];
 MultiBodySystemTools.filterJoints(jointsArr, justSix, SixDoFJoint.class);
 assertEquals(1, justSix.length);
 assertTrue(justSix[0] instanceof SixDoFJoint);
 Boolean clean = false;
 for(JointBasics joint: justRevolutes)
 {
   if(joint instanceof RevoluteJoint)
   {
    clean = true;
   }
   else
   {
    clean = false;
   }
   assertTrue(clean);
 }
}

RandomFloatingRevoluteJointChain floatingChain = new RandomFloatingRevoluteJointChain(random, numberOfJoints);
SixDoFJoint floatingJoint = floatingChain.getRootJoint();
List<RevoluteJoint> revoluteJoints = floatingChain.getRevoluteJoints();

@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testFilterJoints()
{
 Vector3D[] jointAxes = {X, Y, Z, Y, X};
 RandomFloatingRevoluteJointChain chain = new RandomFloatingRevoluteJointChain(random, jointAxes);
 JointBasics[] jointsArr = MultiBodySystemTools.collectSubtreeJoints(chain.getElevator());
 RevoluteJoint[] justRevolutes = MultiBodySystemTools.filterJoints(jointsArr, RevoluteJoint.class);
 assertEquals(jointsArr.length - 1, justRevolutes.length);
 SixDoFJoint[] justSix = MultiBodySystemTools.filterJoints(jointsArr, SixDoFJoint.class);
 assertEquals(1, justSix.length);
 assertTrue(justSix[0] instanceof SixDoFJoint);
 Boolean clean = false;
 for(JointBasics joint: justRevolutes)
 {
   if(joint instanceof RevoluteJoint)
   {
    clean = true;
   }
   else
   {
    clean = false;
   }
   assertTrue(clean);
 }
}

@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testSetVelocities()
{
 Vector3D[] jointAxes = {X, Y, Z, Y, X};
 RandomFloatingRevoluteJointChain chain = new RandomFloatingRevoluteJointChain(random, jointAxes);
 JointBasics[] jointsArray = MultiBodySystemTools.collectSubtreeJoints(chain.getElevator());
 DenseMatrix64F jointVelocities = new DenseMatrix64F(MultiBodySystemTools.computeDegreesOfFreedom(jointsArray), 1);
 for(int i = 0; i < jointVelocities.getNumRows() * jointVelocities.getNumCols(); i++)
 {
   jointVelocities.set(i, random.nextDouble());
 }
 MultiBodySystemTools.insertJointsState(jointsArray, JointStateType.VELOCITY, jointVelocities);
 DenseMatrix64F sixDoFVeloc = new DenseMatrix64F(6, 1);
 jointsArray[0].getJointVelocity(0, sixDoFVeloc);
 for(int i = 0; i < 6; i++)
 {
   assertEquals("Should be equal velocitiess", jointVelocities.get(i), sixDoFVeloc.get(i), epsilon);
 }
 OneDoFJointBasics joint;
 for(int i = 6; i < jointVelocities.getNumRows() * jointVelocities.getNumCols(); i++)
 {
   joint = (OneDoFJointBasics)jointsArray[i - 5]; //1 - 6
   assertEquals("Should be equal velocities", jointVelocities.get(i), joint.getQd(), epsilon);
 }
}

RandomFloatingRevoluteJointChain randomFloatingChain = new RandomFloatingRevoluteJointChain(random, jointAxes);
GeometricJacobian jacobian = new GeometricJacobian(randomFloatingChain.getRootJoint().getSuccessor(), randomFloatingChain.getLeafBody(),
                randomFloatingChain.getLeafBody().getBodyFixedFrame());

@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testSetDesiredAccelerations()
{
 Vector3D[] jointAxes = {X, Y, Z, Y, X};
 RandomFloatingRevoluteJointChain chain = new RandomFloatingRevoluteJointChain(random, jointAxes);
 JointBasics[] jointsArray = MultiBodySystemTools.collectSubtreeJoints(chain.getElevator());
 DenseMatrix64F jointAccelerations = new DenseMatrix64F(MultiBodySystemTools.computeDegreesOfFreedom(jointsArray), 1);
 for(int i = 0; i < jointAccelerations.getNumRows() * jointAccelerations.getNumCols(); i++)
 {
   jointAccelerations.set(i, random.nextDouble());
 }
 MultiBodySystemTools.insertJointsState(jointsArray, JointStateType.ACCELERATION, jointAccelerations);
 DenseMatrix64F sixDoFAccel = new DenseMatrix64F(6, 1);
 jointsArray[0].getJointAcceleration(0, sixDoFAccel);
 for(int i = 0; i < 6; i++)
 {
   assertEquals("Should be equal accelerations", jointAccelerations.get(i), sixDoFAccel.get(i), epsilon);
 }
 OneDoFJointBasics joint;
 for(int i = 6; i < jointAccelerations.getNumRows() * jointAccelerations.getNumCols(); i++)
 {
   joint = (OneDoFJointBasics)jointsArray[i - 5]; //1 - 6
   assertEquals("Should be equal accelerations", jointAccelerations.get(i), joint.getQdd(), epsilon);
 }
}

@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testAddRevoluteJoint_String_RigidBody_Vector3d_Vector3d()
{
 Vector3D[] jointAxes = {X, Y, Z, Y, X};
 RandomFloatingRevoluteJointChain chain = new RandomFloatingRevoluteJointChain(random, jointAxes);
 chain.nextState(random, JointStateType.CONFIGURATION, JointStateType.VELOCITY);
 RigidBodyBasics[] rootBodies = {chain.getElevator()};
 JointBasics[] jointsArray = MultiBodySystemTools.collectSubtreeJoints(rootBodies);
 RigidBodyBasics[] partialBodiesArray = ScrewTools.computeSubtreeSuccessors(chain.getElevator());
 RigidBodyBasics[] bodiesArray = new RigidBodyBasics[partialBodiesArray.length + 1];
 bodiesArray[0] = chain.getElevator();
 for(int i = 0; i < partialBodiesArray.length; i++)
 {
   bodiesArray[i+1] = partialBodiesArray[i];
 }
 String jointName = "joint";
 RigidBodyBasics parentBody = bodiesArray[bodiesArray.length - 1];
 Vector3D jointOffset = RandomGeometry.nextVector3D(random, 5.0);
 Vector3D jointAxis = RandomGeometry.nextVector3D(random, 5.0);
 RevoluteJoint joint = new RevoluteJoint(jointName, parentBody, jointOffset, jointAxis);
 assertEquals("Should be equal", jointName, joint.getName());
 assertTrue(parentBody.equals(joint.getPredecessor()));
 assertTrue(jointAxis.equals(joint.getJointAxis()));
}

@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testPackJointVelocitiesMatrix_Iterable()
{
 Vector3D[] jointAxes = {X, Y, Z, Y, X};
 RandomFloatingRevoluteJointChain chain = new RandomFloatingRevoluteJointChain(random, jointAxes);
 JointBasics[] jointsArray = MultiBodySystemTools.collectSubtreeJoints(chain.getElevator());
 ArrayList<JointBasics> jointsList = new ArrayList<JointBasics>();
 for(int i = 0; i < jointsArray.length; i++)
 {
   jointsList.add(jointsArray[i]);
 }
 DenseMatrix64F originalVelocities = new DenseMatrix64F(MultiBodySystemTools.computeDegreesOfFreedom(jointsArray), 1);
 for(int i = 0; i < originalVelocities.getNumRows() * originalVelocities.getNumCols(); i++)
 {       //create original matrix
   originalVelocities.set(i, random.nextDouble());
 }
 MultiBodySystemTools.insertJointsState(jointsArray, JointStateType.VELOCITY, originalVelocities); //set velocities from matrix
 DenseMatrix64F newVelocities = new DenseMatrix64F(MultiBodySystemTools.computeDegreesOfFreedom(jointsArray), 1);
 MultiBodySystemTools.extractJointsState(jointsList, JointStateType.VELOCITY, newVelocities);//pack new matrix
 for(int i = 0; i < jointsArray.length; i++)
 {
   assertEquals("Should be equal velocities", originalVelocities.get(i), newVelocities.get(i), epsilon);
 }
}

@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testPackJointVelocitiesMatrix_Array()
{
 Vector3D[] jointAxes = {X, Y, Z, Y, X};
 RandomFloatingRevoluteJointChain chain = new RandomFloatingRevoluteJointChain(random, jointAxes);
 JointBasics[] jointsArray = MultiBodySystemTools.collectSubtreeJoints(chain.getElevator());
 DenseMatrix64F originalVelocities = new DenseMatrix64F(MultiBodySystemTools.computeDegreesOfFreedom(jointsArray), 1);
 for(int i = 0; i < originalVelocities.getNumRows() * originalVelocities.getNumCols(); i++)
 {       //create original matrix
   originalVelocities.set(i, random.nextDouble());
 }
 MultiBodySystemTools.insertJointsState(jointsArray, JointStateType.VELOCITY, originalVelocities); //set velocities from matrix
 DenseMatrix64F newVelocities = new DenseMatrix64F(MultiBodySystemTools.computeDegreesOfFreedom(jointsArray), 1);
 MultiBodySystemTools.extractJointsState(jointsArray, JointStateType.VELOCITY, newVelocities);//pack new matrix
 for(int i = 0; i < jointsArray.length; i++)
 {
   assertEquals("Should be equal velocities", originalVelocities.get(i), newVelocities.get(i), epsilon);
 }
}

@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testPackDesiredJointAccelerationsMatrix()
{
 Vector3D[] jointAxes = {X, Y, Z, Y, X};
 RandomFloatingRevoluteJointChain chain = new RandomFloatingRevoluteJointChain(random, jointAxes);
 JointBasics[] jointsArray = MultiBodySystemTools.collectSubtreeJoints(chain.getElevator());
 DenseMatrix64F originalAccel = new DenseMatrix64F(MultiBodySystemTools.computeDegreesOfFreedom(jointsArray), 1);
 for(int i = 0; i < originalAccel.getNumRows() * originalAccel.getNumCols(); i++)
 {       //create original matrix
   originalAccel.set(i, random.nextDouble());
 }
 MultiBodySystemTools.insertJointsState(jointsArray, JointStateType.ACCELERATION, originalAccel); //set velocities from matrix
 DenseMatrix64F newAccelerations = new DenseMatrix64F(MultiBodySystemTools.computeDegreesOfFreedom(jointsArray), 1);
 MultiBodySystemTools.extractJointsState(jointsArray, JointStateType.ACCELERATION, newAccelerations);//pack new matrix
 for(int i = 0; i < jointsArray.length; i++)
 {
   assertEquals("Should be equal velocities", originalAccel.get(i), newAccelerations.get(i), epsilon);
 }
}

@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testComputeDegreesOfFreedom_Array()
{
 Vector3D[] jointAxes = {X, Y, Z, Y, X};
 RandomFloatingRevoluteJointChain chain = new RandomFloatingRevoluteJointChain(random, jointAxes);
 chain.nextState(random, JointStateType.CONFIGURATION, JointStateType.VELOCITY);
 RigidBodyBasics[] rootBodies = {chain.getElevator()};
 JointBasics[] jointsArray = MultiBodySystemTools.collectSubtreeJoints(rootBodies);
 RigidBodyBasics[] partialBodiesArray = ScrewTools.computeSubtreeSuccessors(chain.getElevator());
 RigidBodyBasics[] bodiesArray = new RigidBodyBasics[partialBodiesArray.length + 1];
 bodiesArray[0] = chain.getElevator();
 for(int i = 0; i < partialBodiesArray.length; i++)
 {
   bodiesArray[i+1] = partialBodiesArray[i];
 }
 int result = MultiBodySystemTools.computeDegreesOfFreedom(jointsArray);
 assertEquals(11, result);
}

@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testComputeIndicesForJoint()
{
 Vector3D[] jointAxes = {X, Y, Z, Y, X};
 RandomFloatingRevoluteJointChain chain = new RandomFloatingRevoluteJointChain(random, jointAxes);
 JointBasics[] jointsArr = MultiBodySystemTools.collectSubtreeJoints(chain.getElevator());
 JointBasics rootJoint = jointsArr[0];
 JointBasics testJoint4 = jointsArr[5];
 
 TIntArrayList indices = new TIntArrayList();
 ScrewTools.computeIndicesForJoint(jointsArr, indices, testJoint4, rootJoint);
 assertEquals(7, indices.size());
 for(int i = 0; i < rootJoint.getDegreesOfFreedom(); i++)
 {
   assertEquals(i, indices.get(i));
 }
 assertEquals(10, indices.get(indices.size() - 1));
}

@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testComputeDegreesOfFreedom_Iterable()
{
 Vector3D[] jointAxes = {X, Y, Z, Y, X};
 RandomFloatingRevoluteJointChain chain = new RandomFloatingRevoluteJointChain(random, jointAxes);
 chain.nextState(random, JointStateType.CONFIGURATION, JointStateType.VELOCITY);
 JointBasics[] jointsArray = MultiBodySystemTools.collectSubtreeJoints(chain.getElevator());
 ArrayList<JointBasics> jointsList = new ArrayList<JointBasics>(jointsArray.length);
 RigidBodyBasics[] partialBodiesArray = ScrewTools.computeSubtreeSuccessors(chain.getElevator());
 RigidBodyBasics[] bodiesArray = new RigidBodyBasics[partialBodiesArray.length + 1];
 bodiesArray[0] = chain.getElevator();
 for(int i = 0; i < partialBodiesArray.length; i++)
 {
   bodiesArray[i+1] = partialBodiesArray[i];
 }
 MultiBodySystemTools.computeDegreesOfFreedom(jointsList);
}

@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testCreateGravitationalSpatialAcceleration()
{
 Vector3D[] jointAxes = {X, Y, Z, Y, X};
 RandomFloatingRevoluteJointChain chain = new RandomFloatingRevoluteJointChain(random, jointAxes);
 chain.nextState(random, JointStateType.CONFIGURATION, JointStateType.VELOCITY);
 double gravity = RandomNumbers.nextDouble(random, 100.0);
 SpatialAcceleration result = ScrewTools.
    createGravitationalSpatialAcceleration(chain.getElevator(), gravity);
 Vector3DReadOnly angularPart = result.getAngularPart();
 Vector3D zeroes = new Vector3D(0.0, 0.0, 0.0);
 assertTrue(angularPart.epsilonEquals(zeroes, epsilon));
 Vector3DReadOnly linearPart = result.getLinearPart();
 assertEquals(zeroes.getX(), linearPart.getX(), epsilon);
 assertEquals(zeroes.getY(), linearPart.getY(), epsilon);
 assertEquals(gravity, linearPart.getZ(), epsilon);
}

@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testExtractRevoluteJoints()
{
 Vector3D[] jointAxes = {X, Y, Z, Y, X};
 RandomFloatingRevoluteJointChain chain = new RandomFloatingRevoluteJointChain(random, jointAxes);
 JointBasics[] jointsArr = MultiBodySystemTools.collectSubtreeJoints(chain.getElevator());
 RevoluteJoint[] revoluteJoints = MultiBodySystemTools.filterJoints(jointsArr, RevoluteJoint.class);
 assertEquals(jointsArr.length - 1, revoluteJoints.length);
 for(int i = 0; i < revoluteJoints.length; i++)
 {
   assertEquals("testJoint" + i, revoluteJoints[i].getName());
 }
}

@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testComputeNumberOfJointsOfType()
{
 Vector3D[] jointAxes = {X, Y, Z, Y, X};
 RandomFloatingRevoluteJointChain chain = new RandomFloatingRevoluteJointChain(random, jointAxes);
 JointBasics[] jointsArr = MultiBodySystemTools.collectSubtreeJoints(chain.getElevator());
 int number6DoF = MultiBodySystemTools.computeNumberOfJointsOfType(SixDoFJoint.class, jointsArr);
 int numberRev = MultiBodySystemTools.computeNumberOfJointsOfType(RevoluteJoint.class, jointsArr);
 assertEquals(1, number6DoF);
 assertEquals(jointsArr.length - 1, numberRev);      
}