@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testAddRevoluteJoint_String_RigidBody_Transform3D_Vector3d() { String jointName = "joint"; RigidBodyBasics parentBody = new RigidBody("body", ReferenceFrame.getWorldFrame()); RigidBodyTransform transformToParent = EuclidCoreRandomTools.nextRigidBodyTransform(random); Vector3D jointAxis = RandomGeometry.nextVector3D(random, 5.0); RevoluteJoint joint = new RevoluteJoint(jointName, parentBody, transformToParent, jointAxis); assertEquals("Should be equal", jointName, joint.getName()); assertTrue(parentBody.equals(joint.getPredecessor())); assertTrue(jointAxis.equals(joint.getJointAxis())); }
Vector3D axis = new Vector3D(idRevoluteJoint.getJointAxis()); PinJoint scsRevoluteJoint = new PinJoint(jointName, offsetVector, scsRobot, axis); scsJoint = scsRevoluteJoint;
@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())); }
Twist expectedTwist = new Twist(bodyFrame, worldFrame, bodyFrame); FrameVector3D jointAxis = new FrameVector3D(joint.getJointAxis()); cumulatedAngularVelocity.changeFrame(jointAxis.getReferenceFrame()); cumulatedAngularVelocity.scaleAdd(joint.getQd(), jointAxis, cumulatedAngularVelocity);
FrameVector3D jointAxis = new FrameVector3D(parentJoint.getJointAxis()); cumulatedAngularVelocity.changeFrame(jointAxis.getReferenceFrame()); cumulatedAngularVelocity.scaleAdd(parentJoint.getQd(), jointAxis, cumulatedAngularVelocity);
FrameVector3D masterJointAxis = new FrameVector3D(masterJointA.getJointAxis()); masterJointAxis.changeFrame(masterJointA.getFrameBeforeJoint()); frameBeforeFourBarWithZAlongJointAxis = GeometryTools masterJointAxis); FrameVector3D masterAxis = new FrameVector3D(masterJointA.getJointAxis()); FrameVector3D jointBAxis = new FrameVector3D(passiveJointB.getJointAxis()); FrameVector3D jointCAxis = new FrameVector3D(passiveJointC.getJointAxis());