Add optional pre/post chain segment to the default robot_chain

cartesian_controller_base/CMakeLists.txt

@@ -22,9 +22,11 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
   controller_interface
   kdl_parser
+  geometry_msgs
   trajectory_msgs
   control_toolbox
   eigen_conversions
+  kdl_conversions
   dynamic_reconfigure
   pluginlib
 )
@@ -70,11 +72,9 @@ find_package(catkin REQUIRED COMPONENTS
 # )
 
 ## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
+add_service_files(DIRECTORY srv FILES
+  SetKinematicChainOffsets.srv
+)
 
 ## Generate actions in the 'action' folder
 # add_action_files(
@@ -84,10 +84,10 @@ find_package(catkin REQUIRED COMPONENTS
 # )
 
 ## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   std_msgs  # Or other packages containing msgs
-# )
+generate_messages(
+   DEPENDENCIES
+   geometry_msgs
+)
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##
@@ -123,7 +123,7 @@ generate_dynamic_reconfigure_options(
 catkin_package(
   INCLUDE_DIRS include
   LIBRARIES cartesian_controller_base ik_solvers
-  CATKIN_DEPENDS roscpp controller_interface kdl_parser trajectory_msgs control_toolbox eigen_conversions dynamic_reconfigure pluginlib
+  CATKIN_DEPENDS roscpp controller_interface kdl_parser geometry_msgs trajectory_msgs control_toolbox kdl_conversions eigen_conversions dynamic_reconfigure pluginlib
 #  DEPENDS system_lib
 )
 

cartesian_controller_base/include/cartesian_controller_base/DampedLeastSquaresSolver.h

@@ -102,6 +102,11 @@ class DampedLeastSquaresSolver : public IKSolver
               const KDL::JntArray& upper_pos_limits,
               const KDL::JntArray& lower_pos_limits);
 
+    /**
+     * @brief Update the robot kinematic chain of the solver
+     */
+    virtual bool updateChain(const KDL::Chain& chain);
+
   private:
     std::shared_ptr<KDL::ChainJntToJacSolver> m_jnt_jacobian_solver;
     KDL::Jacobian m_jnt_jacobian;

cartesian_controller_base/include/cartesian_controller_base/ForwardDynamicsSolver.h

@@ -123,6 +123,11 @@ class ForwardDynamicsSolver : public IKSolver
               const KDL::JntArray& upper_pos_limits,
               const KDL::JntArray& lower_pos_limits);
 
+    /**
+     * @brief Update the robot kinematic chain of the solver
+     */
+    bool updateChain(const KDL::Chain& chain);
+
   private:
 
     //! Build a generic robot model for control

cartesian_controller_base/include/cartesian_controller_base/IKSolver.h

@@ -151,6 +151,11 @@ class IKSolver
                       const KDL::JntArray& upper_pos_limits,
                       const KDL::JntArray& lower_pos_limits);
 
+    /**
+     * @brief Update the robot kinematic chain of the solver
+     */
+    virtual bool updateChain(const KDL::Chain& chain);
+
     /**
      * @brief Update the robot kinematics of the solver
      *

cartesian_controller_base/include/cartesian_controller_base/JacobianTransposeSolver.h

@@ -95,6 +95,11 @@ class JacobianTransposeSolver : public IKSolver
               const KDL::JntArray& upper_pos_limits,
               const KDL::JntArray& lower_pos_limits);
 
+    /**
+     * @brief Update the robot kinematic chain of the solver
+     */
+    virtual bool updateChain(const KDL::Chain& chain);
+
   private:
     std::shared_ptr<KDL::ChainJntToJacSolver> m_jnt_jacobian_solver;
     KDL::Jacobian m_jnt_jacobian;

cartesian_controller_base/include/cartesian_controller_base/SelectivelyDampedLeastSquaresSolver.h

@@ -101,6 +101,11 @@ class SelectivelyDampedLeastSquaresSolver : public IKSolver
               const KDL::JntArray& upper_pos_limits,
               const KDL::JntArray& lower_pos_limits);
 
