secondJointIndex %= numberOfJoints; MovingReferenceFrame frameOne = joints.get(firstJointIndex).getFrameAfterJoint(); MovingReferenceFrame frameTwo = joints.get(secondJointIndex).getFrameAfterJoint(); joints.get(0).getPredecessor().updateFramesRecursively(); zUpFramesToMovingZUpFrames.keySet().forEach(ReferenceFrame::update); zUpFramesToMovingZUpFrames.values().forEach(ReferenceFrame::update);
@Override public String toString() { String qAsString = String.format(EuclidCoreIOTools.DEFAULT_FORMAT, getQ()); String qdAsString = String.format(EuclidCoreIOTools.DEFAULT_FORMAT, getQd()); String qddAsString = String.format(EuclidCoreIOTools.DEFAULT_FORMAT, getQdd()); String tauAsString = String.format(EuclidCoreIOTools.DEFAULT_FORMAT, getTau()); return super.toString() + ", q: " + qAsString + ", qd: " + qdAsString + ", qdd: " + qddAsString + ", tau: " + tauAsString; } }
/** * Generates a random kinematic tree composed of rigid-bodies and 1-DoF joints. * <p> * The joints and rigid-bodies have random physical parameters. * </p> * <p> * The generated multi-body system is a kinematic tree, i.e. every rigid-body can have one or more * child joint(s). * </p> * * @param random the random generator to use. * @param prefix provides a common prefix used for all the joint and rigid-body names. * @param rootBody the root to which the kinematic tree is to be attached. * @param numberOfJoints how many joints the kinematic tree should be composed of. * @return the list of all the joints composing the kinematic tree. */ public static List<OneDoFJoint> nextOneDoFJointTree(Random random, String prefix, RigidBodyBasics rootBody, int numberOfJoints) { List<OneDoFJoint> oneDoFJoints = new ArrayList<>(); RigidBodyBasics predecessor = rootBody; for (int i = 0; i < numberOfJoints; i++) { OneDoFJoint joint = nextOneDoFJoint(random, prefix + "Joint" + i, predecessor); nextRigidBody(random, prefix + "Body" + i, joint); oneDoFJoints.add(joint); predecessor = oneDoFJoints.get(random.nextInt(oneDoFJoints.size())).getSuccessor(); } return SubtreeStreams.from(OneDoFJoint.class, rootBody.getChildrenJoints()).collect(Collectors.toList()); }
List<OneDoFJointBasics> jointsInFuture = Arrays.asList(MultiBodySystemFactories.cloneOneDoFJointKinematicChain(joints.toArray(new OneDoFJointBasics[numberOfJoints]))); TwistCalculator twistCalculator = new TwistCalculator(worldFrame, joints.get(0).getPredecessor()); double q = joints.get(jointIndex).getQ() + dt * joints.get(jointIndex).getQd(); jointsInFuture.get(jointIndex).setQ(q); joints.get(0).updateFramesRecursively(); jointsInFuture.get(0).updateFramesRecursively();
RigidBodyBasics rootBody = MultiBodySystemTools.getRootBody(joints.get(0).getSuccessor()); RigidBodyBasics randomEndEffector = joints.get(randomEndEffectorIndex).getSuccessor(); GeometricJacobian rootJacobian = new GeometricJacobian(rootBody, randomEndEffector, randomEndEffector.getBodyFixedFrame()); rootJacobian.compute(); RigidBodyBasics randomBase = joints.get(random.nextInt(randomEndEffectorIndex + 1)).getPredecessor(); GeometricJacobian jacobian = new GeometricJacobian(randomBase, randomEndEffector, randomEndEffector.getBodyFixedFrame()); jacobian.compute();
joints.get(0).getPredecessor().updateFramesRecursively(); joints.get(0).getPredecessor().updateFramesRecursively(); jointFramesToFDFrames.keySet().forEach(MovingReferenceFrame::update); jointFramesToFDFrames.values().forEach(MovingReferenceFrame::update);
RigidBodyBasics rootBody = MultiBodySystemTools.getRootBody(joints.get(0).getPredecessor()); RigidBodyBasics rootBodyInFuture = MultiBodySystemFactories.cloneMultiBodySystem(rootBody, worldFrame, "Test"); List<OneDoFJointBasics> jointsInFuture = SubtreeStreams.fromChildren(OneDoFJointBasics.class, rootBodyInFuture).collect(Collectors.toList()); double q = joints.get(jointIndex).getQ() + dt * joints.get(jointIndex).getQd(); jointsInFuture.get(jointIndex).setQ(q); joints.get(0).updateFramesRecursively(); jointsInFuture.get(0).updateFramesRecursively();
ReferenceFrame predecessorFrame = getPredecessor().getBodyFixedFrame(); ReferenceFrame successorFrame = getSuccessor().getBodyFixedFrame();
joints.get(0).getPredecessor().updateFramesRecursively(); joints.get(0).getPredecessor().updateFramesRecursively();
secondJointIndex %= numberOfJoints; MovingReferenceFrame frameOne = joints.get(firstJointIndex).getFrameAfterJoint(); MovingReferenceFrame frameTwo = joints.get(secondJointIndex).getFrameAfterJoint(); joints.get(0).getPredecessor().updateFramesRecursively(); joints.get(0).getPredecessor().updateFramesRecursively(); jointFramesToFDFrames.keySet().forEach(MovingReferenceFrame::update); jointFramesToFDFrames.values().forEach(MovingReferenceFrame::update);
joints.get(0).getPredecessor().updateFramesRecursively(); zUpFramesToMovingZUpFrames.keySet().forEach(ReferenceFrame::update); zUpFramesToMovingZUpFrames.values().forEach(ReferenceFrame::update);
secondJointIndex %= numberOfJoints; MovingReferenceFrame frameOne = joints.get(firstJointIndex).getFrameAfterJoint(); MovingReferenceFrame frameTwo = joints.get(secondJointIndex).getFrameAfterJoint(); joints.get(0).getPredecessor().updateFramesRecursively(); joints.get(0).getPredecessor().updateFramesRecursively(); jointFramesToFDFrames.keySet().forEach(MovingReferenceFrame::update); jointFramesToFDFrames.values().forEach(MovingReferenceFrame::update);
@ContinuousIntegrationTest(estimatedDuration = 0.1) @Test(timeout = 30000) public void testAgainstTwistCalculatorWithChainRobot() throws Exception { Random random = new Random(435423L); int numberOfJoints = 100; List<OneDoFJoint> joints = MultiBodySystemRandomTools.nextOneDoFJointChain(random, numberOfJoints); TwistCalculator twistCalculator = new TwistCalculator(worldFrame, joints.get(0).getPredecessor()); Twist actualTwist = new Twist(); Twist expectedTwist = new Twist(); for (int i = 0; i < 100; i++) { MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -1.0, 1.0, joints); MultiBodySystemRandomTools.nextState(random, JointStateType.VELOCITY, -1.0, 1.0, joints); joints.get(0).getPredecessor().updateFramesRecursively(); twistCalculator.compute(); for (int jointIndex = 0; jointIndex < numberOfJoints; jointIndex++) { OneDoFJointBasics joint = joints.get(jointIndex); RigidBodyBasics body = joint.getSuccessor(); MovingReferenceFrame bodyFrame = body.getBodyFixedFrame(); twistCalculator.getTwistOfBody(body, expectedTwist); bodyFrame.getTwistOfFrame(actualTwist); MecanoTestTools.assertTwistEquals(expectedTwist, actualTwist, 1.0e-5); } } }
secondJointIndex %= numberOfJoints; MovingReferenceFrame nonZUpFrameOne = joints.get(firstJointIndex).getFrameAfterJoint(); MovingReferenceFrame nonZUpFrameTwo = joints.get(secondJointIndex).getFrameAfterJoint(); MovingZUpFrame frameOne = new MovingZUpFrame(nonZUpFrameOne, nonZUpFrameOne.getName() + "ZUp"); MovingZUpFrame frameTwo = new MovingZUpFrame(nonZUpFrameTwo, nonZUpFrameTwo.getName() + "ZUp"); joints.get(0).getPredecessor().updateFramesRecursively(); joints.get(0).getPredecessor().updateFramesRecursively(); jointFramesToFDFrames.keySet().forEach(MovingReferenceFrame::update); jointFramesToFDFrames.values().forEach(MovingReferenceFrame::update);
List<OneDoFJoint> joints = MultiBodySystemRandomTools.nextOneDoFJointTree(random, numberOfJoints); TwistCalculator twistCalculator = new TwistCalculator(worldFrame, joints.get(0).getPredecessor()); Twist actualTwist = new Twist(); Twist expectedTwist = new Twist(); joints.get(0).getPredecessor().updateFramesRecursively();
secondJointIndex %= numberOfJoints; MovingReferenceFrame nonZUpFrameOne = joints.get(firstJointIndex).getFrameAfterJoint(); MovingReferenceFrame nonZUpFrameTwo = joints.get(secondJointIndex).getFrameAfterJoint(); MovingZUpFrame frameOne = new MovingZUpFrame(nonZUpFrameOne, nonZUpFrameOne.getName() + "ZUp"); MovingZUpFrame frameTwo = new MovingZUpFrame(nonZUpFrameTwo, nonZUpFrameTwo.getName() + "ZUp"); joints.get(0).getPredecessor().updateFramesRecursively(); zUpFramesToMovingZUpFrames.keySet().forEach(ReferenceFrame::update); zUpFramesToMovingZUpFrames.values().forEach(ReferenceFrame::update);