public static float getAngle(Quat4f q) { return 2.0f * (float) Math.acos(q.getW()); }
/** * Convert quaternion to rotation matrix and store as rotational * component of this transform. * * @param quat */ public void setRotation(Quat4f quat) { setRotationWithQuaternion(quat.getX(), quat.getY(), quat.getZ(), quat.getW()); }
public static void convertQuaternionToYawPitchRoll(Quat4f quaternion, double[] yawPitchRollToPack) { convertQuaternionToYawPitchRoll(quaternion.getX(), quaternion.getY(), quaternion.getZ(), quaternion.getW(), yawPitchRollToPack); }
/** * Convert quaternion to rotation matrix. * * @param quat */ @Override public void setRotation(Quat4f quat) { computeScale(); double yy2 = 2.0 * quat.getY() * quat.getY(); double zz2 = 2.0 * quat.getZ() * quat.getZ(); double xx2 = 2.0 * quat.getX() * quat.getX(); double xy2 = 2.0 * quat.getX() * quat.getY(); double wz2 = 2.0 * quat.getW() * quat.getZ(); double xz2 = 2.0 * quat.getX() * quat.getZ(); double wy2 = 2.0 * quat.getW() * quat.getY(); double yz2 = 2.0 * quat.getY() * quat.getZ(); double wx2 = 2.0 * quat.getW() * quat.getX(); mat00 = (1.0 - yy2 - zz2) * scale1; mat01 = (xy2 - wz2) * scale2; mat02 = (xz2 + wy2) * scale3; mat10 = (xy2 + wz2) * scale1; mat11 = (1.0 - xx2 - zz2) * scale2; mat12 = (yz2 - wx2) * scale3; mat20 = (xz2 - wy2) * scale1; mat21 = (yz2 + wx2) * scale2; mat22 = (1.0 - xx2 - yy2) * scale3; }
private void updateFullRobotModel(RobotConfigurationData robotConfigurationData, FullRobotModel model, ForceSensorDataHolder forceSensorDataHolder) { FullRobotModelCache fullRobotModelCache = getFullRobotModelCache(model); FloatingInverseDynamicsJoint rootJoint = model.getRootJoint(); if (robotConfigurationData.jointNameHash != fullRobotModelCache.jointNameHash) { System.out.println(robotConfigurationData.jointNameHash); System.out.println(fullRobotModelCache.jointNameHash); throw new RuntimeException("Joint names do not match for RobotConfigurationData"); } float[] newJointAngles = robotConfigurationData.getJointAngles(); for (int i = 0; i < newJointAngles.length; i++) { fullRobotModelCache.allJoints[i].setQ(newJointAngles[i]); } Vector3f translation = robotConfigurationData.getPelvisTranslation(); rootJoint.setPosition(translation.getX(), translation.getY(), translation.getZ()); Quat4f orientation = robotConfigurationData.getPelvisOrientation(); rootJoint.setRotation(orientation.getX(), orientation.getY(), orientation.getZ(), orientation.getW()); rootJoint.getPredecessor().updateFramesRecursively(); if (forceSensorDataHolder != null) { for (int i = 0; i < forceSensorDataHolder.getForceSensorDefinitions().size(); i++) { forceSensorDataHolder.get(forceSensorDataHolder.getForceSensorDefinitions().get(i)).setWrench( robotConfigurationData.getMomentAndForceVectorForSensor(i)); } } }
/** * Set this transform to have translation described in vector and a rotation * equal to the Quat4f quat. * * @param quat */ public final void set(Quat4f quat, Vector3f vector, double scalex, double scaley, double scalez) { double yy2 = 2.0 * quat.getY() * quat.getY(); double zz2 = 2.0 * quat.getZ() * quat.getZ(); double xx2 = 2.0 * quat.getX() * quat.getX(); double xy2 = 2.0 * quat.getX() * quat.getY(); double wz2 = 2.0 * quat.getW() * quat.getZ(); double xz2 = 2.0 * quat.getX() * quat.getZ(); double wy2 = 2.0 * quat.getW() * quat.getY(); double yz2 = 2.0 * quat.getY() * quat.getZ(); double wx2 = 2.0 * quat.getW() * quat.getX(); mat00 = (1.0 - yy2 - zz2) * scalex; mat01 = (xy2 - wz2) * scaley; mat02 = (xz2 + wy2) * scalez; mat10 = (xy2 + wz2) * scalex; mat11 = (1.0 - xx2 - zz2) * scaley; mat12 = (yz2 - wx2) * scalez; mat20 = (xz2 - wy2) * scalex; mat21 = (yz2 + wx2) * scaley; mat22 = (1.0 - xx2 - yy2) * scalez; mat03 = vector.getX(); mat13 = vector.getY(); mat23 = vector.getZ(); }
/** * Set this transform to have translation described in vector and a rotation * equal to the Quat4f quat. * * @param quat */ public final void set(Quat4f quat, Vector3f vector, double scale) { double yy2 = 2.0 * quat.getY() * quat.getY(); double zz2 = 2.0 * quat.getZ() * quat.getZ(); double xx2 = 2.0 * quat.getX() * quat.getX(); double xy2 = 2.0 * quat.getX() * quat.getY(); double wz2 = 2.0 * quat.getW() * quat.getZ(); double xz2 = 2.0 * quat.getX() * quat.getZ(); double wy2 = 2.0 * quat.getW() * quat.getY(); double yz2 = 2.0 * quat.getY() * quat.getZ(); double wx2 = 2.0 * quat.getW() * quat.getX(); mat00 = (1.0 - yy2 - zz2) * scale; mat01 = (xy2 - wz2) * scale; mat02 = (xz2 + wy2) * scale; mat10 = (xy2 + wz2) * scale; mat11 = (1.0 - xx2 - zz2) * scale; mat12 = (yz2 - wx2) * scale; mat20 = (xz2 - wy2) * scale; mat21 = (yz2 + wx2) * scale; mat22 = (1.0 - xx2 - yy2) * scale; setTranslation(vector.getX(),vector.getY(),vector.getZ()); }
public void updateFullRobotModel(KinematicsToolboxOutputStatus solution) { if (jointsHashCode != solution.jointNameHash) throw new RuntimeException("Hashes are different."); for (int i = 0; i < oneDoFJoints.length; i++) { float q = solution.getJointAngles()[i]; OneDoFJoint joint = oneDoFJoints[i]; joint.setQ(q); } Vector3f translation = solution.getPelvisTranslation(); rootJoint.setPosition(translation.getX(), translation.getY(), translation.getZ()); Quat4f orientation = solution.getPelvisOrientation(); rootJoint.setRotation(orientation.getX(), orientation.getY(), orientation.getZ(), orientation.getW()); fullRobotModelToUseForConversion.updateFrames(); }
rootJoint.setPosition(translation.getX(), translation.getY(), translation.getZ()); Quat4f orientation = robotConfigurationData.getPelvisOrientation(); rootJoint.setRotation(orientation.getX(), orientation.getY(), orientation.getZ(), orientation.getW()); rootJoint.getPredecessor().updateFramesRecursively();
rootJoint.setPosition(translation.getX(), translation.getY(), translation.getZ()); Quat4f orientation = robotConfigurationData.getPelvisOrientation(); rootJoint.setRotation(orientation.getX(), orientation.getY(), orientation.getZ(), orientation.getW()); rootJoint.getPredecessor().updateFramesRecursively();
/** * Set this transform to have translation described in vector and a rotation * equal to the Quat4f quat. * * @param quat */ public final void set(Quat4f quat, Vector3f vector, Vector3f scales) { double yy2 = 2.0 * quat.getY() * quat.getY(); double zz2 = 2.0 * quat.getZ() * quat.getZ(); double xx2 = 2.0 * quat.getX() * quat.getX(); double xy2 = 2.0 * quat.getX() * quat.getY(); double wz2 = 2.0 * quat.getW() * quat.getZ(); double xz2 = 2.0 * quat.getX() * quat.getZ(); double wy2 = 2.0 * quat.getW() * quat.getY(); double yz2 = 2.0 * quat.getY() * quat.getZ(); double wx2 = 2.0 * quat.getW() * quat.getX(); mat00 = (1.0 - yy2 - zz2) * scales.getX(); mat01 = (xy2 - wz2) * scales.getY(); mat02 = (xz2 + wy2) * scales.getZ(); mat10 = (xy2 + wz2) * scales.getX(); mat11 = (1.0 - xx2 - zz2) * scales.getY(); mat12 = (yz2 - wx2) * scales.getZ(); mat20 = (xz2 - wy2) * scales.getX(); mat21 = (yz2 + wx2) * scales.getY(); mat22 = (1.0 - xx2 - yy2) * scales.getZ(); setTranslation(vector.getX(),vector.getY(),vector.getZ()); }
public static Transform j3dTransform3DToJMETransform(RigidBodyTransform transform3D) { Quat4f quat = new Quat4f(); javax.vecmath.Vector3f vector = new javax.vecmath.Vector3f(); transform3D.get(quat, vector); Vector3f jmeVector = new Vector3f(vector.getX(), vector.getY(), vector.getZ()); Quaternion jmeQuat = new Quaternion(quat.getX(), quat.getY(), quat.getZ(), quat.getW()); Transform ret = new Transform(jmeVector, jmeQuat, new Vector3f(1.0f, 1.0f, 1.0f)); return ret; }
private static Transform j3dTransform3DToJMETransform(RigidBodyTransform transform3D) { javax.vecmath.Quat4f quat = new javax.vecmath.Quat4f(); javax.vecmath.Vector3f vector = new javax.vecmath.Vector3f(); transform3D.get(quat, vector); Vector3f jmeVector = new Vector3f(vector.getX(), vector.getY(), vector.getZ()); Quaternion jmeQuat = new Quaternion(quat.getX(), quat.getY(), quat.getZ(), quat.getW()); Transform ret = new Transform(jmeVector, jmeQuat, new Vector3f(1.0f, 1.0f, 1.0f)); return ret; }
desiredRootJoint.setPosition(translation.getX(), translation.getY(), translation.getZ()); Quat4f orientation = robotConfigurationData.getPelvisOrientation(); desiredRootJoint.setRotation(orientation.getX(), orientation.getY(), orientation.getZ(), orientation.getW()); desiredRootJoint.setVelocity(zeroVelocityMatrix, 0);
desiredRootJoint.setPosition(translation.getX(), translation.getY(), translation.getZ()); Quat4f orientation = robotConfigurationData.getPelvisOrientation(); desiredRootJoint.setRotation(orientation.getX(), orientation.getY(), orientation.getZ(), orientation.getW()); desiredRootJoint.setVelocity(zeroVelocityMatrix, 0);
localOrientation.setW(orientation.getW()); localOrientation.setX(orientation.getX()); localOrientation.setY(orientation.getY());