+    /**
+     * @brief Update the robot kinematic chain of the solver
+     */
+    bool updateChain(const KDL::Chain& chain);
+
   private:
     /**
      * @brief Helper function to clamp a column vector

cartesian_controller_base/include/cartesian_controller_base/cartesian_controller_base.h

@@ -59,6 +59,7 @@
 #include <cartesian_controller_base/IKSolver.h>
 #include <cartesian_controller_base/SpatialPDController.h>
 #include <cartesian_controller_base/Utility.h>
+#include <cartesian_controller_base/SetKinematicChainOffsets.h>
 
 // Dynamic reconfigure
 #include <dynamic_reconfigure/server.h>
@@ -200,6 +201,20 @@ class CartesianControllerBase : public controller_interface::Controller<Hardware
     void publishStateFeedback();
 
   private:
+    /**
+     * @brief Allow users to add a chain segment before and/or after the robot chain
+     *
+     * Before:
+     * Empty segment if frame_id is empty, otherwise consist of a fixed segment that will
+     * become the parent of the robot base link.
+     *
+     * After:
+     * Empty segment if frame_id is empty,otherwise consist of a fixed segment that will
+     * be attached to the end effector link.
+     */
+    bool signalChainOffsetsCallback(SetKinematicChainOffsets::Request& req, SetKinematicChainOffsets::Response& res);
+
+    ros::ServiceServer    m_signal_chain_offsets_server;
     std::vector<std::string>                          m_joint_names;
     trajectory_msgs::JointTrajectoryPoint             m_simulated_joint_motion;
     SpatialPDController                              m_spatial_controller;

cartesian_controller_base/include/cartesian_controller_base/cartesian_controller_base.hpp

@@ -48,6 +48,7 @@
 #include <kdl/tree.hpp>
 #include <kdl_parser/kdl_parser.hpp>
 #include <kdl/jntarray.hpp>
+#include <kdl_conversions/kdl_msg.h>
 
 // URDF
 #include <urdf/model.h>
@@ -185,6 +186,9 @@ init(HardwareInterface* hw, ros::NodeHandle& nh)
   // Initialize Cartesian pd controllers
   m_spatial_controller.init(nh);
 
+  // Update chain offsets service
+  m_signal_chain_offsets_server = nh.advertiseService("signal_chain_offsets",&CartesianControllerBase<HardwareInterface>::signalChainOffsetsCallback,this);
+
   // Controller-internal state publishing
   m_feedback_pose_publisher =
     std::make_shared<realtime_tools::RealtimePublisher<geometry_msgs::PoseStamped> >(
@@ -365,6 +369,61 @@ displayInTipLink(const ctrl::Vector6D& vector, const std::string& to)
   return out;
 }
 
+template <class HardwareInterface>
+bool CartesianControllerBase<HardwareInterface>::
+signalChainOffsetsCallback(SetKinematicChainOffsets::Request& req, SetKinematicChainOffsets::Response& res)
+{
+  KDL::Chain new_chain;
+
+  // Add pre chain segment
+  if(!req.pre_robot_chain_offset.header.frame_id.empty())
+  {
+    KDL::Frame pre_chain_frame;
+    tf::poseMsgToKDL(req.pre_robot_chain_offset.pose, pre_chain_frame);
+
+    KDL::Segment pre_segment = KDL::Segment(req.pre_robot_chain_offset.header.frame_id,
+                                            KDL::Joint(KDL::Joint::None),
+                                            pre_chain_frame);
+
+    new_chain.addSegment(pre_segment);
+  }
+
+  // Add robot chain
+  new_chain.addChain(m_robot_chain);
+
+  // Add post chain segment
+  if(!req.post_robot_chain_offset.header.frame_id.empty())
+  {
+    KDL::Frame post_chain_frame;
+    tf::poseMsgToKDL(req.post_robot_chain_offset.pose, post_chain_frame);
+
+    KDL::Segment post_segment = KDL::Segment(req.post_robot_chain_offset.header.frame_id,
+                                            KDL::Joint(KDL::Joint::None),
+                                            post_chain_frame);
+
+    new_chain.addSegment(post_segment);
+  }
+
+  // Update solvers with new chain
+  if(!m_ik_solver->updateChain(new_chain))
+  {
+    res.message = "Failed to update the kinematic chain in the ik solver.";
+    res.success = false;
+
+    return false;
+  }
+
+  KDL::Tree tmp("not_relevant");
+  tmp.addChain(new_chain,"not_relevant");
+  m_forward_kinematics_solver.reset(new KDL::TreeFkSolverPos_recursive(tmp));
+
+  // Fill service result
+  res.message = "Got it.";
+  res.success = true;
+
+  return true;
+}
+
 template <class HardwareInterface>
 void CartesianControllerBase<HardwareInterface>::
 publishStateFeedback()

