TrajectoryController: Use desired state to calculate hold trajectory

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h

@@ -138,6 +138,13 @@ starting(const ros::Time& time)
   time_data.uptime = ros::Time(0.0);
   time_data_.initRT(time_data);
 
+  // Initialize the desired_state with the current state on startup
+  for (unsigned int i = 0; i < joints_.size(); ++i)
+  {
+    desired_state_.position[i] = joints_[i].getPosition();
+    desired_state_.velocity[i] = joints_[i].getVelocity();
+  }
+
   // Hold current position
   setHoldPosition(time_data.uptime);
 
@@ -755,12 +762,14 @@ setHoldPosition(const ros::Time& time, RealtimeGoalHandlePtr gh)
   const unsigned int n_joints = joints_.size();
   for (unsigned int i = 0; i < n_joints; ++i)
   {
-    hold_start_state_.position[0]     =  joints_[i].getPosition();
-    hold_start_state_.velocity[0]     =  joints_[i].getVelocity();
+    // If there is a time delay in the system it is better to use calculate the hold trajectory starting from the
+    // desired position. Otherwise there would be a jerk in the motion.
+    hold_start_state_.position[0]     =  desired_state_.position[i];
+    hold_start_state_.velocity[0]     =  desired_state_.velocity[i];
     hold_start_state_.acceleration[0] =  0.0;
 
-    hold_end_state_.position[0]       =  joints_[i].getPosition();
-    hold_end_state_.velocity[0]       = -joints_[i].getVelocity();
+    hold_end_state_.position[0]       =  desired_state_.position[i];
+    hold_end_state_.velocity[0]       = -desired_state_.velocity[i];
     hold_end_state_.acceleration[0]   =  0.0;
 
     (*hold_trajectory_ptr_)[i].front().init(start_time,  hold_start_state_,

joint_trajectory_controller/test/joint_trajectory_controller_test.cpp

@@ -29,6 +29,7 @@
 
 #include <algorithm>
 #include <cmath>
+#include <limits>
 
 #include <boost/shared_ptr.hpp>
 #include <boost/thread/mutex.hpp>
@@ -39,6 +40,7 @@
 #include <actionlib/client/simple_action_client.h>
 
 #include <std_msgs/Float64.h>
+#include <std_msgs/Bool.h>
 #include <control_msgs/FollowJointTrajectoryAction.h>
 #include <control_msgs/JointTrajectoryControllerState.h>
 #include <control_msgs/QueryTrajectoryState.h>
@@ -62,6 +64,9 @@ class JointTrajectoryControllerTest : public ::testing::Test
     joint_names[0] = "joint1";
     joint_names[1] = "joint2";
 
+    controller_min_actual_velocity.resize(n_joints);
+    controller_max_actual_velocity.resize(n_joints);
+
     trajectory_msgs::JointTrajectoryPoint point;
     point.positions.resize(n_joints, 0.0);
     point.velocities.resize(n_joints, 0.0);
@@ -96,6 +101,9 @@ class JointTrajectoryControllerTest : public ::testing::Test
     // Smoothing publisher (determines how well the robot follows a trajectory)
     smoothing_pub = ros::NodeHandle().advertise<std_msgs::Float64>("smoothing", 1);
 
+    // Delay publisher (allows to simulate a delay of one cycle in the hardware interface)
+    delay_pub = ros::NodeHandle().advertise<std_msgs::Bool>("delay", 1);
+
     // Trajectory publisher
     traj_pub = nh.advertise<trajectory_msgs::JointTrajectory>("command", 1);
 
@@ -141,6 +149,7 @@ class JointTrajectoryControllerTest : public ::testing::Test
   ros::Duration long_timeout;
 
   ros::Publisher     smoothing_pub;
+  ros::Publisher     delay_pub;
   ros::Publisher     traj_pub;
   ros::Subscriber    state_sub;
   ros::ServiceClient query_state_service;
@@ -149,11 +158,20 @@ class JointTrajectoryControllerTest : public ::testing::Test
 
 
   StateConstPtr controller_state;
+  std::vector<double> controller_min_actual_velocity;
+  std::vector<double> controller_max_actual_velocity;
 
   void stateCB(const StateConstPtr& state)
   {
     boost::mutex::scoped_lock lock(mutex);
     controller_state = state;
+
+    std::transform(controller_min_actual_velocity.begin(), controller_min_actual_velocity.end(),
+                   state->actual.velocities.begin(), controller_min_actual_velocity.begin(),
+                   std::min<double>);
+    std::transform(controller_max_actual_velocity.begin(), controller_max_actual_velocity.end(),
+                   state->actual.velocities.begin(), controller_max_actual_velocity.begin(),
+                   std::max<double>);
   }
 
   StateConstPtr getState()
@@ -177,6 +195,28 @@ class JointTrajectoryControllerTest : public ::testing::Test
     return init_ok;
   }
 
+  std::vector<double> getMinActualVelocity()
+  {
+    boost::mutex::scoped_lock lock(mutex);
+    return controller_min_actual_velocity;
+  }
+
+  std::vector<double> getMaxActualVelocity()
+  {
+    boost::mutex::scoped_lock lock(mutex);
+    return controller_max_actual_velocity;
+  }
+
+  void resetActualVelocityObserver()
+  {
+    boost::mutex::scoped_lock lock(mutex);
+    for(size_t i = 0; i < controller_min_actual_velocity.size(); ++i)
+    {
+      controller_min_actual_velocity[i] = std::numeric_limits<double>::infinity();
+      controller_max_actual_velocity[i] = -std::numeric_limits<double>::infinity();
+    }
+  }
+
   static bool waitForState(const ActionClientPtr& action_client,
                            const actionlib::SimpleClientGoalState& state,
                            const ros::Duration& timeout)
@@ -731,6 +771,52 @@ TEST_F(JointTrajectoryControllerTest, emptyTopicCancelsActionTraj)
   }
 }
 
