From b10c2b7cb2431b94309af32b3547fdad25a26ff5 Mon Sep 17 00:00:00 2001 From: agutenkunst Date: Fri, 16 Mar 2018 14:08:35 +0100 Subject: [PATCH] TrajectoryController: Use desired state to calculate hold trajectory (#297) * Use desired state to calculate hold trajectory * add test case that fails without e53b364 See pull request for fix/jerk_on_stop for detailed description. This commit adds a delay subscriber to rrbot to emulate a hardware controller with dead time. The jump backwards in the actual position is observed in joint_trajectory_controller_test by extending the stateCB. The new testCase fails, if the velocity of the actual position is negative although motion should only be forwards. * add test case with upper bound (simulating a wall) Adding a topic to rrbot to set an upper bound allows to test the actual position after setHoldPosition() has been called. * preserve jump, if stop_trajectory_duration==0 old behaviour for stop_trajectory_duration==0 * incorporate review comments * use start_time variable in setHoldPosition * doxygen documentation in header file * drop sleep() in testcases in favor of waiting for PREEMPTED/SUCCEEDED state to speedup the tests a bit and stay below the former timeout of 85s --- joint_trajectory_controller/CMakeLists.txt | 5 + .../joint_trajectory_controller.h | 7 +- .../joint_trajectory_controller_impl.h | 80 +++++--- .../joint_trajectory_controller_stopramp.test | 39 ++++ .../test/joint_trajectory_controller_test.cpp | 173 +++++++++++++++++- joint_trajectory_controller/test/rrbot.cpp | 49 ++++- .../test/rrbot_controllers.yaml | 2 + .../test/rrbot_controllers_stopramp.yaml | 16 ++ 8 files changed, 333 insertions(+), 38 deletions(-) create mode 100644 joint_trajectory_controller/test/joint_trajectory_controller_stopramp.test create mode 100644 joint_trajectory_controller/test/rrbot_controllers_stopramp.yaml diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 27da66732..076bfaf0d 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -96,6 +96,11 @@ if(CATKIN_ENABLE_TESTING) test/joint_trajectory_controller_test.cpp) target_link_libraries(joint_trajectory_controller_test ${catkin_LIBRARIES}) + add_rostest_gtest(joint_trajectory_controller_stopramp_test + test/joint_trajectory_controller_stopramp.test + test/joint_trajectory_controller_test.cpp) + target_link_libraries(joint_trajectory_controller_stopramp_test ${catkin_LIBRARIES}) + add_rostest_gtest(joint_trajectory_controller_vel_test test/joint_trajectory_controller_vel.test test/joint_trajectory_controller_test.cpp) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.h b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.h index 14ea0bb6a..11da26eaf 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.h +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.h @@ -208,7 +208,7 @@ class JointTrajectoryController : public controller_interface::Controller successful_joint_traj_; bool allow_partial_joints_goal_; @@ -240,8 +240,9 @@ class JointTrajectoryController : public controller_interface::Controller void JointTrajectoryController:: setHoldPosition(const ros::Time& time, RealtimeGoalHandlePtr gh) { - // Settle position in a fixed time. We do the following: - // - Create segment that goes from current (pos,vel) to (pos,-vel) in 2x the desired stop time - // - Assuming segment symmetry, sample segment at its midpoint (desired stop time). It should have zero velocity - // - Create segment that goes from current state to above zero velocity state, in the desired time - // NOTE: The symmetry assumption from the second point above might not hold for all possible segment types - assert(joint_names_.size() == hold_trajectory_ptr_->size()); typename Segment::State hold_start_state_ = typename Segment::State(1); typename Segment::State hold_end_state_ = typename Segment::State(1); - + const unsigned int n_joints = joints_.size(); const typename Segment::Time start_time = time.toSec(); - const typename Segment::Time end_time = time.toSec() + stop_trajectory_duration_; - const typename Segment::Time end_time_2x = time.toSec() + 2.0 * stop_trajectory_duration_; - // Create segment that goes from current (pos,vel) to (pos,-vel) - const unsigned int n_joints = joints_.size(); - for (unsigned int i = 0; i < n_joints; ++i) + if(stop_trajectory_duration_ == 0.0) { - hold_start_state_.position[0] = joints_[i].getPosition(); - hold_start_state_.velocity[0] = joints_[i].getVelocity(); - hold_start_state_.acceleration[0] = 0.0; + // stop at current actual position + for (unsigned int i = 0; i < n_joints; ++i) + { + hold_start_state_.position[0] = joints_[i].getPosition(); + hold_start_state_.velocity[0] = 0.0; + hold_start_state_.acceleration[0] = 0.0; + (*hold_trajectory_ptr_)[i].front().init(start_time, hold_start_state_, + start_time, hold_start_state_); + // Set goal handle for the segment + (*hold_trajectory_ptr_)[i].front().setGoalHandle(gh); + } + } + else + { + // Settle position in a fixed time. We do the following: + // - Create segment that goes from current (pos,vel) to (pos,-vel) in 2x the desired stop time + // - Assuming segment symmetry, sample segment at its midpoint (desired stop time). It should have zero velocity + // - Create segment that goes from current state to above zero velocity state, in the desired time + // NOTE: The symmetry assumption from the second point above might not hold for all possible segment types - hold_end_state_.position[0] = joints_[i].getPosition(); - hold_end_state_.velocity[0] = -joints_[i].getVelocity(); - hold_end_state_.acceleration[0] = 0.0; + const typename Segment::Time end_time = time.toSec() + stop_trajectory_duration_; + const typename Segment::Time end_time_2x = time.toSec() + 2.0 * stop_trajectory_duration_; - (*hold_trajectory_ptr_)[i].front().init(start_time, hold_start_state_, - end_time_2x, hold_end_state_); + // Create segment that goes from current (pos,vel) to (pos,-vel) + for (unsigned int i = 0; i < n_joints; ++i) + { + // If there is a time delay in the system it is better to 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] = desired_state_.position[i]; + hold_end_state_.velocity[0] = -desired_state_.velocity[i]; + hold_end_state_.acceleration[0] = 0.0; - // Sample segment at its midpoint, that should have zero velocity - (*hold_trajectory_ptr_)[i].front().sample(end_time, hold_end_state_); + (*hold_trajectory_ptr_)[i].front().init(start_time, hold_start_state_, + end_time_2x, hold_end_state_); - // Now create segment that goes from current state to one with zero end velocity - (*hold_trajectory_ptr_)[i].front().init(start_time, hold_start_state_, - end_time, hold_end_state_); + // Sample segment at its midpoint, that should have zero velocity + (*hold_trajectory_ptr_)[i].front().sample(end_time, hold_end_state_); - // Set goal handle for the segment - (*hold_trajectory_ptr_)[i].front().setGoalHandle(gh); + // Now create segment that goes from current state to one with zero end velocity + (*hold_trajectory_ptr_)[i].front().init(start_time, hold_start_state_, + end_time, hold_end_state_); + + // Set goal handle for the segment + (*hold_trajectory_ptr_)[i].front().setGoalHandle(gh); + } } curr_trajectory_box_.set(hold_trajectory_ptr_); } diff --git a/joint_trajectory_controller/test/joint_trajectory_controller_stopramp.test b/joint_trajectory_controller/test/joint_trajectory_controller_stopramp.test new file mode 100644 index 000000000..a36beef39 --- /dev/null +++ b/joint_trajectory_controller/test/joint_trajectory_controller_stopramp.test @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/joint_trajectory_controller/test/joint_trajectory_controller_test.cpp b/joint_trajectory_controller/test/joint_trajectory_controller_test.cpp index 40fa1b076..9a360b9fd 100644 --- a/joint_trajectory_controller/test/joint_trajectory_controller_test.cpp +++ b/joint_trajectory_controller/test/joint_trajectory_controller_test.cpp @@ -29,6 +29,7 @@ #include #include +#include #include #include @@ -39,6 +40,7 @@ #include #include +#include #include #include #include @@ -59,13 +61,17 @@ class JointTrajectoryControllerTest : public ::testing::Test : nh("rrbot_controller"), short_timeout(1.0), long_timeout(10.0), - controller_state() + controller_state(), + stop_trajectory_duration(0.0) { n_joints = (2); joint_names.resize(n_joints); 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); @@ -100,6 +106,12 @@ class JointTrajectoryControllerTest : public ::testing::Test // Smoothing publisher (determines how well the robot follows a trajectory) smoothing_pub = ros::NodeHandle().advertise("smoothing", 1); + // Delay publisher (allows to simulate a delay of one cycle in the hardware interface) + delay_pub = ros::NodeHandle().advertise("delay", 1); + + // Upper bound publisher (allows to simulate a wall) + upper_bound_pub = ros::NodeHandle().advertise("upper_bound", 1); + // Trajectory publisher traj_pub = nh.advertise("command", 1); @@ -121,6 +133,8 @@ class JointTrajectoryControllerTest : public ::testing::Test const std::string action_server_name = nh.getNamespace() + "/follow_joint_trajectory"; action_client.reset(new ActionClient(action_server_name)); action_client2.reset(new ActionClient(action_server_name)); + + nh.getParam("stop_trajectory_duration", stop_trajectory_duration); } ~JointTrajectoryControllerTest() @@ -150,6 +164,8 @@ class JointTrajectoryControllerTest : public ::testing::Test ros::Duration long_timeout; ros::Publisher smoothing_pub; + ros::Publisher delay_pub; + ros::Publisher upper_bound_pub; ros::Publisher traj_pub; ros::Subscriber state_sub; ros::ServiceClient query_state_service; @@ -161,11 +177,22 @@ class JointTrajectoryControllerTest : public ::testing::Test StateConstPtr controller_state; + std::vector controller_min_actual_velocity; + std::vector controller_max_actual_velocity; + + double stop_trajectory_duration; 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); + std::transform(controller_max_actual_velocity.begin(), controller_max_actual_velocity.end(), + state->actual.velocities.begin(), controller_max_actual_velocity.begin(), + std::max); } StateConstPtr getState() @@ -189,6 +216,28 @@ class JointTrajectoryControllerTest : public ::testing::Test return init_ok; } + std::vector getMinActualVelocity() + { + boost::mutex::scoped_lock lock(mutex); + return controller_min_actual_velocity; + } + + std::vector 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::infinity(); + controller_max_actual_velocity[i] = -std::numeric_limits::infinity(); + } + } + static bool waitForState(const ActionClientPtr& action_client, const actionlib::SimpleClientGoalState& state, const ros::Duration& timeout) @@ -750,7 +799,6 @@ TEST_F(JointTrajectoryControllerTest, emptyTopicCancelsActionTraj) trajectory_msgs::JointTrajectory traj_empty; traj_pub.publish(traj_empty); ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::PREEMPTED, short_timeout)); - ros::Duration(2.0).sleep(); // Stopping takes some time // Check that we're not on the start state StateConstPtr state1 = getState(); @@ -768,6 +816,123 @@ TEST_F(JointTrajectoryControllerTest, emptyTopicCancelsActionTraj) } } +TEST_F(JointTrajectoryControllerTest, emptyTopicCancelsActionTrajWithDelay) +{ + // check stop ramp for trajectory duration > 0 + if(stop_trajectory_duration > 0) + { + 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, short_timeout)); + + // Velocity should be continuous so all axes >= 0 + std::vector 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(); + } + } + else + { + SUCCEED(); + } +} + +TEST_F(JointTrajectoryControllerTest, emptyTopicCancelsActionTrajWithDelayStopZero) +{ + // check set position = actual position for stop_duration == 0 + 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 and clip position at a wall + std_msgs::Float64 upper_bound; + { + std_msgs::Bool delay; + delay.data = true; + delay_pub.publish(delay); + upper_bound.data = traj_goal.trajectory.points.front().positions.front() / 3.; + upper_bound_pub.publish(upper_bound); + 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, short_timeout)); + + StateConstPtr state1 = getState(); + + if(stop_trajectory_duration == 0.0) + { + // Here we expect that the desired position is equal to the actual position of the robot, + // which is given through upper_bound by construction. + EXPECT_NEAR(state1->desired.positions[0], upper_bound.data, EPS); // first joint + } + else + { + // stop ramp should be calculated using the desired position + // so it is greater than the upper bound + EXPECT_GT(state1->desired.positions[0], upper_bound.data); // first joint + } + + // Restore perfect control + { + std_msgs::Bool delay; + delay.data = false; + delay_pub.publish(delay); + std_msgs::Float64 upper_bound; + upper_bound.data = std::numeric_limits::infinity(); + upper_bound_pub.publish(upper_bound); + ros::Duration(0.5).sleep(); + } +} + TEST_F(JointTrajectoryControllerTest, emptyActionCancelsTopicTraj) { ASSERT_TRUE(initState()); @@ -792,7 +957,7 @@ TEST_F(JointTrajectoryControllerTest, emptyActionCancelsTopicTraj) empty_goal.trajectory.joint_names = traj.joint_names; action_client->sendGoal(empty_goal); ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::ACTIVE, short_timeout)); - ros::Duration(2.0).sleep(); // Stopping takes some time + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::SUCCEEDED, short_timeout)); // Check that we're not on the start state StateConstPtr state1 = getState(); @@ -836,7 +1001,7 @@ TEST_F(JointTrajectoryControllerTest, emptyActionCancelsActionTraj) action_client2->sendGoal(empty_goal); ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::PREEMPTED, short_timeout)); ASSERT_TRUE(waitForState(action_client2, SimpleClientGoalState::ACTIVE, short_timeout)); - ros::Duration(2.0).sleep(); // Stopping takes some time + ASSERT_TRUE(waitForState(action_client2, SimpleClientGoalState::SUCCEEDED, short_timeout)); // Check that we're not on the start state StateConstPtr state1 = getState(); diff --git a/joint_trajectory_controller/test/rrbot.cpp b/joint_trajectory_controller/test/rrbot.cpp index f304f9a71..8b41d711e 100644 --- a/joint_trajectory_controller/test/rrbot.cpp +++ b/joint_trajectory_controller/test/rrbot.cpp @@ -30,6 +30,7 @@ // ROS #include #include +#include // ros_control #include @@ -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,14 @@ 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); + + // Upper bound subscriber: set positions greater than this value are clipped + upper_bound_sub_ = ros::NodeHandle().subscribe("upper_bound", 1, &RRbot::upper_boundCB, this); + upper_bound_.initRT(std::numeric_limits::infinity()); } ros::Time getTime() const {return ros::Time::now();} @@ -90,20 +101,41 @@ class RRbot : public hardware_interface::RobotHW void write() { const double smoothing = *(smoothing_.readFromRT()); + const bool delay = *(delay_.readFromRT()); + const double upper_bound = *(upper_bound_.readFromRT()); + for (unsigned int i = 0; i < 2; ++i) { - if(active_interface_[i] == "hardware_interface::PositionJointInterface") + // if delay is true, use position from previous cycle + if(delay != 0) { - vel_[i] = (pos_cmd_[i] - pos_[i]) / getPeriod().toSec(); + std::swap(pos_cmd_[i], pos_lastcmd_[i]); + std::swap(vel_cmd_[i], vel_lastcmd_[i]); + } + else + { + 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") { vel_[i] = (1.0 - smoothing) * vel_cmd_[i]; pos_[i] = pos_[i] + vel_[i] * getPeriod().toSec(); } + + // clip position at upper bound + if(pos_[i] > upper_bound) + { + pos_[i] = upper_bound; + vel_[i] = 0.0; + } } } @@ -146,6 +178,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 +189,15 @@ class RRbot : public hardware_interface::RobotHW realtime_tools::RealtimeBuffer smoothing_; void smoothingCB(const std_msgs::Float64& smoothing) {smoothing_.writeFromNonRT(smoothing.data);} - ros::Subscriber smoothing_sub_; + + realtime_tools::RealtimeBuffer delay_; + void delayCB(const std_msgs::Bool& delay) {delay_.writeFromNonRT(delay.data);} + ros::Subscriber delay_sub_; + + realtime_tools::RealtimeBuffer upper_bound_; + void upper_boundCB(const std_msgs::Float64& upper_bound) {upper_bound_.writeFromNonRT(upper_bound.data);} + ros::Subscriber upper_bound_sub_; }; int main(int argc, char **argv) diff --git a/joint_trajectory_controller/test/rrbot_controllers.yaml b/joint_trajectory_controller/test/rrbot_controllers.yaml index 4623945c4..c28e9bf19 100644 --- a/joint_trajectory_controller/test/rrbot_controllers.yaml +++ b/joint_trajectory_controller/test/rrbot_controllers.yaml @@ -12,3 +12,5 @@ rrbot_controller: joint2: goal: 0.01 trajectory: 0.05 + stop_trajectory_duration: 0.0 + state_publish_rate: 100 diff --git a/joint_trajectory_controller/test/rrbot_controllers_stopramp.yaml b/joint_trajectory_controller/test/rrbot_controllers_stopramp.yaml new file mode 100644 index 000000000..c2ac32611 --- /dev/null +++ b/joint_trajectory_controller/test/rrbot_controllers_stopramp.yaml @@ -0,0 +1,16 @@ +rrbot_controller: + type: "position_controllers/JointTrajectoryController" + joints: + - joint1 + - joint2 + + constraints: + goal_time: 0.5 + joint1: + goal: 0.01 + trajectory: 0.05 + joint2: + goal: 0.01 + trajectory: 0.05 + stop_trajectory_duration: 0.1 + state_publish_rate: 100