public Quat4d getActualOrientation() { return rootJoint.getQuaternion(); }
assertEquals(floatingJointB.getQy().getValue(), floatingJointA.getQy().getValue(), EPSILON); assertEquals(floatingJointB.getQz().getValue(), floatingJointA.getQz().getValue(), EPSILON); EuclidCoreTestTools.assertQuaternionEquals(floatingJointB.getQuaternion(), floatingJointA.getQuaternion(), EPSILON);
public void setRobotStateToMatchOtherRobot(FloatingRootJointRobot otherRobot) { otherRobot.update(); FloatingJoint floatingJoint = robot.getRootJoint(); FloatingJoint otherFloatingJoint = otherRobot.getRootJoint(); Tuple3DBasics position = new Point3D(); Tuple3DBasics velocity = new Vector3D(); otherFloatingJoint.getPositionAndVelocity(position, velocity); floatingJoint.setPositionAndVelocity(position, velocity); Quaternion rotation = new Quaternion(); otherFloatingJoint.getQuaternion(rotation); floatingJoint.setQuaternion(rotation); Vector3D angularVelocityInBody = otherFloatingJoint.getAngularVelocityInBody(); floatingJoint.setAngularVelocityInBody(angularVelocityInBody); ArrayList<OneDegreeOfFreedomJoint> oneDegreeOfFreedomJoints = new ArrayList<OneDegreeOfFreedomJoint>(); robot.getAllOneDegreeOfFreedomJoints(oneDegreeOfFreedomJoints); ArrayList<OneDegreeOfFreedomJoint> otherOneDegreeOfFreedomJoints = new ArrayList<OneDegreeOfFreedomJoint>(); otherRobot.getAllOneDegreeOfFreedomJoints(otherOneDegreeOfFreedomJoints); for (int i = 0; i < oneDegreeOfFreedomJoints.size(); i++) { OneDegreeOfFreedomJoint oneDegreeOfFreedomJoint = oneDegreeOfFreedomJoints.get(i); OneDegreeOfFreedomJoint otherOneDegreeOfFreedomJoint = otherOneDegreeOfFreedomJoints.get(i); oneDegreeOfFreedomJoint.setQ(otherOneDegreeOfFreedomJoint.getQYoVariable().getDoubleValue()); oneDegreeOfFreedomJoint.setQd(otherOneDegreeOfFreedomJoint.getQDYoVariable().getDoubleValue()); } }
assertEquals(0.0, pinnedJointA.getQy().getValue(), EPSILON); assertEquals(0.0, pinnedJointA.getQz().getValue(), EPSILON); EuclidCoreTestTools.assertQuaternionEquals(new Quaternion(), pinnedJointA.getQuaternion(), EPSILON);