PinJointRobotSensor(PinJoint joint) { q = joint.getQYoVariable(); qd = joint.getQDYoVariable(); qdd = joint.getQDDYoVariable(); tau_actual = joint.getTauYoVariable(); }
@Test// timeout=300000 public void testFloatingJointAndPinJointWithMassiveBody() throws UnreasonableAccelerationException { Random random = new Random(1659L); Robot robot = new Robot("r1"); robot.setGravity(0.0); FloatingJoint root1 = new FloatingJoint("root1", new Vector3D(), robot); robot.addRootJoint(root1); Link floatingBody = new Link("floatingBody"); floatingBody.setMass(random.nextDouble()); floatingBody.setComOffset(random.nextDouble(), random.nextDouble(), random.nextDouble()); floatingBody.setMomentOfInertia(getRotationalInertiaMatrixOfSolidEllipsoid(floatingBody.getMass(), random.nextDouble(), random.nextDouble(), random.nextDouble())); root1.setLink(floatingBody); Vector3D offset = EuclidCoreRandomTools.nextVector3D(random); PinJoint pin1 = new PinJoint("pin1", offset, robot, EuclidCoreRandomTools.nextVector3D(random)); pin1.setLink(massiveLink()); root1.addJoint(pin1); pin1.setTau(random.nextDouble()); robot.doDynamicsButDoNotIntegrate(); double scalarInertiaAboutJointAxis = computeScalarInertiaAroundJointAxis(floatingBody, pin1); double torqueFromDynamics = scalarInertiaAboutJointAxis * pin1.getQDDYoVariable().getDoubleValue(); assertEquals(pin1.getTauYoVariable().getDoubleValue(), torqueFromDynamics, pin1.getTauYoVariable().getDoubleValue() * 1e-3); }
@Test// timeout=300000 public void testOffsetAtCenterOfMassWithCantileveredBeam() throws UnreasonableAccelerationException { double massOne = 7.21; Robot robot = new Robot("JointWrenchSensorTest"); PinJoint pinJointOne = createPinJointWithHangingMass("pinJointOne", massOne, Axis.Y, robot); Vector3D comOffsetFromJointOne = new Vector3D(); pinJointOne.getLink().getComOffset(comOffsetFromJointOne); JointWrenchSensor jointWrenchSensorOne = new JointWrenchSensor("jointWrenchSensorOne", comOffsetFromJointOne, robot); pinJointOne.addJointWrenchSensor(jointWrenchSensorOne); assertTrue(jointWrenchSensorOne == pinJointOne.getJointWrenchSensor()); robot.addRootJoint(pinJointOne); pinJointOne.setQ(Math.PI/2.0); pinJointOne.setTau(massOne * robot.getGravityZ() * comOffsetFromJointOne.getZ()); robot.doDynamicsAndIntegrate(0.0001); double jointAcceleration = pinJointOne.getQDDYoVariable().getDoubleValue(); assertEquals(0.0, jointAcceleration, 1e-7); Vector3D expectedJointForce = new Vector3D(-massOne * robot.getGravityZ(), 0.0, 0.0); Vector3D expectedJointTorque = new Vector3D(); assertJointWrenchEquals(jointWrenchSensorOne, expectedJointForce, expectedJointTorque); }
robot2.doDynamicsButDoNotIntegrate(); assertEquals(pin1.getQDDYoVariable().getDoubleValue(), pin1.getQDDYoVariable().getDoubleValue(), 1e-8);
assertEquals(joint1.getQDDYoVariable().getDoubleValue(), joint1B.getQDDYoVariable().getDoubleValue(), epsilon); assertEquals(joint2.getQDDYoVariable().getDoubleValue(), joint2B.getQDDYoVariable().getDoubleValue(), epsilon);