Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add allowed_start_tolerance_joints parameter (Port moveit#3287) #3309

Merged
merged 5 commits into from
Feb 6, 2025
Merged

Add allowed_start_tolerance_joints parameter (Port moveit#3287) #3309

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
eae07e5
Add joints_allowed_start_tolerance parameter (moveit/moveit#3287)
Hugal31 Feb 26, 2024
858442e
Port to moveit2
DaniGarciaLopez Feb 4, 2025
0fedc17
Allow joints_allowed_start_tolerance to be updated dynamically
DaniGarciaLopez Feb 4, 2025
274b1bd
Apply formatting
DaniGarciaLopez Feb 4, 2025
7dc0087
Rename joints_allowed_start_tolerance to allowed_start_tolerance_joints
DaniGarciaLopez Feb 5, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 6 additions & 0 deletions ...tion_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.hpp
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Loading