Feature/reconfigure cart compliance controller

cartesian_compliance_controller/src/cartesian_compliance_controller.cpp

@@ -99,19 +99,7 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes
     return TYPE::ERROR;
   }
 
-  // Make sure compliance link is part of the robot chain
-  m_compliance_ref_link = get_node()->get_parameter("compliance_ref_link").as_string();
-  if(!Base::robotChainContains(m_compliance_ref_link))
-  {
-    RCLCPP_ERROR_STREAM(get_node()->get_logger(),
-                        m_compliance_ref_link << " is not part of the kinematic chain from "
-                                              << Base::m_robot_base_link << " to "
-                                              << Base::m_end_effector_link);
-    return TYPE::ERROR;
-  }
 
-  // Make sure sensor wrenches are interpreted correctly
-  ForceBase::setFtSensorReferenceFrame(m_compliance_ref_link);
 
   return TYPE::SUCCESS;
 }
@@ -126,6 +114,21 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes
   {
     return TYPE::ERROR;
   }
+    // Make sure compliance link is part of the robot chain
+  m_compliance_ref_link = get_node()->get_parameter("compliance_ref_link").as_string();
+  if(!Base::robotChainContains(m_compliance_ref_link))
+  {
+    RCLCPP_ERROR_STREAM(get_node()->get_logger(), "[CartesianComplianceController]: " <<
+                        m_compliance_ref_link << " is not part of the kinematic chain from "
+                                              << Base::m_robot_base_link << " to "
+                                              << Base::m_end_effector_link);
+    release_interfaces();                                              
+    return TYPE::ERROR;
+  }
+
+  // Make sure sensor wrenches are interpreted correctly
+  ForceBase::setFtSensorReferenceFrame(m_compliance_ref_link);
+
   return TYPE::SUCCESS;
 }
 

cartesian_controller_base/src/ForwardDynamicsSolver.cpp

@@ -147,7 +147,10 @@ namespace cartesian_controller_base{
     m_jnt_space_inertia.resize(m_number_joints);
 
     // Set the initial value if provided at runtime, else use default value.
-    m_min = nh->declare_parameter<double>(m_params + "/link_mass", 0.1);
+    if(!nh->has_parameter(m_params + "/link_mass"))
+      m_min = nh->declare_parameter<double>(m_params + "/link_mass", 0.1);
+    else
+      m_min = nh->get_parameter(m_params + "/link_mass").get_value<double>();
 
     RCLCPP_INFO(nh->get_logger(), "Forward dynamics solver initialized");
     RCLCPP_INFO(nh->get_logger(), "Forward dynamics solver has control over %i joints", m_number_joints);

cartesian_controller_base/src/cartesian_controller_base.cpp

@@ -154,7 +154,79 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes
     return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
   }
 
-  // Get kinematics specific configuration
+
+    // Get names of actuated joints
+  m_joint_names = get_node()->get_parameter("joints").as_string_array();
+  if (m_joint_names.empty())
+  {
+    RCLCPP_ERROR(get_node()->get_logger(), "joints array is empty");
+    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
+  }
+  // Check command interfaces.
+  // We support position, velocity, or both.
+  m_cmd_interface_types = get_node()->get_parameter("command_interfaces").as_string_array();
+  if (m_cmd_interface_types.empty())
+  {
+    RCLCPP_ERROR(get_node()->get_logger(), "No command_interfaces specified");
+    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
+  }
+  for (const auto& type : m_cmd_interface_types)
+  {
+    RCLCPP_INFO_STREAM(get_node()->get_logger(), type);
+    if (type != hardware_interface::HW_IF_POSITION && type != hardware_interface::HW_IF_VELOCITY)
+    {
+      RCLCPP_ERROR(
+        get_node()->get_logger(),
+        "Unsupported command interface: %s. Choose position or velocity",
+        type.c_str());
+      return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
+    }
+  }
+
+
+
+  // Controller-internal state publishing
+  m_feedback_pose_publisher =
+    std::make_shared<realtime_tools::RealtimePublisher<geometry_msgs::msg::PoseStamped> >(
+      get_node()->create_publisher<geometry_msgs::msg::PoseStamped>(
+        std::string(get_node()->get_name()) + "/current_pose", 3));
+
+  m_feedback_twist_publisher =
+    std::make_shared<realtime_tools::RealtimePublisher<geometry_msgs::msg::TwistStamped> >(
+      get_node()->create_publisher<geometry_msgs::msg::TwistStamped>(
+        std::string(get_node()->get_name()) + "/current_twist", 3));
+
+  m_configured = true;
+  RCLCPP_INFO_STREAM(get_node()->get_logger(), "Configured controller");
+  return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianControllerBase::on_deactivate(
+    const rclcpp_lifecycle::State & previous_state)
+{
+  stopCurrentMotion();
+
+  if (m_active)
+  {
+    m_joint_cmd_pos_handles.clear();
+    m_joint_cmd_vel_handles.clear();
+    m_joint_state_pos_handles.clear();
+    this->release_interfaces();
+    m_active = false;
+  }
+  return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianControllerBase::on_activate(
+    const rclcpp_lifecycle::State & previous_state)
+{
+  if (m_active)
+  {
+    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+  }
+
+
+    // Get kinematics specific configuration
   urdf::Model robot_model;
   KDL::Tree   robot_tree;
 
@@ -197,13 +269,7 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes
     return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
   }
 
-  // Get names of actuated joints
-  m_joint_names = get_node()->get_parameter("joints").as_string_array();
-  if (m_joint_names.empty())
-  {
-    RCLCPP_ERROR(get_node()->get_logger(), "joints array is empty");
-    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
-  }
+
 
   // Parse joint limits
   KDL::JntArray upper_pos_limits(m_joint_names.size());
@@ -239,65 +305,7 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes
   // Initialize Cartesian pd controllers
   m_spatial_controller.init(get_node());
 
-  // Check command interfaces.
-  // We support position, velocity, or both.
-  m_cmd_interface_types = get_node()->get_parameter("command_interfaces").as_string_array();
-  if (m_cmd_interface_types.empty())
-  {
-    RCLCPP_ERROR(get_node()->get_logger(), "No command_interfaces specified");
-    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
-  }
-  for (const auto& type : m_cmd_interface_types)
-  {
-    if (type != hardware_interface::HW_IF_POSITION && type != hardware_interface::HW_IF_VELOCITY)
-    {
-      RCLCPP_ERROR(
-        get_node()->get_logger(),
-        "Unsupported command interface: %s. Choose position or velocity",
-        type.c_str());
-      return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
-    }
-  }
-
-  // Controller-internal state publishing
-  m_feedback_pose_publisher =
-    std::make_shared<realtime_tools::RealtimePublisher<geometry_msgs::msg::PoseStamped> >(
-      get_node()->create_publisher<geometry_msgs::msg::PoseStamped>(
-        std::string(get_node()->get_name()) + "/current_pose", 3));
 
-  m_feedback_twist_publisher =
-    std::make_shared<realtime_tools::RealtimePublisher<geometry_msgs::msg::TwistStamped> >(
-      get_node()->create_publisher<geometry_msgs::msg::TwistStamped>(
-        std::string(get_node()->get_name()) + "/current_twist", 3));
-
-  m_configured = true;
-
-  return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
-}
-
-rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianControllerBase::on_deactivate(
-    const rclcpp_lifecycle::State & previous_state)
-{
-  stopCurrentMotion();
-
-  if (m_active)
-  {
-    m_joint_cmd_pos_handles.clear();
-    m_joint_cmd_vel_handles.clear();
-    m_joint_state_pos_handles.clear();
-    this->release_interfaces();
-    m_active = false;
-  }
-  return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
-}
-
-rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianControllerBase::on_activate(
-    const rclcpp_lifecycle::State & previous_state)
-{
-  if (m_active)
-  {
-    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
-  }
 
   // Get command handles.
   for (const auto& type : m_cmd_interface_types)
@@ -319,6 +327,7 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes
       return CallbackReturn::ERROR;
     }
   }
