Merge pull request #282 from ubi-agni/fix-revolute-angle-computation

fixed RevoluteJointModel::computeVariablePositions
moveit · May 14, 2016 · df74f90 · df74f90
2 parents 8206cf7 + 01c44b8
commit df74f90
Showing 1 changed file with 3 additions and 1 deletion.
diff --git a/robot_model/src/revolute_joint_model.cpp b/robot_model/src/revolute_joint_model.cpp
@@ -244,5 +244,7 @@ void moveit::core::RevoluteJointModel::computeVariablePositions(const Eigen::Aff
 {
   Eigen::Quaterniond q(transf.rotation());
   q.normalize();
-  joint_values[0] = acos(q.w())*2.0f;
+  size_t maxIdx;
+  axis_.array().abs().maxCoeff(&maxIdx);
+  joint_values[0] = 2.*atan2(q.vec()[maxIdx] / axis_[maxIdx], q.w());
 }