From e53b3640ec81d85ac086c320e7a3bf8848fac326 Mon Sep 17 00:00:00 2001 From: Alexander Gutenkunst Date: Mon, 11 Sep 2017 12:55:12 +0200 Subject: [PATCH 1/5] Use desired state to calculate hold trajectory --- .../joint_trajectory_controller_impl.h | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h index ec6753353..75fa64412 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h +++ b/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_, From 9bc9fe7b321c46955109fd7315c8a3da66ae8faf Mon Sep 17 00:00:00 2001 From: Joachim Schleicher Date: Fri, 13 Oct 2017 17:14:41 +0200 Subject: [PATCH 2/5] 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. --- .../test/joint_trajectory_controller_test.cpp | 86 +++++++++++++++++++ joint_trajectory_controller/test/rrbot.cpp | 32 ++++++- .../test/rrbot_controllers.yaml | 2 + 3 files changed, 116 insertions(+), 4 deletions(-) diff --git a/joint_trajectory_controller/test/joint_trajectory_controller_test.cpp b/joint_trajectory_controller/test/joint_trajectory_controller_test.cpp index f6c37100f..0abd07030 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 @@ -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("smoothing", 1); + // Delay publisher (allows to simulate a delay of one cycle in the hardware interface) + delay_pub = ros::NodeHandle().advertise("delay", 1); + // Trajectory publisher traj_pub = nh.advertise("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 controller_min_actual_velocity; + std::vector 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); + 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() @@ -177,6 +195,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) @@ -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 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()); diff --git a/joint_trajectory_controller/test/rrbot.cpp b/joint_trajectory_controller/test/rrbot.cpp index f304f9a71..29919ba8c 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,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 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_; }; 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..c2ac32611 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.1 + state_publish_rate: 100 From e0d7beeb2d8232dfa313b3a5b63d55de16a28e12 Mon Sep 17 00:00:00 2001 From: Joachim Schleicher Date: Mon, 5 Mar 2018 14:50:09 +0100 Subject: [PATCH 3/5] 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. --- joint_trajectory_controller/CMakeLists.txt | 5 ++ .../test/joint_trajectory_controller.test | 2 +- .../joint_trajectory_controller_stopramp.test | 39 ++++++++ .../test/joint_trajectory_controller_test.cpp | 90 +++++++++++++++++-- .../test/joint_trajectory_controller_vel.test | 4 +- joint_trajectory_controller/test/rrbot.cpp | 17 ++++ .../test/rrbot_controllers.yaml | 2 +- .../test/rrbot_controllers_stopramp.yaml | 16 ++++ 8 files changed, 166 insertions(+), 9 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 656c48b93..f775ad636 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/test/joint_trajectory_controller.test b/joint_trajectory_controller/test/joint_trajectory_controller.test index 5c5301e79..e797b3f21 100644 --- a/joint_trajectory_controller/test/joint_trajectory_controller.test +++ b/joint_trajectory_controller/test/joint_trajectory_controller.test @@ -35,5 +35,5 @@ + time-limit="95.0"/> 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..4975fa3fb --- /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 0abd07030..f4d8592b8 100644 --- a/joint_trajectory_controller/test/joint_trajectory_controller_test.cpp +++ b/joint_trajectory_controller/test/joint_trajectory_controller_test.cpp @@ -57,7 +57,8 @@ 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); @@ -104,6 +105,9 @@ class JointTrajectoryControllerTest : public ::testing::Test // 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); @@ -120,6 +124,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 +156,7 @@ class JointTrajectoryControllerTest : public ::testing::Test 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,6 +168,8 @@ class JointTrajectoryControllerTest : public ::testing::Test 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); @@ -773,6 +782,62 @@ 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, long_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 @@ -780,11 +845,14 @@ TEST_F(JointTrajectoryControllerTest, emptyTopicCancelsActionTrajWithDelay) action_client->sendGoal(traj_home_goal); ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::SUCCEEDED, long_timeout)); - // Make robot respond with a delay + // 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(); @@ -802,10 +870,19 @@ TEST_F(JointTrajectoryControllerTest, emptyTopicCancelsActionTrajWithDelay) action_client->sendGoal(empty_goal); ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::SUCCEEDED, long_timeout)); - std::vector minVelocity = getMinActualVelocity(); - for(size_t i = 0; i < minVelocity.size(); ++i) + 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 { - EXPECT_GE(minVelocity[i], 0.); + // 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 @@ -813,6 +890,9 @@ TEST_F(JointTrajectoryControllerTest, emptyTopicCancelsActionTrajWithDelay) 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(); } } diff --git a/joint_trajectory_controller/test/joint_trajectory_controller_vel.test b/joint_trajectory_controller/test/joint_trajectory_controller_vel.test index 474913b80..b72fa8a23 100644 --- a/joint_trajectory_controller/test/joint_trajectory_controller_vel.test +++ b/joint_trajectory_controller/test/joint_trajectory_controller_vel.test @@ -34,6 +34,6 @@ + type="joint_trajectory_controller_vel_test" + time-limit="120.0"/> diff --git a/joint_trajectory_controller/test/rrbot.cpp b/joint_trajectory_controller/test/rrbot.cpp index 29919ba8c..8b41d711e 100644 --- a/joint_trajectory_controller/test/rrbot.cpp +++ b/joint_trajectory_controller/test/rrbot.cpp @@ -87,6 +87,10 @@ class RRbot : public hardware_interface::RobotHW // 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();} @@ -98,9 +102,11 @@ class RRbot : public hardware_interface::RobotHW { 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 delay is true, use position from previous cycle if(delay != 0) { std::swap(pos_cmd_[i], pos_lastcmd_[i]); @@ -123,6 +129,13 @@ class RRbot : public hardware_interface::RobotHW 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; + } } } @@ -181,6 +194,10 @@ class RRbot : public hardware_interface::RobotHW 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 c2ac32611..c28e9bf19 100644 --- a/joint_trajectory_controller/test/rrbot_controllers.yaml +++ b/joint_trajectory_controller/test/rrbot_controllers.yaml @@ -12,5 +12,5 @@ rrbot_controller: joint2: goal: 0.01 trajectory: 0.05 - stop_trajectory_duration: 0.1 + 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 From e717d260347bdedaa46e3b3d28a4fc0e8ea12ee7 Mon Sep 17 00:00:00 2001 From: Joachim Schleicher Date: Mon, 5 Mar 2018 15:58:45 +0100 Subject: [PATCH 4/5] preserve jump, if stop_trajectory_duration==0 old behaviour for stop_trajectory_duration==0 --- .../joint_trajectory_controller_impl.h | 77 +++++++++++-------- 1 file changed, 47 insertions(+), 30 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h index 75fa64412..6e6e5c629 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h @@ -743,47 +743,64 @@ template 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 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) + { + // 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(time.toSec(), hold_start_state_, + time.toSec(), hold_start_state_); + // Set goal handle for the segment + (*hold_trajectory_ptr_)[i].front().setGoalHandle(gh); + } + } + else { - // 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; + // 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] = desired_state_.position[i]; - hold_end_state_.velocity[0] = -desired_state_.velocity[i]; - hold_end_state_.acceleration[0] = 0.0; + 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_; - (*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; - // Sample segment at its midpoint, that should have zero velocity - (*hold_trajectory_ptr_)[i].front().sample(end_time, hold_end_state_); + 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; - // 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_); + (*hold_trajectory_ptr_)[i].front().init(start_time, hold_start_state_, + end_time_2x, hold_end_state_); - // Set goal handle for the segment - (*hold_trajectory_ptr_)[i].front().setGoalHandle(gh); + // Sample segment at its midpoint, that should have zero velocity + (*hold_trajectory_ptr_)[i].front().sample(end_time, 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_); + + // Set goal handle for the segment + (*hold_trajectory_ptr_)[i].front().setGoalHandle(gh); + } } curr_trajectory_box_.set(hold_trajectory_ptr_); } From 7a47b0b02b9ada4dd255d8d5b5198112e0b89341 Mon Sep 17 00:00:00 2001 From: Joachim Schleicher Date: Tue, 6 Mar 2018 14:24:10 +0100 Subject: [PATCH 5/5] 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.h | 7 ++++--- .../joint_trajectory_controller_impl.h | 6 +++--- .../test/joint_trajectory_controller.test | 2 +- .../test/joint_trajectory_controller_stopramp.test | 2 +- .../test/joint_trajectory_controller_test.cpp | 9 ++++----- 5 files changed, 13 insertions(+), 13 deletions(-) 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 + time-limit="85.0"/> diff --git a/joint_trajectory_controller/test/joint_trajectory_controller_stopramp.test b/joint_trajectory_controller/test/joint_trajectory_controller_stopramp.test index 4975fa3fb..a36beef39 100644 --- a/joint_trajectory_controller/test/joint_trajectory_controller_stopramp.test +++ b/joint_trajectory_controller/test/joint_trajectory_controller_stopramp.test @@ -35,5 +35,5 @@ + time-limit="85.0"/> diff --git a/joint_trajectory_controller/test/joint_trajectory_controller_test.cpp b/joint_trajectory_controller/test/joint_trajectory_controller_test.cpp index f4d8592b8..f9a3b7f90 100644 --- a/joint_trajectory_controller/test/joint_trajectory_controller_test.cpp +++ b/joint_trajectory_controller/test/joint_trajectory_controller_test.cpp @@ -762,7 +762,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(); @@ -812,7 +811,7 @@ TEST_F(JointTrajectoryControllerTest, emptyTopicCancelsActionTrajWithDelay) ActionGoal empty_goal; empty_goal.trajectory.joint_names = joint_names; action_client->sendGoal(empty_goal); - ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::SUCCEEDED, long_timeout)); + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::SUCCEEDED, short_timeout)); // Velocity should be continuous so all axes >= 0 std::vector minVelocity = getMinActualVelocity(); @@ -868,7 +867,7 @@ TEST_F(JointTrajectoryControllerTest, emptyTopicCancelsActionTrajWithDelayStopZe ActionGoal empty_goal; empty_goal.trajectory.joint_names = joint_names; action_client->sendGoal(empty_goal); - ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::SUCCEEDED, long_timeout)); + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::SUCCEEDED, short_timeout)); StateConstPtr state1 = getState(); @@ -921,7 +920,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(); @@ -965,7 +964,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();