+
 
   // Get state handles.
   if (!controller_interface::get_ordered_interfaces(state_interfaces_,

cartesian_force_controller/src/cartesian_force_controller.cpp

@@ -89,41 +89,49 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes
     return ret;
   }
 
-  // Make sure sensor link is part of the robot chain
+  m_target_wrench_subscriber = get_node()->create_subscription<geometry_msgs::msg::WrenchStamped>(
+    get_node()->get_name() + std::string("/target_wrench"),
+    10,
+    std::bind(&CartesianForceController::targetWrenchCallback, this, std::placeholders::_1));
+
+
+
+
+  return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianForceController::on_activate(
+    const rclcpp_lifecycle::State & previous_state)
+{
+  RCLCPP_INFO_STREAM(get_node()->get_logger(), "[CartesianForceController] activating");
+  Base::on_activate(previous_state);
+
+
+   // Make sure sensor link is part of the robot chain
   m_ft_sensor_ref_link = get_node()->get_parameter("ft_sensor_ref_link").as_string();
   if(!Base::robotChainContains(m_ft_sensor_ref_link))
   {
-    RCLCPP_ERROR_STREAM(get_node()->get_logger(),
+    RCLCPP_ERROR_STREAM(get_node()->get_logger(), "[CartesianForceController]: " <<
                         m_ft_sensor_ref_link << " is not part of the kinematic chain from "
                                              << Base::m_robot_base_link << " to "
                                              << Base::m_end_effector_link);
+    release_interfaces();
     return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
   }
 
   // Make sure sensor wrenches are interpreted correctly
   setFtSensorReferenceFrame(Base::m_end_effector_link);
 
-  m_target_wrench_subscriber = get_node()->create_subscription<geometry_msgs::msg::WrenchStamped>(
-    get_node()->get_name() + std::string("/target_wrench"),
-    10,
-    std::bind(&CartesianForceController::targetWrenchCallback, this, std::placeholders::_1));
+
+  m_target_wrench.setZero();
+  m_ft_sensor_wrench.setZero();
+
 
   m_ft_sensor_wrench_subscriber =
     get_node()->create_subscription<geometry_msgs::msg::WrenchStamped>(
       get_node()->get_name() + std::string("/ft_sensor_wrench"),
       10,
       std::bind(&CartesianForceController::ftSensorWrenchCallback, this, std::placeholders::_1));
-
-  m_target_wrench.setZero();
-  m_ft_sensor_wrench.setZero();
-
-  return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
-}
-
-rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianForceController::on_activate(
-    const rclcpp_lifecycle::State & previous_state)
-{
-  Base::on_activate(previous_state);
   return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
 }
 

cartesian_motion_controller/src/cartesian_motion_controller.cpp

@@ -97,6 +97,7 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes
 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianMotionController::on_activate(
     const rclcpp_lifecycle::State & previous_state)
 {
+  RCLCPP_INFO_STREAM(get_node()->get_logger(), "[CartesianMotionController] activating");
   Base::on_activate(previous_state);
 
   // Reset simulation with real joint state