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(); }
floatingJointB.getQz().set(floatingJointA.getQz().getValue()); floatingJointB.getQuaternionQs().set(floatingJointA.getQuaternionQs().getValue()); floatingJointB.getQuaternionQx().set(floatingJointA.getQuaternionQx().getValue()); assertEquals(floatingJointB.getQz().getValue(), floatingJointA.getQz().getValue(), EPSILON); EuclidCoreTestTools.assertQuaternionEquals(floatingJointB.getQuaternion(), floatingJointA.getQuaternion(), EPSILON);
assertEquals(0.0, pinnedJointA.getQz().getValue(), EPSILON); EuclidCoreTestTools.assertQuaternionEquals(new Quaternion(), pinnedJointA.getQuaternion(), EPSILON);
public VisualizerRobot(RobotDescription robotDescription) { super(robotDescription); this.reducedRegistry = new YoVariableRegistry(robotDescription.getName()); reducedRegistry.registerVariable(getRootJoint().getQx()); reducedRegistry.registerVariable(getRootJoint().getQy()); reducedRegistry.registerVariable(getRootJoint().getQz()); reducedRegistry.registerVariable(getRootJoint().getQdx()); reducedRegistry.registerVariable(getRootJoint().getQdy()); reducedRegistry.registerVariable(getRootJoint().getQdz()); reducedRegistry.registerVariable(getRootJoint().getQuaternionQs()); reducedRegistry.registerVariable(getRootJoint().getQuaternionQx()); reducedRegistry.registerVariable(getRootJoint().getQuaternionQy()); reducedRegistry.registerVariable(getRootJoint().getQuaternionQz()); reducedRegistry.registerVariable(getRootJoint().getAngularVelocityX()); reducedRegistry.registerVariable(getRootJoint().getAngularVelocityY()); reducedRegistry.registerVariable(getRootJoint().getAngularVelocityZ()); for(OneDegreeOfFreedomJoint joint : getOneDegreeOfFreedomJoints()) { reducedRegistry.registerVariable(joint.getQYoVariable()); reducedRegistry.registerVariable(joint.getQDYoVariable()); } }
public VisualizerRobot(RobotDescription robotDescription) { super(robotDescription, false, false); this.reducedRegistry = new YoVariableRegistry(robotDescription.getName()); reducedRegistry.registerVariable(getRootJoint().getQx()); reducedRegistry.registerVariable(getRootJoint().getQy()); reducedRegistry.registerVariable(getRootJoint().getQz()); reducedRegistry.registerVariable(getRootJoint().getQdx()); reducedRegistry.registerVariable(getRootJoint().getQdy()); reducedRegistry.registerVariable(getRootJoint().getQdz()); reducedRegistry.registerVariable(getRootJoint().getQuaternionQs()); reducedRegistry.registerVariable(getRootJoint().getQuaternionQx()); reducedRegistry.registerVariable(getRootJoint().getQuaternionQy()); reducedRegistry.registerVariable(getRootJoint().getQuaternionQz()); reducedRegistry.registerVariable(getRootJoint().getAngularVelocityX()); reducedRegistry.registerVariable(getRootJoint().getAngularVelocityY()); reducedRegistry.registerVariable(getRootJoint().getAngularVelocityZ()); for(OneDegreeOfFreedomJoint joint : getOneDegreeOfFreedomJoints()) { reducedRegistry.registerVariable(joint.getQYoVariable()); reducedRegistry.registerVariable(joint.getQDYoVariable()); } }
public VisualizerRobot(RobotDescription robotDescription) { super(robotDescription, false, false); this.reducedRegistry = new YoVariableRegistry(robotDescription.getName()); reducedRegistry.registerVariable(getRootJoint().getQx()); reducedRegistry.registerVariable(getRootJoint().getQy()); reducedRegistry.registerVariable(getRootJoint().getQz()); reducedRegistry.registerVariable(getRootJoint().getQdx()); reducedRegistry.registerVariable(getRootJoint().getQdy()); reducedRegistry.registerVariable(getRootJoint().getQdz()); reducedRegistry.registerVariable(getRootJoint().getQuaternionQs()); reducedRegistry.registerVariable(getRootJoint().getQuaternionQx()); reducedRegistry.registerVariable(getRootJoint().getQuaternionQy()); reducedRegistry.registerVariable(getRootJoint().getQuaternionQz()); reducedRegistry.registerVariable(getRootJoint().getAngularVelocityX()); reducedRegistry.registerVariable(getRootJoint().getAngularVelocityY()); reducedRegistry.registerVariable(getRootJoint().getAngularVelocityZ()); for(OneDegreeOfFreedomJoint joint : getOneDegreeOfFreedomJoints()) { reducedRegistry.registerVariable(joint.getQYoVariable()); reducedRegistry.registerVariable(joint.getQDYoVariable()); } }
pin1.getTransformToWorld(pin1ToWorld); root2.setRotationAndTranslation(pin1ToWorld); root2.setPosition(root2.getQx().getDoubleValue(), root2.getQy().getDoubleValue(), root2.getQz().getDoubleValue()); robot2.update();
q_x = rootJoint.getQx(); q_y = rootJoint.getQy(); q_z = rootJoint.getQz(); q_qs = rootJoint.getQuaternionQs(); q_qx = rootJoint.getQuaternionQx();
q_x = rootJoint.getQx(); q_y = rootJoint.getQy(); q_z = rootJoint.getQz(); q_qs = rootJoint.getQuaternionQs(); q_qx = rootJoint.getQuaternionQx();
q_x = rootJoint.getQx(); q_y = rootJoint.getQy(); q_z = rootJoint.getQz(); q_qs = rootJoint.getQuaternionQs(); q_qx = rootJoint.getQuaternionQx();