+TEST_F(JointTrajectoryControllerTest, emptyTopicCancelsActionTrajWithDelay)
+{
+  ASSERT_TRUE(action_client->waitForServer(long_timeout));
+
+  // Go to home configuration, we need known initial conditions
+  traj_home_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+  action_client->sendGoal(traj_home_goal);
+  ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::SUCCEEDED, long_timeout));
+
+  // Make robot respond with a delay
+  {
+    std_msgs::Bool delay;
+    delay.data = true;
+    delay_pub.publish(delay);
+    ros::Duration(0.5).sleep();
+  }
+  resetActualVelocityObserver();
+
+  // Send trajectory
+  traj_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+  action_client->sendGoal(traj_goal);
+  EXPECT_TRUE(waitForState(action_client, SimpleClientGoalState::ACTIVE, short_timeout));
+
+  ros::Duration wait_duration = traj.points.front().time_from_start * 0.5;
+  wait_duration.sleep(); // Wait until half of first segment
+
+  ActionGoal empty_goal;
+  empty_goal.trajectory.joint_names = joint_names;
+  action_client->sendGoal(empty_goal);
+  ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::SUCCEEDED, long_timeout));
+
+  std::vector<double> minVelocity = getMinActualVelocity();
+  for(size_t i = 0; i < minVelocity.size(); ++i)
+  {
+    EXPECT_GE(minVelocity[i], 0.);
+  }
+
+  // Restore perfect control
+  {
+    std_msgs::Bool delay;
+    delay.data = false;
+    delay_pub.publish(delay);
+    ros::Duration(0.5).sleep();
+  }
+}
+
 TEST_F(JointTrajectoryControllerTest, emptyActionCancelsTopicTraj)
 {
   ASSERT_TRUE(initState());

joint_trajectory_controller/test/rrbot.cpp

@@ -30,6 +30,7 @@
 // ROS
 #include <ros/ros.h>
 #include <std_msgs/Float64.h>
+#include <std_msgs/Bool.h>
 
 // ros_control
 #include <controller_manager/controller_manager.h>
@@ -48,7 +49,9 @@ class RRbot : public hardware_interface::RobotHW
     vel_[0] = 0.0; vel_[1] = 0.0;
     eff_[0] = 0.0; eff_[1] = 0.0;
     pos_cmd_[0] = 0.0; pos_cmd_[1] = 0.0;
+    pos_lastcmd_[0] = 0.0; pos_lastcmd_[1] = 0.0;
     vel_cmd_[0] = 0.0; vel_cmd_[1] = 0.0;
+    vel_lastcmd_[0] = 0.0; vel_lastcmd_[1] = 0.0;
 
     // Connect and register the joint state interface
     hardware_interface::JointStateHandle state_handle_1("joint1", &pos_[0], &vel_[0], &eff_[0]);
@@ -80,6 +83,10 @@ class RRbot : public hardware_interface::RobotHW
     // Smoothing subscriber
     smoothing_sub_ = ros::NodeHandle().subscribe("smoothing", 1, &RRbot::smoothingCB, this);
     smoothing_.initRT(0.0);
+
+    // Delay subscriber: delay==0 yields direct control, one cycle delay otherwise
+    delay_sub_ = ros::NodeHandle().subscribe("delay", 1, &RRbot::delayCB, this);
+    delay_.initRT(false);
   }
 
   ros::Time getTime() const {return ros::Time::now();}
