experiment ral (first session)

Author: Hydran00

effort_controller_base/include/effort_controller_base/effort_controller_base.h

@@ -196,6 +196,8 @@ class EffortControllerBase : public controller_interface::ControllerInterface {
   std::string m_end_effector_link;
   std::string m_compliance_ref_link;
   std::string m_robot_base_link;
+  KDL::JntArray m_upper_pos_limits;
+  KDL::JntArray m_lower_pos_limits;
   std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
       m_joint_state_pos_handles;
   std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
@@ -225,6 +227,7 @@ class EffortControllerBase : public controller_interface::ControllerInterface {
   bool m_initialized = {false};
   bool m_configured = {false};
   bool m_active = {false};
+  int m_rate;
   // joint velocity filter
   const double m_dotq_alpha = 0.3;
   // Dynamic parameters

effort_controller_base/src/effort_controller_base.cpp

@@ -93,7 +93,8 @@ EffortControllerBase::on_init() {
     auto executor =
         std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
     executor->add_node(robot_description_listener);
-    while (!robot_description_listener->m_description_received_) {
+    while (!robot_description_listener->m_description_received_ &&
+           rclcpp::ok()) {
       executor->spin_some();
       RCLCPP_INFO_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(),
                            1000, "Waiting for robot description");
@@ -117,7 +118,8 @@ EffortControllerBase::on_configure(
   // Get kinematics specific configuration
   urdf::Model robot_model;
   KDL::Tree robot_tree;
-
+  m_rate = get_node()->get_parameter("update_rate").as_int();
+  RCLCPP_INFO(get_node()->get_logger(), "Rate: %d Hz", m_rate);
   m_robot_base_link = get_node()->get_parameter("robot_base_link").as_string();
   if (m_robot_base_link.empty()) {
     RCLCPP_ERROR(get_node()->get_logger(), "robot_base_link is empty");
@@ -187,8 +189,9 @@ EffortControllerBase::on_configure(
   m_joint_effort_limits.resize(m_joint_number);
 
   // Parse joint limits
-  KDL::JntArray upper_pos_limits(m_joint_number);
-  KDL::JntArray lower_pos_limits(m_joint_number);
+  m_upper_pos_limits.resize(m_joint_number);
+  m_lower_pos_limits.resize(m_joint_number);
+
   for (size_t i = 0; i < m_joint_number; ++i) {
     if (!robot_model.getJoint(m_joint_names[i])) {
       RCLCPP_ERROR(get_node()->get_logger(),
@@ -199,18 +202,25 @@ EffortControllerBase::on_configure(
     }
     if (robot_model.getJoint(m_joint_names[i])->type ==
         urdf::Joint::CONTINUOUS) {
-      upper_pos_limits(i) = std::nan("0");
-      lower_pos_limits(i) = std::nan("0");
+      m_upper_pos_limits(i) = std::nan("0");
+      m_lower_pos_limits(i) = std::nan("0");
       m_joint_effort_limits(i) = std::nan("0");
     } else {
       // Non-existent urdf limits are zero initialized
-      upper_pos_limits(i) =
+      m_upper_pos_limits(i) =
           robot_model.getJoint(m_joint_names[i])->limits->upper;
-      lower_pos_limits(i) =
+      m_lower_pos_limits(i) =
           robot_model.getJoint(m_joint_names[i])->limits->lower;
       m_joint_effort_limits(i) =
           robot_model.getJoint(m_joint_names[i])->limits->effort;
     }
+
+    // print limits
+    RCLCPP_INFO_STREAM(get_node()->get_logger(),
+                       "Joint " << m_joint_names[i] << ": "
+                                << "lower: " << m_lower_pos_limits(i)
+                                << ", upper: " << m_upper_pos_limits(i)
+                                << ", effort: " << m_joint_effort_limits(i));
   }
 
   // Initialize solvers
@@ -224,7 +234,7 @@ EffortControllerBase::on_configure(
   m_ik_solver_vel.reset(new KDL::ChainIkSolverVel_pinv(m_robot_chain));
 
   m_ik_solver.reset(new KDL::ChainIkSolverPos_NR_JL(
-      m_robot_chain, lower_pos_limits, upper_pos_limits, *m_fk_solver,
+      m_robot_chain, m_lower_pos_limits, m_upper_pos_limits, *m_fk_solver,
       *m_ik_solver_vel, 100, 1e-6));
 
   // m_ik_solver.reset(new KDL::ChainIkSolverPos_LMA(m_robot_chain, 1e-4, 1000,
@@ -326,15 +336,15 @@ EffortControllerBase::on_activate(
 
   // Get command handles.
   // Position
-    if (!controller_interface::get_ordered_interfaces(
-            command_interfaces_, m_joint_names,
-            hardware_interface::HW_IF_POSITION, m_joint_cmd_pos_handles)) {
-      RCLCPP_ERROR(get_node()->get_logger(),
-                   "Expected %zu '%s' command interfaces, got %zu.",
-                   m_joint_number, hardware_interface::HW_IF_POSITION,
-                   m_joint_cmd_pos_handles.size());
-      return CallbackReturn::ERROR;
-    }
+  if (!controller_interface::get_ordered_interfaces(
+          command_interfaces_, m_joint_names,
+          hardware_interface::HW_IF_POSITION, m_joint_cmd_pos_handles)) {
+    RCLCPP_ERROR(get_node()->get_logger(),
+                 "Expected %zu '%s' command interfaces, got %zu.",
+                 m_joint_number, hardware_interface::HW_IF_POSITION,
+                 m_joint_cmd_pos_handles.size());
+    return CallbackReturn::ERROR;
+  }
   RCLCPP_INFO(get_node()->get_logger(), "Finished getting command interfaces");
   // Get state handles.
   // Position
@@ -371,9 +381,27 @@ EffortControllerBase::on_activate(
 
 void EffortControllerBase::writeJointEffortCmds(
     ctrl::VectorND &target_joint_positions) {
-    for (size_t i = 0; i < m_joint_number; ++i) {
-      m_joint_cmd_pos_handles[i].get().set_value(target_joint_positions(i));
+  const double max_velocity = 2 * 0.001309; // rad/s
+  const double eps = 1e-4;
+  for (size_t i = 0; i < m_joint_number; ++i) {
+    // enforce joint limits
+    if (target_joint_positions(i) + 2 * max_velocity > m_upper_pos_limits(i)) {
+      // set equal to current
+      target_joint_positions(i) = m_upper_pos_limits(i) - 2 * max_velocity - eps;
+      // RCLCPP_INFO(get_node()->get_logger(),
+      //             "Joint %s target is out of limits, setting to %f",
+      //             m_joint_names[i].c_str(), m_joint_positions(i));
+    } else if (target_joint_positions(i) - 2 * max_velocity <
+               m_lower_pos_limits(i)) {
+      // set equal to current
+      target_joint_positions(i) = m_lower_pos_limits(i) + 2 * max_velocity + eps;
+      // RCLCPP_INFO(get_node()->get_logger(),
+      //             "Joint %s target is out of limits, setting to %f",
+      //             m_joint_names[i].c_str(), m_joint_positions(i));
     }
+
+    m_joint_cmd_pos_handles[i].get().set_value(target_joint_positions(i));
+  }
 }
 
 void EffortControllerBase::computeIKSolution(

kuka_cartesian_impedance_controller/CMakeLists.txt

@@ -11,9 +11,8 @@ option(LOGGING "Enable logging" ON)
 add_compile_options(${ADDITIONAL_COMPILE_OPTIONS})
 
 # Build in release mode if not specified
-if(NOT CMAKE_BUILD_TYPE)
-  set(CMAKE_BUILD_TYPE Release)
-endif()
+set(CMAKE_BUILD_TYPE Release)
+
 
 find_package(Eigen3 3.3 REQUIRED NO_MODULE)
 add_definitions(-DEIGEN_MPL2_ONLY)

kuka_cartesian_impedance_controller/include/kuka_cartesian_impedance_controller/kuka_cartesian_impedance_controller.h

@@ -63,6 +63,7 @@ class KukaCartesianImpedanceController
 
   ctrl::VectorND computeTorque();
   void computeTargetPos();
+  void filterMaximumForce();
 
   using Base = effort_controller_base::EffortControllerBase;
 
@@ -87,6 +88,8 @@ class KukaCartesianImpedanceController
   rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr
       m_target_frame_subscriber;
   rclcpp::Publisher<debug_msg::msg::Debug>::SharedPtr m_data_publisher;
+  rclcpp::Publisher<geometry_msgs::msg::WrenchStamped>::SharedPtr
+      m_wrench_publisher;
   KDL::Frame m_target_frame, m_filtered_frame;
   ctrl::VectorND m_target_joint_position;
 #if LOGGING
@@ -102,9 +105,12 @@ class KukaCartesianImpedanceController
   ctrl::VectorND m_q_starting_pose;
   double m_max_linear_velocity;
   double m_max_angular_velocity;
+  double m_max_impedance_force;
   double m_vel_old = 0.0;
   double current_acc_j0 = 0.0;
   bool m_compensate_dJdq = false;
+  bool m_received_target_frame = false;
+  geometry_msgs::msg::WrenchStamped m_wrench_msg;
 
   double m_last_time;