FloatingJointRobotSensor(FloatingJoint joint) { q_x = joint.getQx(); q_y = joint.getQy(); q_z = joint.getQz(); qd_x = joint.getQdx(); qd_y = joint.getQdy(); qd_z = joint.getQdz(); q_qs = joint.getQuaternionQs(); q_qx = joint.getQuaternionQx(); q_qy = joint.getQuaternionQy(); q_qz = joint.getQuaternionQz(); qd_wx = joint.getAngularVelocityX(); qd_wy = joint.getAngularVelocityY(); qd_wz = joint.getAngularVelocityZ(); qdd_x = joint.getQddx(); qdd_y = joint.getQddy(); qdd_z = joint.getQddz(); qdd_wx = joint.getAngularAccelerationX(); qdd_wy = joint.getAngularAccelerationY(); qdd_wz = joint.getAngularAccelerationZ(); }
assertEquals(floatingJoint1.getQddz().getDoubleValue(), floatingJoint2.getQdd_t2().getDoubleValue(), epsilon); assertEquals(floatingJoint1.getAngularAccelerationX().getDoubleValue(), 0.0, epsilon); assertEquals(floatingJoint1.getAngularAccelerationY().getDoubleValue(), floatingJoint2.getQdd_rot().getDoubleValue(), epsilon); assertEquals(floatingJoint1.getAngularAccelerationZ().getDoubleValue(), 0.0, epsilon);
assertEquals(0.0, pinnedJointA.getQddy().getValue(), EPSILON); assertEquals(0.0, pinnedJointA.getQddz().getValue(), EPSILON); assertEquals(0.0, pinnedJointA.getAngularAccelerationX().getValue(), EPSILON); assertEquals(0.0, pinnedJointA.getAngularAccelerationY().getValue(), EPSILON); assertEquals(0.0, pinnedJointA.getAngularAccelerationZ().getValue(), EPSILON);
assertEquals(floatingJointB.getQddy().getValue(), floatingJointA.getQddy().getValue(), EPSILON); assertEquals(floatingJointB.getQddz().getValue(), floatingJointA.getQddz().getValue(), EPSILON); assertEquals(floatingJointB.getAngularAccelerationX().getValue(), floatingJointA.getAngularAccelerationX().getValue(), EPSILON); assertEquals(floatingJointB.getAngularAccelerationY().getValue(), floatingJointA.getAngularAccelerationY().getValue(), EPSILON); assertEquals(floatingJointB.getAngularAccelerationZ().getValue(), floatingJointA.getAngularAccelerationZ().getValue(), EPSILON);