Adjusting CTG with limiters.

StoglRobotics-forks · Feb 9, 2024 · 1ca51d1 · 1ca51d1
1 parent 34efa25
commit 1ca51d1
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 10 deletions.
diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp
@@ -52,6 +52,7 @@ void Trajectory::set_point_before_trajectory_msg(
 {
   time_before_traj_msg_ = current_time;
   state_before_traj_msg_ = current_point;
+  previous_state_ = current_point;
 }
 
 void Trajectory::update(std::shared_ptr<trajectory_msgs::msg::JointTrajectory> joint_trajectory)
@@ -71,8 +72,6 @@ bool Trajectory::sample(
 {
   THROW_ON_NULLPTR(trajectory_msg_)
 
-  RCUTILS_LOG_ERROR_NAMED("trajectory", "Sampling Trajectory");
-
   if (trajectory_msg_->points.empty())
   {
     start_segment_itr = end();
@@ -107,16 +106,14 @@ bool Trajectory::sample(
   // current time hasn't reached traj time of the first point in the msg yet
   if (sample_time < first_point_timestamp)
   {
-    RCUTILS_LOG_ERROR_NAMED("trajectory", "Before Start time");
     // If interpolation is disabled, just forward the next waypoint
     if (interpolation_method == interpolation_methods::InterpolationMethod::NONE)
     {
       output_state = state_before_traj_msg_;
-      previous_state_ = state_before_traj_msg_;
     }
     else
     {
-      // it changes points only if position and velocity do not exist, but their derivatives
+      // it changes a point only if position and velocity do not exist, but their derivatives
       deduce_from_derivatives(
         state_before_traj_msg_, first_point_in_msg, state_before_traj_msg_.positions.size(),
         (first_point_timestamp - time_before_traj_msg_).seconds());
@@ -130,7 +127,7 @@ bool Trajectory::sample(
 
     if (joint_limiter)
     {
-      joint_limiter->enforce(previous_state_, output_state, period);
+      joint_limiter->enforce(state_before_traj_msg_, output_state, period);
     }
     previous_state_ = output_state;
     return true;
@@ -173,8 +170,6 @@ bool Trajectory::sample(
     }
   }
 
-  RCUTILS_LOG_ERROR_NAMED("trajectory", "Managing last point");
-
   // whole animation has played out - but still continue s interpolating and smoothing
   auto & last_point = trajectory_msg_->points[trajectory_msg_->points.size() - 1];
 

diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
@@ -149,12 +149,12 @@ class TestableJointTrajectoryController
 
   bool has_trivial_traj() const
   {
-    return has_active_trajectory() && traj_external_point_ptr_->has_nontrivial_msg() == false;
+    return has_active_trajectory() && current_trajectory_->has_nontrivial_msg() == false;
   }
 
   bool has_nontrivial_traj()
   {
-    return has_active_trajectory() && traj_external_point_ptr_->has_nontrivial_msg();
+    return has_active_trajectory() && current_trajectory_->has_nontrivial_msg();
   }
 
   double get_cmd_timeout() { return cmd_timeout_; }