Skip to content

Commit

Permalink
Add allowed_start_tolerance_joints parameter (Port #3287) (#3309) (#3328
Browse files Browse the repository at this point in the history 
)

* Add joints_allowed_start_tolerance parameter (moveit/moveit#3287)
... for joint-specific start tolerances for TrajectoryExecutionManager

Co-authored-by: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>

* Port to moveit2

* Allow joints_allowed_start_tolerance to be updated dynamically

* Apply formatting

* Rename joints_allowed_start_tolerance to allowed_start_tolerance_joints

---------

Co-authored-by: Hugal31 <hugo.laloge@gmail.com>
Co-authored-by: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
(cherry picked from commit 9922704)

Co-authored-by: Daniel García López <d.garcialopez@hotmail.com>
  • Loading branch information
@mergify @DaniGarciaLopez
mergify[bot] and DaniGarciaLopez authored Feb 6, 2025
1 parent ddfab7d commit 6c5e6ec
Show file tree
Hide file tree
Showing 3 changed files with 154 additions and 10 deletions.
6 changes: 6 additions & 0 deletions ...tion_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -292,6 +292,10 @@ class MOVEIT_TRAJECTORY_EXECUTION_MANAGER_EXPORT TrajectoryExecutionManager

void loadControllerParams();

double getAllowedStartToleranceJoint(const std::string& joint_name) const;
void setAllowedStartToleranceJoint(const std::string& joint_name, double joint_start_tolerance);
void initializeAllowedStartToleranceJoints();

// Name of this class for logging
const std::string name_ = "trajectory_execution_manager";

Expand Down Expand Up @@ -340,6 +344,8 @@ class MOVEIT_TRAJECTORY_EXECUTION_MANAGER_EXPORT TrajectoryExecutionManager
std::map<std::string, double> 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<std::string, double> allowed_start_tolerance_joints_;
double execution_velocity_scaling_;
bool wait_for_trajectory_completion_;

Expand Down
104 changes: 96 additions & 8 deletions moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -99,6 +99,7 @@ void TrajectoryExecutionManager::initialize()
execution_duration_monitoring_ = true;
execution_velocity_scaling_ = 1.0;
allowed_start_tolerance_ = 0.01;
allowed_start_tolerance_joints_.clear();
wait_for_trajectory_completion_ = true;
control_multi_dof_joint_variables_ = DEFAULT_CONTROL_MULTI_DOF_JOINT_VARIABLES;

Expand Down Expand Up @@ -210,6 +211,8 @@ void TrajectoryExecutionManager::initialize()
controller_mgr_node_->get_parameter("trajectory_execution.control_multi_dof_joint_variables",
control_multi_dof_joint_variables_);

initializeAllowedStartToleranceJoints();

if (manage_controllers_)
{
RCLCPP_INFO(logger_, "Trajectory execution is managing controllers");
Expand Down Expand Up @@ -245,6 +248,10 @@ void TrajectoryExecutionManager::initialize()
{
setAllowedStartTolerance(parameter.as_double());
}
else if (name.find("trajectory_execution.allowed_start_tolerance_joints.") == 0)
{
setAllowedStartToleranceJoint(name, parameter.as_double());
}
else if (name == "trajectory_execution.wait_for_trajectory_completion")
{
setWaitForTrajectoryCompletion(parameter.as_bool());
Expand Down Expand Up @@ -914,10 +921,26 @@ bool TrajectoryExecutionManager::distributeTrajectory(const moveit_msgs::msg::Ro

bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& context) const
{
if (allowed_start_tolerance_ == 0) // skip validation on this magic number
if (allowed_start_tolerance_ == 0 && allowed_start_tolerance_joints_.empty()) // skip validation on this magic number
return true;

RCLCPP_INFO(logger_, "Validating trajectory with allowed_start_tolerance %g", allowed_start_tolerance_);
if (allowed_start_tolerance_joints_.empty())
{
RCLCPP_INFO_STREAM(logger_, "Validating trajectory with allowed_start_tolerance " << allowed_start_tolerance_);
}
else
{
std::ostringstream ss;
for (const auto& [joint_name, joint_start_tolerance] : allowed_start_tolerance_joints_)
{
if (ss.tellp() > 1)
ss << ", "; // skip the comma at the end
ss << joint_name << ": " << joint_start_tolerance;
}
RCLCPP_INFO_STREAM(logger_, "Validating trajectory with allowed_start_tolerance "
<< allowed_start_tolerance_ << " and allowed_start_tolerance_joints {" << ss.str()
<< "}");
}

moveit::core::RobotStatePtr current_state;
if (!csm_->waitForCurrentState(node_->now()) || !(current_state = csm_->getCurrentState()))
Expand Down Expand Up @@ -960,14 +983,15 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont

for (const auto joint : joints)
{
const double joint_start_tolerance = getAllowedStartToleranceJoint(joint->getName());
reference_state.enforcePositionBounds(joint);
current_state->enforcePositionBounds(joint);
if (reference_state.distance(*current_state, joint) > allowed_start_tolerance_)
if (joint_start_tolerance != 0 && reference_state.distance(*current_state, joint) > joint_start_tolerance)
{
RCLCPP_ERROR(logger_,
"Invalid Trajectory: start point deviates from current robot state more than %g at joint '%s'."
"\nEnable DEBUG for detailed state info.",
allowed_start_tolerance_, joint->getName().c_str());
joint_start_tolerance, joint->getName().c_str());
RCLCPP_DEBUG(logger_, "| Joint | Expected | Current |");
for (const auto& joint_name : joint_names)
{
Expand Down Expand Up @@ -1012,10 +1036,12 @@ 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_)
const double joint_start_tolerance = getAllowedStartToleranceJoint(joint_names[i]);
if (joint_start_tolerance != 0 &&
((offset.array() > joint_start_tolerance).any() || rotation.angle() > joint_start_tolerance))
{
RCLCPP_ERROR_STREAM(logger_, "\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;
Expand Down Expand Up @@ -1568,7 +1594,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 && allowed_start_tolerance_joints_.empty()) || !wait_for_trajectory_completion_)
{
RCLCPP_INFO(logger_, "Not waiting for trajectory completion");
return true;
Expand Down Expand Up @@ -1607,8 +1633,9 @@ bool TrajectoryExecutionManager::waitForRobotToStop(const TrajectoryExecutionCon
if (!jm)
continue; // joint vanished from robot state (shouldn't happen), but we don't care

const double joint_start_tolerance = getAllowedStartToleranceJoint(joint_names[i]);
if (fabs(jm->distance(cur_state->getJointPositions(jm), prev_state->getJointPositions(jm))) >
allowed_start_tolerance_)
joint_start_tolerance)
{
moved = true;
no_motion_count = 0;
Expand Down Expand Up @@ -1834,4 +1861,65 @@ void TrajectoryExecutionManager::loadControllerParams()
// }
// }
}

double TrajectoryExecutionManager::getAllowedStartToleranceJoint(const std::string& joint_name) const
{
auto start_tolerance_it = allowed_start_tolerance_joints_.find(joint_name);
return start_tolerance_it != allowed_start_tolerance_joints_.end() ? start_tolerance_it->second :
allowed_start_tolerance_;
}

void TrajectoryExecutionManager::setAllowedStartToleranceJoint(const std::string& parameter_name,
double joint_start_tolerance)
{
if (joint_start_tolerance < 0)
{
RCLCPP_WARN(logger_, "%s has a negative value. The start tolerance value for that joint was not updated.",
parameter_name.c_str());
return;
}

// get the joint name by removing the parameter prefix if necessary
std::string joint_name = parameter_name;
const std::string parameter_prefix = "trajectory_execution.allowed_start_tolerance_joints.";
if (parameter_name.find(parameter_prefix) == 0)
joint_name = joint_name.substr(parameter_prefix.length()); // remove prefix

if (!robot_model_->hasJointModel(joint_name))
{
RCLCPP_WARN(logger_,
"Joint '%s' was not found in the robot model. "
"The start tolerance value for that joint was not updated.",
joint_name.c_str());
return;
}

allowed_start_tolerance_joints_.insert_or_assign(joint_name, joint_start_tolerance);
}

void TrajectoryExecutionManager::initializeAllowedStartToleranceJoints()
{
allowed_start_tolerance_joints_.clear();

// retrieve all parameters under "trajectory_execution.allowed_start_tolerance_joints"
// that correspond to existing joints in the robot model
for (const auto& joint_name : robot_model_->getJointModelNames())
{
double joint_start_tolerance;
const std::string parameter_name = "trajectory_execution.allowed_start_tolerance_joints." + joint_name;
if (node_->get_parameter(parameter_name, joint_start_tolerance))
{
if (joint_start_tolerance < 0)
{
RCLCPP_WARN(logger_,
"%s has a negative value. The start tolerance value for that joint "
"will default to allowed_start_tolerance.",
parameter_name.c_str());
continue;
}
allowed_start_tolerance_joints_.insert({ joint_name, joint_start_tolerance });
}
}
}

} // namespace trajectory_execution_manager
54 changes: 52 additions & 2 deletions moveit_ros/planning/trajectory_execution_manager/test/test_execution_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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/allowed_start_tolerance_joints");
}

protected:
Expand Down Expand Up @@ -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/allowed_start_tolerance_joints/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/allowed_start_tolerance_joints/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)
Expand Down

0 comments on commit 6c5e6ec

Please sign in to comment.