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(0.0, pinnedJointA.getAngularVelocityX().getValue(), EPSILON); assertEquals(0.0, pinnedJointA.getAngularVelocityY().getValue(), EPSILON); assertEquals(0.0, pinnedJointA.getAngularVelocityZ().getValue(), 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()); } }
assertEquals(floatingJointB.getAngularVelocityX().getValue(), floatingJointA.getAngularVelocityX().getValue(), EPSILON); assertEquals(floatingJointB.getAngularVelocityY().getValue(), floatingJointA.getAngularVelocityY().getValue(), EPSILON); assertEquals(floatingJointB.getAngularVelocityZ().getValue(), floatingJointA.getAngularVelocityZ().getValue(), EPSILON);