Avoid "jumps" with states that have tracking error. All test are passing but separatelly. Is there some kind of timeout?

Author: destogl

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -122,6 +122,14 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
     hardware_interface::HW_IF_EFFORT,
   };
 
+  // Parameters for some special cases, e.g. hydraulics powered robots
+  /// Read hardware states only when starting controller. This is useful when robot is not exactly
+  /// following the commanded trajectory.
+  bool hardware_state_has_offset_ = false;
+  trajectory_msgs::msg::JointTrajectoryPoint current_state_when_offset_;
+  /// Allow integration in goal trajectories to accept goals without position or velocity specified
+  bool allow_integration_in_goal_trajectories_ = false;
+
   // The interfaces are defined as the types in 'allowed_interface_types_' member.
   // For convenience, for each type the interfaces are ordered so that i-th position
   // matches i-th index in joint_names_
@@ -131,6 +139,9 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   joint_state_interface_;
 
   /// If true, a velocity feedforward term plus corrective PID term is used
+  // TODO(anyone): This flag is not used for now
+  // There should be PID-approach used as in ROS1:
+  // https://github.com/ros-controls/ros_controllers/blob/noetic-devel/joint_trajectory_controller/include/joint_trajectory_controller/hardware_interface_adapter.h#L283
   bool use_closed_loop_pid_adapter = false;
 
   // TODO(karsten1987): eventually activate and deactive subscriber directly when its supported
@@ -207,7 +218,25 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
     const JointTrajectoryPoint & current_state,
     const JointTrajectoryPoint & state_error);
 
+  void read_state_from_hardware(JointTrajectoryPoint & state);
+
 private:
+  void resize_joint_trajectory_point(
+    trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size)
+  {
+    point.positions.resize(size);
+    if (check_if_interface_type_exists(
+        state_interface_types_, hardware_interface::HW_IF_VELOCITY))
+    {
+      point.velocities.resize(size);
+    }
+    if (check_if_interface_type_exists(
+        state_interface_types_, hardware_interface::HW_IF_ACCELERATION))
+    {
+      point.accelerations.resize(size);
+    }
+  }
+
   bool check_if_interface_type_exists(
     const std::vector<std::string> & interface_type_list, const std::string & interface_type)
   {

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -71,6 +71,10 @@ JointTrajectoryController::init(const std::string & controller_name)
   node_->declare_parameter<double>("state_publish_rate", 50.0);
   node_->declare_parameter<double>("action_monitor_rate", 20.0);
   node_->declare_parameter<bool>("allow_partial_joints_goal", allow_partial_joints_goal_);
+  node_->declare_parameter<bool>(
+    "hardware_state_has_offset", hardware_state_has_offset_);
+  node_->declare_parameter<bool>(
+    "allow_integration_in_goal_trajectories", allow_integration_in_goal_trajectories_);
   node_->declare_parameter<double>("constraints.stopped_velocity_tolerance", 0.01);
   node_->declare_parameter<double>("constraints.goal_time", 0.0);
 
@@ -116,21 +120,6 @@ JointTrajectoryController::update()
     return controller_interface::return_type::SUCCESS;
   }
 
