diff --git a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h index af1e92db212..86047586fe8 100644 --- a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h +++ b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h @@ -265,6 +265,9 @@ class TrajectoryExecutionManager void loadControllerParams(); + double getJointAllowedStartTolerance(std::string const& jointName) const; + void updateJointsAllowedStartTolerance(); + moveit::core::RobotModelConstPtr robot_model_; planning_scene_monitor::CurrentStateMonitorPtr csm_; ros::NodeHandle node_handle_; @@ -310,6 +313,8 @@ class TrajectoryExecutionManager std::map controller_allowed_goal_duration_margin_; double allowed_start_tolerance_; // joint tolerance for validate(): radians for revolute joints + // tolerance per joint, overrides global allowed_start_tolerance_. + std::map joints_allowed_start_tolerance_; double execution_velocity_scaling_; bool wait_for_trajectory_completion_; }; diff --git a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp index 68970ce41f0..77442d1bd36 100644 --- a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp +++ b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp @@ -116,6 +116,7 @@ void TrajectoryExecutionManager::initialize() execution_duration_monitoring_ = true; execution_velocity_scaling_ = 1.0; allowed_start_tolerance_ = 0.01; + joints_allowed_start_tolerance_.clear(); allowed_execution_duration_scaling_ = DEFAULT_CONTROLLER_GOAL_DURATION_SCALING; allowed_goal_duration_margin_ = DEFAULT_CONTROLLER_GOAL_DURATION_MARGIN; @@ -176,6 +177,8 @@ void TrajectoryExecutionManager::initialize() reconfigure_impl_ = new DynamicReconfigureImpl(this); + updateJointsAllowedStartTolerance(); + if (manage_controllers_) ROS_INFO_NAMED(LOGNAME, "Trajectory execution is managing controllers"); else @@ -717,7 +720,7 @@ bool TrajectoryExecutionManager::distributeTrajectory(const moveit_msgs::RobotTr bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& context) const { - if (allowed_start_tolerance_ == 0) // skip validation on this magic number + if (allowed_start_tolerance_ == 0 && joints_allowed_start_tolerance_.empty()) // skip validation on this magic number return true; ROS_DEBUG_NAMED(LOGNAME, "Validating trajectory with allowed_start_tolerance %g", allowed_start_tolerance_); @@ -754,15 +757,16 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont double cur_position = current_state->getJointPositions(jm)[0]; double traj_position = positions[i]; + double joint_start_tolerance = getJointAllowedStartTolerance(joint_names[i]); // normalize positions and compare jm->enforcePositionBounds(&cur_position); jm->enforcePositionBounds(&traj_position); - if (jm->distance(&cur_position, &traj_position) > allowed_start_tolerance_) + if (joint_start_tolerance != 0 && jm->distance(&cur_position, &traj_position) > joint_start_tolerance) { ROS_ERROR_NAMED(LOGNAME, "\nInvalid Trajectory: start point deviates from current robot state more than %g" "\njoint '%s': expected: %g, current: %g", - allowed_start_tolerance_, joint_names[i].c_str(), traj_position, cur_position); + joint_start_tolerance, joint_names[i].c_str(), traj_position, cur_position); return false; } } @@ -799,11 +803,13 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont Eigen::Vector3d offset = cur_transform.translation() - start_transform.translation(); Eigen::AngleAxisd rotation; rotation.fromRotationMatrix(cur_transform.linear().transpose() * start_transform.linear()); - if ((offset.array() > allowed_start_tolerance_).any() || rotation.angle() > allowed_start_tolerance_) + double joint_start_tolerance = getJointAllowedStartTolerance(joint_names[i]); + if (joint_start_tolerance != 0 && + ((offset.array() > joint_start_tolerance).any() || rotation.angle() > joint_start_tolerance)) { ROS_ERROR_STREAM_NAMED(LOGNAME, "\nInvalid Trajectory: start point deviates from current robot state more than " - << allowed_start_tolerance_ << "\nmulti-dof joint '" << joint_names[i] + << joint_start_tolerance << "\nmulti-dof joint '" << joint_names[i] << "': pos delta: " << offset.transpose() << " rot delta: " << rotation.angle()); return false; } @@ -1003,6 +1009,8 @@ void TrajectoryExecutionManager::execute(const ExecutionCompleteCallback& callba stopExecution(false); + updateJointsAllowedStartTolerance(); + // check whether first trajectory starts at current robot state if (!trajectories_.empty() && !validate(*trajectories_.front())) { @@ -1316,7 +1324,7 @@ bool TrajectoryExecutionManager::executePart(std::size_t part_index) bool TrajectoryExecutionManager::waitForRobotToStop(const TrajectoryExecutionContext& context, double wait_time) { // skip waiting for convergence? - if (allowed_start_tolerance_ == 0 || !wait_for_trajectory_completion_) + if ((allowed_start_tolerance_ == 0 && joints_allowed_start_tolerance_.empty()) || !wait_for_trajectory_completion_) { ROS_DEBUG_NAMED(LOGNAME, "Not waiting for trajectory completion"); return true; @@ -1354,7 +1362,8 @@ bool TrajectoryExecutionManager::waitForRobotToStop(const TrajectoryExecutionCon if (!jm) continue; // joint vanished from robot state (shouldn't happen), but we don't care - if (fabs(cur_state->getJointPositions(jm)[0] - prev_state->getJointPositions(jm)[0]) > allowed_start_tolerance_) + double joint_tolerance = getJointAllowedStartTolerance(joint_names[i]); + if (fabs(cur_state->getJointPositions(jm)[0] - prev_state->getJointPositions(jm)[0]) > joint_tolerance) { moved = true; no_motion_count = 0; @@ -1554,4 +1563,27 @@ void TrajectoryExecutionManager::loadControllerParams() } } } + +double TrajectoryExecutionManager::getJointAllowedStartTolerance(std::string const& jointName) const +{ + auto start_tolerance_it = joints_allowed_start_tolerance_.find(jointName); + return start_tolerance_it != joints_allowed_start_tolerance_.end() ? start_tolerance_it->second : + allowed_start_tolerance_; +} + +void TrajectoryExecutionManager::updateJointsAllowedStartTolerance() +{ + joints_allowed_start_tolerance_.clear(); + node_handle_.getParam("trajectory_execution/joints_allowed_start_tolerance", joints_allowed_start_tolerance_); + + // remove negative values + for (auto it = joints_allowed_start_tolerance_.begin(); it != joints_allowed_start_tolerance_.end();) + { + if (it->second < 0) + it = joints_allowed_start_tolerance_.erase(it); + else + ++it; + } +} + } // namespace trajectory_execution_manager diff --git a/moveit_ros/planning/trajectory_execution_manager/test/test_execution_manager.cpp b/moveit_ros/planning/trajectory_execution_manager/test/test_execution_manager.cpp index f6a7683f2b1..21097bbf864 100644 --- a/moveit_ros/planning/trajectory_execution_manager/test/test_execution_manager.cpp +++ b/moveit_ros/planning/trajectory_execution_manager/test/test_execution_manager.cpp @@ -60,15 +60,21 @@ class MoveItCppTest : public ::testing::Test trajectory_execution_manager_ptr = moveit_cpp_ptr->getTrajectoryExecutionManagerNonConst(); traj1.joint_trajectory.joint_names.push_back("panda_joint1"); - traj1.joint_trajectory.points.resize(1); + traj1.joint_trajectory.points.resize(2); traj1.joint_trajectory.points[0].positions.push_back(0.0); + traj1.joint_trajectory.points[1].positions.push_back(0.5); + traj1.joint_trajectory.points[1].time_from_start.fromSec(0.5); traj2 = traj1; traj2.joint_trajectory.joint_names.push_back("panda_joint2"); traj2.joint_trajectory.points[0].positions.push_back(1.0); + traj2.joint_trajectory.points[1].positions.push_back(1.5); traj2.multi_dof_joint_trajectory.joint_names.push_back("panda_joint3"); - traj2.multi_dof_joint_trajectory.points.resize(1); + traj2.multi_dof_joint_trajectory.points.resize(2); traj2.multi_dof_joint_trajectory.points[0].transforms.resize(1); + traj2.multi_dof_joint_trajectory.points[1].transforms.resize(1); + + ros::param::del("~/trajectory_execution/joints_allowed_start_tolerance"); } protected: @@ -108,6 +114,50 @@ TEST_F(MoveItCppTest, PushExecuteAndWaitTest) ASSERT_EQ(last_execution_status, moveit_controller_manager::ExecutionStatus::SUCCEEDED); } +TEST_F(MoveItCppTest, RejectTooFarFromStart) +{ + moveit_msgs::RobotTrajectory traj = traj1; + traj.joint_trajectory.points[0].positions[0] = 0.3; + + trajectory_execution_manager_ptr->setAllowedStartTolerance(0.01); + ASSERT_TRUE(trajectory_execution_manager_ptr->push(traj)); + auto last_execution_status = trajectory_execution_manager_ptr->executeAndWait(); + ASSERT_EQ(last_execution_status, moveit_controller_manager::ExecutionStatus::ABORTED); +} + +TEST_F(MoveItCppTest, AcceptAllowedJointStartTolerance) +{ + moveit_msgs::RobotTrajectory traj = traj1; + traj.joint_trajectory.points[0].positions[0] = 0.3; + + trajectory_execution_manager_ptr->setAllowedStartTolerance(0.01); + ros::param::set("~/trajectory_execution/joints_allowed_start_tolerance/panda_joint1", 0.5); + ASSERT_TRUE(trajectory_execution_manager_ptr->push(traj)); + auto last_execution_status = trajectory_execution_manager_ptr->executeAndWait(); + ASSERT_EQ(last_execution_status, moveit_controller_manager::ExecutionStatus::SUCCEEDED); +} + +TEST_F(MoveItCppTest, DoNotValidateJointStartToleranceZero) +{ + moveit_msgs::RobotTrajectory traj = traj1; + traj.joint_trajectory.points[0].positions[0] = 0.3; + + trajectory_execution_manager_ptr->setAllowedStartTolerance(0); + ASSERT_TRUE(trajectory_execution_manager_ptr->push(traj)); + auto last_execution_status = trajectory_execution_manager_ptr->executeAndWait(); + ASSERT_EQ(last_execution_status, moveit_controller_manager::ExecutionStatus::SUCCEEDED); + + trajectory_execution_manager_ptr->setAllowedStartTolerance(0.1); + ASSERT_TRUE(trajectory_execution_manager_ptr->push(traj)); + last_execution_status = trajectory_execution_manager_ptr->executeAndWait(); + ASSERT_EQ(last_execution_status, moveit_controller_manager::ExecutionStatus::ABORTED); + + ros::param::set("~/trajectory_execution/joints_allowed_start_tolerance/panda_joint1", 0); + ASSERT_TRUE(trajectory_execution_manager_ptr->push(traj)); + last_execution_status = trajectory_execution_manager_ptr->executeAndWait(); + ASSERT_EQ(last_execution_status, moveit_controller_manager::ExecutionStatus::SUCCEEDED); +} + } // namespace moveit_cpp int main(int argc, char** argv)