cartesian_controller_base/package.xml

@@ -42,18 +42,22 @@
   <build_depend>roscpp</build_depend>
   <build_depend>controller_interface</build_depend>
   <build_depend>kdl_parser</build_depend>
+  <build_depend>geometry_msgs</build_depend>
   <build_depend>trajectory_msgs</build_depend>
   <build_depend>control_toolbox</build_depend>
   <build_depend>eigen_conversions</build_depend>
+  <build_depend>kdl_conversions</build_depend>
   <build_depend>dynamic_reconfigure</build_depend>
   <build_depend>pluginlib</build_depend>
 
   <run_depend>roscpp</run_depend>
   <run_depend>controller_interface</run_depend>
   <run_depend>kdl_parser</run_depend>
+  <run_depend>geometry_msgs</run_depend>
   <run_depend>trajectory_msgs</run_depend>
   <run_depend>control_toolbox</run_depend>
   <run_depend>eigen_conversions</run_depend>
+  <run_depend>kdl_conversions</run_depend>
   <run_depend>dynamic_reconfigure</run_depend>
   <run_depend>pluginlib</run_depend>
 

cartesian_controller_base/src/DampedLeastSquaresSolver.cpp

@@ -125,8 +125,7 @@ namespace cartesian_controller_base{
   {
     IKSolver::init(nh, chain, upper_pos_limits, lower_pos_limits);
 
-    m_jnt_jacobian_solver.reset(new KDL::ChainJntToJacSolver(m_chain));
-    m_jnt_jacobian.resize(m_number_joints);
+    updateChain(m_chain);
 
     // Connect dynamic reconfigure and overwrite the default values with values
     // on the parameter server. This is done automatically if parameters with
@@ -143,6 +142,17 @@ namespace cartesian_controller_base{
     return true;
   }
 
+  bool DampedLeastSquaresSolver::updateChain(const KDL::Chain& chain)
+  {
+    IKSolver::updateChain(chain);
+
+    m_jnt_jacobian_solver.reset(new KDL::ChainJntToJacSolver(m_chain));
+    m_jnt_jacobian.resize(m_number_joints);
+
+    return true;
+  }
+
+
     void DampedLeastSquaresSolver::dynamicReconfigureCallback(IKConfig& config, uint32_t level)
     {
       m_alpha = config.alpha;

cartesian_controller_base/src/ForwardDynamicsSolver.cpp

@@ -138,17 +138,8 @@ namespace cartesian_controller_base{
   {
     IKSolver::init(nh, chain, upper_pos_limits, lower_pos_limits);
 
-    if (!buildGenericModel())
-    {
-      ROS_ERROR("ForwardDynamicsSolver: Something went wrong in setting up the internal model.");
+    if(!updateChain(m_chain))
       return false;
-    }
-
-    // Forward dynamics
-    m_jnt_jacobian_solver.reset(new KDL::ChainJntToJacSolver(m_chain));
-    m_jnt_space_inertia_solver.reset(new KDL::ChainDynParam(m_chain,KDL::Vector::Zero()));
-    m_jnt_jacobian.resize(m_number_joints);
-    m_jnt_space_inertia.resize(m_number_joints);
 
     // Connect dynamic reconfigure and overwrite the default values with values
     // on the parameter server. This is done automatically if parameters with
@@ -170,6 +161,26 @@ namespace cartesian_controller_base{
     return true;
   }
 
+  bool ForwardDynamicsSolver::updateChain(const KDL::Chain& chain)
+  {
+    IKSolver::updateChain(chain);
+
+    if (!buildGenericModel())
+    {
+      ROS_ERROR("ForwardDynamicsSolver: Something went wrong in setting up the internal model.");
+      return false;
+    }
+
+    // Forward dynamics
+    m_jnt_jacobian_solver.reset(new KDL::ChainJntToJacSolver(m_chain));
+    m_jnt_space_inertia_solver.reset(new KDL::ChainDynParam(m_chain,KDL::Vector::Zero()));
+    m_jnt_jacobian.resize(m_number_joints);
+    m_jnt_space_inertia.resize(m_number_joints);
+
+    return true;
+  }
+
+
   bool ForwardDynamicsSolver::buildGenericModel()
   {
     // Set all masses and inertias to minimal (yet stable) values.

cartesian_controller_base/src/IKSolver.cpp

@@ -111,15 +111,27 @@ namespace cartesian_controller_base{
                       const KDL::JntArray& lower_pos_limits)
   {
     // Initialize
+    IKSolver::updateChain(chain);
+    m_upper_pos_limits           = upper_pos_limits;
+    m_lower_pos_limits           = lower_pos_limits;
+
+    return true;
+  }
+
+  bool IKSolver::updateChain(const KDL::Chain& chain)
+  {
+    // Guard against multiple initialization
+    // Also fixes a segfault...
+    if(&chain == &m_chain)
+      return true;
+
     m_chain = chain;
     m_number_joints              = m_chain.getNrOfJoints();
     m_current_positions.data     = ctrl::VectorND::Zero(m_number_joints);
     m_current_velocities.data    = ctrl::VectorND::Zero(m_number_joints);
     m_current_accelerations.data = ctrl::VectorND::Zero(m_number_joints);
     m_last_positions.data        = ctrl::VectorND::Zero(m_number_joints);
     m_last_velocities.data       = ctrl::VectorND::Zero(m_number_joints);
-    m_upper_pos_limits           = upper_pos_limits;
-    m_lower_pos_limits           = lower_pos_limits;
 
     // Forward kinematics
     m_fk_pos_solver.reset(new KDL::ChainFkSolverPos_recursive(m_chain));

cartesian_controller_base/src/JacobianTransposeSolver.cpp

@@ -116,6 +116,15 @@ namespace cartesian_controller_base{
   {
     IKSolver::init(nh, chain, upper_pos_limits, lower_pos_limits);
 
+    updateChain(m_chain);
+
+    return true;
+  }
+
+  bool JacobianTransposeSolver::updateChain(const KDL::Chain& chain)
+  {
+    IKSolver::updateChain(chain);
+
     m_jnt_jacobian_solver.reset(new KDL::ChainJntToJacSolver(m_chain));
     m_jnt_jacobian.resize(m_number_joints);
 

cartesian_controller_base/src/SelectivelyDampedLeastSquaresSolver.cpp

@@ -148,6 +148,15 @@ namespace cartesian_controller_base{
   {
     IKSolver::init(nh, chain, upper_pos_limits, lower_pos_limits);
 
+    updateChain(m_chain);
+
+    return true;
+  }
+
+  bool SelectivelyDampedLeastSquaresSolver::updateChain(const KDL::Chain& chain)
+  {
+    IKSolver::updateChain(chain);
+
     m_jnt_jacobian_solver.reset(new KDL::ChainJntToJacSolver(m_chain));
     m_jnt_jacobian.resize(m_number_joints);
 

cartesian_controller_base/srv/SetKinematicChainOffsets.srv

@@ -0,0 +1,8 @@
+# Ability to add a chain before or after the robot chain (parsed by the URDF)
+
+# Removes the chain if the frame_id is empty. Otherwise, overwrites the previous chain.
+geometry_msgs/PoseStamped pre_robot_chain_offset    # Attached before the robot base link
+geometry_msgs/PoseStamped post_robot_chain_offset   # Attached after end effector link
+---
+bool success
+string message