@@ -90,14 +97,26 @@ class RRbot : public hardware_interface::RobotHW
   void write()
   {
     const double smoothing = *(smoothing_.readFromRT());
+    const bool delay = *(delay_.readFromRT());
+
     for (unsigned int i = 0; i < 2; ++i)
     {
-      if(active_interface_[i] == "hardware_interface::PositionJointInterface")
+      if(delay != 0)
+      {
+        std::swap(pos_cmd_[i], pos_lastcmd_[i]);
+        std::swap(vel_cmd_[i], vel_lastcmd_[i]);
+      }
+      else
       {
-        vel_[i] = (pos_cmd_[i] - pos_[i]) / getPeriod().toSec();
+        pos_lastcmd_[i] = pos_cmd_[i];
+        vel_lastcmd_[i] = vel_cmd_[i];
+      }
 
+      if(active_interface_[i] == "hardware_interface::PositionJointInterface")
+      {
         const double next_pos = smoothing * pos_[i] +  (1.0 - smoothing) * pos_cmd_[i];
-        pos_[i] = next_pos;        
+        vel_[i] = (next_pos - pos_[i]) / getPeriod().toSec();
+        pos_[i] = next_pos;
       }
       else if(active_interface_[i] == "hardware_interface::VelocityJointInterface")
       {
@@ -146,6 +165,8 @@ class RRbot : public hardware_interface::RobotHW
   hardware_interface::VelocityJointInterface jnt_vel_interface_;
   double pos_cmd_[2];
   double vel_cmd_[2];
+  double pos_lastcmd_[2];
+  double vel_lastcmd_[2];
   double pos_[2];
   double vel_[2];
   double eff_[2];
@@ -155,8 +176,11 @@ class RRbot : public hardware_interface::RobotHW
 
   realtime_tools::RealtimeBuffer<double> smoothing_;
   void smoothingCB(const std_msgs::Float64& smoothing) {smoothing_.writeFromNonRT(smoothing.data);}
-
   ros::Subscriber smoothing_sub_;
+
+  realtime_tools::RealtimeBuffer<bool> delay_;
+  void delayCB(const std_msgs::Bool& delay) {delay_.writeFromNonRT(delay.data);}
+  ros::Subscriber delay_sub_;
 };
 
 int main(int argc, char **argv)

joint_trajectory_controller/test/rrbot_controllers.yaml

@@ -12,3 +12,5 @@ rrbot_controller:
     joint2:
       goal:       0.01
       trajectory: 0.05
+  stop_trajectory_duration: 0.1
+  state_publish_rate: 100