public void setRigidBodies(RigidBodyBasics originalBody, RigidBodyBasics targetBody) { jointPairs.clear(); JointBasics[] originalJoints = MultiBodySystemTools.collectSubtreeJoints(originalBody); JointBasics[] targetJoints = MultiBodySystemTools.collectSubtreeJoints(targetBody); for(int i = 0; i < originalJoints.length; i++) { JointBasics originalJoint = originalJoints[i]; JointBasics targetJoint = targetJoints[i]; areJointsTheSame(originalJoint, targetJoint); ImmutablePair<JointBasics, JointBasics> jointPair = new ImmutablePair<JointBasics, JointBasics>(originalJoint, targetJoint); jointPairs.add(jointPair); } }
public static RigidBodyBasics[] computeSubtreeSuccessors(RigidBodyBasics... bodies) { return MultiBodySystemTools.collectSuccessors(MultiBodySystemTools.collectSubtreeJoints(bodies)); }
private Set<RigidBodyBasics> getExcludedRigidBodies() { Set<RigidBodyBasics> excludedBodies = new LinkedHashSet<RigidBodyBasics>(); for (RigidBodyBasics rigidBody : exclusions) { excludedBodies.add(rigidBody); RigidBodyBasics[] subTree = MultiBodySystemTools.collectSuccessors(MultiBodySystemTools.collectSubtreeJoints(rigidBody)); excludedBodies.addAll(Arrays.asList(subTree)); } return excludedBodies; }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testComputeSubtreeJoints_RigidBody() { List<RigidBodyBasics> bodies = new ArrayList<RigidBodyBasics>(); bodies.add(elevator); bodies.add(elevator); JointBasics[] fromBodies = MultiBodySystemTools.collectSubtreeJoints(elevator, elevator); JointReadOnly[] fromBodiesList = MultiBodySystemTools.collectSubtreeJoints(bodies); assertEquals("These should be equal", fromBodies.length, fromBodiesList.length); for(int i = 0; i < fromBodies.length; i++) { assertTrue(fromBodies[i].equals(fromBodiesList[i])); } }
private void getAllJointsExcludingHands(ArrayList<OneDoFJointBasics> jointsToPack) { getOneDoFJoints(jointsToPack); for (RobotSide robotSide : RobotSide.values) { RigidBodyBasics hand = getHand(robotSide); if (hand != null) { OneDoFJointBasics[] fingerJoints = MultiBodySystemTools.filterJoints(MultiBodySystemTools.collectSubtreeJoints(hand), OneDoFJointBasics.class); for (OneDoFJointBasics fingerJoint : fingerJoints) { jointsToPack.remove(fingerJoint); } } } }
public static void getAllJointsExcludingHands(ArrayList<OneDoFJointBasics> jointsToPack, FullHumanoidRobotModel model) { model.getOneDoFJoints(jointsToPack); for (RobotSide robotSide : RobotSide.values) { RigidBodyBasics hand = model.getHand(robotSide); if (hand != null) { OneDoFJointBasics[] fingerJoints = MultiBodySystemTools.filterJoints(MultiBodySystemTools.collectSubtreeJoints(hand), OneDoFJointBasics.class); for (OneDoFJointBasics fingerJoint : fingerJoints) { jointsToPack.remove(fingerJoint); } } } } }
/** * Creates a new {@code TwistCalculator} that will compute all the twists of all the rigid-bodies * of the system to which {@code body} belongs. * * @param inertialFrame non-moving frame with respect to which the twists are computed. Typically * {@link ReferenceFrame#getWorldFrame()} is used here. * @param body a body that belongs to the system this twist calculator will be available for. */ public TwistCalculator(ReferenceFrame inertialFrame, RigidBodyBasics body) { this.inertialFrame = inertialFrame; this.rootBody = MultiBodySystemTools.getRootBody(body); this.rootTwist = new Twist(rootBody.getBodyFixedFrame(), inertialFrame, rootBody.getBodyFixedFrame()); int numberOfRigidBodies = MultiBodySystemTools.collectSubtreeSuccessors(MultiBodySystemTools.collectSubtreeJoints(rootBody)).length; while (unnassignedTwists.size() < numberOfRigidBodies) unnassignedTwists.add(new Twist()); assignedTwists = new ArrayList<>(numberOfRigidBodies); rigidBodiesWithAssignedTwist = new ArrayList<>(numberOfRigidBodies); assignedTwists.add(rootTwist); rigidBodiesWithAssignedTwist.add(rootBody); rigidBodyToAssignedTwistIndex.put(rootBody, new MutableInt(0)); }
private void checkRobotMatchesData(String robotName, RigidBodyBasics rootBody, HSSFSheet descriptionSheet) { String robotNameInWorkbook = descriptionSheet.getRow(0).getCell(1).getStringCellValue(); if (!robotName.equals(robotNameInWorkbook)) { throw new RuntimeException("Trying to load the data for another robot: Loading data for " + robotName + ", workbook contains data for " + robotNameInWorkbook); } ArrayList<String> jointNames = new ArrayList<>(); int currentIndexValue = 2; HSSFRow currentRow = descriptionSheet.getRow(11); HSSFCell currentCell = currentRow.getCell(currentIndexValue); while (currentCell != null) { jointNames.add(currentCell.getStringCellValue()); currentCell = currentRow.getCell(currentIndexValue++); } JointBasics[] joints = ScrewTools.findJointsWithNames(MultiBodySystemTools.collectSubtreeJoints(rootBody), jointNames.toArray(new String[0])); OneDoFJointBasics[] oneDoFJoints = MultiBodySystemTools.filterJoints(joints, OneDoFJointBasics.class); if (oneDoFJoints.length != jointNames.size()) { throw new RuntimeException("Could not find all the joints"); } }
public static Momentum computeCoMMomentum(RigidBodyBasics elevator, ReferenceFrame centerOfMassFrame, CentroidalMomentumCalculator centroidalMomentumMatrix) { DenseMatrix64F mat = centroidalMomentumMatrix.getCentroidalMomentumMatrix(); JointBasics[] jointList = MultiBodySystemTools.collectSubtreeJoints(elevator); DenseMatrix64F jointVelocities = new DenseMatrix64F(MultiBodySystemTools.computeDegreesOfFreedom(jointList), 1); MultiBodySystemTools.extractJointsState(jointList, JointStateType.VELOCITY, jointVelocities); DenseMatrix64F comMomentumMatrix = MatrixTools.mult(mat, jointVelocities); Momentum comMomentum = new Momentum(centerOfMassFrame, comMomentumMatrix); return comMomentum; } }
@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); }
@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 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 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); } }
public DifferentialIDMassMatrixCalculator(ReferenceFrame inertialFrame, RigidBodyBasics rootBody) { SpatialAcceleration zeroRootAcceleration = ScrewTools.createGravitationalSpatialAcceleration(rootBody, 0.0); idCalculator = new InverseDynamicsCalculator(rootBody, false, true); idCalculator.setRootAcceleration(zeroRootAcceleration); jointsInOrder = MultiBodySystemTools.collectSubtreeJoints(rootBody); totalNumberOfDoFs = MultiBodySystemTools.computeDegreesOfFreedom(jointsInOrder); massMatrix = new DenseMatrix64F(totalNumberOfDoFs, totalNumberOfDoFs); storedJointDesiredAccelerations = new DenseMatrix64F(totalNumberOfDoFs, 1); storedJointVelocities = new DenseMatrix64F(totalNumberOfDoFs, 1); tmpDesiredJointAccelerationsMatrix = new DenseMatrix64F(totalNumberOfDoFs, 1); tmpTauMatrix = new DenseMatrix64F(totalNumberOfDoFs, 1); for (JointBasics joint : jointsInOrder) { ReferenceFrame bodyFixedFrame = joint.getSuccessor().getBodyFixedFrame(); Wrench jointWrench = new Wrench(bodyFixedFrame, bodyFixedFrame); storedJointWrenches.put(joint, jointWrench); } }
@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 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 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 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 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); } }