From 9a4c51d0def962b2f5362d681263fc6b25b1fa76 Mon Sep 17 00:00:00 2001 From: beatrizleon Date: Thu, 26 May 2016 07:04:54 +0100 Subject: [PATCH 1/3] jtc: Enable sending trajectories with a partial set of joints --- joint_trajectory_controller/CMakeLists.txt | 5 + .../init_joint_trajectory.h | 280 +++++--- .../joint_trajectory_controller.h | 44 +- .../joint_trajectory_controller_impl.h | 297 ++++---- .../joint_trajectory_segment.h | 89 +-- .../joint_trajectory_controller/tolerances.h | 61 ++ .../test/init_joint_trajectory_test.cpp | 198 +++--- .../joint_partial_trajectory_controller.test | 35 + ...int_partial_trajectory_controller_test.cpp | 637 ++++++++++++++++++ .../test/joint_trajectory_controller_test.cpp | 2 +- .../test/joint_trajectory_segment_test.cpp | 129 +--- .../test/rrbot_partial_controllers.yaml | 15 + 12 files changed, 1299 insertions(+), 493 deletions(-) create mode 100644 joint_trajectory_controller/test/joint_partial_trajectory_controller.test create mode 100644 joint_trajectory_controller/test/joint_partial_trajectory_controller_test.cpp create mode 100644 joint_trajectory_controller/test/rrbot_partial_controllers.yaml diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 985e04280..98862e610 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -88,6 +88,11 @@ if(CATKIN_ENABLE_TESTING) test/joint_trajectory_controller.test test/joint_trajectory_controller_test.cpp) target_link_libraries(joint_trajectory_controller_test ${catkin_LIBRARIES}) + + add_rostest_gtest(joint_partial_trajectory_controller_test + test/joint_partial_trajectory_controller.test + test/joint_partial_trajectory_controller_test.cpp) + target_link_libraries(joint_partial_trajectory_controller_test ${catkin_LIBRARIES}) endif() # Install diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/init_joint_trajectory.h b/joint_trajectory_controller/include/joint_trajectory_controller/init_joint_trajectory.h index 75cf1889f..37269efb6 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/init_joint_trajectory.h +++ b/joint_trajectory_controller/include/joint_trajectory_controller/init_joint_trajectory.h @@ -57,19 +57,19 @@ namespace joint_trajectory_controller namespace internal { /** - * \return The permutation vector between two containers. - * If \p t1 is "{A, B, C, D}" and \p t2 is "{B, D, A, C}", the associated permutation vector is - * "{2, 0, 3, 1}". + * \return The map between \p t1 indices (implicitly encoded in return vector indices) to \t2 indices. + * If \p t1 is "{C, B}" and \p t2 is "{A, B, C, D}", the associated mapping vector is + * "{2, 1}". */ template -inline std::vector permutation(const T& t1, const T& t2) +inline std::vector mapping(const T& t1, const T& t2) { typedef unsigned int SizeType; + + // t1 must be a subset of t2 + if (t1.size() > t2.size()) {return std::vector();} - // Arguments must have the same size - if (t1.size() != t2.size()) {return std::vector();} - - std::vector permutation_vector(t1.size()); // Return value + std::vector mapping_vector(t1.size()); // Return value for (typename T::const_iterator t1_it = t1.begin(); t1_it != t1.end(); ++t1_it) { typename T::const_iterator t2_it = std::find(t2.begin(), t2.end(), *t1_it); @@ -78,10 +78,10 @@ inline std::vector permutation(const T& t1, const T& t2) { const SizeType t1_dist = std::distance(t1.begin(), t1_it); const SizeType t2_dist = std::distance(t2.begin(), t2_it); - permutation_vector[t1_dist] = t2_dist; + mapping_vector[t1_dist] = t2_dist; } } - return permutation_vector; + return mapping_vector; } } // namespace @@ -95,7 +95,9 @@ struct InitJointTrajectoryOptions { typedef realtime_tools::RealtimeServerGoalHandle RealtimeGoalHandle; typedef boost::shared_ptr RealtimeGoalHandlePtr; - typedef typename Trajectory::value_type::Scalar Scalar; + typedef typename Trajectory::value_type TrajectoryPerJoint; + typedef typename TrajectoryPerJoint::value_type Segment; + typedef typename Segment::Scalar Scalar; InitJointTrajectoryOptions() : current_trajectory(0), @@ -103,7 +105,8 @@ struct InitJointTrajectoryOptions angle_wraparound(0), rt_goal_handle(), default_tolerances(0), - other_time_base(0) + other_time_base(0), + allow_partial_joints_goal(false) {} Trajectory* current_trajectory; @@ -112,6 +115,13 @@ struct InitJointTrajectoryOptions RealtimeGoalHandlePtr rt_goal_handle; SegmentTolerances* default_tolerances; ros::Time* other_time_base; + bool allow_partial_joints_goal; +}; + +template +bool isNotEmpty(typename Trajectory::value_type trajPerJoint) +{ + return !trajPerJoint.empty(); }; /** @@ -164,7 +174,6 @@ struct InitJointTrajectoryOptions * Segment(const ros::Time& traj_start_time, * const trajectory_msgs::JointTrajectoryPoint& start_point, * const trajectory_msgs::JointTrajectoryPoint& end_point, - * const std::vector& permutation, * const std::vector& position_offset) * \endcode * The following function must also be defined to properly handle continuous joints: @@ -184,8 +193,10 @@ Trajectory initJointTrajectory(const trajectory_msgs::JointTrajectory& msg const InitJointTrajectoryOptions& options = InitJointTrajectoryOptions()) { - typedef typename Trajectory::value_type Segment; + typedef typename Trajectory::value_type TrajectoryPerJoint; + typedef typename TrajectoryPerJoint::value_type Segment; typedef typename Segment::Scalar Scalar; + typedef typename TrajectoryPerJoint::const_iterator TrajIter; const unsigned int n_joints = msg.joint_names.size(); @@ -239,13 +250,35 @@ Trajectory initJointTrajectory(const trajectory_msgs::JointTrajectory& msg o_msg_start_time = msg_start_time; } - // Permutation vector mapping the expected joint order to the message joint order - // If unspecified, a trivial map (no permutation) is computed const std::vector joint_names = has_joint_names ? *(options.joint_names) : msg.joint_names; - std::vector permutation_vector = internal::permutation(joint_names, msg.joint_names); + if (has_angle_wraparound) + { + // Preconditions + const unsigned int n_angle_wraparound = options.angle_wraparound->size(); + if (n_angle_wraparound != joint_names.size()) + { + ROS_ERROR("Cannot create trajectory from message. " + "Vector specifying whether joints wrap around has an invalid size."); + return Trajectory(); + } + } + + // If partial joints goals are not allowed, goal should specify all controller joints + if (!options.allow_partial_joints_goal) + { + if (msg.joint_names.size() != joint_names.size()) + { + ROS_ERROR("Cannot create trajectory from message. It does not contain the expected joints."); + return Trajectory(); + } + } + + // Mapping vector contains the map between the message joint order and the expected joint order + // If unspecified, a trivial map is computed + std::vector mapping_vector = internal::mapping(msg.joint_names,joint_names); - if (permutation_vector.empty()) + if (mapping_vector.empty()) { ROS_ERROR("Cannot create trajectory from message. It does not contain the expected joints."); return Trajectory(); @@ -264,17 +297,17 @@ Trajectory initJointTrajectory(const trajectory_msgs::JointTrajectory& msg // This point is used later on in this function, but is computed here, in advance because if the trajectory message // contains a trajectory in the past, we can quickly return without spending additional computational resources std::vector::const_iterator - it = findPoint(msg, time); // Points to last point occurring before current time (NOTE: Using time, not o_time) - if (it == msg.points.end()) + msg_it = findPoint(msg, time); // Points to last point occurring before current time (NOTE: Using time, not o_time) + if (msg_it == msg.points.end()) { - it = msg.points.begin(); // Entire trajectory is after current time + msg_it = msg.points.begin(); // Entire trajectory is after current time } else { - ++it; // Points to first point after current time OR sequence end - if (it == msg.points.end()) + ++msg_it; // Points to first point after current time OR sequence end + if (msg_it == msg.points.end()) { - ros::Duration last_point_dur = time - (msg_start_time + (--it)->time_from_start); + ros::Duration last_point_dur = time - (msg_start_time + (--msg_it)->time_from_start); ROS_WARN_STREAM("Dropping all " << msg.points.size() << " trajectory point(s), as they occur before the current time.\n" << "Last point is " << std::fixed << std::setprecision(3) << last_point_dur.toSec() << @@ -283,8 +316,8 @@ Trajectory initJointTrajectory(const trajectory_msgs::JointTrajectory& msg } else { - ros::Duration next_point_dur = msg_start_time + it->time_from_start - time; - ROS_WARN_STREAM("Dropping first " << std::distance(msg.points.begin(), it) << + ros::Duration next_point_dur = msg_start_time + msg_it->time_from_start - time; + ROS_WARN_STREAM("Dropping first " << std::distance(msg.points.begin(), msg_it) << " trajectory point(s) out of " << msg.points.size() << ", as they occur before the current time.\n" << "First valid point will be reached in " << std::fixed << std::setprecision(3) << @@ -297,91 +330,156 @@ Trajectory initJointTrajectory(const trajectory_msgs::JointTrajectory& msg // - Useful segments of new trajectory (contained in ROS message) Trajectory result_traj; // Currently empty - // Initialize offsets due to wrapping joints to zero - std::vector position_offset(n_joints, 0.0); - - // Bridge current trajectory to new one + // Set active goal to segments after current time if (has_current_trajectory) { - const Trajectory& curr_traj = *(options.current_trajectory); - - // Get the last time and state that will be executed from the current trajectory - const typename Segment::Time last_curr_time = std::max(o_msg_start_time.toSec(), o_time.toSec()); // Important! - typename Segment::State last_curr_state; - sample(curr_traj, last_curr_time, last_curr_state); - - // Get the first time and state that will be executed from the new trajectory - const typename Segment::Time first_new_time = o_msg_start_time.toSec() + (it->time_from_start).toSec(); - typename Segment::State first_new_state(*it, permutation_vector); // Here offsets are not yet applied + result_traj = *(options.current_trajectory); - // Compute offsets due to wrapping joints - if (has_angle_wraparound) + //Iterate to all segments after current time to set the new goal handler + for (unsigned int joint_id=0; joint_id < joint_names.size();joint_id++) { - position_offset = wraparoundOffset(last_curr_state.position, - first_new_state.position, - *(options.angle_wraparound)); - if (position_offset.empty()) + const TrajectoryPerJoint& curr_joint_traj = result_traj[joint_id]; + TrajIter active_seg = findSegment(curr_joint_traj, o_time.toSec()); // Currently active segment + + while (std::distance(active_seg, curr_joint_traj.end())>=1) { - ROS_ERROR("Cannot create trajectory from message. " - "Vector specifying whether joints wrap around has an invalid size."); - return Trajectory(); + (result_traj[joint_id])[std::distance(curr_joint_traj.begin(),active_seg)].setGoalHandle(options.rt_goal_handle); + ++active_seg; } } + } + else + result_traj.resize(joint_names.size()); + + //Iterate through the joints that are in the message, in the order of the mapping vector + //for (unsigned int joint_id=0; joint_id < joint_names.size();joint_id++) + for (unsigned int msg_joint_it=0; msg_joint_it < mapping_vector.size();msg_joint_it++) + { + std::vector::const_iterator it = msg_it; + if (!isValid(*it, it->positions.size())) + throw(std::invalid_argument("Size mismatch in trajectory point position, velocity or acceleration data.")); - // Apply offset to first state that will be executed from the new trajectory - first_new_state = typename Segment::State(*it, permutation_vector, position_offset); // Now offsets are applied + TrajectoryPerJoint result_traj_per_joint; // Currently empty + unsigned int joint_id = mapping_vector[msg_joint_it]; - // Add useful segments of current trajectory to result + // Initialize offsets due to wrapping joints to zero + std::vector position_offset(1, 0.0); + + //Initialize segment tolerance per joint + SegmentTolerancesPerJoint tolerances_per_joint; + tolerances_per_joint.state_tolerance = tolerances.state_tolerance[joint_id]; + tolerances_per_joint.goal_state_tolerance = tolerances.goal_state_tolerance[joint_id]; + tolerances_per_joint.goal_time_tolerance = tolerances.goal_time_tolerance; + + // Bridge current trajectory to new one + if (has_current_trajectory) { - typedef typename Trajectory::const_iterator TrajIter; - TrajIter first = findSegment(curr_traj, o_time.toSec()); // Currently active segment - TrajIter last = findSegment(curr_traj, last_curr_time); // Segment active when new trajectory starts - if (first == curr_traj.end() || last == curr_traj.end()) + const TrajectoryPerJoint& curr_joint_traj = (*options.current_trajectory)[joint_id]; + + // Get the last time and state that will be executed from the current trajectory + const typename Segment::Time last_curr_time = std::max(o_msg_start_time.toSec(), o_time.toSec()); // Important! + typename Segment::State last_curr_state; + sample(curr_joint_traj, last_curr_time, last_curr_state); + + // Get the first time and state that will be executed from the new trajectory + trajectory_msgs::JointTrajectoryPoint point_per_joint; + if (!it->positions.empty()) {point_per_joint.positions.resize(1, it->positions[msg_joint_it]);} + if (!it->velocities.empty()) {point_per_joint.velocities.resize(1, it->velocities[msg_joint_it]);} + if (!it->accelerations.empty()) {point_per_joint.accelerations.resize(1, it->accelerations[msg_joint_it]);} + point_per_joint.time_from_start = it->time_from_start; + + const typename Segment::Time first_new_time = o_msg_start_time.toSec() + (it->time_from_start).toSec(); + typename Segment::State first_new_state(point_per_joint); // Here offsets are not yet applied + + // Compute offsets due to wrapping joints + if (has_angle_wraparound) + { + position_offset[0] = wraparoundJointOffset(last_curr_state.position[0], + first_new_state.position[0], + (*options.angle_wraparound)[joint_id]); + } + + // Apply offset to first state that will be executed from the new trajectory + first_new_state = typename Segment::State(point_per_joint, position_offset); // Now offsets are applied + + // Add useful segments of current trajectory to result { - ROS_ERROR("Unexpected error: Could not find segments in current trajectory. Please contact the package maintainer."); - return Trajectory(); + TrajIter first = findSegment(curr_joint_traj, o_time.toSec()); // Currently active segment + TrajIter last = findSegment(curr_joint_traj, last_curr_time); // Segment active when new trajectory starts + if (first == curr_joint_traj.end() || last == curr_joint_traj.end()) + { + ROS_ERROR("Unexpected error: Could not find segments in current trajectory. Please contact the package maintainer."); + return Trajectory(); + } + result_traj_per_joint.insert(result_traj_per_joint.begin(), first, ++last); // Range [first,last) will still be executed } - result_traj.insert(result_traj.begin(), first, ++last); // Range [first,last) will still be executed + + // Add segment bridging current and new trajectories to result + Segment bridge_seg(last_curr_time, last_curr_state, + first_new_time, first_new_state); + bridge_seg.setGoalHandle(options.rt_goal_handle); + if (has_rt_goal_handle) {bridge_seg.setTolerances(tolerances_per_joint);} + result_traj_per_joint.push_back(bridge_seg); } - // Add segment bridging current and new trajectories to result - Segment bridge_seg(last_curr_time, last_curr_state, - first_new_time, first_new_state); - bridge_seg.setGoalHandle(options.rt_goal_handle); - if (has_rt_goal_handle) {bridge_seg.setTolerances(tolerances);} - result_traj.push_back(bridge_seg); - } + // Constants used in log statement at the end + const unsigned int num_old_segments = result_traj_per_joint.size() -1; + const unsigned int num_new_segments = std::distance(it, msg.points.end()) -1; - // Constants used in log statement at the end - const unsigned int num_old_segments = result_traj.size() -1; - const unsigned int num_new_segments = std::distance(it, msg.points.end()) -1; + // Add useful segments of new trajectory to result + // - Construct all trajectory segments occurring after current time + // - As long as there remain two trajectory points we can construct the next trajectory segment + while (std::distance(it, msg.points.end()) >= 2) + { + std::vector::const_iterator next_it = it; ++next_it; + + trajectory_msgs::JointTrajectoryPoint it_point_per_joint, next_it_point_per_joint; + + if (!isValid(*it, it->positions.size())) + throw(std::invalid_argument("Size mismatch in trajectory point position, velocity or acceleration data.")); + if (!it->positions.empty()) {it_point_per_joint.positions.resize(1, it->positions[msg_joint_it]);} + if (!it->velocities.empty()) {it_point_per_joint.velocities.resize(1, it->velocities[msg_joint_it]);} + if (!it->accelerations.empty()) {it_point_per_joint.accelerations.resize(1, it->accelerations[msg_joint_it]);} + it_point_per_joint.time_from_start = it->time_from_start; + + if (!isValid(*next_it, next_it->positions.size())) + throw(std::invalid_argument("Size mismatch in trajectory point position, velocity or acceleration data.")); + if (!next_it->positions.empty()) {next_it_point_per_joint.positions.resize(1, next_it->positions[msg_joint_it]);} + if (!next_it->velocities.empty()) {next_it_point_per_joint.velocities.resize(1, next_it->velocities[msg_joint_it]);} + if (!next_it->accelerations.empty()) {next_it_point_per_joint.accelerations.resize(1, next_it->accelerations[msg_joint_it]);} + next_it_point_per_joint.time_from_start = next_it->time_from_start; + + Segment segment(o_msg_start_time, it_point_per_joint, next_it_point_per_joint, position_offset); + segment.setGoalHandle(options.rt_goal_handle); + if (has_rt_goal_handle) {segment.setTolerances(tolerances_per_joint);} + result_traj_per_joint.push_back(segment); + ++it; + } - // Add useful segments of new trajectory to result - // - Construct all trajectory segments occurring after current time - // - As long as there remain two trajectory points we can construct the next trajectory segment - while (std::distance(it, msg.points.end()) >= 2) - { - std::vector::const_iterator next_it = it; ++next_it; - Segment segment(o_msg_start_time, *it, *next_it, permutation_vector, position_offset); - segment.setGoalHandle(options.rt_goal_handle); - if (has_rt_goal_handle) {segment.setTolerances(tolerances);} - result_traj.push_back(segment); - ++it; + // Useful debug info + std::stringstream log_str; + log_str << "Trajectory of joint " << joint_names[joint_id] << "has " << result_traj_per_joint.size() << " segments"; + if (has_current_trajectory) + { + log_str << ":"; + log_str << "\n- " << num_old_segments << " segment(s) will still be executed from previous trajectory."; + log_str << "\n- 1 segment added for transitioning between the current trajectory and first point of the input message."; + if (num_new_segments > 0) {log_str << "\n- " << num_new_segments << " new segments (" << (num_new_segments + 1) << + " points) taken from the input trajectory.";} + } + else {log_str << ".";} + ROS_DEBUG_STREAM(log_str.str()); + + if (result_traj_per_joint.size() > 0) + result_traj[joint_id] = result_traj_per_joint; } - // Useful debug info - std::stringstream log_str; - log_str << "Trajectory has " << result_traj.size() << " segments"; - if (has_current_trajectory) + // If the trajectory for all joints is empty, empty the trajectory vector + typename Trajectory::const_iterator trajIter = std::find_if (result_traj.begin(), result_traj.end(), isNotEmpty); + if (trajIter == result_traj.end()) { - log_str << ":"; - log_str << "\n- " << num_old_segments << " segment(s) will still be executed from previous trajectory."; - log_str << "\n- 1 segment added for transitioning between the current trajectory and first point of the input message."; - if (num_new_segments > 0) {log_str << "\n- " << num_new_segments << " new segments (" << (num_new_segments + 1) << - " points) taken from the input trajectory.";} + result_traj.clear(); } - else {log_str << ".";} - ROS_DEBUG_STREAM(log_str.str()); return result_traj; } 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 4aaf29116..7b5f469d2 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 @@ -40,6 +40,7 @@ // Boost #include #include +#include // ROS #include @@ -166,8 +167,10 @@ class JointTrajectoryController : public controller_interface::Controller StatePublisherPtr; typedef JointTrajectorySegment Segment; - typedef std::vector Trajectory; + typedef std::vector TrajectoryPerJoint; + typedef std::vector Trajectory; typedef boost::shared_ptr TrajectoryPtr; + typedef boost::shared_ptr TrajectoryPerJointPtr; typedef realtime_tools::RealtimeBox TrajectoryBox; typedef typename Segment::Scalar Scalar; @@ -194,11 +197,11 @@ class JointTrajectoryController : public controller_interface::Controller time_data_; @@ -206,6 +209,8 @@ class JointTrajectoryController : public controller_interface::Controller successful_joint_traj_; + bool allow_partial_joints_goal_; // ROS API ros::NodeHandle controller_nh_; @@ -241,33 +246,6 @@ class JointTrajectoryController : public controller_interface::Controller -inline void JointTrajectoryController:: -checkPathTolerances(const typename Segment::State& state_error, - const Segment& segment) -{ - const RealtimeGoalHandlePtr rt_segment_goal = segment.getGoalHandle(); - const SegmentTolerances& tolerances = segment.getTolerances(); - if (!checkStateTolerance(state_error, tolerances.state_tolerance)) - { - rt_segment_goal->preallocated_result_->error_code = - control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED; - rt_segment_goal->setAborted(rt_segment_goal->preallocated_result_); - rt_active_goal_.reset(); - } -} - -template -inline void JointTrajectoryController:: -checkGoalTolerances(const typename Segment::State& state_error, - const Segment& segment) -{ - // Controller uptime - const ros::Time uptime = time_data_.readFromRT()->uptime; - - // Checks that we have ended inside the goal tolerances - const RealtimeGoalHandlePtr rt_segment_goal = segment.getGoalHandle(); - const SegmentTolerances& tolerances = segment.getTolerances(); - const bool inside_goal_tolerances = checkStateTolerance(state_error, tolerances.goal_state_tolerance); - - if (inside_goal_tolerances) - { - rt_segment_goal->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::SUCCESSFUL; - rt_segment_goal->setSucceeded(rt_segment_goal->preallocated_result_); - rt_active_goal_.reset(); - } - else if (uptime.toSec() < segment.endTime() + tolerances.goal_time_tolerance) - { - // Still have some time left to meet the goal state tolerances - } - else - { - if (verbose_) - { - ROS_ERROR_STREAM_NAMED(name_,"Goal tolerances failed"); - // Check the tolerances one more time to output the errors that occures - checkStateTolerance(state_error, tolerances.goal_state_tolerance, true); - } - - rt_segment_goal->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED; - rt_segment_goal->setAborted(rt_segment_goal->preallocated_result_); - rt_active_goal_.reset(); - } -} - template JointTrajectoryController:: JointTrajectoryController() @@ -277,6 +223,13 @@ bool JointTrajectoryController::init(HardwareInt } ROS_DEBUG_STREAM_NAMED(name_, "Stop trajectory has a duration of " << stop_trajectory_duration_ << "s."); + // Checking if partial trajectories are allowed + controller_nh_.param("allow_partial_joints_goal", allow_partial_joints_goal_, false); + if (allow_partial_joints_goal_) + { + ROS_DEBUG_NAMED(name_, "Goals with partial set of joints are allowed"); + } + // List of controlled joints joint_names_ = getStrings(controller_nh_, "joints"); if (joint_names_.empty()) {return false;} @@ -344,14 +297,26 @@ bool JointTrajectoryController::init(HardwareInt this); // Preeallocate resources - current_state_ = typename Segment::State(n_joints); - desired_state_ = typename Segment::State(n_joints); - state_error_ = typename Segment::State(n_joints); - hold_start_state_ = typename Segment::State(n_joints); - hold_end_state_ = typename Segment::State(n_joints); + current_state_ = typename Segment::State(n_joints); + desired_state_ = typename Segment::State(n_joints); + state_error_ = typename Segment::State(n_joints); + desired_joint_state_ = typename Segment::State(1); + state_joint_error_ = typename Segment::State(1); - Segment hold_segment(0.0, current_state_, 0.0, current_state_); - hold_trajectory_ptr_->resize(1, hold_segment); + successful_joint_traj_ = boost::dynamic_bitset<>(joints_.size()); + + // Initialize trajectory with all joints + typename Segment::State current_joint_state_ = typename Segment::State(1); + for (unsigned int i = 0; i < n_joints; ++i) + { + current_joint_state_.position[0]= current_state_.position[i]; + current_joint_state_.velocity[0]= current_state_.velocity[i]; + Segment hold_segment(0.0, current_joint_state_, 0.0, current_joint_state_); + + TrajectoryPerJoint joint_segment; + joint_segment.resize(1, hold_segment); + hold_trajectory_ptr_->push_back(joint_segment); + } { state_publisher_->lock(); @@ -393,16 +358,6 @@ update(const ros::Time& time, const ros::Duration& period) // fetch the currently followed trajectory, it has been updated by the non-rt thread with something that starts in the // next control cycle, leaving the current cycle without a valid trajectory. - // Update desired state: sample trajectory at current time - typename Trajectory::const_iterator segment_it = sample(curr_traj, time_data.uptime.toSec(), desired_state_); - if (curr_traj.end() == segment_it) - { - // Non-realtime safe, but should never happen under normal operation - ROS_ERROR_NAMED(name_, - "Unexpected error: No trajectory defined at current time. Please contact the package maintainer."); - return; - } - // Update current state and state error for (unsigned int i = 0; i < joints_.size(); ++i) { @@ -410,31 +365,95 @@ update(const ros::Time& time, const ros::Duration& period) current_state_.velocity[i] = joints_[i].getVelocity(); // There's no acceleration data available in a joint handle - state_error_.position[i] = desired_state_.position[i] - current_state_.position[i]; - state_error_.velocity[i] = desired_state_.velocity[i] - current_state_.velocity[i]; + typename TrajectoryPerJoint::const_iterator segment_it = sample(curr_traj[i], time_data.uptime.toSec(), desired_joint_state_); + if (curr_traj[i].end() == segment_it) + { + // Non-realtime safe, but should never happen under normal operation + ROS_ERROR_NAMED(name_, + "Unexpected error: No trajectory defined at current time. Please contact the package maintainer."); + return; + } + desired_state_.position[i] = desired_joint_state_.position[0]; + desired_state_.velocity[i] = desired_joint_state_.velocity[0]; + desired_state_.acceleration[i] = 0.0; + + state_joint_error_.position[0] = desired_joint_state_.position[0] - current_state_.position[i]; + state_joint_error_.velocity[0] = desired_joint_state_.velocity[0] - current_state_.velocity[i]; + state_joint_error_.acceleration[0] = 0.0; + + state_error_.position[i] = desired_joint_state_.position[0] - current_state_.position[i]; + state_error_.velocity[i] = desired_joint_state_.velocity[0] - current_state_.velocity[i]; state_error_.acceleration[i] = 0.0; - } - // Check tolerances if segment corresponds to currently active action goal - const RealtimeGoalHandlePtr rt_segment_goal = segment_it->getGoalHandle(); - if (rt_segment_goal && rt_segment_goal == rt_active_goal_) - { - // Check tolerances - if (time_data.uptime.toSec() < segment_it->endTime()) + //Check tolerances + const RealtimeGoalHandlePtr rt_segment_goal = segment_it->getGoalHandle(); + if (rt_segment_goal && rt_segment_goal == rt_active_goal_) { - // Currently executing a segment: check path tolerances - checkPathTolerances(state_error_, - *segment_it); + // Check tolerances + if (time_data.uptime.toSec() < segment_it->endTime()) + { + // Currently executing a segment: check path tolerances + const SegmentTolerancesPerJoint& joint_tolerances = segment_it->getTolerances(); + if (!checkStateTolerancePerJoint(state_joint_error_, joint_tolerances.state_tolerance)) + { + if (verbose_) + { + ROS_ERROR_STREAM_NAMED(name_,"Path tolerances failed for joint: " << joint_names_[i]); + checkStateTolerancePerJoint(state_joint_error_, joint_tolerances.state_tolerance, true); + } + rt_segment_goal->preallocated_result_->error_code = + control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED; + rt_segment_goal->setAborted(rt_segment_goal->preallocated_result_); + rt_active_goal_.reset(); + successful_joint_traj_.reset(); + } + } + else if (segment_it == --curr_traj[i].end()) + { + if (verbose_) + ROS_DEBUG_STREAM_THROTTLE_NAMED(1,name_,"Finished executing last segment, checking goal tolerances"); + + // Controller uptime + const ros::Time uptime = time_data_.readFromRT()->uptime; + + // Checks that we have ended inside the goal tolerances + const SegmentTolerancesPerJoint& tolerances = segment_it->getTolerances(); + const bool inside_goal_tolerances = checkStateTolerancePerJoint(state_joint_error_, tolerances.goal_state_tolerance); + + if (inside_goal_tolerances) + { + successful_joint_traj_[i] = 1; + } + else if (uptime.toSec() < segment_it->endTime() + tolerances.goal_time_tolerance) + { + // Still have some time left to meet the goal state tolerances + } + else + { + if (verbose_) + { + ROS_ERROR_STREAM_NAMED(name_,"Goal tolerances failed for joint: "<< joint_names_[i]); + // Check the tolerances one more time to output the errors that occurs + checkStateTolerancePerJoint(state_joint_error_, tolerances.goal_state_tolerance, true); + } + + rt_segment_goal->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED; + rt_segment_goal->setAborted(rt_segment_goal->preallocated_result_); + rt_active_goal_.reset(); + successful_joint_traj_.reset(); + } + } } - else if (segment_it == --curr_traj.end()) - { - if (verbose_) - ROS_DEBUG_STREAM_THROTTLE_NAMED(1,name_,"Finished executing last segement, checking goal tolerances"); + } - // Finished executing the LAST segment: check goal tolerances - checkGoalTolerances(state_error_, - *segment_it); - } + //If there is an active goal and all segments finished successfully then set goal as succeeded + RealtimeGoalHandlePtr current_active_goal(rt_active_goal_); + if (current_active_goal and successful_joint_traj_.count() == joints_.size()) + { + current_active_goal->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::SUCCESSFUL; + current_active_goal->setSucceeded(current_active_goal->preallocated_result_); + rt_active_goal_.reset(); + successful_joint_traj_.reset(); } // Hardware interface adapter: Generate and send commands @@ -500,12 +519,13 @@ updateTrajectoryCommand(const JointTrajectoryConstPtr& msg, RealtimeGoalHandlePt curr_trajectory_box_.get(curr_traj_ptr); Options options; - options.other_time_base = &next_update_uptime; - options.current_trajectory = curr_traj_ptr.get(); - options.joint_names = &joint_names_; - options.angle_wraparound = &angle_wraparound_; - options.rt_goal_handle = gh; - options.default_tolerances = &default_tolerances_; + options.other_time_base = &next_update_uptime; + options.current_trajectory = curr_traj_ptr.get(); + options.joint_names = &joint_names_; + options.angle_wraparound = &angle_wraparound_; + options.rt_goal_handle = gh; + options.default_tolerances = &default_tolerances_; + options.allow_partial_joints_goal = allow_partial_joints_goal_; // Update currently executing trajectory try @@ -540,7 +560,7 @@ template void JointTrajectoryController:: goalCB(GoalHandle gh) { - ROS_DEBUG_STREAM_NAMED(name_,"Recieved new action goal"); + ROS_DEBUG_STREAM_NAMED(name_,"Received new action goal"); // Precondition: Running controller if (!this->isRunning()) @@ -552,11 +572,24 @@ goalCB(GoalHandle gh) return; } - // Goal should specify all controller joints (they can be ordered differently). Reject if this is not the case - using internal::permutation; - std::vector permutation_vector = permutation(joint_names_, gh.getGoal()->trajectory.joint_names); + // If partial joints goals are not allowed, goal should specify all controller joints + if (!allow_partial_joints_goal_) + { + if (gh.getGoal()->trajectory.joint_names.size() != joint_names_.size()) + { + ROS_ERROR_NAMED(name_, "Joints on incoming goal don't match the controller joints."); + control_msgs::FollowJointTrajectoryResult result; + result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS; + gh.setRejected(result); + return; + } + } - if (permutation_vector.empty()) + // Goal should specify valid controller joints (they can be ordered differently). Reject if this is not the case + using internal::mapping; + std::vector mapping_vector = mapping(gh.getGoal()->trajectory.joint_names, joint_names_); + + if (mapping_vector.empty()) { ROS_ERROR_NAMED(name_, "Joints on incoming goal don't match the controller joints."); control_msgs::FollowJointTrajectoryResult result; @@ -639,19 +672,28 @@ queryStateService(control_msgs::QueryTrajectoryState::Request& req, curr_trajectory_box_.get(curr_traj_ptr); Trajectory& curr_traj = *curr_traj_ptr; - typename Segment::State state; - typename Trajectory::const_iterator segment_it = sample(curr_traj, sample_time.toSec(), state); - if (curr_traj.end() == segment_it) + typename Segment::State response_point = typename Segment::State(joint_names_.size()); + + for (unsigned int i = 0; i < joints_.size(); ++i) { - ROS_ERROR_STREAM_NAMED(name_, "Requested sample time preceeds trajectory start time."); - return false; - } + typename Segment::State state; + typename TrajectoryPerJoint::const_iterator segment_it = sample(curr_traj[i], sample_time.toSec(), state); + if (curr_traj[i].end() == segment_it) + { + ROS_ERROR_STREAM_NAMED(name_, "Requested sample time precedes trajectory start time."); + return false; + } + response_point.position[i] = state.position[0]; + response_point.velocity[i] = state.velocity[0]; + response_point.acceleration[i] = state.acceleration[0]; + } + // Populate response resp.name = joint_names_; - resp.position = state.position; - resp.velocity = state.velocity; - resp.acceleration = state.acceleration; + resp.position = response_point.position; + resp.velocity = response_point.velocity; + resp.acceleration = response_point.acceleration; return true; } @@ -691,7 +733,10 @@ setHoldPosition(const ros::Time& time) // - 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(1 == hold_trajectory_ptr_->size()); + 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_; @@ -701,24 +746,24 @@ setHoldPosition(const ros::Time& time) const unsigned int n_joints = joints_.size(); for (unsigned int i = 0; i < n_joints; ++i) { - hold_start_state_.position[i] = joints_[i].getPosition(); - hold_start_state_.velocity[i] = joints_[i].getVelocity(); - hold_start_state_.acceleration[i] = 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; - hold_end_state_.position[i] = joints_[i].getPosition(); - hold_end_state_.velocity[i] = -joints_[i].getVelocity(); - hold_end_state_.acceleration[i] = 0.0; - } - hold_trajectory_ptr_->front().init(start_time, hold_start_state_, - end_time_2x, hold_end_state_); + hold_end_state_.position[0] = joints_[i].getPosition(); + hold_end_state_.velocity[0] = -joints_[i].getVelocity(); + hold_end_state_.acceleration[0] = 0.0; - // Sample segment at its midpoint, that should have zero velocity - hold_trajectory_ptr_->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_->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_); + // 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_); + } curr_trajectory_box_.set(hold_trajectory_ptr_); } diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_segment.h b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_segment.h index 3c024c7cf..6915a7950 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_segment.h +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_segment.h @@ -85,31 +85,18 @@ class JointTrajectorySegment : public Segment /** * \param point Trajectory point. * - * \param permutation Permutation vector for mapping the joint order of a \p point to a desired order. - * For instance, if \p point contains data associated to joints "{B, D, A, C}", and we are interested in - * constructing a segment with joints ordered as "{A, B, C, D}", the permutation vector should - * be set to "{2, 0, 3, 1}". - * If unspecified (empty), the joint order of \p point is preserved; if specified, its size must coincide with that - * of \p point. - * This vector can be computed using the \ref trajectory_interface::internal::permutation() - * "permutation" utility function. - * - * \param position_offset Position offset to applpy to the data in \p point. This parameter is useful for handling + * \param position_offset Position offset to apply to the data in \p point. This parameter is useful for handling * joints that wrap around (ie. continuous), to compensate for multi-turn offsets. * If unspecified (empty), zero offsets are applied; if specified, its size must coincide with that of \p point. * - * \note The offsets in \p position_offsets correspond to joints not ordered according to \p point, but to joints - * in the expected order, that is \p point with \p permutation applied to it. */ State(const trajectory_msgs::JointTrajectoryPoint& point, - const std::vector& permutation = std::vector(), const std::vector& position_offset = std::vector()) { - init(point, permutation, position_offset); + init(point, position_offset); } void init(const trajectory_msgs::JointTrajectoryPoint& point, - const std::vector& permutation = std::vector(), const std::vector& position_offset = std::vector()) { using std::invalid_argument; @@ -121,17 +108,6 @@ class JointTrajectorySegment : public Segment { throw(invalid_argument("Size mismatch in trajectory point position, velocity or acceleration data.")); } - if (!permutation.empty() && joint_dim != permutation.size()) - { - throw(invalid_argument("Size mismatch between trajectory point and permutation vector.")); - } - for (unsigned int i = 0; i < permutation.size(); ++i) - { - if (permutation[i] >= joint_dim) - { - throw(invalid_argument("Permutation vector contains out-of-range indices.")); - } - } if (!position_offset.empty() && joint_dim != position_offset.size()) { throw(invalid_argument("Size mismatch between trajectory point " @@ -145,15 +121,12 @@ class JointTrajectorySegment : public Segment for (unsigned int i = 0; i < joint_dim; ++i) { - // Apply permutation only if it was specified, otherwise preserve original message order - const unsigned int id = permutation.empty() ? i : permutation[i]; - // Apply position offset only if it was specified const Scalar offset = position_offset.empty() ? 0.0 : position_offset[i]; - if (!point.positions.empty()) {this->position[i] = point.positions[id] + offset;} - if (!point.velocities.empty()) {this->velocity[i] = point.velocities[id];} - if (!point.accelerations.empty()) {this->acceleration[i] = point.accelerations[id];} + if (!point.positions.empty()) {this->position[i] = point.positions[i] + offset;} + if (!point.velocities.empty()) {this->velocity[i] = point.velocities[i];} + if (!point.accelerations.empty()) {this->acceleration[i] = point.accelerations[i];} } } }; @@ -171,7 +144,7 @@ class JointTrajectorySegment : public Segment const Time& end_time, const State& end_state) : rt_goal_handle_(), - tolerances_(start_state.position.size()) + tolerances_() { Segment::init(start_time, start_state, end_time, end_state); } @@ -183,7 +156,6 @@ class JointTrajectorySegment : public Segment * segment start time. * \param start_point Start state in ROS message format. * \param end_point End state in ROS message format. - * \param permutation See \ref JointTrajectorySegment::State. * \param position_offset See \ref JointTrajectorySegment::State. * * \throw std::invalid_argument If input parameters are inconsistent and a valid segment can't be constructed. @@ -191,10 +163,9 @@ class JointTrajectorySegment : public Segment JointTrajectorySegment(const ros::Time& traj_start_time, const trajectory_msgs::JointTrajectoryPoint& start_point, const trajectory_msgs::JointTrajectoryPoint& end_point, - const std::vector& permutation = std::vector(), const std::vector& position_offset = std::vector()) : rt_goal_handle_(), - tolerances_(start_point.positions.size()) + tolerances_() { if (start_point.positions.size() != end_point.positions.size()) { @@ -207,8 +178,8 @@ class JointTrajectorySegment : public Segment try { - const State start_state(start_point, permutation, position_offset); - const State end_state(end_point, permutation, position_offset); + const State start_state(start_point, position_offset); + const State end_state(end_point, position_offset); this->init(start_time, start_state, end_time, end_state); @@ -227,51 +198,45 @@ class JointTrajectorySegment : public Segment void setGoalHandle(RealtimeGoalHandlePtr rt_goal_handle) {rt_goal_handle_ = rt_goal_handle;} /** \return Tolerances this segment is associated to. */ - const SegmentTolerances& getTolerances() const {return tolerances_;} + const SegmentTolerancesPerJoint& getTolerances() const {return tolerances_;} /** \brief Set the tolerances this segment is associated to. */ - void setTolerances(const SegmentTolerances& tolerances) {tolerances_ = tolerances;} + void setTolerances(const SegmentTolerancesPerJoint& tolerances) {tolerances_ = tolerances;} private: RealtimeGoalHandlePtr rt_goal_handle_; - SegmentTolerances tolerances_; + SegmentTolerancesPerJoint tolerances_; }; /** * \param prev_position Previous position from which to compute the wraparound offset. * \param next_position Next position from which to compute the wraparound offset. - * \param angle_wraparound Vector of booleans where true values correspond to joints that wrap around - * (ie. are continuous). Offsets will be computed only for these joints, otherwise they are set to zero. - * \return Wraparound offsets that should be applied to \p next_position such that no multi-turns are performed when + * \param angle_wraparound Boolean where true value corresponds to a joint that wrap around + * (ie. is continuous). Offset will be computed only for this joint, otherwise it is set to zero. + * \return Wraparound offset that should be applied to \p next_position such that no multi-turns are performed when * transitioning from \p prev_position. * \tparam Scalar Scalar type. */ template -std::vector wraparoundOffset(const std::vector& prev_position, - const std::vector& next_position, - const std::vector& angle_wraparound) +Scalar wraparoundJointOffset(const Scalar& prev_position, + const Scalar& next_position, + const bool& angle_wraparound) { - // Preconditions - const unsigned int n_joints = angle_wraparound.size(); - if (n_joints != prev_position.size() || n_joints != next_position.size()) {return std::vector();} - // Return value - std::vector pos_offset(n_joints, 0.0); + Scalar pos_offset = 0.0; - for (unsigned int i = 0; i < angle_wraparound.size(); ++i) + if (angle_wraparound) { - if (angle_wraparound[i]) - { - Scalar dist = angles::shortest_angular_distance(prev_position[i], next_position[i]); + Scalar dist = angles::shortest_angular_distance(prev_position, next_position); - // Deal with singularity at M_PI shortest distance - if (std::abs(dist) - M_PI < 1e-9) - { - dist = next_position[i] > prev_position[i] ? std::abs(dist) : -std::abs(dist); - } - pos_offset[i] = (prev_position[i] + dist) - next_position[i]; + // Deal with singularity at M_PI shortest distance + if (std::abs(dist) - M_PI < 1e-9) + { + dist = next_position > prev_position ? std::abs(dist) : -std::abs(dist); } + pos_offset = (prev_position + dist) - next_position; } + return pos_offset; } diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.h b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.h index efc06580a..02e03e0f4 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.h +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.h @@ -88,6 +88,28 @@ struct SegmentTolerances Scalar goal_time_tolerance; }; +/** + * \brief Trajectory segment tolerances per Joint + */ +template +struct SegmentTolerancesPerJoint +{ + SegmentTolerancesPerJoint() + : state_tolerance(static_cast(0.0)), + goal_state_tolerance(static_cast(0.0)), + goal_time_tolerance(static_cast(0.0)) + {} + + /** State tolerances that appply during segment execution. */ + StateTolerances state_tolerance; + + /** State tolerances that apply for the goal state only.*/ + StateTolerances goal_state_tolerance; + + /** Extra time after the segment end time allowed to reach the goal state tolerances. */ + Scalar goal_time_tolerance; +}; + /** * \param state_error State error to check. * \param state_tolerance State tolerances to check \p state_error against. @@ -136,6 +158,45 @@ inline bool checkStateTolerance(const State& return true; } +/** + * \param state_error State error to check. + * \param state_tolerance State tolerances to check \p state_error against. + * \param show_errors If the joint that violate its tolerance should be output to console. NOT REALTIME if true + * \return True if \p state_error fulfills \p state_tolerance. + */ +template +inline bool checkStateTolerancePerJoint(const State& state_error, + const StateTolerances& state_tolerance, + bool show_errors = false) +{ + + using std::abs; + + const bool is_valid = !(state_tolerance.position > 0.0 && abs(state_error.position[0]) > state_tolerance.position) && + !(state_tolerance.velocity > 0.0 && abs(state_error.velocity[0]) > state_tolerance.velocity) && + !(state_tolerance.acceleration > 0.0 && abs(state_error.acceleration[0]) > state_tolerance.acceleration); + + if (!is_valid) + { + if( show_errors ) + { + ROS_ERROR_STREAM_NAMED("tolerances","Path state tolerances failed:"); + + if (state_tolerance.position > 0.0 && abs(state_error.position[0]) > state_tolerance.position) + ROS_ERROR_STREAM_NAMED("tolerances","Position Error: " << state_error.position[0] << + " Position Tolerance: " << state_tolerance.position); + if (state_tolerance.velocity > 0.0 && abs(state_error.velocity[0]) > state_tolerance.velocity) + ROS_ERROR_STREAM_NAMED("tolerances","Velocity Error: " << state_error.velocity[0] << + " Velocity Tolerance: " << state_tolerance.velocity); + if (state_tolerance.acceleration > 0.0 && abs(state_error.acceleration[0]) > state_tolerance.acceleration) + ROS_ERROR_STREAM_NAMED("tolerances","Acceleration Error: " << state_error.acceleration[0] << + " Acceleration Tolerance: " << state_tolerance.acceleration); + } + return false; + } + return true; +} + /** * \brief Update data in \p tols from data in \p msg_tol. * diff --git a/joint_trajectory_controller/test/init_joint_trajectory_test.cpp b/joint_trajectory_controller/test/init_joint_trajectory_test.cpp index b26985dcf..c72035c1d 100644 --- a/joint_trajectory_controller/test/init_joint_trajectory_test.cpp +++ b/joint_trajectory_controller/test/init_joint_trajectory_test.cpp @@ -45,61 +45,73 @@ using std::string; const double EPS = 1e-9; typedef JointTrajectorySegment > Segment; -typedef vector Trajectory; +typedef vector TrajectoryPerJoint; +typedef vector Trajectory; -TEST(PermutationTest, Permutation) +TEST(MappingTest, Mapping) { vector t1(4); - t1[0] = "A"; - t1[1] = "B"; - t1[2] = "C"; - t1[3] = "D"; + t1[0] = "B"; + t1[1] = "D"; + t1[2] = "A"; + t1[3] = "C"; vector t2(4); - t2[0] = "B"; - t2[1] = "D"; - t2[2] = "A"; - t2[3] = "C"; + t2[0] = "A"; + t2[1] = "B"; + t2[2] = "C"; + t2[3] = "D"; - typedef vector PermutationType; - PermutationType ground_truth(4); + typedef vector MappingType; + MappingType ground_truth(4); ground_truth[0] = 2; ground_truth[1] = 0; ground_truth[2] = 3; ground_truth[3] = 1; - // Mismatching sizes: Return empty permutation vector + // Mismatching sizes (t2 smaller than t1): Return empty mapping vector { vector t2_bad(1,"A"); - PermutationType perm = internal::permutation(t1, t2_bad); - EXPECT_TRUE(perm.empty()); + MappingType mapping = internal::mapping(t1, t2_bad); + EXPECT_TRUE(mapping.empty()); } - // Mismatching contents: Return empty permutation vector + // Mismatching contents: Return empty mapping vector { vector t2_bad(3,"A"); - PermutationType perm = internal::permutation(t1, t2_bad); - EXPECT_TRUE(perm.empty()); + MappingType mapping = internal::mapping(t1, t2_bad); + EXPECT_TRUE(mapping.empty()); } // Valid parameters { - PermutationType perm = internal::permutation(t1, t2); - EXPECT_EQ(ground_truth.size(), perm.size()); - EXPECT_EQ(ground_truth[0], perm[0]); - EXPECT_EQ(ground_truth[1], perm[1]); - EXPECT_EQ(ground_truth[2], perm[2]); - EXPECT_EQ(ground_truth[3], perm[3]); + MappingType mapping = internal::mapping(t1, t2); + EXPECT_EQ(ground_truth.size(), mapping.size()); + EXPECT_EQ(ground_truth[3], mapping[0]); + EXPECT_EQ(ground_truth[2], mapping[1]); + EXPECT_EQ(ground_truth[1], mapping[2]); + EXPECT_EQ(ground_truth[0], mapping[3]); } - // Valid parameters, inverse parameter order yields inverse permutation vector + // Valid parameters, inverse parameter order yields inverse mapping vector { - PermutationType perm = internal::permutation(t2, t1); - EXPECT_EQ(ground_truth.size(), perm.size()); - EXPECT_EQ(ground_truth[3], perm[0]); - EXPECT_EQ(ground_truth[2], perm[1]); - EXPECT_EQ(ground_truth[1], perm[2]); - EXPECT_EQ(ground_truth[0], perm[3]); + MappingType mapping = internal::mapping(t2, t1); + EXPECT_EQ(ground_truth.size(), mapping.size()); + EXPECT_EQ(ground_truth[0], mapping[0]); + EXPECT_EQ(ground_truth[1], mapping[1]); + EXPECT_EQ(ground_truth[2], mapping[2]); + EXPECT_EQ(ground_truth[3], mapping[3]); + } + + // Valid parameters, partial trajectory + { + vector t1_partial(2); + t1_partial[0] = "B"; + t1_partial[1] = "D"; + MappingType mapping = internal::mapping(t1_partial, t2); + EXPECT_EQ(t1_partial.size(), mapping.size()); + EXPECT_EQ(1, mapping[0]); + EXPECT_EQ(3, mapping[1]); } } @@ -140,11 +152,15 @@ class InitTrajectoryTest : public ::testing::Test state[1].velocity.push_back(1.0); state[1].acceleration.push_back(1.0); - curr_traj.push_back(Segment(0.0, state[0], + TrajectoryPerJoint joint_segment; + + joint_segment.push_back(Segment(0.0, state[0], 1.0, state[1])); - curr_traj.push_back(Segment(3.0, state[1], + joint_segment.push_back(Segment(3.0, state[1], 10.0, state[0])); // This is just junk that should be discarded in the tests + + curr_traj.push_back(joint_segment); } protected: @@ -182,14 +198,14 @@ TEST_F(InitTrajectoryTest, InitLogic) { const ros::Time time = msg_start_time; Trajectory trajectory = initJointTrajectory(trajectory_msg, time); - EXPECT_EQ(points.size() - 1, trajectory.size()); + EXPECT_EQ(points.size() - 1, trajectory[0].size()); } // First point: Return partial trajectory (last segment) { const ros::Time time = msg_start_time + msg_points.begin()->time_from_start; Trajectory trajectory = initJointTrajectory(trajectory_msg, time); - EXPECT_EQ(points.size() - 2, trajectory.size()); + EXPECT_EQ(points.size() - 2, trajectory[0].size()); } // Between the first and second points: Return partial trajectory (last segment) @@ -197,7 +213,7 @@ TEST_F(InitTrajectoryTest, InitLogic) const ros::Time time = msg_start_time + ros::Duration((msg_points.begin()->time_from_start + (++msg_points.begin())->time_from_start).toSec() / 2.0); Trajectory trajectory = initJointTrajectory(trajectory_msg, time); - EXPECT_EQ(msg_points.size() - 2, trajectory.size()); + EXPECT_EQ(msg_points.size() - 2, trajectory[0].size()); } // Second point: Return empty trajectory @@ -249,23 +265,23 @@ TEST_F(InitTrajectoryTest, InitLogicCombine) // Before first point, starting the current trajectory: Return 4 segments: Last of current + bridge + full message { - const ros::Time time(curr_traj[0].startTime()); + const ros::Time time(curr_traj[0][0].startTime()); Trajectory trajectory = initJointTrajectory(trajectory_msg, time, options); - EXPECT_EQ(points.size() + 1, trajectory.size()); + EXPECT_EQ(points.size() + 1, trajectory[0].size()); } // Before first point: Return 4 segments: Last of current + bridge + full message { const ros::Time time = msg_start_time; Trajectory trajectory = initJointTrajectory(trajectory_msg, time, options); - EXPECT_EQ(points.size() + 1, trajectory.size()); + EXPECT_EQ(points.size() + 1, trajectory[0].size()); } // First point: Return partial trajectory, 3 segments: Last of current + bridge + last of message { const ros::Time time = msg_start_time + msg_points.begin()->time_from_start; Trajectory trajectory = initJointTrajectory(trajectory_msg, time, options); - EXPECT_EQ(points.size(), trajectory.size()); + EXPECT_EQ(points.size(), trajectory[0].size()); } // Between the first and second points: Same as before @@ -273,14 +289,14 @@ TEST_F(InitTrajectoryTest, InitLogicCombine) const ros::Time time = msg_start_time + ros::Duration((msg_points.begin()->time_from_start + (++msg_points.begin())->time_from_start).toSec() / 2.0); Trajectory trajectory = initJointTrajectory(trajectory_msg, time, options); - EXPECT_EQ(msg_points.size(), trajectory.size()); + EXPECT_EQ(msg_points.size(), trajectory[0].size()); } // Second point: Return partial trajectory, 2 segments: Last of current + bridge (only one point of message made it) { const ros::Time time = msg_start_time + (++msg_points.begin())->time_from_start; Trajectory trajectory = initJointTrajectory(trajectory_msg, time, options); - EXPECT_EQ(msg_points.size() - 1, trajectory.size()); + EXPECT_EQ(msg_points.size() - 1, trajectory[0].size()); } // Between the second and third points: Same as before @@ -288,7 +304,7 @@ TEST_F(InitTrajectoryTest, InitLogicCombine) const ros::Time time = msg_start_time + ros::Duration(((++msg_points.begin())->time_from_start + (--msg_points.end())->time_from_start).toSec() / 2.0); Trajectory trajectory = initJointTrajectory(trajectory_msg, time, options); - EXPECT_EQ(msg_points.size() - 1, trajectory.size()); + EXPECT_EQ(msg_points.size() - 1, trajectory[0].size()); } // Last point: Return empty trajectory @@ -314,12 +330,12 @@ TEST_F(InitTrajectoryTest, InitValues) // Input time is before first point: Return full trajectory (2 segments) const ros::Time time = msg_start_time; Trajectory trajectory = initJointTrajectory(trajectory_msg, time); - ASSERT_EQ(points.size() - 1, trajectory.size()); + ASSERT_EQ(points.size() - 1, trajectory[0].size()); // Check all segment start/end times and states - for (unsigned int i = 0; i < trajectory.size(); ++i) + for (unsigned int i = 0; i < trajectory[0].size(); ++i) { - const Segment& segment = trajectory[i]; + const Segment& segment = trajectory[0][i]; const JointTrajectoryPoint& p_start = msg_points[i]; const JointTrajectoryPoint& p_end = msg_points[i + 1]; @@ -354,17 +370,17 @@ TEST_F(InitTrajectoryTest, InitValues) TEST_F(InitTrajectoryTest, InitValuesCombine) { // Before first point: Return 4 segments: Last of current + bridge + full message - const ros::Time time(curr_traj[0].startTime()); + const ros::Time time(curr_traj[0][0].startTime()); InitJointTrajectoryOptions options; options.current_trajectory = &curr_traj; Trajectory trajectory = initJointTrajectory(trajectory_msg, time, options); - ASSERT_EQ(points.size() + 1, trajectory.size()); + ASSERT_EQ(points.size() + 1, trajectory[0].size()); // Check current trajectory segment start/end times and states (only one) { - const Segment& ref_segment = curr_traj[0]; - const Segment& segment = trajectory[0]; + const Segment& ref_segment = curr_traj[0][0]; + const Segment& segment = trajectory[0][0]; // Check start/end times { @@ -395,8 +411,8 @@ TEST_F(InitTrajectoryTest, InitValuesCombine) // Check bridge trajectory segment start/end times and states (only one) { - const Segment& ref_segment = curr_traj[0]; - const Segment& segment = trajectory[1]; + const Segment& ref_segment = curr_traj[0][0]; + const Segment& segment = trajectory[0][1]; const vector& msg_points = trajectory_msg.points; @@ -433,12 +449,12 @@ TEST_F(InitTrajectoryTest, InitValuesCombine) } // Check all segment start/end times and states (2 segments) - for (unsigned int traj_it = 2, msg_it = 0; traj_it < trajectory.size(); ++traj_it, ++msg_it) + for (unsigned int traj_it = 2, msg_it = 0; traj_it < trajectory[0].size(); ++traj_it, ++msg_it) { const ros::Time msg_start_time = trajectory_msg.header.stamp; const vector& msg_points = trajectory_msg.points; - const Segment& segment = trajectory[traj_it]; + const Segment& segment = trajectory[0][traj_it]; const JointTrajectoryPoint& p_start = msg_points[msg_it]; const JointTrajectoryPoint& p_end = msg_points[msg_it + 1]; @@ -470,7 +486,7 @@ TEST_F(InitTrajectoryTest, InitValuesCombine) } } -TEST_F(InitTrajectoryTest, JointPermutation) +TEST_F(InitTrajectoryTest, JointMapping) { // Add an extra joint to the trajectory message created in the fixture trajectory_msg.points[0].positions.push_back(-2.0); @@ -492,11 +508,13 @@ TEST_F(InitTrajectoryTest, JointPermutation) Trajectory trajectory = initJointTrajectory(trajectory_msg, trajectory_msg.header.stamp); // Check position values only - const Segment& segment = trajectory[0]; + const Segment& segment_foo_joint = trajectory[0][0]; + const Segment& segment_bar_joint = trajectory[1][0]; typename Segment::State state; - segment.sample(segment.startTime(), state); + segment_foo_joint.sample(segment_foo_joint.startTime(), state); EXPECT_EQ(trajectory_msg.points[0].positions[0], state.position[0]); - EXPECT_EQ(trajectory_msg.points[0].positions[1], state.position[1]); + segment_bar_joint.sample(segment_bar_joint.startTime(), state); + EXPECT_EQ(trajectory_msg.points[0].positions[1], state.position[0]); } // Reference joint names vector that reverses joint order @@ -510,13 +528,17 @@ TEST_F(InitTrajectoryTest, JointPermutation) Trajectory trajectory = initJointTrajectory(trajectory_msg, trajectory_msg.header.stamp, options); // Check position values only - const Segment& segment = trajectory[0]; - typename Segment::State state; - segment.sample(segment.startTime(), state); - EXPECT_NE(trajectory_msg.points[0].positions[0], state.position[0]); - EXPECT_NE(trajectory_msg.points[0].positions[1], state.position[1]); - EXPECT_EQ(trajectory_msg.points[0].positions[0], state.position[1]); - EXPECT_EQ(trajectory_msg.points[0].positions[1], state.position[0]); + const Segment& segment_bar_joint = trajectory[0][0]; + const Segment& segment_foo_joint = trajectory[1][0]; + + typename Segment::State state_foo_joint; + typename Segment::State state_bar_joint; + segment_foo_joint.sample(segment_foo_joint.startTime(), state_foo_joint); + segment_bar_joint.sample(segment_bar_joint.startTime(), state_bar_joint); + EXPECT_NE(trajectory_msg.points[0].positions[0], state_bar_joint.position[0]); + EXPECT_NE(trajectory_msg.points[0].positions[1], state_foo_joint.position[0]); + EXPECT_EQ(trajectory_msg.points[0].positions[0], state_foo_joint.position[0]); + EXPECT_EQ(trajectory_msg.points[0].positions[1], state_bar_joint.position[0]); } // Reference joint names size mismatch @@ -553,19 +575,19 @@ TEST_F(InitTrajectoryTest, WrappingSpec) } // Before first point: Return 4 segments: Last of current + bridge + full message - const ros::Time time(curr_traj[0].startTime()); + const ros::Time time(curr_traj[0][0].startTime()); std::vector angle_wraparound(1, true); InitJointTrajectoryOptions options; options.current_trajectory = &curr_traj; options.angle_wraparound = &angle_wraparound; Trajectory trajectory = initJointTrajectory(trajectory_msg, time, options); - ASSERT_EQ(points.size() + 1, trajectory.size()); + ASSERT_EQ(points.size() + 1, trajectory[0].size()); // Check current trajectory segment start/end times and states (only one) { - const Segment& ref_segment = curr_traj[0]; - const Segment& segment = trajectory[0]; + const Segment& ref_segment = curr_traj[0][0]; + const Segment& segment = trajectory[0][0]; // Check start/end times { @@ -596,8 +618,8 @@ TEST_F(InitTrajectoryTest, WrappingSpec) // Check bridge trajectory segment start/end times and states (only one) { - const Segment& ref_segment = curr_traj[0]; - const Segment& segment = trajectory[1]; + const Segment& ref_segment = curr_traj[0][0]; + const Segment& segment = trajectory[0][1]; const vector& msg_points = trajectory_msg.points; @@ -634,12 +656,12 @@ TEST_F(InitTrajectoryTest, WrappingSpec) } // Check all segment start/end times and states (2 segments) - for (unsigned int traj_it = 2, msg_it = 0; traj_it < trajectory.size(); ++traj_it, ++msg_it) + for (unsigned int traj_it = 2, msg_it = 0; traj_it < trajectory[0].size(); ++traj_it, ++msg_it) { const ros::Time msg_start_time = trajectory_msg.header.stamp; const vector& msg_points = trajectory_msg.points; - const Segment& segment = trajectory[traj_it]; + const Segment& segment = trajectory[0][traj_it]; const JointTrajectoryPoint& p_start = msg_points[msg_it]; const JointTrajectoryPoint& p_end = msg_points[msg_it + 1]; @@ -696,18 +718,18 @@ TEST_F(InitTrajectoryTest, IgnoreWrappingSpec) } // Before first point: Return 2 segments: Full message - const ros::Time time(curr_traj[0].startTime()); + const ros::Time time(curr_traj[0][0].startTime()); std::vector angle_wraparound(1, true); InitJointTrajectoryOptions options; options.angle_wraparound = &angle_wraparound; Trajectory trajectory = initJointTrajectory(trajectory_msg, time, options); - ASSERT_EQ(points.size() - 1, trajectory.size()); + ASSERT_EQ(points.size() - 1, trajectory[0].size()); // Check all segment start/end times and states (2 segments) - for (unsigned int i = 0; i < trajectory.size(); ++i) + for (unsigned int i = 0; i < trajectory[0].size(); ++i) { - const Segment& segment = trajectory[i]; + const Segment& segment = trajectory[0][i]; const JointTrajectoryPoint& p_start = msg_points[i]; const JointTrajectoryPoint& p_end = msg_points[i + 1]; @@ -751,21 +773,21 @@ TEST_F(InitTrajectoryTest, GoalHandleTest) RealtimeGoalHandlePtr rt_goal(new RealtimeGoalHandle(gh)); // Before first point: Return 4 segments: Last of current + bridge + full message - const ros::Time time(curr_traj[0].startTime()); + const ros::Time time(curr_traj[0][0].startTime()); InitJointTrajectoryOptions options; options.current_trajectory = &curr_traj; options.rt_goal_handle = rt_goal; Trajectory trajectory = initJointTrajectory(trajectory_msg, time, options); - ASSERT_EQ(points.size() + 1, trajectory.size()); + ASSERT_EQ(points.size() + 1, trajectory[0].size()); // Segment of current trajectory preserves existing null goal handle - ASSERT_FALSE(trajectory[0].getGoalHandle()); + ASSERT_FALSE(trajectory[0][0].getGoalHandle()); // All further segments should be associated to the new goal handle - for (unsigned int i = 1; i < trajectory.size(); ++i) // NOTE: Start index != 0 + for (unsigned int i = 1; i < trajectory[0].size(); ++i) // NOTE: Start index != 0 { - ASSERT_EQ(rt_goal, trajectory[i].getGoalHandle()); + ASSERT_EQ(rt_goal, trajectory[0][i].getGoalHandle()); } } @@ -783,12 +805,12 @@ TEST_F(InitTrajectoryTest, OtherTimeBase) // Before first point: Return 4 segments: Last of current + bridge + full message const ros::Time time = msg_start_time; Trajectory trajectory = initJointTrajectory(trajectory_msg, time, options); - ASSERT_EQ(points.size() + 1, trajectory.size()); + ASSERT_EQ(points.size() + 1, trajectory[0].size()); // Check current trajectory segment start/end times (only one). Time offset does NOT apply { - const Segment& ref_segment = curr_traj[0]; - const Segment& segment = trajectory[0]; + const Segment& ref_segment = curr_traj[0][0]; + const Segment& segment = trajectory[0][0]; // Check start/end times { @@ -799,7 +821,7 @@ TEST_F(InitTrajectoryTest, OtherTimeBase) // Check bridge trajectory segment start/end times and states (only one). Time offset applies to end state only { - const Segment& segment = trajectory[1]; + const Segment& segment = trajectory[0][1]; const vector& msg_points = trajectory_msg.points; @@ -817,12 +839,12 @@ TEST_F(InitTrajectoryTest, OtherTimeBase) } // Check all segment start/end times and states (2 segments). Time offset does apply - for (unsigned int traj_it = 2, msg_it = 0; traj_it < trajectory.size(); ++traj_it, ++msg_it) + for (unsigned int traj_it = 2, msg_it = 0; traj_it < trajectory[0].size(); ++traj_it, ++msg_it) { const ros::Time msg_start_time = trajectory_msg.header.stamp; const vector& msg_points = trajectory_msg.points; - const Segment& segment = trajectory[traj_it]; + const Segment& segment = trajectory[0][traj_it]; const JointTrajectoryPoint& p_start = msg_points[msg_it]; const JointTrajectoryPoint& p_end = msg_points[msg_it + 1]; diff --git a/joint_trajectory_controller/test/joint_partial_trajectory_controller.test b/joint_trajectory_controller/test/joint_partial_trajectory_controller.test new file mode 100644 index 000000000..7962df69c --- /dev/null +++ b/joint_trajectory_controller/test/joint_partial_trajectory_controller.test @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/joint_trajectory_controller/test/joint_partial_trajectory_controller_test.cpp b/joint_trajectory_controller/test/joint_partial_trajectory_controller_test.cpp new file mode 100644 index 000000000..270cfb48a --- /dev/null +++ b/joint_trajectory_controller/test/joint_partial_trajectory_controller_test.cpp @@ -0,0 +1,637 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2013, PAL Robotics S.L. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of PAL Robotics S.L. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +/// \author Adolfo Rodriguez Tsouroukdissian + +#include +#include + +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include + +// Floating-point value comparison threshold +const double EPS = 0.01; + +using actionlib::SimpleClientGoalState; + +class JointPartialTrajectoryControllerTest : public ::testing::Test +{ +public: + JointPartialTrajectoryControllerTest() + : nh("rrbot_controller"), + short_timeout(1.0), + long_timeout(10.0), + controller_state() + { + n_joints = (2); + n_partial_joints = (2); + joint_names.resize(n_joints); + joint_names[0] = "joint1"; + joint_names[1] = "joint2"; + + trajectory_msgs::JointTrajectoryPoint point; + point.positions.resize(n_joints, 0.0); + point.velocities.resize(n_joints, 0.0); + point.accelerations.resize(n_joints, 0.0); + + // Go home trajectory + traj_home.joint_names = joint_names; + traj_home.points.resize(1, point); + traj_home.points[0].time_from_start = ros::Duration(1.0); + + // Three-point trajectory + points.resize(3, point); + points[0].positions[0] = M_PI / 4.0; + points[0].positions[1] = 0.0; + points[0].time_from_start = ros::Duration(1.0); + + points[1].positions[0] = 0.0; + points[1].positions[1] = -M_PI / 4.0; + points[1].time_from_start = ros::Duration(2.0); + + points[2].positions[0] = -M_PI / 4.0; + points[2].positions[1] = M_PI / 4.0; + points[2].time_from_start = ros::Duration(4.0); + + traj.joint_names = joint_names; + traj.points = points; + + // Partial trajectory + traj_partial.joint_names.resize(1, "joint2"); + points.resize(3, point); + points[0].positions[0] = M_PI / 4.0; + points[0].time_from_start = ros::Duration(1.0); + + points[1].positions[0] = M_PI / 2.0; + points[1].time_from_start = ros::Duration(2.0); + + points[2].positions[0] = -M_PI / 2.0; + points[2].time_from_start = ros::Duration(4.0); + traj_partial.points = points; + + // Action goals + traj_home_goal.trajectory = traj_home; + traj_goal.trajectory = traj; + traj_partial_goal.trajectory = traj_partial; + + // Smoothing publisher (determines how well the robot follows a trajectory) + smoothing_pub = ros::NodeHandle().advertise("smoothing", 1); + + // Trajectory publisher + traj_pub = nh.advertise("command", 1); + + // State subscriber + state_sub = nh.subscribe("state", + 1, + &JointPartialTrajectoryControllerTest::stateCB, + this); + + // Query state service client + query_state_service = nh.serviceClient("query_state"); + + // Action client + 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)); + } + + ~JointPartialTrajectoryControllerTest() + { + state_sub.shutdown(); // This is important, to make sure that the callback is not woken up later in the destructor + } + +protected: + typedef actionlib::SimpleActionClient ActionClient; + typedef boost::shared_ptr ActionClientPtr; + typedef control_msgs::FollowJointTrajectoryGoal ActionGoal; + typedef control_msgs::JointTrajectoryControllerStateConstPtr StateConstPtr; + + boost::mutex mutex; + ros::NodeHandle nh; + + unsigned int n_joints; + unsigned int n_partial_joints; + std::vector joint_names; + std::vector points; + + trajectory_msgs::JointTrajectory traj_home; + trajectory_msgs::JointTrajectory traj; + trajectory_msgs::JointTrajectory traj_partial; + ActionGoal traj_home_goal; + ActionGoal traj_goal; + ActionGoal traj_partial_goal; + + ros::Duration short_timeout; + ros::Duration long_timeout; + + ros::Publisher smoothing_pub; + ros::Publisher traj_pub; + ros::Subscriber state_sub; + ros::ServiceClient query_state_service; + ActionClientPtr action_client; + ActionClientPtr action_client2; + + + StateConstPtr controller_state; + + void stateCB(const StateConstPtr& state) + { + boost::mutex::scoped_lock lock(mutex); + controller_state = state; + } + + StateConstPtr getState() + { + boost::mutex::scoped_lock lock(mutex); + return controller_state; + } + + bool initState(const ros::Duration& timeout = ros::Duration(5.0)) + { + bool init_ok = false; + ros::Time start_time = ros::Time::now(); + while (!init_ok && (ros::Time::now() - start_time) < timeout) + { + { + boost::mutex::scoped_lock lock(mutex); + init_ok = controller_state && !controller_state->joint_names.empty(); + } + ros::Duration(0.1).sleep(); + } + return init_ok; + } + + static bool waitForState(const ActionClientPtr& action_client, + const actionlib::SimpleClientGoalState& state, + const ros::Duration& timeout) + { + using ros::Time; + using ros::Duration; + + Time start_time = Time::now(); + while (action_client->getState() != state && ros::ok()) + { + if (timeout >= Duration(0.0) && (Time::now() - start_time) > timeout) {return false;} // Timed-out + ros::Duration(0.01).sleep(); + } + return true; + } +}; + +// Invalid messages //////////////////////////////////////////////////////////////////////////////////////////////////// + +TEST_F(JointPartialTrajectoryControllerTest, invalidMessages) +{ + ASSERT_TRUE(initState()); + ASSERT_TRUE(action_client->waitForServer(long_timeout)); + + // Incompatible joint names + { + ActionGoal bad_goal = traj_home_goal; + bad_goal.trajectory.joint_names[0] = "bad_name"; + + bad_goal.trajectory.header.stamp = ros::Time(0); // Start immediately + action_client->sendGoal(bad_goal); + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::REJECTED, long_timeout)); + EXPECT_EQ(action_client->getResult()->error_code, control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS); + } + + // No position data + { + ActionGoal bad_goal = traj_home_goal; + bad_goal.trajectory.points[0].positions.clear(); + + bad_goal.trajectory.header.stamp = ros::Time(0); // Start immediately + action_client->sendGoal(bad_goal); + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::REJECTED, long_timeout)); + EXPECT_EQ(action_client->getResult()->error_code, control_msgs::FollowJointTrajectoryResult::INVALID_GOAL); + } + + // Incompatible data sizes + { + ActionGoal bad_goal = traj_home_goal; + bad_goal.trajectory.points[0].positions.pop_back(); + + bad_goal.trajectory.header.stamp = ros::Time(0); // Start immediately + action_client->sendGoal(bad_goal); + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::REJECTED, long_timeout)); + EXPECT_EQ(action_client->getResult()->error_code, control_msgs::FollowJointTrajectoryResult::INVALID_GOAL); + } + + // Non-strictly increasing waypoint times + { + ActionGoal bad_goal = traj_goal; + bad_goal.trajectory.points[2].time_from_start = bad_goal.trajectory.points[1].time_from_start; + + action_client->sendGoal(bad_goal); + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::REJECTED, long_timeout)); + EXPECT_EQ(action_client->getResult()->error_code, control_msgs::FollowJointTrajectoryResult::INVALID_GOAL); + } + + // Empty trajectory through action interface + // NOTE: Sending an empty trajectory through the topic interface cancels execution of all queued segments, but + // an empty trajectory is interpreted by the action interface as not having the correct joint names. + { + ActionGoal empty_goal; + action_client->sendGoal(empty_goal); + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::REJECTED, long_timeout)); + EXPECT_EQ(action_client->getResult()->error_code, control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS); + } +} + +// Partial trajectory execution ////////////////////////////////////////////////////////////////////////////////////// + +TEST_F(JointPartialTrajectoryControllerTest, allowPartialTrajIfSpecified) +{ + ASSERT_TRUE(initState()); + ASSERT_TRUE(action_client->waitForServer(long_timeout)); + + // Send partial trajectory. + action_client->sendGoal(traj_partial_goal); + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::ACTIVE, long_timeout)); +} + +// Uninterrupted Partial trajectory execution ////////////////////////////////////////////////////////////////////////////////// + +TEST_F(JointPartialTrajectoryControllerTest, topicSinglePartialTraj) +{ + ASSERT_TRUE(initState()); + + // Send partial trajectory + traj_partial.header.stamp = ros::Time(0); // Start immediately + traj_pub.publish(traj_partial); + ros::Duration wait_duration = traj_partial.points.back().time_from_start + ros::Duration(0.5); + wait_duration.sleep(); // Wait until done + + // Validate state topic values + StateConstPtr state = getState(); + + EXPECT_NEAR(traj_partial.points.back().positions[0], state->desired.positions[1], EPS); + EXPECT_NEAR(traj_partial.points.back().velocities[0], state->desired.velocities[1], EPS); + EXPECT_NEAR(traj_partial.points.back().accelerations[0], state->desired.accelerations[1], EPS); + + EXPECT_NEAR(traj_partial.points.back().positions[0], state->actual.positions[1], EPS); + EXPECT_NEAR(traj_partial.points.back().velocities[0], state->actual.velocities[1], EPS); + + EXPECT_NEAR(0.0, state->error.positions[1], EPS); + EXPECT_NEAR(0.0, state->error.velocities[1], EPS); + + // Validate query state service + control_msgs::QueryTrajectoryState srv; + srv.request.time = ros::Time::now(); + ASSERT_TRUE(query_state_service.call(srv)); + for (unsigned int i = 0; i < n_joints; ++i) + { + EXPECT_NEAR(state->desired.positions[i], srv.response.position[i], EPS); + EXPECT_NEAR(state->desired.velocities[i], srv.response.velocity[i], EPS); + EXPECT_NEAR(state->desired.accelerations[i], srv.response.acceleration[i], EPS); + } +} + +TEST_F(JointPartialTrajectoryControllerTest, actionSinglePartialTraj) +{ + ASSERT_TRUE(initState()); + ASSERT_TRUE(action_client->waitForServer(long_timeout)); + + // Send partial trajectory + traj_partial_goal.trajectory.header.stamp = ros::Time(0); // Start immediately + action_client->sendGoal(traj_partial_goal); + + // Wait until done + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::SUCCEEDED, long_timeout)); + EXPECT_EQ(action_client->getResult()->error_code, control_msgs::FollowJointTrajectoryResult::SUCCESSFUL); + ros::Duration(0.5).sleep(); // Allows values to settle to within EPS, especially accelerations + + // Validate state topic values + StateConstPtr state = getState(); + + EXPECT_NEAR(traj_partial.points.back().positions[0], state->desired.positions[1], EPS); + EXPECT_NEAR(traj_partial.points.back().velocities[0], state->desired.velocities[1], EPS); + EXPECT_NEAR(traj_partial.points.back().accelerations[0], state->desired.accelerations[1], EPS); + + EXPECT_NEAR(traj_partial.points.back().positions[0], state->actual.positions[1], EPS); + EXPECT_NEAR(traj_partial.points.back().velocities[0], state->actual.velocities[1], EPS); + + EXPECT_NEAR(0.0, state->error.positions[1], EPS); + EXPECT_NEAR(0.0, state->error.velocities[1], EPS); +} + +// Trajectory replacement ////////////////////////////////////////////////////////////////////////////////////////////// + +TEST_F(JointPartialTrajectoryControllerTest, topicReplacesTopicTraj) +{ + ASSERT_TRUE(initState()); + ASSERT_TRUE(action_client->waitForServer(long_timeout)); + + // Send trajectory + traj.header.stamp = ros::Time(0); // Start immediately + traj_pub.publish(traj); + ros::Duration wait_duration = traj.points.front().time_from_start; + wait_duration.sleep(); // Wait until ~first waypoint + + // Send partial trajectory that preempts the previous one + traj_partial.header.stamp = ros::Time(0); // Start immediately + traj_pub.publish(traj_partial); + wait_duration = traj_partial.points.back().time_from_start + ros::Duration(0.5); + wait_duration.sleep(); // Wait until done + + // Check that we're at the original trajectory for joint1 and on the partial trajectory for joint2 + StateConstPtr state = getState(); + EXPECT_NEAR(traj.points.back().positions[0], state->desired.positions[0], EPS); + EXPECT_NEAR(traj.points.back().velocities[0], state->desired.velocities[0], EPS); + EXPECT_NEAR(traj.points.back().accelerations[0], state->desired.accelerations[0], EPS); + + EXPECT_NEAR(traj_partial.points.back().positions[0], state->desired.positions[1], EPS); + EXPECT_NEAR(traj_partial.points.back().velocities[0], state->desired.velocities[1], EPS); + EXPECT_NEAR(traj_partial.points.back().accelerations[0], state->desired.accelerations[1], EPS); +} + +TEST_F(JointPartialTrajectoryControllerTest, actionReplacesActionTraj) +{ + ASSERT_TRUE(initState()); + ASSERT_TRUE(action_client->waitForServer(long_timeout)); + + // Send trajectory + traj_goal.trajectory.header.stamp = ros::Time(0); // Start immediately + action_client->sendGoal(traj_goal); + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::ACTIVE, short_timeout)); + + ros::Duration wait_duration = traj.points.front().time_from_start; + wait_duration.sleep(); // Wait until ~first waypoint + + // Send partial trajectory that preempts the previous one + traj_partial_goal.trajectory.header.stamp = ros::Time(0); // Start immediately + action_client2->sendGoal(traj_partial_goal); + ASSERT_TRUE(waitForState(action_client2, SimpleClientGoalState::ACTIVE, short_timeout)); + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::PREEMPTED, short_timeout)); + //ROS_ERROR_STREAM("state: "<< action_client->getState().toString()); + + // Wait until done + ASSERT_TRUE(waitForState(action_client2, SimpleClientGoalState::SUCCEEDED, long_timeout)); + ros::Duration(0.5).sleep(); // Allows values to settle to within EPS, especially accelerations + + // Check that we're at the original trajectory for joint1 and on the partial trajectory for joint2 + StateConstPtr state = getState(); + EXPECT_NEAR(traj.points.back().positions[0], state->desired.positions[0], EPS); + EXPECT_NEAR(traj.points.back().velocities[0], state->desired.velocities[0], EPS); + EXPECT_NEAR(traj.points.back().accelerations[0], state->desired.accelerations[0], EPS); + + EXPECT_NEAR(traj_partial.points.back().positions[0], state->desired.positions[1], EPS); + EXPECT_NEAR(traj_partial.points.back().velocities[0], state->desired.velocities[1], EPS); + EXPECT_NEAR(traj_partial.points.back().accelerations[0], state->desired.accelerations[1], EPS); +} + +TEST_F(JointPartialTrajectoryControllerTest, actionReplacesTopicTraj) +{ + ASSERT_TRUE(initState()); + ASSERT_TRUE(action_client->waitForServer(long_timeout)); + + // Send trajectory + traj.header.stamp = ros::Time(0); // Start immediately + traj_pub.publish(traj); + ros::Duration wait_duration = traj.points.front().time_from_start; + wait_duration.sleep(); // Wait until ~first waypoint + + // Send partial trajectory that preempts the previous one + traj_partial_goal.trajectory.header.stamp = ros::Time(0); // Start immediately + action_client->sendGoal(traj_partial_goal); + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::ACTIVE, short_timeout)); + + // Wait until done + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::SUCCEEDED, long_timeout)); + ros::Duration(0.5).sleep(); // Allows values to settle to within EPS, especially accelerations + + // Check that we're at the original trajectory for joint1 and on the partial trajectory for joint2 + StateConstPtr state = getState(); + EXPECT_NEAR(traj.points.back().positions[0], state->desired.positions[0], EPS); + EXPECT_NEAR(traj.points.back().velocities[0], state->desired.velocities[0], EPS); + EXPECT_NEAR(traj.points.back().accelerations[0], state->desired.accelerations[0], EPS); + + EXPECT_NEAR(traj_partial.points.back().positions[0], state->desired.positions[1], EPS); + EXPECT_NEAR(traj_partial.points.back().velocities[0], state->desired.velocities[1], EPS); + EXPECT_NEAR(traj_partial.points.back().accelerations[0], state->desired.accelerations[1], EPS); +} + +TEST_F(JointPartialTrajectoryControllerTest, topicReplacesActionTraj) +{ + ASSERT_TRUE(initState()); + ASSERT_TRUE(action_client->waitForServer(long_timeout)); + + // Send trajectory + traj_goal.trajectory.header.stamp = ros::Time(0); // Start immediately + action_client->sendGoal(traj_goal); + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::ACTIVE, short_timeout)); + + ros::Duration wait_duration = traj.points.front().time_from_start; + wait_duration.sleep(); // Wait until ~first waypoint + + // Send partial trajectory that preempts the previous one + traj_partial.header.stamp = ros::Time(0); // Start immediately + traj_pub.publish(traj_partial); + + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::PREEMPTED, short_timeout)); + + wait_duration = traj_partial.points.back().time_from_start + ros::Duration(0.5); + wait_duration.sleep(); // Wait until done + + // Check that we're at the original trajectory for joint1 and on the partial trajectory for joint2 + StateConstPtr state = getState(); + EXPECT_NEAR(traj.points.back().positions[0], state->desired.positions[0], EPS); + EXPECT_NEAR(traj.points.back().velocities[0], state->desired.velocities[0], EPS); + EXPECT_NEAR(traj.points.back().accelerations[0], state->desired.accelerations[0], EPS); + + EXPECT_NEAR(traj_partial.points.back().positions[0], state->desired.positions[1], EPS); + EXPECT_NEAR(traj_partial.points.back().velocities[0], state->desired.velocities[1], EPS); + EXPECT_NEAR(traj_partial.points.back().accelerations[0], state->desired.accelerations[1], EPS); +} + +// Ignore old trajectory points //////////////////////////////////////////////////////////////////////////////////////// + +TEST_F(JointPartialTrajectoryControllerTest, ignoreOldTopicTraj) +{ + ASSERT_TRUE(initState()); + + // Send trajectory + traj.header.stamp = ros::Time(0); // Start immediately + traj_pub.publish(traj); + ros::Duration wait_duration = traj.points.front().time_from_start; + wait_duration.sleep(); // Wait until ~first waypoint + + // Send partial trajectory with all points occuring in the past. Should not preempts the previous one + traj_partial.header.stamp = ros::Time::now() - traj_partial.points.back().time_from_start - ros::Duration(0.5); + traj_pub.publish(traj_partial); + + wait_duration = traj.points.back().time_from_start - traj.points.front().time_from_start + ros::Duration(0.5); + wait_duration.sleep(); // Wait until first trajectory is done + + // Check that we're at the original trajectory end (Joint2 has not moved according to traj_partial) + StateConstPtr state = getState(); + for (unsigned int i = 0; i < n_joints; ++i) + { + EXPECT_NEAR(traj.points.back().positions[i], state->desired.positions[i], EPS); + EXPECT_NEAR(traj.points.back().velocities[i], state->desired.velocities[i], EPS); + EXPECT_NEAR(traj.points.back().accelerations[i], state->desired.accelerations[i], EPS); + } +} + +TEST_F(JointPartialTrajectoryControllerTest, ignoreOldActionTraj) +{ + ASSERT_TRUE(initState()); + ASSERT_TRUE(action_client->waitForServer(long_timeout)); + + // Send trajectory + traj_goal.trajectory.header.stamp = ros::Time(0); // Start immediately + action_client->sendGoal(traj_goal); + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::ACTIVE, short_timeout)); + + ros::Duration wait_duration = traj.points.front().time_from_start; + wait_duration.sleep(); // Wait until ~first waypoint + + // Send partial trajectory with all points occuring in the past. Should not preempts the previous one + traj_partial_goal.trajectory.header.stamp = ros::Time::now() - traj_partial.points.back().time_from_start - ros::Duration(0.5); + action_client2->sendGoal(traj_partial_goal); + ASSERT_TRUE(waitForState(action_client2, SimpleClientGoalState::REJECTED, short_timeout)); + + // Wait until done + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::SUCCEEDED, long_timeout)); + ros::Duration(0.5).sleep(); // Allows values to settle to within EPS, especially accelerations + + // Check that we're at the original trajectory end (Joint2 has not moved according to traj_partial) + StateConstPtr state = getState(); + for (unsigned int i = 0; i < n_joints; ++i) + { + EXPECT_NEAR(traj.points.back().positions[i], state->desired.positions[i], EPS); + EXPECT_NEAR(traj.points.back().velocities[i], state->desired.velocities[i], EPS); + EXPECT_NEAR(traj.points.back().accelerations[i], state->desired.accelerations[i], EPS); + } +} + +TEST_F(JointPartialTrajectoryControllerTest, ignorePartiallyOldActionTraj) +{ + ASSERT_TRUE(initState()); + ASSERT_TRUE(action_client->waitForServer(long_timeout)); + + // Send trajectory + ActionGoal first_goal = traj_goal; + first_goal.trajectory.header.stamp = ros::Time(0); // Start immediately + action_client->sendGoal(first_goal); + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::ACTIVE, short_timeout)); + + ros::Duration wait_duration = first_goal.trajectory.points.front().time_from_start; + wait_duration.sleep(); // Wait until ~first waypoint + + // Send partial trajectory with only the last point not occuring in the past. Only the last point should be executed + traj_partial_goal.trajectory.header.stamp = ros::Time::now() - traj.points.back().time_from_start + ros::Duration(1.0); + action_client2->sendGoal(traj_partial_goal); + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::PREEMPTED, short_timeout)); + ASSERT_TRUE(waitForState(action_client2, SimpleClientGoalState::ACTIVE, short_timeout)); + + // Wait until done + ASSERT_TRUE(waitForState(action_client2, SimpleClientGoalState::SUCCEEDED, long_timeout)); + ros::Duration(0.5).sleep(); // Allows values to settle to within EPS, especially accelerations + + // Check that we're at the original trajectory for joint1 and on the partial trajectory for joint2 + StateConstPtr state = getState(); + EXPECT_NEAR(traj.points.back().positions[0], state->desired.positions[0], EPS); + EXPECT_NEAR(traj.points.back().velocities[0], state->desired.velocities[0], EPS); + EXPECT_NEAR(traj.points.back().accelerations[0], state->desired.accelerations[0], EPS); + + EXPECT_NEAR(traj_partial.points.back().positions[0], state->desired.positions[1], EPS); + EXPECT_NEAR(traj_partial.points.back().velocities[0], state->desired.velocities[1], EPS); + EXPECT_NEAR(traj_partial.points.back().accelerations[0], state->desired.accelerations[1], EPS); +} + +TEST_F(JointPartialTrajectoryControllerTest, executeParitalActionTrajInFuture) +{ + ASSERT_TRUE(initState()); + ASSERT_TRUE(action_client->waitForServer(long_timeout)); + + // Send trajectory + ActionGoal first_goal = traj_goal; + first_goal.trajectory.header.stamp = ros::Time(0); // Start immediately + action_client->sendGoal(first_goal); + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::ACTIVE, short_timeout)); + + ros::Duration wait_duration = first_goal.trajectory.points.front().time_from_start; + wait_duration.sleep(); // Wait until ~first waypoint + + // Send partial trajectory with only the last point not occuring in the past. Only the last point should be executed + traj_partial_goal.trajectory.header.stamp = ros::Time::now() + traj.points.back().time_from_start + ros::Duration(2.0); + action_client2->sendGoal(traj_partial_goal); + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::PREEMPTED, short_timeout)); + ASSERT_TRUE(waitForState(action_client2, SimpleClientGoalState::ACTIVE, short_timeout)); + + // Wait until first traj is done + wait_duration = first_goal.trajectory.points.back().time_from_start; + wait_duration.sleep(); // Wait until ~first waypoint + + StateConstPtr state = getState(); + for (unsigned int i = 0; i < n_joints; ++i) + { + EXPECT_NEAR(traj.points.back().positions[i], state->desired.positions[i], EPS); + EXPECT_NEAR(traj.points.back().velocities[i], state->desired.velocities[i], EPS); + EXPECT_NEAR(traj.points.back().accelerations[i], state->desired.accelerations[i], EPS); + } + + // Wait until done + ASSERT_TRUE(waitForState(action_client2, SimpleClientGoalState::SUCCEEDED, long_timeout)); + ros::Duration(0.5).sleep(); // Allows values to settle to within EPS, especially accelerations + + // Check that we're at the original trajectory for joint1 and on the partial trajectory for joint2 + state = getState(); + EXPECT_NEAR(traj.points.back().positions[0], state->desired.positions[0], EPS); + EXPECT_NEAR(traj.points.back().velocities[0], state->desired.velocities[0], EPS); + EXPECT_NEAR(traj.points.back().accelerations[0], state->desired.accelerations[0], EPS); + + EXPECT_NEAR(traj_partial.points.back().positions[0], state->desired.positions[1], EPS); + EXPECT_NEAR(traj_partial.points.back().velocities[0], state->desired.velocities[1], EPS); + EXPECT_NEAR(traj_partial.points.back().accelerations[0], state->desired.accelerations[1], EPS); +} + + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "joint_partial_trajectory_controller_test"); + + ros::AsyncSpinner spinner(1); + spinner.start(); + int ret = RUN_ALL_TESTS(); + spinner.stop(); + ros::shutdown(); + return ret; +} diff --git a/joint_trajectory_controller/test/joint_trajectory_controller_test.cpp b/joint_trajectory_controller/test/joint_trajectory_controller_test.cpp index bb357c5ba..7dfd1ea48 100644 --- a/joint_trajectory_controller/test/joint_trajectory_controller_test.cpp +++ b/joint_trajectory_controller/test/joint_trajectory_controller_test.cpp @@ -248,7 +248,7 @@ TEST_F(JointTrajectoryControllerTest, invalidMessages) ASSERT_TRUE(initState()); ASSERT_TRUE(action_client->waitForServer(long_timeout)); - // Invalid size + // Invalid size (No partial joints goals allowed) { trajectory_msgs::JointTrajectoryPoint point; point.positions.resize(1, 0.0); diff --git a/joint_trajectory_controller/test/joint_trajectory_segment_test.cpp b/joint_trajectory_controller/test/joint_trajectory_segment_test.cpp index 546dec449..14ac71cd0 100644 --- a/joint_trajectory_controller/test/joint_trajectory_segment_test.cpp +++ b/joint_trajectory_controller/test/joint_trajectory_segment_test.cpp @@ -41,7 +41,7 @@ using namespace trajectory_msgs; const double EPS = 1e-9; typedef JointTrajectorySegment > Segment; -typedef std::vector PermutationType; +typedef std::vector MappingType; TEST(WraparoundOffsetTest, WrappingPositions) { @@ -60,10 +60,10 @@ TEST(WraparoundOffsetTest, WrappingPositions) // No wrapping joints { std::vector angle_wraparound(2, false); - std::vector wraparound_offset = wraparoundOffset(pos1, pos2, angle_wraparound); - EXPECT_NEAR(pos1.size(), wraparound_offset.size(), EPS); - EXPECT_NEAR(0.0, wraparound_offset[0], EPS); - EXPECT_NEAR(0.0, wraparound_offset[1], EPS); + double wraparound_offset1 = wraparoundJointOffset(pos1[0], pos2[0], angle_wraparound[0]); + double wraparound_offset2 = wraparoundJointOffset(pos1[1], pos2[1], angle_wraparound[1]); + EXPECT_NEAR(0.0, wraparound_offset1, EPS); + EXPECT_NEAR(0.0, wraparound_offset2, EPS); } // Single wrapping joint @@ -74,18 +74,18 @@ TEST(WraparoundOffsetTest, WrappingPositions) // From state1 to state2 { - std::vector wraparound_offset = wraparoundOffset(pos1, pos2, angle_wraparound); - EXPECT_NEAR(pos1.size(), wraparound_offset.size(), EPS); - EXPECT_NEAR(-two_pi, wraparound_offset[0], EPS); - EXPECT_NEAR(0.0, wraparound_offset[1], EPS); + double wraparound_offset1 = wraparoundJointOffset(pos1[0], pos2[0], angle_wraparound[0]); + double wraparound_offset2 = wraparoundJointOffset(pos1[1], pos2[1], angle_wraparound[1]); + EXPECT_NEAR(-two_pi, wraparound_offset1, EPS); + EXPECT_NEAR(0.0, wraparound_offset2, EPS); } // From state2 to state1 { - std::vector wraparound_offset = wraparoundOffset(pos2, pos1, angle_wraparound); - EXPECT_NEAR(pos1.size(), wraparound_offset.size(), EPS); - EXPECT_NEAR(two_pi, wraparound_offset[0], EPS); - EXPECT_NEAR(0.0, wraparound_offset[1], EPS); + double wraparound_offset1 = wraparoundJointOffset(pos2[0], pos1[0], angle_wraparound[0]); + double wraparound_offset2 = wraparoundJointOffset(pos2[1], pos1[1], angle_wraparound[1]); + EXPECT_NEAR(two_pi, wraparound_offset1, EPS); + EXPECT_NEAR(0.0, wraparound_offset2, EPS); } } @@ -95,18 +95,18 @@ TEST(WraparoundOffsetTest, WrappingPositions) // From state1 to state2 { - std::vector wraparound_offset = wraparoundOffset(pos1, pos2, angle_wraparound); - EXPECT_NEAR(pos1.size(), wraparound_offset.size(), EPS); - EXPECT_NEAR(-two_pi, wraparound_offset[0], EPS); - EXPECT_NEAR(-2.0 * two_pi, wraparound_offset[1], EPS); + double wraparound_offset1 = wraparoundJointOffset(pos1[0], pos2[0], angle_wraparound[0]); + double wraparound_offset2 = wraparoundJointOffset(pos1[1], pos2[1], angle_wraparound[1]); + EXPECT_NEAR(-two_pi, wraparound_offset1, EPS); + EXPECT_NEAR(-2.0 * two_pi, wraparound_offset2, EPS); } // From state2 to state1 { - std::vector wraparound_offset = wraparoundOffset(pos2, pos1, angle_wraparound); - EXPECT_NEAR(pos1.size(), wraparound_offset.size(), EPS); - EXPECT_NEAR(two_pi, wraparound_offset[0], EPS); - EXPECT_NEAR(2.0 * two_pi, wraparound_offset[1], EPS); + double wraparound_offset1 = wraparoundJointOffset(pos2[0], pos1[0], angle_wraparound[0]); + double wraparound_offset2 = wraparoundJointOffset(pos2[1], pos1[1], angle_wraparound[1]); + EXPECT_NEAR(two_pi, wraparound_offset1, EPS); + EXPECT_NEAR(2.0 * two_pi, wraparound_offset2, EPS); } } } @@ -122,16 +122,14 @@ TEST(WraparoundOffsetTest, WrappingPositionsPiSingularity) std::vector angle_wraparound(1, true); // From state1 to state2 { - std::vector wraparound_offset = wraparoundOffset(pos1, pos2, angle_wraparound); - EXPECT_EQ(pos1.size(), wraparound_offset.size()); - EXPECT_NEAR(0.0, wraparound_offset[0], EPS); + double wraparound_offset1 = wraparoundJointOffset(pos1[0], pos2[0], angle_wraparound[0]); + EXPECT_NEAR(0.0, wraparound_offset1, EPS); } // From state2 to state1 { - std::vector wraparound_offset = wraparoundOffset(pos2, pos1, angle_wraparound); - EXPECT_EQ(pos1.size(), wraparound_offset.size()); - EXPECT_NEAR(0.0, wraparound_offset[0], EPS); + double wraparound_offset1 = wraparoundJointOffset(pos2[0], pos1[0], angle_wraparound[0]); + EXPECT_NEAR(0.0, wraparound_offset1, EPS); } } @@ -154,18 +152,18 @@ TEST(WraparoundOffsetTest, NonWrappingPositions) // From state1 to state2 { - std::vector wraparound_offset = wraparoundOffset(pos1, pos2, angle_wraparound); - EXPECT_NEAR(pos1.size(), wraparound_offset.size(), EPS); - EXPECT_NEAR(0.0, wraparound_offset[0], EPS); - EXPECT_NEAR(0.0, wraparound_offset[1], EPS); + double wraparound_offset1 = wraparoundJointOffset(pos1[0], pos2[0], angle_wraparound[0]); + double wraparound_offset2 = wraparoundJointOffset(pos1[1], pos2[1], angle_wraparound[1]); + EXPECT_NEAR(0.0, wraparound_offset1, EPS); + EXPECT_NEAR(0.0, wraparound_offset2, EPS); } // From state2 to state1 { - std::vector wraparound_offset = wraparoundOffset(pos2, pos1, angle_wraparound); - EXPECT_NEAR(pos1.size(), wraparound_offset.size(), EPS); - EXPECT_NEAR(0.0, wraparound_offset[0], EPS); - EXPECT_NEAR(0.0, wraparound_offset[1], EPS); + double wraparound_offset1 = wraparoundJointOffset(pos2[0], pos1[0], angle_wraparound[0]); + double wraparound_offset2 = wraparoundJointOffset(pos2[1], pos1[1], angle_wraparound[1]); + EXPECT_NEAR(0.0, wraparound_offset1, EPS); + EXPECT_NEAR(0.0, wraparound_offset2, EPS); } } } @@ -239,28 +237,11 @@ TEST_F(JointTrajectorySegmentTest, InvalidSegmentConstruction) catch (const std::invalid_argument& ex) {ROS_ERROR_STREAM(ex.what());} } - // Invalid permutation vector size - { - PermutationType permutation(2, 1); - EXPECT_THROW(Segment(traj_start_time, p_start, p_end, permutation), std::invalid_argument); - try {Segment(traj_start_time, p_start, p_end, permutation);} - catch (const std::invalid_argument& ex) {ROS_ERROR_STREAM(ex.what());} - } - - // Invalid permutation vector indices - { - PermutationType permutation(1, 1); - EXPECT_THROW(Segment(traj_start_time, p_start, p_end, permutation), std::invalid_argument); - try {Segment(traj_start_time, p_start, p_end, permutation);} - catch (const std::invalid_argument& ex) {ROS_ERROR_STREAM(ex.what());} - } - // Invalid joint wraparound specification { - PermutationType permutation; std::vector pos_offset(2); - EXPECT_THROW(Segment(traj_start_time, p_start, p_end, permutation, pos_offset), std::invalid_argument); - try {Segment(traj_start_time, p_start, p_end, permutation, pos_offset);} + EXPECT_THROW(Segment(traj_start_time, p_start, p_end, pos_offset), std::invalid_argument); + try {Segment(traj_start_time, p_start, p_end, pos_offset);} catch (const std::invalid_argument& ex) {ROS_ERROR_STREAM(ex.what());} } } @@ -372,7 +353,7 @@ TEST_F(JointTrajectorySegmentTest, CrossValidateSegmentConstruction) } // TODO: Test with dimension four data -TEST_F(JointTrajectorySegmentTest, PermutationTest) +TEST_F(JointTrajectorySegmentTest, MappingTest) { // Add an extra joint to the trajectory point messages created in the fixture p_start.positions.push_back(-p_start.positions[0]); @@ -381,7 +362,7 @@ TEST_F(JointTrajectorySegmentTest, PermutationTest) p_end.positions.push_back(-p_end.positions[0]); p_end.velocities.push_back(-p_end.velocities[0]); - // No permutation vector + // No mapping vector { // Construct segment from ROS message EXPECT_NO_THROW(Segment(traj_start_time, p_start, p_end)); @@ -393,42 +374,6 @@ TEST_F(JointTrajectorySegmentTest, PermutationTest) EXPECT_EQ(p_start.positions[0], state.position[0]); EXPECT_EQ(p_start.positions[1], state.position[1]); } - - // Permutation vector preserving trajectory message joint order - { - PermutationType permutation(2); - permutation[0] = 0; - permutation[1] = 1; - - // Construct segment from ROS message - EXPECT_NO_THROW(Segment(traj_start_time, p_start, p_end, permutation)); - Segment segment(traj_start_time, p_start, p_end, permutation); - - // Check position values of start state only - typename Segment::State state; - segment.sample(segment.startTime(), state); - EXPECT_EQ(p_start.positions[0], state.position[0]); - EXPECT_EQ(p_start.positions[1], state.position[1]); - } - - // Permutation vector reversing trajectory message joint order - { - PermutationType permutation(2); - permutation[0] = 1; - permutation[1] = 0; - - // Construct segment from ROS message - EXPECT_NO_THROW(Segment(traj_start_time, p_start, p_end, permutation)); - Segment segment(traj_start_time, p_start, p_end, permutation); - - // Check position values of start state only - typename Segment::State state; - segment.sample(segment.startTime(), state); - EXPECT_NE(p_start.positions[0], state.position[0]); - EXPECT_NE(p_start.positions[1], state.position[1]); - EXPECT_EQ(p_start.positions[0], state.position[1]); - EXPECT_EQ(p_start.positions[1], state.position[0]); - } } int main(int argc, char** argv) diff --git a/joint_trajectory_controller/test/rrbot_partial_controllers.yaml b/joint_trajectory_controller/test/rrbot_partial_controllers.yaml new file mode 100644 index 000000000..e7a319544 --- /dev/null +++ b/joint_trajectory_controller/test/rrbot_partial_controllers.yaml @@ -0,0 +1,15 @@ +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 + allow_partial_joints_goal: true From 8ebf99b727e19145b63a82de2754b0fd1b5af7f1 Mon Sep 17 00:00:00 2001 From: beatrizleon Date: Thu, 26 May 2016 07:04:54 +0100 Subject: [PATCH 2/3] jtc: Enable sending trajectories with a partial set of joints --- .../init_joint_trajectory.h | 280 +++++--- .../joint_trajectory_controller.h | 44 +- .../joint_trajectory_controller_impl.h | 297 ++++---- .../joint_trajectory_segment.h | 89 +-- .../joint_trajectory_controller/tolerances.h | 61 ++ .../test/init_joint_trajectory_test.cpp | 198 +++--- .../joint_partial_trajectory_controller.test | 35 + ...int_partial_trajectory_controller_test.cpp | 637 ++++++++++++++++++ .../test/joint_trajectory_controller_test.cpp | 2 +- .../test/joint_trajectory_segment_test.cpp | 129 +--- .../test/rrbot_partial_controllers.yaml | 15 + 11 files changed, 1294 insertions(+), 493 deletions(-) create mode 100644 joint_trajectory_controller/test/joint_partial_trajectory_controller.test create mode 100644 joint_trajectory_controller/test/joint_partial_trajectory_controller_test.cpp create mode 100644 joint_trajectory_controller/test/rrbot_partial_controllers.yaml diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/init_joint_trajectory.h b/joint_trajectory_controller/include/joint_trajectory_controller/init_joint_trajectory.h index 75cf1889f..37269efb6 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/init_joint_trajectory.h +++ b/joint_trajectory_controller/include/joint_trajectory_controller/init_joint_trajectory.h @@ -57,19 +57,19 @@ namespace joint_trajectory_controller namespace internal { /** - * \return The permutation vector between two containers. - * If \p t1 is "{A, B, C, D}" and \p t2 is "{B, D, A, C}", the associated permutation vector is - * "{2, 0, 3, 1}". + * \return The map between \p t1 indices (implicitly encoded in return vector indices) to \t2 indices. + * If \p t1 is "{C, B}" and \p t2 is "{A, B, C, D}", the associated mapping vector is + * "{2, 1}". */ template -inline std::vector permutation(const T& t1, const T& t2) +inline std::vector mapping(const T& t1, const T& t2) { typedef unsigned int SizeType; + + // t1 must be a subset of t2 + if (t1.size() > t2.size()) {return std::vector();} - // Arguments must have the same size - if (t1.size() != t2.size()) {return std::vector();} - - std::vector permutation_vector(t1.size()); // Return value + std::vector mapping_vector(t1.size()); // Return value for (typename T::const_iterator t1_it = t1.begin(); t1_it != t1.end(); ++t1_it) { typename T::const_iterator t2_it = std::find(t2.begin(), t2.end(), *t1_it); @@ -78,10 +78,10 @@ inline std::vector permutation(const T& t1, const T& t2) { const SizeType t1_dist = std::distance(t1.begin(), t1_it); const SizeType t2_dist = std::distance(t2.begin(), t2_it); - permutation_vector[t1_dist] = t2_dist; + mapping_vector[t1_dist] = t2_dist; } } - return permutation_vector; + return mapping_vector; } } // namespace @@ -95,7 +95,9 @@ struct InitJointTrajectoryOptions { typedef realtime_tools::RealtimeServerGoalHandle RealtimeGoalHandle; typedef boost::shared_ptr RealtimeGoalHandlePtr; - typedef typename Trajectory::value_type::Scalar Scalar; + typedef typename Trajectory::value_type TrajectoryPerJoint; + typedef typename TrajectoryPerJoint::value_type Segment; + typedef typename Segment::Scalar Scalar; InitJointTrajectoryOptions() : current_trajectory(0), @@ -103,7 +105,8 @@ struct InitJointTrajectoryOptions angle_wraparound(0), rt_goal_handle(), default_tolerances(0), - other_time_base(0) + other_time_base(0), + allow_partial_joints_goal(false) {} Trajectory* current_trajectory; @@ -112,6 +115,13 @@ struct InitJointTrajectoryOptions RealtimeGoalHandlePtr rt_goal_handle; SegmentTolerances* default_tolerances; ros::Time* other_time_base; + bool allow_partial_joints_goal; +}; + +template +bool isNotEmpty(typename Trajectory::value_type trajPerJoint) +{ + return !trajPerJoint.empty(); }; /** @@ -164,7 +174,6 @@ struct InitJointTrajectoryOptions * Segment(const ros::Time& traj_start_time, * const trajectory_msgs::JointTrajectoryPoint& start_point, * const trajectory_msgs::JointTrajectoryPoint& end_point, - * const std::vector& permutation, * const std::vector& position_offset) * \endcode * The following function must also be defined to properly handle continuous joints: @@ -184,8 +193,10 @@ Trajectory initJointTrajectory(const trajectory_msgs::JointTrajectory& msg const InitJointTrajectoryOptions& options = InitJointTrajectoryOptions()) { - typedef typename Trajectory::value_type Segment; + typedef typename Trajectory::value_type TrajectoryPerJoint; + typedef typename TrajectoryPerJoint::value_type Segment; typedef typename Segment::Scalar Scalar; + typedef typename TrajectoryPerJoint::const_iterator TrajIter; const unsigned int n_joints = msg.joint_names.size(); @@ -239,13 +250,35 @@ Trajectory initJointTrajectory(const trajectory_msgs::JointTrajectory& msg o_msg_start_time = msg_start_time; } - // Permutation vector mapping the expected joint order to the message joint order - // If unspecified, a trivial map (no permutation) is computed const std::vector joint_names = has_joint_names ? *(options.joint_names) : msg.joint_names; - std::vector permutation_vector = internal::permutation(joint_names, msg.joint_names); + if (has_angle_wraparound) + { + // Preconditions + const unsigned int n_angle_wraparound = options.angle_wraparound->size(); + if (n_angle_wraparound != joint_names.size()) + { + ROS_ERROR("Cannot create trajectory from message. " + "Vector specifying whether joints wrap around has an invalid size."); + return Trajectory(); + } + } + + // If partial joints goals are not allowed, goal should specify all controller joints + if (!options.allow_partial_joints_goal) + { + if (msg.joint_names.size() != joint_names.size()) + { + ROS_ERROR("Cannot create trajectory from message. It does not contain the expected joints."); + return Trajectory(); + } + } + + // Mapping vector contains the map between the message joint order and the expected joint order + // If unspecified, a trivial map is computed + std::vector mapping_vector = internal::mapping(msg.joint_names,joint_names); - if (permutation_vector.empty()) + if (mapping_vector.empty()) { ROS_ERROR("Cannot create trajectory from message. It does not contain the expected joints."); return Trajectory(); @@ -264,17 +297,17 @@ Trajectory initJointTrajectory(const trajectory_msgs::JointTrajectory& msg // This point is used later on in this function, but is computed here, in advance because if the trajectory message // contains a trajectory in the past, we can quickly return without spending additional computational resources std::vector::const_iterator - it = findPoint(msg, time); // Points to last point occurring before current time (NOTE: Using time, not o_time) - if (it == msg.points.end()) + msg_it = findPoint(msg, time); // Points to last point occurring before current time (NOTE: Using time, not o_time) + if (msg_it == msg.points.end()) { - it = msg.points.begin(); // Entire trajectory is after current time + msg_it = msg.points.begin(); // Entire trajectory is after current time } else { - ++it; // Points to first point after current time OR sequence end - if (it == msg.points.end()) + ++msg_it; // Points to first point after current time OR sequence end + if (msg_it == msg.points.end()) { - ros::Duration last_point_dur = time - (msg_start_time + (--it)->time_from_start); + ros::Duration last_point_dur = time - (msg_start_time + (--msg_it)->time_from_start); ROS_WARN_STREAM("Dropping all " << msg.points.size() << " trajectory point(s), as they occur before the current time.\n" << "Last point is " << std::fixed << std::setprecision(3) << last_point_dur.toSec() << @@ -283,8 +316,8 @@ Trajectory initJointTrajectory(const trajectory_msgs::JointTrajectory& msg } else { - ros::Duration next_point_dur = msg_start_time + it->time_from_start - time; - ROS_WARN_STREAM("Dropping first " << std::distance(msg.points.begin(), it) << + ros::Duration next_point_dur = msg_start_time + msg_it->time_from_start - time; + ROS_WARN_STREAM("Dropping first " << std::distance(msg.points.begin(), msg_it) << " trajectory point(s) out of " << msg.points.size() << ", as they occur before the current time.\n" << "First valid point will be reached in " << std::fixed << std::setprecision(3) << @@ -297,91 +330,156 @@ Trajectory initJointTrajectory(const trajectory_msgs::JointTrajectory& msg // - Useful segments of new trajectory (contained in ROS message) Trajectory result_traj; // Currently empty - // Initialize offsets due to wrapping joints to zero - std::vector position_offset(n_joints, 0.0); - - // Bridge current trajectory to new one + // Set active goal to segments after current time if (has_current_trajectory) { - const Trajectory& curr_traj = *(options.current_trajectory); - - // Get the last time and state that will be executed from the current trajectory - const typename Segment::Time last_curr_time = std::max(o_msg_start_time.toSec(), o_time.toSec()); // Important! - typename Segment::State last_curr_state; - sample(curr_traj, last_curr_time, last_curr_state); - - // Get the first time and state that will be executed from the new trajectory - const typename Segment::Time first_new_time = o_msg_start_time.toSec() + (it->time_from_start).toSec(); - typename Segment::State first_new_state(*it, permutation_vector); // Here offsets are not yet applied + result_traj = *(options.current_trajectory); - // Compute offsets due to wrapping joints - if (has_angle_wraparound) + //Iterate to all segments after current time to set the new goal handler + for (unsigned int joint_id=0; joint_id < joint_names.size();joint_id++) { - position_offset = wraparoundOffset(last_curr_state.position, - first_new_state.position, - *(options.angle_wraparound)); - if (position_offset.empty()) + const TrajectoryPerJoint& curr_joint_traj = result_traj[joint_id]; + TrajIter active_seg = findSegment(curr_joint_traj, o_time.toSec()); // Currently active segment + + while (std::distance(active_seg, curr_joint_traj.end())>=1) { - ROS_ERROR("Cannot create trajectory from message. " - "Vector specifying whether joints wrap around has an invalid size."); - return Trajectory(); + (result_traj[joint_id])[std::distance(curr_joint_traj.begin(),active_seg)].setGoalHandle(options.rt_goal_handle); + ++active_seg; } } + } + else + result_traj.resize(joint_names.size()); + + //Iterate through the joints that are in the message, in the order of the mapping vector + //for (unsigned int joint_id=0; joint_id < joint_names.size();joint_id++) + for (unsigned int msg_joint_it=0; msg_joint_it < mapping_vector.size();msg_joint_it++) + { + std::vector::const_iterator it = msg_it; + if (!isValid(*it, it->positions.size())) + throw(std::invalid_argument("Size mismatch in trajectory point position, velocity or acceleration data.")); - // Apply offset to first state that will be executed from the new trajectory - first_new_state = typename Segment::State(*it, permutation_vector, position_offset); // Now offsets are applied + TrajectoryPerJoint result_traj_per_joint; // Currently empty + unsigned int joint_id = mapping_vector[msg_joint_it]; - // Add useful segments of current trajectory to result + // Initialize offsets due to wrapping joints to zero + std::vector position_offset(1, 0.0); + + //Initialize segment tolerance per joint + SegmentTolerancesPerJoint tolerances_per_joint; + tolerances_per_joint.state_tolerance = tolerances.state_tolerance[joint_id]; + tolerances_per_joint.goal_state_tolerance = tolerances.goal_state_tolerance[joint_id]; + tolerances_per_joint.goal_time_tolerance = tolerances.goal_time_tolerance; + + // Bridge current trajectory to new one + if (has_current_trajectory) { - typedef typename Trajectory::const_iterator TrajIter; - TrajIter first = findSegment(curr_traj, o_time.toSec()); // Currently active segment - TrajIter last = findSegment(curr_traj, last_curr_time); // Segment active when new trajectory starts - if (first == curr_traj.end() || last == curr_traj.end()) + const TrajectoryPerJoint& curr_joint_traj = (*options.current_trajectory)[joint_id]; + + // Get the last time and state that will be executed from the current trajectory + const typename Segment::Time last_curr_time = std::max(o_msg_start_time.toSec(), o_time.toSec()); // Important! + typename Segment::State last_curr_state; + sample(curr_joint_traj, last_curr_time, last_curr_state); + + // Get the first time and state that will be executed from the new trajectory + trajectory_msgs::JointTrajectoryPoint point_per_joint; + if (!it->positions.empty()) {point_per_joint.positions.resize(1, it->positions[msg_joint_it]);} + if (!it->velocities.empty()) {point_per_joint.velocities.resize(1, it->velocities[msg_joint_it]);} + if (!it->accelerations.empty()) {point_per_joint.accelerations.resize(1, it->accelerations[msg_joint_it]);} + point_per_joint.time_from_start = it->time_from_start; + + const typename Segment::Time first_new_time = o_msg_start_time.toSec() + (it->time_from_start).toSec(); + typename Segment::State first_new_state(point_per_joint); // Here offsets are not yet applied + + // Compute offsets due to wrapping joints + if (has_angle_wraparound) + { + position_offset[0] = wraparoundJointOffset(last_curr_state.position[0], + first_new_state.position[0], + (*options.angle_wraparound)[joint_id]); + } + + // Apply offset to first state that will be executed from the new trajectory + first_new_state = typename Segment::State(point_per_joint, position_offset); // Now offsets are applied + + // Add useful segments of current trajectory to result { - ROS_ERROR("Unexpected error: Could not find segments in current trajectory. Please contact the package maintainer."); - return Trajectory(); + TrajIter first = findSegment(curr_joint_traj, o_time.toSec()); // Currently active segment + TrajIter last = findSegment(curr_joint_traj, last_curr_time); // Segment active when new trajectory starts + if (first == curr_joint_traj.end() || last == curr_joint_traj.end()) + { + ROS_ERROR("Unexpected error: Could not find segments in current trajectory. Please contact the package maintainer."); + return Trajectory(); + } + result_traj_per_joint.insert(result_traj_per_joint.begin(), first, ++last); // Range [first,last) will still be executed } - result_traj.insert(result_traj.begin(), first, ++last); // Range [first,last) will still be executed + + // Add segment bridging current and new trajectories to result + Segment bridge_seg(last_curr_time, last_curr_state, + first_new_time, first_new_state); + bridge_seg.setGoalHandle(options.rt_goal_handle); + if (has_rt_goal_handle) {bridge_seg.setTolerances(tolerances_per_joint);} + result_traj_per_joint.push_back(bridge_seg); } - // Add segment bridging current and new trajectories to result - Segment bridge_seg(last_curr_time, last_curr_state, - first_new_time, first_new_state); - bridge_seg.setGoalHandle(options.rt_goal_handle); - if (has_rt_goal_handle) {bridge_seg.setTolerances(tolerances);} - result_traj.push_back(bridge_seg); - } + // Constants used in log statement at the end + const unsigned int num_old_segments = result_traj_per_joint.size() -1; + const unsigned int num_new_segments = std::distance(it, msg.points.end()) -1; - // Constants used in log statement at the end - const unsigned int num_old_segments = result_traj.size() -1; - const unsigned int num_new_segments = std::distance(it, msg.points.end()) -1; + // Add useful segments of new trajectory to result + // - Construct all trajectory segments occurring after current time + // - As long as there remain two trajectory points we can construct the next trajectory segment + while (std::distance(it, msg.points.end()) >= 2) + { + std::vector::const_iterator next_it = it; ++next_it; + + trajectory_msgs::JointTrajectoryPoint it_point_per_joint, next_it_point_per_joint; + + if (!isValid(*it, it->positions.size())) + throw(std::invalid_argument("Size mismatch in trajectory point position, velocity or acceleration data.")); + if (!it->positions.empty()) {it_point_per_joint.positions.resize(1, it->positions[msg_joint_it]);} + if (!it->velocities.empty()) {it_point_per_joint.velocities.resize(1, it->velocities[msg_joint_it]);} + if (!it->accelerations.empty()) {it_point_per_joint.accelerations.resize(1, it->accelerations[msg_joint_it]);} + it_point_per_joint.time_from_start = it->time_from_start; + + if (!isValid(*next_it, next_it->positions.size())) + throw(std::invalid_argument("Size mismatch in trajectory point position, velocity or acceleration data.")); + if (!next_it->positions.empty()) {next_it_point_per_joint.positions.resize(1, next_it->positions[msg_joint_it]);} + if (!next_it->velocities.empty()) {next_it_point_per_joint.velocities.resize(1, next_it->velocities[msg_joint_it]);} + if (!next_it->accelerations.empty()) {next_it_point_per_joint.accelerations.resize(1, next_it->accelerations[msg_joint_it]);} + next_it_point_per_joint.time_from_start = next_it->time_from_start; + + Segment segment(o_msg_start_time, it_point_per_joint, next_it_point_per_joint, position_offset); + segment.setGoalHandle(options.rt_goal_handle); + if (has_rt_goal_handle) {segment.setTolerances(tolerances_per_joint);} + result_traj_per_joint.push_back(segment); + ++it; + } - // Add useful segments of new trajectory to result - // - Construct all trajectory segments occurring after current time - // - As long as there remain two trajectory points we can construct the next trajectory segment - while (std::distance(it, msg.points.end()) >= 2) - { - std::vector::const_iterator next_it = it; ++next_it; - Segment segment(o_msg_start_time, *it, *next_it, permutation_vector, position_offset); - segment.setGoalHandle(options.rt_goal_handle); - if (has_rt_goal_handle) {segment.setTolerances(tolerances);} - result_traj.push_back(segment); - ++it; + // Useful debug info + std::stringstream log_str; + log_str << "Trajectory of joint " << joint_names[joint_id] << "has " << result_traj_per_joint.size() << " segments"; + if (has_current_trajectory) + { + log_str << ":"; + log_str << "\n- " << num_old_segments << " segment(s) will still be executed from previous trajectory."; + log_str << "\n- 1 segment added for transitioning between the current trajectory and first point of the input message."; + if (num_new_segments > 0) {log_str << "\n- " << num_new_segments << " new segments (" << (num_new_segments + 1) << + " points) taken from the input trajectory.";} + } + else {log_str << ".";} + ROS_DEBUG_STREAM(log_str.str()); + + if (result_traj_per_joint.size() > 0) + result_traj[joint_id] = result_traj_per_joint; } - // Useful debug info - std::stringstream log_str; - log_str << "Trajectory has " << result_traj.size() << " segments"; - if (has_current_trajectory) + // If the trajectory for all joints is empty, empty the trajectory vector + typename Trajectory::const_iterator trajIter = std::find_if (result_traj.begin(), result_traj.end(), isNotEmpty); + if (trajIter == result_traj.end()) { - log_str << ":"; - log_str << "\n- " << num_old_segments << " segment(s) will still be executed from previous trajectory."; - log_str << "\n- 1 segment added for transitioning between the current trajectory and first point of the input message."; - if (num_new_segments > 0) {log_str << "\n- " << num_new_segments << " new segments (" << (num_new_segments + 1) << - " points) taken from the input trajectory.";} + result_traj.clear(); } - else {log_str << ".";} - ROS_DEBUG_STREAM(log_str.str()); return result_traj; } 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 4aaf29116..7b5f469d2 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 @@ -40,6 +40,7 @@ // Boost #include #include +#include // ROS #include @@ -166,8 +167,10 @@ class JointTrajectoryController : public controller_interface::Controller StatePublisherPtr; typedef JointTrajectorySegment Segment; - typedef std::vector Trajectory; + typedef std::vector TrajectoryPerJoint; + typedef std::vector Trajectory; typedef boost::shared_ptr TrajectoryPtr; + typedef boost::shared_ptr TrajectoryPerJointPtr; typedef realtime_tools::RealtimeBox TrajectoryBox; typedef typename Segment::Scalar Scalar; @@ -194,11 +197,11 @@ class JointTrajectoryController : public controller_interface::Controller time_data_; @@ -206,6 +209,8 @@ class JointTrajectoryController : public controller_interface::Controller successful_joint_traj_; + bool allow_partial_joints_goal_; // ROS API ros::NodeHandle controller_nh_; @@ -241,33 +246,6 @@ class JointTrajectoryController : public controller_interface::Controller -inline void JointTrajectoryController:: -checkPathTolerances(const typename Segment::State& state_error, - const Segment& segment) -{ - const RealtimeGoalHandlePtr rt_segment_goal = segment.getGoalHandle(); - const SegmentTolerances& tolerances = segment.getTolerances(); - if (!checkStateTolerance(state_error, tolerances.state_tolerance)) - { - rt_segment_goal->preallocated_result_->error_code = - control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED; - rt_segment_goal->setAborted(rt_segment_goal->preallocated_result_); - rt_active_goal_.reset(); - } -} - -template -inline void JointTrajectoryController:: -checkGoalTolerances(const typename Segment::State& state_error, - const Segment& segment) -{ - // Controller uptime - const ros::Time uptime = time_data_.readFromRT()->uptime; - - // Checks that we have ended inside the goal tolerances - const RealtimeGoalHandlePtr rt_segment_goal = segment.getGoalHandle(); - const SegmentTolerances& tolerances = segment.getTolerances(); - const bool inside_goal_tolerances = checkStateTolerance(state_error, tolerances.goal_state_tolerance); - - if (inside_goal_tolerances) - { - rt_segment_goal->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::SUCCESSFUL; - rt_segment_goal->setSucceeded(rt_segment_goal->preallocated_result_); - rt_active_goal_.reset(); - } - else if (uptime.toSec() < segment.endTime() + tolerances.goal_time_tolerance) - { - // Still have some time left to meet the goal state tolerances - } - else - { - if (verbose_) - { - ROS_ERROR_STREAM_NAMED(name_,"Goal tolerances failed"); - // Check the tolerances one more time to output the errors that occures - checkStateTolerance(state_error, tolerances.goal_state_tolerance, true); - } - - rt_segment_goal->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED; - rt_segment_goal->setAborted(rt_segment_goal->preallocated_result_); - rt_active_goal_.reset(); - } -} - template JointTrajectoryController:: JointTrajectoryController() @@ -276,6 +222,13 @@ bool JointTrajectoryController::init(HardwareInt } ROS_DEBUG_STREAM_NAMED(name_, "Stop trajectory has a duration of " << stop_trajectory_duration_ << "s."); + // Checking if partial trajectories are allowed + controller_nh_.param("allow_partial_joints_goal", allow_partial_joints_goal_, false); + if (allow_partial_joints_goal_) + { + ROS_DEBUG_NAMED(name_, "Goals with partial set of joints are allowed"); + } + // List of controlled joints joint_names_ = getStrings(controller_nh_, "joints"); if (joint_names_.empty()) {return false;} @@ -343,14 +296,26 @@ bool JointTrajectoryController::init(HardwareInt this); // Preeallocate resources - current_state_ = typename Segment::State(n_joints); - desired_state_ = typename Segment::State(n_joints); - state_error_ = typename Segment::State(n_joints); - hold_start_state_ = typename Segment::State(n_joints); - hold_end_state_ = typename Segment::State(n_joints); + current_state_ = typename Segment::State(n_joints); + desired_state_ = typename Segment::State(n_joints); + state_error_ = typename Segment::State(n_joints); + desired_joint_state_ = typename Segment::State(1); + state_joint_error_ = typename Segment::State(1); - Segment hold_segment(0.0, current_state_, 0.0, current_state_); - hold_trajectory_ptr_->resize(1, hold_segment); + successful_joint_traj_ = boost::dynamic_bitset<>(joints_.size()); + + // Initialize trajectory with all joints + typename Segment::State current_joint_state_ = typename Segment::State(1); + for (unsigned int i = 0; i < n_joints; ++i) + { + current_joint_state_.position[0]= current_state_.position[i]; + current_joint_state_.velocity[0]= current_state_.velocity[i]; + Segment hold_segment(0.0, current_joint_state_, 0.0, current_joint_state_); + + TrajectoryPerJoint joint_segment; + joint_segment.resize(1, hold_segment); + hold_trajectory_ptr_->push_back(joint_segment); + } { state_publisher_->lock(); @@ -392,16 +357,6 @@ update(const ros::Time& time, const ros::Duration& period) // fetch the currently followed trajectory, it has been updated by the non-rt thread with something that starts in the // next control cycle, leaving the current cycle without a valid trajectory. - // Update desired state: sample trajectory at current time - typename Trajectory::const_iterator segment_it = sample(curr_traj, time_data.uptime.toSec(), desired_state_); - if (curr_traj.end() == segment_it) - { - // Non-realtime safe, but should never happen under normal operation - ROS_ERROR_NAMED(name_, - "Unexpected error: No trajectory defined at current time. Please contact the package maintainer."); - return; - } - // Update current state and state error for (unsigned int i = 0; i < joints_.size(); ++i) { @@ -409,31 +364,95 @@ update(const ros::Time& time, const ros::Duration& period) current_state_.velocity[i] = joints_[i].getVelocity(); // There's no acceleration data available in a joint handle - state_error_.position[i] = desired_state_.position[i] - current_state_.position[i]; - state_error_.velocity[i] = desired_state_.velocity[i] - current_state_.velocity[i]; + typename TrajectoryPerJoint::const_iterator segment_it = sample(curr_traj[i], time_data.uptime.toSec(), desired_joint_state_); + if (curr_traj[i].end() == segment_it) + { + // Non-realtime safe, but should never happen under normal operation + ROS_ERROR_NAMED(name_, + "Unexpected error: No trajectory defined at current time. Please contact the package maintainer."); + return; + } + desired_state_.position[i] = desired_joint_state_.position[0]; + desired_state_.velocity[i] = desired_joint_state_.velocity[0]; + desired_state_.acceleration[i] = 0.0; + + state_joint_error_.position[0] = desired_joint_state_.position[0] - current_state_.position[i]; + state_joint_error_.velocity[0] = desired_joint_state_.velocity[0] - current_state_.velocity[i]; + state_joint_error_.acceleration[0] = 0.0; + + state_error_.position[i] = desired_joint_state_.position[0] - current_state_.position[i]; + state_error_.velocity[i] = desired_joint_state_.velocity[0] - current_state_.velocity[i]; state_error_.acceleration[i] = 0.0; - } - // Check tolerances if segment corresponds to currently active action goal - const RealtimeGoalHandlePtr rt_segment_goal = segment_it->getGoalHandle(); - if (rt_segment_goal && rt_segment_goal == rt_active_goal_) - { - // Check tolerances - if (time_data.uptime.toSec() < segment_it->endTime()) + //Check tolerances + const RealtimeGoalHandlePtr rt_segment_goal = segment_it->getGoalHandle(); + if (rt_segment_goal && rt_segment_goal == rt_active_goal_) { - // Currently executing a segment: check path tolerances - checkPathTolerances(state_error_, - *segment_it); + // Check tolerances + if (time_data.uptime.toSec() < segment_it->endTime()) + { + // Currently executing a segment: check path tolerances + const SegmentTolerancesPerJoint& joint_tolerances = segment_it->getTolerances(); + if (!checkStateTolerancePerJoint(state_joint_error_, joint_tolerances.state_tolerance)) + { + if (verbose_) + { + ROS_ERROR_STREAM_NAMED(name_,"Path tolerances failed for joint: " << joint_names_[i]); + checkStateTolerancePerJoint(state_joint_error_, joint_tolerances.state_tolerance, true); + } + rt_segment_goal->preallocated_result_->error_code = + control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED; + rt_segment_goal->setAborted(rt_segment_goal->preallocated_result_); + rt_active_goal_.reset(); + successful_joint_traj_.reset(); + } + } + else if (segment_it == --curr_traj[i].end()) + { + if (verbose_) + ROS_DEBUG_STREAM_THROTTLE_NAMED(1,name_,"Finished executing last segment, checking goal tolerances"); + + // Controller uptime + const ros::Time uptime = time_data_.readFromRT()->uptime; + + // Checks that we have ended inside the goal tolerances + const SegmentTolerancesPerJoint& tolerances = segment_it->getTolerances(); + const bool inside_goal_tolerances = checkStateTolerancePerJoint(state_joint_error_, tolerances.goal_state_tolerance); + + if (inside_goal_tolerances) + { + successful_joint_traj_[i] = 1; + } + else if (uptime.toSec() < segment_it->endTime() + tolerances.goal_time_tolerance) + { + // Still have some time left to meet the goal state tolerances + } + else + { + if (verbose_) + { + ROS_ERROR_STREAM_NAMED(name_,"Goal tolerances failed for joint: "<< joint_names_[i]); + // Check the tolerances one more time to output the errors that occurs + checkStateTolerancePerJoint(state_joint_error_, tolerances.goal_state_tolerance, true); + } + + rt_segment_goal->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED; + rt_segment_goal->setAborted(rt_segment_goal->preallocated_result_); + rt_active_goal_.reset(); + successful_joint_traj_.reset(); + } + } } - else if (segment_it == --curr_traj.end()) - { - if (verbose_) - ROS_DEBUG_STREAM_THROTTLE_NAMED(1,name_,"Finished executing last segement, checking goal tolerances"); + } - // Finished executing the LAST segment: check goal tolerances - checkGoalTolerances(state_error_, - *segment_it); - } + //If there is an active goal and all segments finished successfully then set goal as succeeded + RealtimeGoalHandlePtr current_active_goal(rt_active_goal_); + if (current_active_goal and successful_joint_traj_.count() == joints_.size()) + { + current_active_goal->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::SUCCESSFUL; + current_active_goal->setSucceeded(current_active_goal->preallocated_result_); + rt_active_goal_.reset(); + successful_joint_traj_.reset(); } // Hardware interface adapter: Generate and send commands @@ -499,12 +518,13 @@ updateTrajectoryCommand(const JointTrajectoryConstPtr& msg, RealtimeGoalHandlePt curr_trajectory_box_.get(curr_traj_ptr); Options options; - options.other_time_base = &next_update_uptime; - options.current_trajectory = curr_traj_ptr.get(); - options.joint_names = &joint_names_; - options.angle_wraparound = &angle_wraparound_; - options.rt_goal_handle = gh; - options.default_tolerances = &default_tolerances_; + options.other_time_base = &next_update_uptime; + options.current_trajectory = curr_traj_ptr.get(); + options.joint_names = &joint_names_; + options.angle_wraparound = &angle_wraparound_; + options.rt_goal_handle = gh; + options.default_tolerances = &default_tolerances_; + options.allow_partial_joints_goal = allow_partial_joints_goal_; // Update currently executing trajectory try @@ -539,7 +559,7 @@ template void JointTrajectoryController:: goalCB(GoalHandle gh) { - ROS_DEBUG_STREAM_NAMED(name_,"Recieved new action goal"); + ROS_DEBUG_STREAM_NAMED(name_,"Received new action goal"); // Precondition: Running controller if (!this->isRunning()) @@ -551,11 +571,24 @@ goalCB(GoalHandle gh) return; } - // Goal should specify all controller joints (they can be ordered differently). Reject if this is not the case - using internal::permutation; - std::vector permutation_vector = permutation(joint_names_, gh.getGoal()->trajectory.joint_names); + // If partial joints goals are not allowed, goal should specify all controller joints + if (!allow_partial_joints_goal_) + { + if (gh.getGoal()->trajectory.joint_names.size() != joint_names_.size()) + { + ROS_ERROR_NAMED(name_, "Joints on incoming goal don't match the controller joints."); + control_msgs::FollowJointTrajectoryResult result; + result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS; + gh.setRejected(result); + return; + } + } - if (permutation_vector.empty()) + // Goal should specify valid controller joints (they can be ordered differently). Reject if this is not the case + using internal::mapping; + std::vector mapping_vector = mapping(gh.getGoal()->trajectory.joint_names, joint_names_); + + if (mapping_vector.empty()) { ROS_ERROR_NAMED(name_, "Joints on incoming goal don't match the controller joints."); control_msgs::FollowJointTrajectoryResult result; @@ -638,19 +671,28 @@ queryStateService(control_msgs::QueryTrajectoryState::Request& req, curr_trajectory_box_.get(curr_traj_ptr); Trajectory& curr_traj = *curr_traj_ptr; - typename Segment::State state; - typename Trajectory::const_iterator segment_it = sample(curr_traj, sample_time.toSec(), state); - if (curr_traj.end() == segment_it) + typename Segment::State response_point = typename Segment::State(joint_names_.size()); + + for (unsigned int i = 0; i < joints_.size(); ++i) { - ROS_ERROR_STREAM_NAMED(name_, "Requested sample time preceeds trajectory start time."); - return false; - } + typename Segment::State state; + typename TrajectoryPerJoint::const_iterator segment_it = sample(curr_traj[i], sample_time.toSec(), state); + if (curr_traj[i].end() == segment_it) + { + ROS_ERROR_STREAM_NAMED(name_, "Requested sample time precedes trajectory start time."); + return false; + } + response_point.position[i] = state.position[0]; + response_point.velocity[i] = state.velocity[0]; + response_point.acceleration[i] = state.acceleration[0]; + } + // Populate response resp.name = joint_names_; - resp.position = state.position; - resp.velocity = state.velocity; - resp.acceleration = state.acceleration; + resp.position = response_point.position; + resp.velocity = response_point.velocity; + resp.acceleration = response_point.acceleration; return true; } @@ -690,7 +732,10 @@ setHoldPosition(const ros::Time& time) // - 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(1 == hold_trajectory_ptr_->size()); + 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_; @@ -700,24 +745,24 @@ setHoldPosition(const ros::Time& time) const unsigned int n_joints = joints_.size(); for (unsigned int i = 0; i < n_joints; ++i) { - hold_start_state_.position[i] = joints_[i].getPosition(); - hold_start_state_.velocity[i] = joints_[i].getVelocity(); - hold_start_state_.acceleration[i] = 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; - hold_end_state_.position[i] = joints_[i].getPosition(); - hold_end_state_.velocity[i] = -joints_[i].getVelocity(); - hold_end_state_.acceleration[i] = 0.0; - } - hold_trajectory_ptr_->front().init(start_time, hold_start_state_, - end_time_2x, hold_end_state_); + hold_end_state_.position[0] = joints_[i].getPosition(); + hold_end_state_.velocity[0] = -joints_[i].getVelocity(); + hold_end_state_.acceleration[0] = 0.0; - // Sample segment at its midpoint, that should have zero velocity - hold_trajectory_ptr_->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_->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_); + // 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_); + } curr_trajectory_box_.set(hold_trajectory_ptr_); } diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_segment.h b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_segment.h index 3c024c7cf..6915a7950 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_segment.h +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_segment.h @@ -85,31 +85,18 @@ class JointTrajectorySegment : public Segment /** * \param point Trajectory point. * - * \param permutation Permutation vector for mapping the joint order of a \p point to a desired order. - * For instance, if \p point contains data associated to joints "{B, D, A, C}", and we are interested in - * constructing a segment with joints ordered as "{A, B, C, D}", the permutation vector should - * be set to "{2, 0, 3, 1}". - * If unspecified (empty), the joint order of \p point is preserved; if specified, its size must coincide with that - * of \p point. - * This vector can be computed using the \ref trajectory_interface::internal::permutation() - * "permutation" utility function. - * - * \param position_offset Position offset to applpy to the data in \p point. This parameter is useful for handling + * \param position_offset Position offset to apply to the data in \p point. This parameter is useful for handling * joints that wrap around (ie. continuous), to compensate for multi-turn offsets. * If unspecified (empty), zero offsets are applied; if specified, its size must coincide with that of \p point. * - * \note The offsets in \p position_offsets correspond to joints not ordered according to \p point, but to joints - * in the expected order, that is \p point with \p permutation applied to it. */ State(const trajectory_msgs::JointTrajectoryPoint& point, - const std::vector& permutation = std::vector(), const std::vector& position_offset = std::vector()) { - init(point, permutation, position_offset); + init(point, position_offset); } void init(const trajectory_msgs::JointTrajectoryPoint& point, - const std::vector& permutation = std::vector(), const std::vector& position_offset = std::vector()) { using std::invalid_argument; @@ -121,17 +108,6 @@ class JointTrajectorySegment : public Segment { throw(invalid_argument("Size mismatch in trajectory point position, velocity or acceleration data.")); } - if (!permutation.empty() && joint_dim != permutation.size()) - { - throw(invalid_argument("Size mismatch between trajectory point and permutation vector.")); - } - for (unsigned int i = 0; i < permutation.size(); ++i) - { - if (permutation[i] >= joint_dim) - { - throw(invalid_argument("Permutation vector contains out-of-range indices.")); - } - } if (!position_offset.empty() && joint_dim != position_offset.size()) { throw(invalid_argument("Size mismatch between trajectory point " @@ -145,15 +121,12 @@ class JointTrajectorySegment : public Segment for (unsigned int i = 0; i < joint_dim; ++i) { - // Apply permutation only if it was specified, otherwise preserve original message order - const unsigned int id = permutation.empty() ? i : permutation[i]; - // Apply position offset only if it was specified const Scalar offset = position_offset.empty() ? 0.0 : position_offset[i]; - if (!point.positions.empty()) {this->position[i] = point.positions[id] + offset;} - if (!point.velocities.empty()) {this->velocity[i] = point.velocities[id];} - if (!point.accelerations.empty()) {this->acceleration[i] = point.accelerations[id];} + if (!point.positions.empty()) {this->position[i] = point.positions[i] + offset;} + if (!point.velocities.empty()) {this->velocity[i] = point.velocities[i];} + if (!point.accelerations.empty()) {this->acceleration[i] = point.accelerations[i];} } } }; @@ -171,7 +144,7 @@ class JointTrajectorySegment : public Segment const Time& end_time, const State& end_state) : rt_goal_handle_(), - tolerances_(start_state.position.size()) + tolerances_() { Segment::init(start_time, start_state, end_time, end_state); } @@ -183,7 +156,6 @@ class JointTrajectorySegment : public Segment * segment start time. * \param start_point Start state in ROS message format. * \param end_point End state in ROS message format. - * \param permutation See \ref JointTrajectorySegment::State. * \param position_offset See \ref JointTrajectorySegment::State. * * \throw std::invalid_argument If input parameters are inconsistent and a valid segment can't be constructed. @@ -191,10 +163,9 @@ class JointTrajectorySegment : public Segment JointTrajectorySegment(const ros::Time& traj_start_time, const trajectory_msgs::JointTrajectoryPoint& start_point, const trajectory_msgs::JointTrajectoryPoint& end_point, - const std::vector& permutation = std::vector(), const std::vector& position_offset = std::vector()) : rt_goal_handle_(), - tolerances_(start_point.positions.size()) + tolerances_() { if (start_point.positions.size() != end_point.positions.size()) { @@ -207,8 +178,8 @@ class JointTrajectorySegment : public Segment try { - const State start_state(start_point, permutation, position_offset); - const State end_state(end_point, permutation, position_offset); + const State start_state(start_point, position_offset); + const State end_state(end_point, position_offset); this->init(start_time, start_state, end_time, end_state); @@ -227,51 +198,45 @@ class JointTrajectorySegment : public Segment void setGoalHandle(RealtimeGoalHandlePtr rt_goal_handle) {rt_goal_handle_ = rt_goal_handle;} /** \return Tolerances this segment is associated to. */ - const SegmentTolerances& getTolerances() const {return tolerances_;} + const SegmentTolerancesPerJoint& getTolerances() const {return tolerances_;} /** \brief Set the tolerances this segment is associated to. */ - void setTolerances(const SegmentTolerances& tolerances) {tolerances_ = tolerances;} + void setTolerances(const SegmentTolerancesPerJoint& tolerances) {tolerances_ = tolerances;} private: RealtimeGoalHandlePtr rt_goal_handle_; - SegmentTolerances tolerances_; + SegmentTolerancesPerJoint tolerances_; }; /** * \param prev_position Previous position from which to compute the wraparound offset. * \param next_position Next position from which to compute the wraparound offset. - * \param angle_wraparound Vector of booleans where true values correspond to joints that wrap around - * (ie. are continuous). Offsets will be computed only for these joints, otherwise they are set to zero. - * \return Wraparound offsets that should be applied to \p next_position such that no multi-turns are performed when + * \param angle_wraparound Boolean where true value corresponds to a joint that wrap around + * (ie. is continuous). Offset will be computed only for this joint, otherwise it is set to zero. + * \return Wraparound offset that should be applied to \p next_position such that no multi-turns are performed when * transitioning from \p prev_position. * \tparam Scalar Scalar type. */ template -std::vector wraparoundOffset(const std::vector& prev_position, - const std::vector& next_position, - const std::vector& angle_wraparound) +Scalar wraparoundJointOffset(const Scalar& prev_position, + const Scalar& next_position, + const bool& angle_wraparound) { - // Preconditions - const unsigned int n_joints = angle_wraparound.size(); - if (n_joints != prev_position.size() || n_joints != next_position.size()) {return std::vector();} - // Return value - std::vector pos_offset(n_joints, 0.0); + Scalar pos_offset = 0.0; - for (unsigned int i = 0; i < angle_wraparound.size(); ++i) + if (angle_wraparound) { - if (angle_wraparound[i]) - { - Scalar dist = angles::shortest_angular_distance(prev_position[i], next_position[i]); + Scalar dist = angles::shortest_angular_distance(prev_position, next_position); - // Deal with singularity at M_PI shortest distance - if (std::abs(dist) - M_PI < 1e-9) - { - dist = next_position[i] > prev_position[i] ? std::abs(dist) : -std::abs(dist); - } - pos_offset[i] = (prev_position[i] + dist) - next_position[i]; + // Deal with singularity at M_PI shortest distance + if (std::abs(dist) - M_PI < 1e-9) + { + dist = next_position > prev_position ? std::abs(dist) : -std::abs(dist); } + pos_offset = (prev_position + dist) - next_position; } + return pos_offset; } diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.h b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.h index efc06580a..02e03e0f4 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.h +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.h @@ -88,6 +88,28 @@ struct SegmentTolerances Scalar goal_time_tolerance; }; +/** + * \brief Trajectory segment tolerances per Joint + */ +template +struct SegmentTolerancesPerJoint +{ + SegmentTolerancesPerJoint() + : state_tolerance(static_cast(0.0)), + goal_state_tolerance(static_cast(0.0)), + goal_time_tolerance(static_cast(0.0)) + {} + + /** State tolerances that appply during segment execution. */ + StateTolerances state_tolerance; + + /** State tolerances that apply for the goal state only.*/ + StateTolerances goal_state_tolerance; + + /** Extra time after the segment end time allowed to reach the goal state tolerances. */ + Scalar goal_time_tolerance; +}; + /** * \param state_error State error to check. * \param state_tolerance State tolerances to check \p state_error against. @@ -136,6 +158,45 @@ inline bool checkStateTolerance(const State& return true; } +/** + * \param state_error State error to check. + * \param state_tolerance State tolerances to check \p state_error against. + * \param show_errors If the joint that violate its tolerance should be output to console. NOT REALTIME if true + * \return True if \p state_error fulfills \p state_tolerance. + */ +template +inline bool checkStateTolerancePerJoint(const State& state_error, + const StateTolerances& state_tolerance, + bool show_errors = false) +{ + + using std::abs; + + const bool is_valid = !(state_tolerance.position > 0.0 && abs(state_error.position[0]) > state_tolerance.position) && + !(state_tolerance.velocity > 0.0 && abs(state_error.velocity[0]) > state_tolerance.velocity) && + !(state_tolerance.acceleration > 0.0 && abs(state_error.acceleration[0]) > state_tolerance.acceleration); + + if (!is_valid) + { + if( show_errors ) + { + ROS_ERROR_STREAM_NAMED("tolerances","Path state tolerances failed:"); + + if (state_tolerance.position > 0.0 && abs(state_error.position[0]) > state_tolerance.position) + ROS_ERROR_STREAM_NAMED("tolerances","Position Error: " << state_error.position[0] << + " Position Tolerance: " << state_tolerance.position); + if (state_tolerance.velocity > 0.0 && abs(state_error.velocity[0]) > state_tolerance.velocity) + ROS_ERROR_STREAM_NAMED("tolerances","Velocity Error: " << state_error.velocity[0] << + " Velocity Tolerance: " << state_tolerance.velocity); + if (state_tolerance.acceleration > 0.0 && abs(state_error.acceleration[0]) > state_tolerance.acceleration) + ROS_ERROR_STREAM_NAMED("tolerances","Acceleration Error: " << state_error.acceleration[0] << + " Acceleration Tolerance: " << state_tolerance.acceleration); + } + return false; + } + return true; +} + /** * \brief Update data in \p tols from data in \p msg_tol. * diff --git a/joint_trajectory_controller/test/init_joint_trajectory_test.cpp b/joint_trajectory_controller/test/init_joint_trajectory_test.cpp index b26985dcf..c72035c1d 100644 --- a/joint_trajectory_controller/test/init_joint_trajectory_test.cpp +++ b/joint_trajectory_controller/test/init_joint_trajectory_test.cpp @@ -45,61 +45,73 @@ using std::string; const double EPS = 1e-9; typedef JointTrajectorySegment > Segment; -typedef vector Trajectory; +typedef vector TrajectoryPerJoint; +typedef vector Trajectory; -TEST(PermutationTest, Permutation) +TEST(MappingTest, Mapping) { vector t1(4); - t1[0] = "A"; - t1[1] = "B"; - t1[2] = "C"; - t1[3] = "D"; + t1[0] = "B"; + t1[1] = "D"; + t1[2] = "A"; + t1[3] = "C"; vector t2(4); - t2[0] = "B"; - t2[1] = "D"; - t2[2] = "A"; - t2[3] = "C"; + t2[0] = "A"; + t2[1] = "B"; + t2[2] = "C"; + t2[3] = "D"; - typedef vector PermutationType; - PermutationType ground_truth(4); + typedef vector MappingType; + MappingType ground_truth(4); ground_truth[0] = 2; ground_truth[1] = 0; ground_truth[2] = 3; ground_truth[3] = 1; - // Mismatching sizes: Return empty permutation vector + // Mismatching sizes (t2 smaller than t1): Return empty mapping vector { vector t2_bad(1,"A"); - PermutationType perm = internal::permutation(t1, t2_bad); - EXPECT_TRUE(perm.empty()); + MappingType mapping = internal::mapping(t1, t2_bad); + EXPECT_TRUE(mapping.empty()); } - // Mismatching contents: Return empty permutation vector + // Mismatching contents: Return empty mapping vector { vector t2_bad(3,"A"); - PermutationType perm = internal::permutation(t1, t2_bad); - EXPECT_TRUE(perm.empty()); + MappingType mapping = internal::mapping(t1, t2_bad); + EXPECT_TRUE(mapping.empty()); } // Valid parameters { - PermutationType perm = internal::permutation(t1, t2); - EXPECT_EQ(ground_truth.size(), perm.size()); - EXPECT_EQ(ground_truth[0], perm[0]); - EXPECT_EQ(ground_truth[1], perm[1]); - EXPECT_EQ(ground_truth[2], perm[2]); - EXPECT_EQ(ground_truth[3], perm[3]); + MappingType mapping = internal::mapping(t1, t2); + EXPECT_EQ(ground_truth.size(), mapping.size()); + EXPECT_EQ(ground_truth[3], mapping[0]); + EXPECT_EQ(ground_truth[2], mapping[1]); + EXPECT_EQ(ground_truth[1], mapping[2]); + EXPECT_EQ(ground_truth[0], mapping[3]); } - // Valid parameters, inverse parameter order yields inverse permutation vector + // Valid parameters, inverse parameter order yields inverse mapping vector { - PermutationType perm = internal::permutation(t2, t1); - EXPECT_EQ(ground_truth.size(), perm.size()); - EXPECT_EQ(ground_truth[3], perm[0]); - EXPECT_EQ(ground_truth[2], perm[1]); - EXPECT_EQ(ground_truth[1], perm[2]); - EXPECT_EQ(ground_truth[0], perm[3]); + MappingType mapping = internal::mapping(t2, t1); + EXPECT_EQ(ground_truth.size(), mapping.size()); + EXPECT_EQ(ground_truth[0], mapping[0]); + EXPECT_EQ(ground_truth[1], mapping[1]); + EXPECT_EQ(ground_truth[2], mapping[2]); + EXPECT_EQ(ground_truth[3], mapping[3]); + } + + // Valid parameters, partial trajectory + { + vector t1_partial(2); + t1_partial[0] = "B"; + t1_partial[1] = "D"; + MappingType mapping = internal::mapping(t1_partial, t2); + EXPECT_EQ(t1_partial.size(), mapping.size()); + EXPECT_EQ(1, mapping[0]); + EXPECT_EQ(3, mapping[1]); } } @@ -140,11 +152,15 @@ class InitTrajectoryTest : public ::testing::Test state[1].velocity.push_back(1.0); state[1].acceleration.push_back(1.0); - curr_traj.push_back(Segment(0.0, state[0], + TrajectoryPerJoint joint_segment; + + joint_segment.push_back(Segment(0.0, state[0], 1.0, state[1])); - curr_traj.push_back(Segment(3.0, state[1], + joint_segment.push_back(Segment(3.0, state[1], 10.0, state[0])); // This is just junk that should be discarded in the tests + + curr_traj.push_back(joint_segment); } protected: @@ -182,14 +198,14 @@ TEST_F(InitTrajectoryTest, InitLogic) { const ros::Time time = msg_start_time; Trajectory trajectory = initJointTrajectory(trajectory_msg, time); - EXPECT_EQ(points.size() - 1, trajectory.size()); + EXPECT_EQ(points.size() - 1, trajectory[0].size()); } // First point: Return partial trajectory (last segment) { const ros::Time time = msg_start_time + msg_points.begin()->time_from_start; Trajectory trajectory = initJointTrajectory(trajectory_msg, time); - EXPECT_EQ(points.size() - 2, trajectory.size()); + EXPECT_EQ(points.size() - 2, trajectory[0].size()); } // Between the first and second points: Return partial trajectory (last segment) @@ -197,7 +213,7 @@ TEST_F(InitTrajectoryTest, InitLogic) const ros::Time time = msg_start_time + ros::Duration((msg_points.begin()->time_from_start + (++msg_points.begin())->time_from_start).toSec() / 2.0); Trajectory trajectory = initJointTrajectory(trajectory_msg, time); - EXPECT_EQ(msg_points.size() - 2, trajectory.size()); + EXPECT_EQ(msg_points.size() - 2, trajectory[0].size()); } // Second point: Return empty trajectory @@ -249,23 +265,23 @@ TEST_F(InitTrajectoryTest, InitLogicCombine) // Before first point, starting the current trajectory: Return 4 segments: Last of current + bridge + full message { - const ros::Time time(curr_traj[0].startTime()); + const ros::Time time(curr_traj[0][0].startTime()); Trajectory trajectory = initJointTrajectory(trajectory_msg, time, options); - EXPECT_EQ(points.size() + 1, trajectory.size()); + EXPECT_EQ(points.size() + 1, trajectory[0].size()); } // Before first point: Return 4 segments: Last of current + bridge + full message { const ros::Time time = msg_start_time; Trajectory trajectory = initJointTrajectory(trajectory_msg, time, options); - EXPECT_EQ(points.size() + 1, trajectory.size()); + EXPECT_EQ(points.size() + 1, trajectory[0].size()); } // First point: Return partial trajectory, 3 segments: Last of current + bridge + last of message { const ros::Time time = msg_start_time + msg_points.begin()->time_from_start; Trajectory trajectory = initJointTrajectory(trajectory_msg, time, options); - EXPECT_EQ(points.size(), trajectory.size()); + EXPECT_EQ(points.size(), trajectory[0].size()); } // Between the first and second points: Same as before @@ -273,14 +289,14 @@ TEST_F(InitTrajectoryTest, InitLogicCombine) const ros::Time time = msg_start_time + ros::Duration((msg_points.begin()->time_from_start + (++msg_points.begin())->time_from_start).toSec() / 2.0); Trajectory trajectory = initJointTrajectory(trajectory_msg, time, options); - EXPECT_EQ(msg_points.size(), trajectory.size()); + EXPECT_EQ(msg_points.size(), trajectory[0].size()); } // Second point: Return partial trajectory, 2 segments: Last of current + bridge (only one point of message made it) { const ros::Time time = msg_start_time + (++msg_points.begin())->time_from_start; Trajectory trajectory = initJointTrajectory(trajectory_msg, time, options); - EXPECT_EQ(msg_points.size() - 1, trajectory.size()); + EXPECT_EQ(msg_points.size() - 1, trajectory[0].size()); } // Between the second and third points: Same as before @@ -288,7 +304,7 @@ TEST_F(InitTrajectoryTest, InitLogicCombine) const ros::Time time = msg_start_time + ros::Duration(((++msg_points.begin())->time_from_start + (--msg_points.end())->time_from_start).toSec() / 2.0); Trajectory trajectory = initJointTrajectory(trajectory_msg, time, options); - EXPECT_EQ(msg_points.size() - 1, trajectory.size()); + EXPECT_EQ(msg_points.size() - 1, trajectory[0].size()); } // Last point: Return empty trajectory @@ -314,12 +330,12 @@ TEST_F(InitTrajectoryTest, InitValues) // Input time is before first point: Return full trajectory (2 segments) const ros::Time time = msg_start_time; Trajectory trajectory = initJointTrajectory(trajectory_msg, time); - ASSERT_EQ(points.size() - 1, trajectory.size()); + ASSERT_EQ(points.size() - 1, trajectory[0].size()); // Check all segment start/end times and states - for (unsigned int i = 0; i < trajectory.size(); ++i) + for (unsigned int i = 0; i < trajectory[0].size(); ++i) { - const Segment& segment = trajectory[i]; + const Segment& segment = trajectory[0][i]; const JointTrajectoryPoint& p_start = msg_points[i]; const JointTrajectoryPoint& p_end = msg_points[i + 1]; @@ -354,17 +370,17 @@ TEST_F(InitTrajectoryTest, InitValues) TEST_F(InitTrajectoryTest, InitValuesCombine) { // Before first point: Return 4 segments: Last of current + bridge + full message - const ros::Time time(curr_traj[0].startTime()); + const ros::Time time(curr_traj[0][0].startTime()); InitJointTrajectoryOptions options; options.current_trajectory = &curr_traj; Trajectory trajectory = initJointTrajectory(trajectory_msg, time, options); - ASSERT_EQ(points.size() + 1, trajectory.size()); + ASSERT_EQ(points.size() + 1, trajectory[0].size()); // Check current trajectory segment start/end times and states (only one) { - const Segment& ref_segment = curr_traj[0]; - const Segment& segment = trajectory[0]; + const Segment& ref_segment = curr_traj[0][0]; + const Segment& segment = trajectory[0][0]; // Check start/end times { @@ -395,8 +411,8 @@ TEST_F(InitTrajectoryTest, InitValuesCombine) // Check bridge trajectory segment start/end times and states (only one) { - const Segment& ref_segment = curr_traj[0]; - const Segment& segment = trajectory[1]; + const Segment& ref_segment = curr_traj[0][0]; + const Segment& segment = trajectory[0][1]; const vector& msg_points = trajectory_msg.points; @@ -433,12 +449,12 @@ TEST_F(InitTrajectoryTest, InitValuesCombine) } // Check all segment start/end times and states (2 segments) - for (unsigned int traj_it = 2, msg_it = 0; traj_it < trajectory.size(); ++traj_it, ++msg_it) + for (unsigned int traj_it = 2, msg_it = 0; traj_it < trajectory[0].size(); ++traj_it, ++msg_it) { const ros::Time msg_start_time = trajectory_msg.header.stamp; const vector& msg_points = trajectory_msg.points; - const Segment& segment = trajectory[traj_it]; + const Segment& segment = trajectory[0][traj_it]; const JointTrajectoryPoint& p_start = msg_points[msg_it]; const JointTrajectoryPoint& p_end = msg_points[msg_it + 1]; @@ -470,7 +486,7 @@ TEST_F(InitTrajectoryTest, InitValuesCombine) } } -TEST_F(InitTrajectoryTest, JointPermutation) +TEST_F(InitTrajectoryTest, JointMapping) { // Add an extra joint to the trajectory message created in the fixture trajectory_msg.points[0].positions.push_back(-2.0); @@ -492,11 +508,13 @@ TEST_F(InitTrajectoryTest, JointPermutation) Trajectory trajectory = initJointTrajectory(trajectory_msg, trajectory_msg.header.stamp); // Check position values only - const Segment& segment = trajectory[0]; + const Segment& segment_foo_joint = trajectory[0][0]; + const Segment& segment_bar_joint = trajectory[1][0]; typename Segment::State state; - segment.sample(segment.startTime(), state); + segment_foo_joint.sample(segment_foo_joint.startTime(), state); EXPECT_EQ(trajectory_msg.points[0].positions[0], state.position[0]); - EXPECT_EQ(trajectory_msg.points[0].positions[1], state.position[1]); + segment_bar_joint.sample(segment_bar_joint.startTime(), state); + EXPECT_EQ(trajectory_msg.points[0].positions[1], state.position[0]); } // Reference joint names vector that reverses joint order @@ -510,13 +528,17 @@ TEST_F(InitTrajectoryTest, JointPermutation) Trajectory trajectory = initJointTrajectory(trajectory_msg, trajectory_msg.header.stamp, options); // Check position values only - const Segment& segment = trajectory[0]; - typename Segment::State state; - segment.sample(segment.startTime(), state); - EXPECT_NE(trajectory_msg.points[0].positions[0], state.position[0]); - EXPECT_NE(trajectory_msg.points[0].positions[1], state.position[1]); - EXPECT_EQ(trajectory_msg.points[0].positions[0], state.position[1]); - EXPECT_EQ(trajectory_msg.points[0].positions[1], state.position[0]); + const Segment& segment_bar_joint = trajectory[0][0]; + const Segment& segment_foo_joint = trajectory[1][0]; + + typename Segment::State state_foo_joint; + typename Segment::State state_bar_joint; + segment_foo_joint.sample(segment_foo_joint.startTime(), state_foo_joint); + segment_bar_joint.sample(segment_bar_joint.startTime(), state_bar_joint); + EXPECT_NE(trajectory_msg.points[0].positions[0], state_bar_joint.position[0]); + EXPECT_NE(trajectory_msg.points[0].positions[1], state_foo_joint.position[0]); + EXPECT_EQ(trajectory_msg.points[0].positions[0], state_foo_joint.position[0]); + EXPECT_EQ(trajectory_msg.points[0].positions[1], state_bar_joint.position[0]); } // Reference joint names size mismatch @@ -553,19 +575,19 @@ TEST_F(InitTrajectoryTest, WrappingSpec) } // Before first point: Return 4 segments: Last of current + bridge + full message - const ros::Time time(curr_traj[0].startTime()); + const ros::Time time(curr_traj[0][0].startTime()); std::vector angle_wraparound(1, true); InitJointTrajectoryOptions options; options.current_trajectory = &curr_traj; options.angle_wraparound = &angle_wraparound; Trajectory trajectory = initJointTrajectory(trajectory_msg, time, options); - ASSERT_EQ(points.size() + 1, trajectory.size()); + ASSERT_EQ(points.size() + 1, trajectory[0].size()); // Check current trajectory segment start/end times and states (only one) { - const Segment& ref_segment = curr_traj[0]; - const Segment& segment = trajectory[0]; + const Segment& ref_segment = curr_traj[0][0]; + const Segment& segment = trajectory[0][0]; // Check start/end times { @@ -596,8 +618,8 @@ TEST_F(InitTrajectoryTest, WrappingSpec) // Check bridge trajectory segment start/end times and states (only one) { - const Segment& ref_segment = curr_traj[0]; - const Segment& segment = trajectory[1]; + const Segment& ref_segment = curr_traj[0][0]; + const Segment& segment = trajectory[0][1]; const vector& msg_points = trajectory_msg.points; @@ -634,12 +656,12 @@ TEST_F(InitTrajectoryTest, WrappingSpec) } // Check all segment start/end times and states (2 segments) - for (unsigned int traj_it = 2, msg_it = 0; traj_it < trajectory.size(); ++traj_it, ++msg_it) + for (unsigned int traj_it = 2, msg_it = 0; traj_it < trajectory[0].size(); ++traj_it, ++msg_it) { const ros::Time msg_start_time = trajectory_msg.header.stamp; const vector& msg_points = trajectory_msg.points; - const Segment& segment = trajectory[traj_it]; + const Segment& segment = trajectory[0][traj_it]; const JointTrajectoryPoint& p_start = msg_points[msg_it]; const JointTrajectoryPoint& p_end = msg_points[msg_it + 1]; @@ -696,18 +718,18 @@ TEST_F(InitTrajectoryTest, IgnoreWrappingSpec) } // Before first point: Return 2 segments: Full message - const ros::Time time(curr_traj[0].startTime()); + const ros::Time time(curr_traj[0][0].startTime()); std::vector angle_wraparound(1, true); InitJointTrajectoryOptions options; options.angle_wraparound = &angle_wraparound; Trajectory trajectory = initJointTrajectory(trajectory_msg, time, options); - ASSERT_EQ(points.size() - 1, trajectory.size()); + ASSERT_EQ(points.size() - 1, trajectory[0].size()); // Check all segment start/end times and states (2 segments) - for (unsigned int i = 0; i < trajectory.size(); ++i) + for (unsigned int i = 0; i < trajectory[0].size(); ++i) { - const Segment& segment = trajectory[i]; + const Segment& segment = trajectory[0][i]; const JointTrajectoryPoint& p_start = msg_points[i]; const JointTrajectoryPoint& p_end = msg_points[i + 1]; @@ -751,21 +773,21 @@ TEST_F(InitTrajectoryTest, GoalHandleTest) RealtimeGoalHandlePtr rt_goal(new RealtimeGoalHandle(gh)); // Before first point: Return 4 segments: Last of current + bridge + full message - const ros::Time time(curr_traj[0].startTime()); + const ros::Time time(curr_traj[0][0].startTime()); InitJointTrajectoryOptions options; options.current_trajectory = &curr_traj; options.rt_goal_handle = rt_goal; Trajectory trajectory = initJointTrajectory(trajectory_msg, time, options); - ASSERT_EQ(points.size() + 1, trajectory.size()); + ASSERT_EQ(points.size() + 1, trajectory[0].size()); // Segment of current trajectory preserves existing null goal handle - ASSERT_FALSE(trajectory[0].getGoalHandle()); + ASSERT_FALSE(trajectory[0][0].getGoalHandle()); // All further segments should be associated to the new goal handle - for (unsigned int i = 1; i < trajectory.size(); ++i) // NOTE: Start index != 0 + for (unsigned int i = 1; i < trajectory[0].size(); ++i) // NOTE: Start index != 0 { - ASSERT_EQ(rt_goal, trajectory[i].getGoalHandle()); + ASSERT_EQ(rt_goal, trajectory[0][i].getGoalHandle()); } } @@ -783,12 +805,12 @@ TEST_F(InitTrajectoryTest, OtherTimeBase) // Before first point: Return 4 segments: Last of current + bridge + full message const ros::Time time = msg_start_time; Trajectory trajectory = initJointTrajectory(trajectory_msg, time, options); - ASSERT_EQ(points.size() + 1, trajectory.size()); + ASSERT_EQ(points.size() + 1, trajectory[0].size()); // Check current trajectory segment start/end times (only one). Time offset does NOT apply { - const Segment& ref_segment = curr_traj[0]; - const Segment& segment = trajectory[0]; + const Segment& ref_segment = curr_traj[0][0]; + const Segment& segment = trajectory[0][0]; // Check start/end times { @@ -799,7 +821,7 @@ TEST_F(InitTrajectoryTest, OtherTimeBase) // Check bridge trajectory segment start/end times and states (only one). Time offset applies to end state only { - const Segment& segment = trajectory[1]; + const Segment& segment = trajectory[0][1]; const vector& msg_points = trajectory_msg.points; @@ -817,12 +839,12 @@ TEST_F(InitTrajectoryTest, OtherTimeBase) } // Check all segment start/end times and states (2 segments). Time offset does apply - for (unsigned int traj_it = 2, msg_it = 0; traj_it < trajectory.size(); ++traj_it, ++msg_it) + for (unsigned int traj_it = 2, msg_it = 0; traj_it < trajectory[0].size(); ++traj_it, ++msg_it) { const ros::Time msg_start_time = trajectory_msg.header.stamp; const vector& msg_points = trajectory_msg.points; - const Segment& segment = trajectory[traj_it]; + const Segment& segment = trajectory[0][traj_it]; const JointTrajectoryPoint& p_start = msg_points[msg_it]; const JointTrajectoryPoint& p_end = msg_points[msg_it + 1]; diff --git a/joint_trajectory_controller/test/joint_partial_trajectory_controller.test b/joint_trajectory_controller/test/joint_partial_trajectory_controller.test new file mode 100644 index 000000000..7962df69c --- /dev/null +++ b/joint_trajectory_controller/test/joint_partial_trajectory_controller.test @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/joint_trajectory_controller/test/joint_partial_trajectory_controller_test.cpp b/joint_trajectory_controller/test/joint_partial_trajectory_controller_test.cpp new file mode 100644 index 000000000..270cfb48a --- /dev/null +++ b/joint_trajectory_controller/test/joint_partial_trajectory_controller_test.cpp @@ -0,0 +1,637 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2013, PAL Robotics S.L. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of PAL Robotics S.L. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +/// \author Adolfo Rodriguez Tsouroukdissian + +#include +#include + +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include + +// Floating-point value comparison threshold +const double EPS = 0.01; + +using actionlib::SimpleClientGoalState; + +class JointPartialTrajectoryControllerTest : public ::testing::Test +{ +public: + JointPartialTrajectoryControllerTest() + : nh("rrbot_controller"), + short_timeout(1.0), + long_timeout(10.0), + controller_state() + { + n_joints = (2); + n_partial_joints = (2); + joint_names.resize(n_joints); + joint_names[0] = "joint1"; + joint_names[1] = "joint2"; + + trajectory_msgs::JointTrajectoryPoint point; + point.positions.resize(n_joints, 0.0); + point.velocities.resize(n_joints, 0.0); + point.accelerations.resize(n_joints, 0.0); + + // Go home trajectory + traj_home.joint_names = joint_names; + traj_home.points.resize(1, point); + traj_home.points[0].time_from_start = ros::Duration(1.0); + + // Three-point trajectory + points.resize(3, point); + points[0].positions[0] = M_PI / 4.0; + points[0].positions[1] = 0.0; + points[0].time_from_start = ros::Duration(1.0); + + points[1].positions[0] = 0.0; + points[1].positions[1] = -M_PI / 4.0; + points[1].time_from_start = ros::Duration(2.0); + + points[2].positions[0] = -M_PI / 4.0; + points[2].positions[1] = M_PI / 4.0; + points[2].time_from_start = ros::Duration(4.0); + + traj.joint_names = joint_names; + traj.points = points; + + // Partial trajectory + traj_partial.joint_names.resize(1, "joint2"); + points.resize(3, point); + points[0].positions[0] = M_PI / 4.0; + points[0].time_from_start = ros::Duration(1.0); + + points[1].positions[0] = M_PI / 2.0; + points[1].time_from_start = ros::Duration(2.0); + + points[2].positions[0] = -M_PI / 2.0; + points[2].time_from_start = ros::Duration(4.0); + traj_partial.points = points; + + // Action goals + traj_home_goal.trajectory = traj_home; + traj_goal.trajectory = traj; + traj_partial_goal.trajectory = traj_partial; + + // Smoothing publisher (determines how well the robot follows a trajectory) + smoothing_pub = ros::NodeHandle().advertise("smoothing", 1); + + // Trajectory publisher + traj_pub = nh.advertise("command", 1); + + // State subscriber + state_sub = nh.subscribe("state", + 1, + &JointPartialTrajectoryControllerTest::stateCB, + this); + + // Query state service client + query_state_service = nh.serviceClient("query_state"); + + // Action client + 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)); + } + + ~JointPartialTrajectoryControllerTest() + { + state_sub.shutdown(); // This is important, to make sure that the callback is not woken up later in the destructor + } + +protected: + typedef actionlib::SimpleActionClient ActionClient; + typedef boost::shared_ptr ActionClientPtr; + typedef control_msgs::FollowJointTrajectoryGoal ActionGoal; + typedef control_msgs::JointTrajectoryControllerStateConstPtr StateConstPtr; + + boost::mutex mutex; + ros::NodeHandle nh; + + unsigned int n_joints; + unsigned int n_partial_joints; + std::vector joint_names; + std::vector points; + + trajectory_msgs::JointTrajectory traj_home; + trajectory_msgs::JointTrajectory traj; + trajectory_msgs::JointTrajectory traj_partial; + ActionGoal traj_home_goal; + ActionGoal traj_goal; + ActionGoal traj_partial_goal; + + ros::Duration short_timeout; + ros::Duration long_timeout; + + ros::Publisher smoothing_pub; + ros::Publisher traj_pub; + ros::Subscriber state_sub; + ros::ServiceClient query_state_service; + ActionClientPtr action_client; + ActionClientPtr action_client2; + + + StateConstPtr controller_state; + + void stateCB(const StateConstPtr& state) + { + boost::mutex::scoped_lock lock(mutex); + controller_state = state; + } + + StateConstPtr getState() + { + boost::mutex::scoped_lock lock(mutex); + return controller_state; + } + + bool initState(const ros::Duration& timeout = ros::Duration(5.0)) + { + bool init_ok = false; + ros::Time start_time = ros::Time::now(); + while (!init_ok && (ros::Time::now() - start_time) < timeout) + { + { + boost::mutex::scoped_lock lock(mutex); + init_ok = controller_state && !controller_state->joint_names.empty(); + } + ros::Duration(0.1).sleep(); + } + return init_ok; + } + + static bool waitForState(const ActionClientPtr& action_client, + const actionlib::SimpleClientGoalState& state, + const ros::Duration& timeout) + { + using ros::Time; + using ros::Duration; + + Time start_time = Time::now(); + while (action_client->getState() != state && ros::ok()) + { + if (timeout >= Duration(0.0) && (Time::now() - start_time) > timeout) {return false;} // Timed-out + ros::Duration(0.01).sleep(); + } + return true; + } +}; + +// Invalid messages //////////////////////////////////////////////////////////////////////////////////////////////////// + +TEST_F(JointPartialTrajectoryControllerTest, invalidMessages) +{ + ASSERT_TRUE(initState()); + ASSERT_TRUE(action_client->waitForServer(long_timeout)); + + // Incompatible joint names + { + ActionGoal bad_goal = traj_home_goal; + bad_goal.trajectory.joint_names[0] = "bad_name"; + + bad_goal.trajectory.header.stamp = ros::Time(0); // Start immediately + action_client->sendGoal(bad_goal); + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::REJECTED, long_timeout)); + EXPECT_EQ(action_client->getResult()->error_code, control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS); + } + + // No position data + { + ActionGoal bad_goal = traj_home_goal; + bad_goal.trajectory.points[0].positions.clear(); + + bad_goal.trajectory.header.stamp = ros::Time(0); // Start immediately + action_client->sendGoal(bad_goal); + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::REJECTED, long_timeout)); + EXPECT_EQ(action_client->getResult()->error_code, control_msgs::FollowJointTrajectoryResult::INVALID_GOAL); + } + + // Incompatible data sizes + { + ActionGoal bad_goal = traj_home_goal; + bad_goal.trajectory.points[0].positions.pop_back(); + + bad_goal.trajectory.header.stamp = ros::Time(0); // Start immediately + action_client->sendGoal(bad_goal); + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::REJECTED, long_timeout)); + EXPECT_EQ(action_client->getResult()->error_code, control_msgs::FollowJointTrajectoryResult::INVALID_GOAL); + } + + // Non-strictly increasing waypoint times + { + ActionGoal bad_goal = traj_goal; + bad_goal.trajectory.points[2].time_from_start = bad_goal.trajectory.points[1].time_from_start; + + action_client->sendGoal(bad_goal); + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::REJECTED, long_timeout)); + EXPECT_EQ(action_client->getResult()->error_code, control_msgs::FollowJointTrajectoryResult::INVALID_GOAL); + } + + // Empty trajectory through action interface + // NOTE: Sending an empty trajectory through the topic interface cancels execution of all queued segments, but + // an empty trajectory is interpreted by the action interface as not having the correct joint names. + { + ActionGoal empty_goal; + action_client->sendGoal(empty_goal); + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::REJECTED, long_timeout)); + EXPECT_EQ(action_client->getResult()->error_code, control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS); + } +} + +// Partial trajectory execution ////////////////////////////////////////////////////////////////////////////////////// + +TEST_F(JointPartialTrajectoryControllerTest, allowPartialTrajIfSpecified) +{ + ASSERT_TRUE(initState()); + ASSERT_TRUE(action_client->waitForServer(long_timeout)); + + // Send partial trajectory. + action_client->sendGoal(traj_partial_goal); + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::ACTIVE, long_timeout)); +} + +// Uninterrupted Partial trajectory execution ////////////////////////////////////////////////////////////////////////////////// + +TEST_F(JointPartialTrajectoryControllerTest, topicSinglePartialTraj) +{ + ASSERT_TRUE(initState()); + + // Send partial trajectory + traj_partial.header.stamp = ros::Time(0); // Start immediately + traj_pub.publish(traj_partial); + ros::Duration wait_duration = traj_partial.points.back().time_from_start + ros::Duration(0.5); + wait_duration.sleep(); // Wait until done + + // Validate state topic values + StateConstPtr state = getState(); + + EXPECT_NEAR(traj_partial.points.back().positions[0], state->desired.positions[1], EPS); + EXPECT_NEAR(traj_partial.points.back().velocities[0], state->desired.velocities[1], EPS); + EXPECT_NEAR(traj_partial.points.back().accelerations[0], state->desired.accelerations[1], EPS); + + EXPECT_NEAR(traj_partial.points.back().positions[0], state->actual.positions[1], EPS); + EXPECT_NEAR(traj_partial.points.back().velocities[0], state->actual.velocities[1], EPS); + + EXPECT_NEAR(0.0, state->error.positions[1], EPS); + EXPECT_NEAR(0.0, state->error.velocities[1], EPS); + + // Validate query state service + control_msgs::QueryTrajectoryState srv; + srv.request.time = ros::Time::now(); + ASSERT_TRUE(query_state_service.call(srv)); + for (unsigned int i = 0; i < n_joints; ++i) + { + EXPECT_NEAR(state->desired.positions[i], srv.response.position[i], EPS); + EXPECT_NEAR(state->desired.velocities[i], srv.response.velocity[i], EPS); + EXPECT_NEAR(state->desired.accelerations[i], srv.response.acceleration[i], EPS); + } +} + +TEST_F(JointPartialTrajectoryControllerTest, actionSinglePartialTraj) +{ + ASSERT_TRUE(initState()); + ASSERT_TRUE(action_client->waitForServer(long_timeout)); + + // Send partial trajectory + traj_partial_goal.trajectory.header.stamp = ros::Time(0); // Start immediately + action_client->sendGoal(traj_partial_goal); + + // Wait until done + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::SUCCEEDED, long_timeout)); + EXPECT_EQ(action_client->getResult()->error_code, control_msgs::FollowJointTrajectoryResult::SUCCESSFUL); + ros::Duration(0.5).sleep(); // Allows values to settle to within EPS, especially accelerations + + // Validate state topic values + StateConstPtr state = getState(); + + EXPECT_NEAR(traj_partial.points.back().positions[0], state->desired.positions[1], EPS); + EXPECT_NEAR(traj_partial.points.back().velocities[0], state->desired.velocities[1], EPS); + EXPECT_NEAR(traj_partial.points.back().accelerations[0], state->desired.accelerations[1], EPS); + + EXPECT_NEAR(traj_partial.points.back().positions[0], state->actual.positions[1], EPS); + EXPECT_NEAR(traj_partial.points.back().velocities[0], state->actual.velocities[1], EPS); + + EXPECT_NEAR(0.0, state->error.positions[1], EPS); + EXPECT_NEAR(0.0, state->error.velocities[1], EPS); +} + +// Trajectory replacement ////////////////////////////////////////////////////////////////////////////////////////////// + +TEST_F(JointPartialTrajectoryControllerTest, topicReplacesTopicTraj) +{ + ASSERT_TRUE(initState()); + ASSERT_TRUE(action_client->waitForServer(long_timeout)); + + // Send trajectory + traj.header.stamp = ros::Time(0); // Start immediately + traj_pub.publish(traj); + ros::Duration wait_duration = traj.points.front().time_from_start; + wait_duration.sleep(); // Wait until ~first waypoint + + // Send partial trajectory that preempts the previous one + traj_partial.header.stamp = ros::Time(0); // Start immediately + traj_pub.publish(traj_partial); + wait_duration = traj_partial.points.back().time_from_start + ros::Duration(0.5); + wait_duration.sleep(); // Wait until done + + // Check that we're at the original trajectory for joint1 and on the partial trajectory for joint2 + StateConstPtr state = getState(); + EXPECT_NEAR(traj.points.back().positions[0], state->desired.positions[0], EPS); + EXPECT_NEAR(traj.points.back().velocities[0], state->desired.velocities[0], EPS); + EXPECT_NEAR(traj.points.back().accelerations[0], state->desired.accelerations[0], EPS); + + EXPECT_NEAR(traj_partial.points.back().positions[0], state->desired.positions[1], EPS); + EXPECT_NEAR(traj_partial.points.back().velocities[0], state->desired.velocities[1], EPS); + EXPECT_NEAR(traj_partial.points.back().accelerations[0], state->desired.accelerations[1], EPS); +} + +TEST_F(JointPartialTrajectoryControllerTest, actionReplacesActionTraj) +{ + ASSERT_TRUE(initState()); + ASSERT_TRUE(action_client->waitForServer(long_timeout)); + + // Send trajectory + traj_goal.trajectory.header.stamp = ros::Time(0); // Start immediately + action_client->sendGoal(traj_goal); + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::ACTIVE, short_timeout)); + + ros::Duration wait_duration = traj.points.front().time_from_start; + wait_duration.sleep(); // Wait until ~first waypoint + + // Send partial trajectory that preempts the previous one + traj_partial_goal.trajectory.header.stamp = ros::Time(0); // Start immediately + action_client2->sendGoal(traj_partial_goal); + ASSERT_TRUE(waitForState(action_client2, SimpleClientGoalState::ACTIVE, short_timeout)); + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::PREEMPTED, short_timeout)); + //ROS_ERROR_STREAM("state: "<< action_client->getState().toString()); + + // Wait until done + ASSERT_TRUE(waitForState(action_client2, SimpleClientGoalState::SUCCEEDED, long_timeout)); + ros::Duration(0.5).sleep(); // Allows values to settle to within EPS, especially accelerations + + // Check that we're at the original trajectory for joint1 and on the partial trajectory for joint2 + StateConstPtr state = getState(); + EXPECT_NEAR(traj.points.back().positions[0], state->desired.positions[0], EPS); + EXPECT_NEAR(traj.points.back().velocities[0], state->desired.velocities[0], EPS); + EXPECT_NEAR(traj.points.back().accelerations[0], state->desired.accelerations[0], EPS); + + EXPECT_NEAR(traj_partial.points.back().positions[0], state->desired.positions[1], EPS); + EXPECT_NEAR(traj_partial.points.back().velocities[0], state->desired.velocities[1], EPS); + EXPECT_NEAR(traj_partial.points.back().accelerations[0], state->desired.accelerations[1], EPS); +} + +TEST_F(JointPartialTrajectoryControllerTest, actionReplacesTopicTraj) +{ + ASSERT_TRUE(initState()); + ASSERT_TRUE(action_client->waitForServer(long_timeout)); + + // Send trajectory + traj.header.stamp = ros::Time(0); // Start immediately + traj_pub.publish(traj); + ros::Duration wait_duration = traj.points.front().time_from_start; + wait_duration.sleep(); // Wait until ~first waypoint + + // Send partial trajectory that preempts the previous one + traj_partial_goal.trajectory.header.stamp = ros::Time(0); // Start immediately + action_client->sendGoal(traj_partial_goal); + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::ACTIVE, short_timeout)); + + // Wait until done + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::SUCCEEDED, long_timeout)); + ros::Duration(0.5).sleep(); // Allows values to settle to within EPS, especially accelerations + + // Check that we're at the original trajectory for joint1 and on the partial trajectory for joint2 + StateConstPtr state = getState(); + EXPECT_NEAR(traj.points.back().positions[0], state->desired.positions[0], EPS); + EXPECT_NEAR(traj.points.back().velocities[0], state->desired.velocities[0], EPS); + EXPECT_NEAR(traj.points.back().accelerations[0], state->desired.accelerations[0], EPS); + + EXPECT_NEAR(traj_partial.points.back().positions[0], state->desired.positions[1], EPS); + EXPECT_NEAR(traj_partial.points.back().velocities[0], state->desired.velocities[1], EPS); + EXPECT_NEAR(traj_partial.points.back().accelerations[0], state->desired.accelerations[1], EPS); +} + +TEST_F(JointPartialTrajectoryControllerTest, topicReplacesActionTraj) +{ + ASSERT_TRUE(initState()); + ASSERT_TRUE(action_client->waitForServer(long_timeout)); + + // Send trajectory + traj_goal.trajectory.header.stamp = ros::Time(0); // Start immediately + action_client->sendGoal(traj_goal); + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::ACTIVE, short_timeout)); + + ros::Duration wait_duration = traj.points.front().time_from_start; + wait_duration.sleep(); // Wait until ~first waypoint + + // Send partial trajectory that preempts the previous one + traj_partial.header.stamp = ros::Time(0); // Start immediately + traj_pub.publish(traj_partial); + + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::PREEMPTED, short_timeout)); + + wait_duration = traj_partial.points.back().time_from_start + ros::Duration(0.5); + wait_duration.sleep(); // Wait until done + + // Check that we're at the original trajectory for joint1 and on the partial trajectory for joint2 + StateConstPtr state = getState(); + EXPECT_NEAR(traj.points.back().positions[0], state->desired.positions[0], EPS); + EXPECT_NEAR(traj.points.back().velocities[0], state->desired.velocities[0], EPS); + EXPECT_NEAR(traj.points.back().accelerations[0], state->desired.accelerations[0], EPS); + + EXPECT_NEAR(traj_partial.points.back().positions[0], state->desired.positions[1], EPS); + EXPECT_NEAR(traj_partial.points.back().velocities[0], state->desired.velocities[1], EPS); + EXPECT_NEAR(traj_partial.points.back().accelerations[0], state->desired.accelerations[1], EPS); +} + +// Ignore old trajectory points //////////////////////////////////////////////////////////////////////////////////////// + +TEST_F(JointPartialTrajectoryControllerTest, ignoreOldTopicTraj) +{ + ASSERT_TRUE(initState()); + + // Send trajectory + traj.header.stamp = ros::Time(0); // Start immediately + traj_pub.publish(traj); + ros::Duration wait_duration = traj.points.front().time_from_start; + wait_duration.sleep(); // Wait until ~first waypoint + + // Send partial trajectory with all points occuring in the past. Should not preempts the previous one + traj_partial.header.stamp = ros::Time::now() - traj_partial.points.back().time_from_start - ros::Duration(0.5); + traj_pub.publish(traj_partial); + + wait_duration = traj.points.back().time_from_start - traj.points.front().time_from_start + ros::Duration(0.5); + wait_duration.sleep(); // Wait until first trajectory is done + + // Check that we're at the original trajectory end (Joint2 has not moved according to traj_partial) + StateConstPtr state = getState(); + for (unsigned int i = 0; i < n_joints; ++i) + { + EXPECT_NEAR(traj.points.back().positions[i], state->desired.positions[i], EPS); + EXPECT_NEAR(traj.points.back().velocities[i], state->desired.velocities[i], EPS); + EXPECT_NEAR(traj.points.back().accelerations[i], state->desired.accelerations[i], EPS); + } +} + +TEST_F(JointPartialTrajectoryControllerTest, ignoreOldActionTraj) +{ + ASSERT_TRUE(initState()); + ASSERT_TRUE(action_client->waitForServer(long_timeout)); + + // Send trajectory + traj_goal.trajectory.header.stamp = ros::Time(0); // Start immediately + action_client->sendGoal(traj_goal); + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::ACTIVE, short_timeout)); + + ros::Duration wait_duration = traj.points.front().time_from_start; + wait_duration.sleep(); // Wait until ~first waypoint + + // Send partial trajectory with all points occuring in the past. Should not preempts the previous one + traj_partial_goal.trajectory.header.stamp = ros::Time::now() - traj_partial.points.back().time_from_start - ros::Duration(0.5); + action_client2->sendGoal(traj_partial_goal); + ASSERT_TRUE(waitForState(action_client2, SimpleClientGoalState::REJECTED, short_timeout)); + + // Wait until done + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::SUCCEEDED, long_timeout)); + ros::Duration(0.5).sleep(); // Allows values to settle to within EPS, especially accelerations + + // Check that we're at the original trajectory end (Joint2 has not moved according to traj_partial) + StateConstPtr state = getState(); + for (unsigned int i = 0; i < n_joints; ++i) + { + EXPECT_NEAR(traj.points.back().positions[i], state->desired.positions[i], EPS); + EXPECT_NEAR(traj.points.back().velocities[i], state->desired.velocities[i], EPS); + EXPECT_NEAR(traj.points.back().accelerations[i], state->desired.accelerations[i], EPS); + } +} + +TEST_F(JointPartialTrajectoryControllerTest, ignorePartiallyOldActionTraj) +{ + ASSERT_TRUE(initState()); + ASSERT_TRUE(action_client->waitForServer(long_timeout)); + + // Send trajectory + ActionGoal first_goal = traj_goal; + first_goal.trajectory.header.stamp = ros::Time(0); // Start immediately + action_client->sendGoal(first_goal); + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::ACTIVE, short_timeout)); + + ros::Duration wait_duration = first_goal.trajectory.points.front().time_from_start; + wait_duration.sleep(); // Wait until ~first waypoint + + // Send partial trajectory with only the last point not occuring in the past. Only the last point should be executed + traj_partial_goal.trajectory.header.stamp = ros::Time::now() - traj.points.back().time_from_start + ros::Duration(1.0); + action_client2->sendGoal(traj_partial_goal); + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::PREEMPTED, short_timeout)); + ASSERT_TRUE(waitForState(action_client2, SimpleClientGoalState::ACTIVE, short_timeout)); + + // Wait until done + ASSERT_TRUE(waitForState(action_client2, SimpleClientGoalState::SUCCEEDED, long_timeout)); + ros::Duration(0.5).sleep(); // Allows values to settle to within EPS, especially accelerations + + // Check that we're at the original trajectory for joint1 and on the partial trajectory for joint2 + StateConstPtr state = getState(); + EXPECT_NEAR(traj.points.back().positions[0], state->desired.positions[0], EPS); + EXPECT_NEAR(traj.points.back().velocities[0], state->desired.velocities[0], EPS); + EXPECT_NEAR(traj.points.back().accelerations[0], state->desired.accelerations[0], EPS); + + EXPECT_NEAR(traj_partial.points.back().positions[0], state->desired.positions[1], EPS); + EXPECT_NEAR(traj_partial.points.back().velocities[0], state->desired.velocities[1], EPS); + EXPECT_NEAR(traj_partial.points.back().accelerations[0], state->desired.accelerations[1], EPS); +} + +TEST_F(JointPartialTrajectoryControllerTest, executeParitalActionTrajInFuture) +{ + ASSERT_TRUE(initState()); + ASSERT_TRUE(action_client->waitForServer(long_timeout)); + + // Send trajectory + ActionGoal first_goal = traj_goal; + first_goal.trajectory.header.stamp = ros::Time(0); // Start immediately + action_client->sendGoal(first_goal); + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::ACTIVE, short_timeout)); + + ros::Duration wait_duration = first_goal.trajectory.points.front().time_from_start; + wait_duration.sleep(); // Wait until ~first waypoint + + // Send partial trajectory with only the last point not occuring in the past. Only the last point should be executed + traj_partial_goal.trajectory.header.stamp = ros::Time::now() + traj.points.back().time_from_start + ros::Duration(2.0); + action_client2->sendGoal(traj_partial_goal); + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::PREEMPTED, short_timeout)); + ASSERT_TRUE(waitForState(action_client2, SimpleClientGoalState::ACTIVE, short_timeout)); + + // Wait until first traj is done + wait_duration = first_goal.trajectory.points.back().time_from_start; + wait_duration.sleep(); // Wait until ~first waypoint + + StateConstPtr state = getState(); + for (unsigned int i = 0; i < n_joints; ++i) + { + EXPECT_NEAR(traj.points.back().positions[i], state->desired.positions[i], EPS); + EXPECT_NEAR(traj.points.back().velocities[i], state->desired.velocities[i], EPS); + EXPECT_NEAR(traj.points.back().accelerations[i], state->desired.accelerations[i], EPS); + } + + // Wait until done + ASSERT_TRUE(waitForState(action_client2, SimpleClientGoalState::SUCCEEDED, long_timeout)); + ros::Duration(0.5).sleep(); // Allows values to settle to within EPS, especially accelerations + + // Check that we're at the original trajectory for joint1 and on the partial trajectory for joint2 + state = getState(); + EXPECT_NEAR(traj.points.back().positions[0], state->desired.positions[0], EPS); + EXPECT_NEAR(traj.points.back().velocities[0], state->desired.velocities[0], EPS); + EXPECT_NEAR(traj.points.back().accelerations[0], state->desired.accelerations[0], EPS); + + EXPECT_NEAR(traj_partial.points.back().positions[0], state->desired.positions[1], EPS); + EXPECT_NEAR(traj_partial.points.back().velocities[0], state->desired.velocities[1], EPS); + EXPECT_NEAR(traj_partial.points.back().accelerations[0], state->desired.accelerations[1], EPS); +} + + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "joint_partial_trajectory_controller_test"); + + ros::AsyncSpinner spinner(1); + spinner.start(); + int ret = RUN_ALL_TESTS(); + spinner.stop(); + ros::shutdown(); + return ret; +} diff --git a/joint_trajectory_controller/test/joint_trajectory_controller_test.cpp b/joint_trajectory_controller/test/joint_trajectory_controller_test.cpp index bb357c5ba..7dfd1ea48 100644 --- a/joint_trajectory_controller/test/joint_trajectory_controller_test.cpp +++ b/joint_trajectory_controller/test/joint_trajectory_controller_test.cpp @@ -248,7 +248,7 @@ TEST_F(JointTrajectoryControllerTest, invalidMessages) ASSERT_TRUE(initState()); ASSERT_TRUE(action_client->waitForServer(long_timeout)); - // Invalid size + // Invalid size (No partial joints goals allowed) { trajectory_msgs::JointTrajectoryPoint point; point.positions.resize(1, 0.0); diff --git a/joint_trajectory_controller/test/joint_trajectory_segment_test.cpp b/joint_trajectory_controller/test/joint_trajectory_segment_test.cpp index 546dec449..14ac71cd0 100644 --- a/joint_trajectory_controller/test/joint_trajectory_segment_test.cpp +++ b/joint_trajectory_controller/test/joint_trajectory_segment_test.cpp @@ -41,7 +41,7 @@ using namespace trajectory_msgs; const double EPS = 1e-9; typedef JointTrajectorySegment > Segment; -typedef std::vector PermutationType; +typedef std::vector MappingType; TEST(WraparoundOffsetTest, WrappingPositions) { @@ -60,10 +60,10 @@ TEST(WraparoundOffsetTest, WrappingPositions) // No wrapping joints { std::vector angle_wraparound(2, false); - std::vector wraparound_offset = wraparoundOffset(pos1, pos2, angle_wraparound); - EXPECT_NEAR(pos1.size(), wraparound_offset.size(), EPS); - EXPECT_NEAR(0.0, wraparound_offset[0], EPS); - EXPECT_NEAR(0.0, wraparound_offset[1], EPS); + double wraparound_offset1 = wraparoundJointOffset(pos1[0], pos2[0], angle_wraparound[0]); + double wraparound_offset2 = wraparoundJointOffset(pos1[1], pos2[1], angle_wraparound[1]); + EXPECT_NEAR(0.0, wraparound_offset1, EPS); + EXPECT_NEAR(0.0, wraparound_offset2, EPS); } // Single wrapping joint @@ -74,18 +74,18 @@ TEST(WraparoundOffsetTest, WrappingPositions) // From state1 to state2 { - std::vector wraparound_offset = wraparoundOffset(pos1, pos2, angle_wraparound); - EXPECT_NEAR(pos1.size(), wraparound_offset.size(), EPS); - EXPECT_NEAR(-two_pi, wraparound_offset[0], EPS); - EXPECT_NEAR(0.0, wraparound_offset[1], EPS); + double wraparound_offset1 = wraparoundJointOffset(pos1[0], pos2[0], angle_wraparound[0]); + double wraparound_offset2 = wraparoundJointOffset(pos1[1], pos2[1], angle_wraparound[1]); + EXPECT_NEAR(-two_pi, wraparound_offset1, EPS); + EXPECT_NEAR(0.0, wraparound_offset2, EPS); } // From state2 to state1 { - std::vector wraparound_offset = wraparoundOffset(pos2, pos1, angle_wraparound); - EXPECT_NEAR(pos1.size(), wraparound_offset.size(), EPS); - EXPECT_NEAR(two_pi, wraparound_offset[0], EPS); - EXPECT_NEAR(0.0, wraparound_offset[1], EPS); + double wraparound_offset1 = wraparoundJointOffset(pos2[0], pos1[0], angle_wraparound[0]); + double wraparound_offset2 = wraparoundJointOffset(pos2[1], pos1[1], angle_wraparound[1]); + EXPECT_NEAR(two_pi, wraparound_offset1, EPS); + EXPECT_NEAR(0.0, wraparound_offset2, EPS); } } @@ -95,18 +95,18 @@ TEST(WraparoundOffsetTest, WrappingPositions) // From state1 to state2 { - std::vector wraparound_offset = wraparoundOffset(pos1, pos2, angle_wraparound); - EXPECT_NEAR(pos1.size(), wraparound_offset.size(), EPS); - EXPECT_NEAR(-two_pi, wraparound_offset[0], EPS); - EXPECT_NEAR(-2.0 * two_pi, wraparound_offset[1], EPS); + double wraparound_offset1 = wraparoundJointOffset(pos1[0], pos2[0], angle_wraparound[0]); + double wraparound_offset2 = wraparoundJointOffset(pos1[1], pos2[1], angle_wraparound[1]); + EXPECT_NEAR(-two_pi, wraparound_offset1, EPS); + EXPECT_NEAR(-2.0 * two_pi, wraparound_offset2, EPS); } // From state2 to state1 { - std::vector wraparound_offset = wraparoundOffset(pos2, pos1, angle_wraparound); - EXPECT_NEAR(pos1.size(), wraparound_offset.size(), EPS); - EXPECT_NEAR(two_pi, wraparound_offset[0], EPS); - EXPECT_NEAR(2.0 * two_pi, wraparound_offset[1], EPS); + double wraparound_offset1 = wraparoundJointOffset(pos2[0], pos1[0], angle_wraparound[0]); + double wraparound_offset2 = wraparoundJointOffset(pos2[1], pos1[1], angle_wraparound[1]); + EXPECT_NEAR(two_pi, wraparound_offset1, EPS); + EXPECT_NEAR(2.0 * two_pi, wraparound_offset2, EPS); } } } @@ -122,16 +122,14 @@ TEST(WraparoundOffsetTest, WrappingPositionsPiSingularity) std::vector angle_wraparound(1, true); // From state1 to state2 { - std::vector wraparound_offset = wraparoundOffset(pos1, pos2, angle_wraparound); - EXPECT_EQ(pos1.size(), wraparound_offset.size()); - EXPECT_NEAR(0.0, wraparound_offset[0], EPS); + double wraparound_offset1 = wraparoundJointOffset(pos1[0], pos2[0], angle_wraparound[0]); + EXPECT_NEAR(0.0, wraparound_offset1, EPS); } // From state2 to state1 { - std::vector wraparound_offset = wraparoundOffset(pos2, pos1, angle_wraparound); - EXPECT_EQ(pos1.size(), wraparound_offset.size()); - EXPECT_NEAR(0.0, wraparound_offset[0], EPS); + double wraparound_offset1 = wraparoundJointOffset(pos2[0], pos1[0], angle_wraparound[0]); + EXPECT_NEAR(0.0, wraparound_offset1, EPS); } } @@ -154,18 +152,18 @@ TEST(WraparoundOffsetTest, NonWrappingPositions) // From state1 to state2 { - std::vector wraparound_offset = wraparoundOffset(pos1, pos2, angle_wraparound); - EXPECT_NEAR(pos1.size(), wraparound_offset.size(), EPS); - EXPECT_NEAR(0.0, wraparound_offset[0], EPS); - EXPECT_NEAR(0.0, wraparound_offset[1], EPS); + double wraparound_offset1 = wraparoundJointOffset(pos1[0], pos2[0], angle_wraparound[0]); + double wraparound_offset2 = wraparoundJointOffset(pos1[1], pos2[1], angle_wraparound[1]); + EXPECT_NEAR(0.0, wraparound_offset1, EPS); + EXPECT_NEAR(0.0, wraparound_offset2, EPS); } // From state2 to state1 { - std::vector wraparound_offset = wraparoundOffset(pos2, pos1, angle_wraparound); - EXPECT_NEAR(pos1.size(), wraparound_offset.size(), EPS); - EXPECT_NEAR(0.0, wraparound_offset[0], EPS); - EXPECT_NEAR(0.0, wraparound_offset[1], EPS); + double wraparound_offset1 = wraparoundJointOffset(pos2[0], pos1[0], angle_wraparound[0]); + double wraparound_offset2 = wraparoundJointOffset(pos2[1], pos1[1], angle_wraparound[1]); + EXPECT_NEAR(0.0, wraparound_offset1, EPS); + EXPECT_NEAR(0.0, wraparound_offset2, EPS); } } } @@ -239,28 +237,11 @@ TEST_F(JointTrajectorySegmentTest, InvalidSegmentConstruction) catch (const std::invalid_argument& ex) {ROS_ERROR_STREAM(ex.what());} } - // Invalid permutation vector size - { - PermutationType permutation(2, 1); - EXPECT_THROW(Segment(traj_start_time, p_start, p_end, permutation), std::invalid_argument); - try {Segment(traj_start_time, p_start, p_end, permutation);} - catch (const std::invalid_argument& ex) {ROS_ERROR_STREAM(ex.what());} - } - - // Invalid permutation vector indices - { - PermutationType permutation(1, 1); - EXPECT_THROW(Segment(traj_start_time, p_start, p_end, permutation), std::invalid_argument); - try {Segment(traj_start_time, p_start, p_end, permutation);} - catch (const std::invalid_argument& ex) {ROS_ERROR_STREAM(ex.what());} - } - // Invalid joint wraparound specification { - PermutationType permutation; std::vector pos_offset(2); - EXPECT_THROW(Segment(traj_start_time, p_start, p_end, permutation, pos_offset), std::invalid_argument); - try {Segment(traj_start_time, p_start, p_end, permutation, pos_offset);} + EXPECT_THROW(Segment(traj_start_time, p_start, p_end, pos_offset), std::invalid_argument); + try {Segment(traj_start_time, p_start, p_end, pos_offset);} catch (const std::invalid_argument& ex) {ROS_ERROR_STREAM(ex.what());} } } @@ -372,7 +353,7 @@ TEST_F(JointTrajectorySegmentTest, CrossValidateSegmentConstruction) } // TODO: Test with dimension four data -TEST_F(JointTrajectorySegmentTest, PermutationTest) +TEST_F(JointTrajectorySegmentTest, MappingTest) { // Add an extra joint to the trajectory point messages created in the fixture p_start.positions.push_back(-p_start.positions[0]); @@ -381,7 +362,7 @@ TEST_F(JointTrajectorySegmentTest, PermutationTest) p_end.positions.push_back(-p_end.positions[0]); p_end.velocities.push_back(-p_end.velocities[0]); - // No permutation vector + // No mapping vector { // Construct segment from ROS message EXPECT_NO_THROW(Segment(traj_start_time, p_start, p_end)); @@ -393,42 +374,6 @@ TEST_F(JointTrajectorySegmentTest, PermutationTest) EXPECT_EQ(p_start.positions[0], state.position[0]); EXPECT_EQ(p_start.positions[1], state.position[1]); } - - // Permutation vector preserving trajectory message joint order - { - PermutationType permutation(2); - permutation[0] = 0; - permutation[1] = 1; - - // Construct segment from ROS message - EXPECT_NO_THROW(Segment(traj_start_time, p_start, p_end, permutation)); - Segment segment(traj_start_time, p_start, p_end, permutation); - - // Check position values of start state only - typename Segment::State state; - segment.sample(segment.startTime(), state); - EXPECT_EQ(p_start.positions[0], state.position[0]); - EXPECT_EQ(p_start.positions[1], state.position[1]); - } - - // Permutation vector reversing trajectory message joint order - { - PermutationType permutation(2); - permutation[0] = 1; - permutation[1] = 0; - - // Construct segment from ROS message - EXPECT_NO_THROW(Segment(traj_start_time, p_start, p_end, permutation)); - Segment segment(traj_start_time, p_start, p_end, permutation); - - // Check position values of start state only - typename Segment::State state; - segment.sample(segment.startTime(), state); - EXPECT_NE(p_start.positions[0], state.position[0]); - EXPECT_NE(p_start.positions[1], state.position[1]); - EXPECT_EQ(p_start.positions[0], state.position[1]); - EXPECT_EQ(p_start.positions[1], state.position[0]); - } } int main(int argc, char** argv) diff --git a/joint_trajectory_controller/test/rrbot_partial_controllers.yaml b/joint_trajectory_controller/test/rrbot_partial_controllers.yaml new file mode 100644 index 000000000..e7a319544 --- /dev/null +++ b/joint_trajectory_controller/test/rrbot_partial_controllers.yaml @@ -0,0 +1,15 @@ +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 + allow_partial_joints_goal: true From c2a903eef2b2c88f7b9a08b1f267a7314ea1de15 Mon Sep 17 00:00:00 2001 From: Beatriz Leon Date: Mon, 31 Oct 2016 09:24:12 -0500 Subject: [PATCH 3/3] Updated copyright info --- .../test/joint_partial_trajectory_controller_test.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/joint_trajectory_controller/test/joint_partial_trajectory_controller_test.cpp b/joint_trajectory_controller/test/joint_partial_trajectory_controller_test.cpp index 270cfb48a..ad45a717d 100644 --- a/joint_trajectory_controller/test/joint_partial_trajectory_controller_test.cpp +++ b/joint_trajectory_controller/test/joint_partial_trajectory_controller_test.cpp @@ -1,5 +1,5 @@ /////////////////////////////////////////////////////////////////////////////// -// Copyright (C) 2013, PAL Robotics S.L. +// Copyright (C) 2016, Shadow Robot Company Ltd. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: @@ -25,7 +25,7 @@ // POSSIBILITY OF SUCH DAMAGE. ////////////////////////////////////////////////////////////////////////////// -/// \author Adolfo Rodriguez Tsouroukdissian +/// \author Beatriz Leon #include #include