-  auto resize_joint_trajectory_point =
-    [&](trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size)
-    {
-      point.positions.resize(size);
-      if (check_if_interface_type_exists(
-          state_interface_types_, hardware_interface::HW_IF_VELOCITY))
-      {
-        point.velocities.resize(size);
-      }
-      if (check_if_interface_type_exists(
-          state_interface_types_, hardware_interface::HW_IF_ACCELERATION))
-      {
-        point.accelerations.resize(size);
-      }
-    };
   auto compute_error_for_joint = [&](JointTrajectoryPoint & error, int index,
       const JointTrajectoryPoint & current, const JointTrajectoryPoint & desired)
     {
@@ -159,20 +148,14 @@ JointTrajectoryController::update()
   if (current_external_msg != *new_external_msg) {
     fill_partial_goal(*new_external_msg);
     sort_to_local_joint_order(*new_external_msg);
+    // TODO(denis): Add here integration of position and velocity
     traj_external_point_ptr_->update(*new_external_msg);
   }
 
   JointTrajectoryPoint state_current, state_desired, state_error;
   const auto joint_num = joint_names_.size();
   resize_joint_trajectory_point(state_current, joint_num);
 
-  auto assign_point_from_interface = [&, joint_num](
-    std::vector<double> & trajectory_point_interface, const auto & joint_inteface)
-    {
-      for (auto index = 0ul; index < joint_num; ++index) {
-        trajectory_point_interface[index] = joint_inteface[index].get().get_value();
-      }
-    };
   // TODO(anyone): can I here also use const on joint_interface since the reference_wrapper is not
   // changed, but its value only?
   auto assign_interface_from_point = [&, joint_num](
@@ -185,35 +168,19 @@ JointTrajectoryController::update()
 
   // current state update
   state_current.time_from_start.set__sec(0);
-
-  // Assign values from the hardware
-  // Position states always exist
-  assign_point_from_interface(state_current.positions, joint_state_interface_[0]);
-  // velocity and acceleration states are optional
-  if (check_if_interface_type_exists(state_interface_types_, hardware_interface::HW_IF_VELOCITY)) {
-    assign_point_from_interface(state_current.velocities, joint_state_interface_[1]);
-    // Acceleration is used only in combination with velocity
-    if (check_if_interface_type_exists(
-        state_interface_types_,
-        hardware_interface::HW_IF_ACCELERATION))
-    {
-      assign_point_from_interface(state_current.accelerations, joint_state_interface_[2]);
-    } else {
-      // Make empty so the property is ignored during interpolation
-      state_current.accelerations.clear();
-    }
-  } else {
-    // Make empty so the property is ignored during interpolation
-    state_current.velocities.clear();
-    state_current.accelerations.clear();
-  }
+  read_state_from_hardware(state_current);
 
   // currently carrying out a trajectory
   if (traj_point_active_ptr_ && (*traj_point_active_ptr_)->has_trajectory_msg() == false) {
     // if sampling the first time, set the point before you sample
     if (!(*traj_point_active_ptr_)->is_sampled_already()) {
-      (*traj_point_active_ptr_)->set_point_before_trajectory_msg(
-        node_->now(), state_current);
+      if (hardware_state_has_offset_) {
+        (*traj_point_active_ptr_)->set_point_before_trajectory_msg(
+          node_->now(), current_state_when_offset_);
+      } else {
+        (*traj_point_active_ptr_)->set_point_before_trajectory_msg(
+          node_->now(), state_current);
+      }
     }
     resize_joint_trajectory_point(state_error, joint_num);
 
@@ -269,6 +236,9 @@ JointTrajectoryController::update()
         }
       }
 
+      // store command as state when hardware state has tracking offset
+      current_state_when_offset_ = state_desired;
+
       if (rt_active_goal_) {
         // send feedback
         auto feedback = std::make_shared<FollowJTrajAction::Feedback>();
@@ -330,6 +300,40 @@ JointTrajectoryController::update()
   return controller_interface::return_type::SUCCESS;
 }
 
+void JointTrajectoryController::read_state_from_hardware(JointTrajectoryPoint & state)
+{
+  const auto joint_num = joint_names_.size();
+  auto assign_point_from_interface = [&, joint_num](
+    std::vector<double> & trajectory_point_interface, const auto & joint_inteface)
+    {
+      for (auto index = 0ul; index < joint_num; ++index) {
+        trajectory_point_interface[index] = joint_inteface[index].get().get_value();
+      }
+    };
+
+  // Assign values from the hardware
+  // Position states always exist
+  assign_point_from_interface(state.positions, joint_state_interface_[0]);
+  // velocity and acceleration states are optional
+  if (check_if_interface_type_exists(state_interface_types_, hardware_interface::HW_IF_VELOCITY)) {
+    assign_point_from_interface(state.velocities, joint_state_interface_[1]);
+    // Acceleration is used only in combination with velocity
+    if (check_if_interface_type_exists(
+        state_interface_types_,
+        hardware_interface::HW_IF_ACCELERATION))
+    {
+      assign_point_from_interface(state.accelerations, joint_state_interface_[2]);
+    } else {
+      // Make empty so the property is ignored during interpolation
+      state.accelerations.clear();
+    }
+  } else {
+    // Make empty so the property is ignored during interpolation
+    state.velocities.clear();
+    state.accelerations.clear();
+  }
+}
+
 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
 JointTrajectoryController::on_configure(const rclcpp_lifecycle::State &)
 {
@@ -487,6 +491,12 @@ JointTrajectoryController::on_configure(const rclcpp_lifecycle::State &)
 
   default_tolerances_ = get_segment_tolerances(*node_, joint_names_);
 
+  // Read parameters customizing controller for special cases
+  hardware_state_has_offset_ =
+    node_->get_parameter("hardware_state_has_offset").get_value<bool>();
+  allow_integration_in_goal_trajectories_ =
+    node_->get_parameter("allow_integration_in_goal_trajectories").get_value<bool>();
+
   // subscriber callback
   // non realtime
   // TODO(karsten): check if traj msg and point time are valid
@@ -653,6 +663,10 @@ JointTrajectoryController::on_activate(const rclcpp_lifecycle::State &)
   traj_point_active_ptr_ = &traj_external_point_ptr_;
   last_state_publish_time_ = node_->now();
 
+  // Initialize current state storage if hardware state has tracking offset
+  resize_joint_trajectory_point(current_state_when_offset_, joint_names_.size());
+  read_state_from_hardware(current_state_when_offset_);
+
   // TODO(karsten1987): activate subscriptions of subscriber
   return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
 }