Merge pull request #1318 from ubi-agni/lma-cleanup

cleanup LMA kinematics solver
moveit · Feb 4, 2019 · c7a31d5 · c7a31d5
2 parents f937cbb + fe15ad4
commit c7a31d5
Show file tree

Hide file tree

Showing 14 changed files with 202 additions and 1,311 deletions.
diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/joint_model.h
@@ -297,6 +297,14 @@ class JointModel
       Return true if changes were made. */
   virtual bool enforcePositionBounds(double* values, const Bounds& other_bounds) const = 0;
 
+  /** Harmonize position of revolute joints, adding/subtracting multiples of 2*Pi to bring them back into bounds.
+   *  Return true if changes were made. */
+  virtual bool harmonizePosition(double* values, const Bounds& other_bounds) const;
+  bool harmonizePosition(double* values) const
+  {
+    return harmonizePosition(values, variable_bounds_);
+  }
+
   /** \brief Check if the set of velocities for the variables of this joint are within bounds. */
   bool satisfiesVelocityBounds(const double* values, double margin = 0.0) const
   {

diff --git a/moveit_core/robot_model/include/moveit/robot_model/revolute_joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/revolute_joint_model.h
@@ -58,6 +58,7 @@ class RevoluteJointModel : public JointModel
                                         const double distance) const override;
   bool enforcePositionBounds(double* values, const Bounds& other_bounds) const override;
   bool satisfiesPositionBounds(const double* values, const Bounds& other_bounds, double margin) const override;
+  bool harmonizePosition(double* values, const Bounds& other_bounds) const override;
 
   void interpolate(const double* from, const double* to, const double t, double* state) const override;
   unsigned int getStateSpaceDimension() const override;

diff --git a/moveit_core/robot_model/src/joint_model.cpp b/moveit_core/robot_model/src/joint_model.cpp
@@ -90,6 +90,11 @@ int JointModel::getLocalVariableIndex(const std::string& variable) const
   return it->second;
 }
 
+bool JointModel::harmonizePosition(double* values, const Bounds& other_bounds) const
+{
+  return false;
+}
+
 bool JointModel::enforceVelocityBounds(double* values, const Bounds& other_bounds) const
 {
   bool change = false;
@@ -234,4 +239,4 @@ std::ostream& operator<<(std::ostream& out, const VariableBounds& b)
 }
 
 }  // end of namespace core
-}  // end of namespace moveit
+}  // end of namespace moveit
diff --git a/moveit_core/robot_model/src/revolute_joint_model.cpp b/moveit_core/robot_model/src/revolute_joint_model.cpp
@@ -174,6 +174,28 @@ bool RevoluteJointModel::satisfiesPositionBounds(const double* values, const Bou
     return !(values[0] < bounds[0].min_position_ - margin || values[0] > bounds[0].max_position_ + margin);
 }
 
+bool RevoluteJointModel::harmonizePosition(double* values, const JointModel::Bounds& other_bounds) const
+{
+  bool modified = false;
+  if (*values < other_bounds[0].min_position_)
+  {
+    while (*values + 2 * M_PI <= other_bounds[0].max_position_)
+    {
+      *values += 2 * M_PI;
+      modified = true;
+    }
+  }
+  else if (*values > other_bounds[0].max_position_)
+  {
+    while (*values - 2 * M_PI >= other_bounds[0].min_position_)
+    {
+      *values -= 2 * M_PI;
+      modified = true;
+    }
+  }
+  return modified;
+}
+
 bool RevoluteJointModel::enforcePositionBounds(double* values, const Bounds& bounds) const
 {
   if (continuous_)

diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h
@@ -1562,6 +1562,17 @@ as the new values that correspond to the group */
       updateMimicJoint(joint);
     }
   }
+
+  /// Call harmonizePosition() for all joints / all joints in group / given joint
+  void harmonizePositions();
+  void harmonizePositions(const JointModelGroup* joint_group);
+  void harmonizePosition(const JointModel* joint)
+  {
+    if (joint->harmonizePosition(position_ + joint->getFirstVariableIndex()))
+      // no need to mark transforms dirty, as the transform hasn't changed
+      updateMimicJoint(joint);
+  }
+
   void enforceVelocityBounds(const JointModel* joint)
   {
     joint->enforceVelocityBounds(velocity_ + joint->getFirstVariableIndex());

diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp
@@ -736,6 +736,18 @@ void RobotState::enforceBounds(const JointModelGroup* joint_group)
     enforceBounds(jm[i]);
 }
 
+void RobotState::harmonizePositions()
+{
+  for (const JointModel* jm : robot_model_->getActiveJointModels())
+    harmonizePosition(jm);
+}
+
+void RobotState::harmonizePositions(const JointModelGroup* joint_group)
+{
+  for (const JointModel* jm : joint_group->getActiveJointModels())
+    harmonizePosition(jm);
+}
+
 std::pair<double, const JointModel*> RobotState::getMinDistanceToPositionBounds() const
 {
   return getMinDistanceToPositionBounds(robot_model_->getActiveJointModels());

diff --git a/moveit_kinematics/lma_kinematics_plugin/CMakeLists.txt b/moveit_kinematics/lma_kinematics_plugin/CMakeLists.txt
@@ -1,8 +1,6 @@
 set(MOVEIT_LIB_NAME moveit_lma_kinematics_plugin)
 
-add_library(${MOVEIT_LIB_NAME} src/lma_kinematics_plugin.cpp
-  src/chainiksolver_pos_lma_jl_mimic.cpp
-  src/chainiksolver_vel_pinv_mimic.cpp)
+add_library(${MOVEIT_LIB_NAME} src/lma_kinematics_plugin.cpp)
 set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION ${${PROJECT_NAME}_VERSION})
 
 target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES})

diff --git a/...a_kinematics_plugin/include/moveit/lma_kinematics_plugin/chainiksolver_pos_lma_jl_mimic.h b/...a_kinematics_plugin/include/moveit/lma_kinematics_plugin/chainiksolver_pos_lma_jl_mimic.h