Skip to content

Commit

Permalink
Add joints_allowed_start_tolerance parameter (moveit#3287)
Browse files Browse the repository at this point in the history 
... for joint-specific start tolerances for TrajectoryExecutionManager

Co-authored-by: Robert Haschke <[email protected]>
  • Loading branch information
@Hugal31 @rhaschke
Hugal31 and rhaschke authored Feb 26, 2024
1 parent f0626e2 commit 76f0009
Show file tree
Hide file tree
Showing 3 changed files with 96 additions and 9 deletions.
5 changes: 5 additions & 0 deletions ...cution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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_;
Expand Down Expand Up @@ -310,6 +313,8 @@ class 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> joints_allowed_start_tolerance_;
double execution_velocity_scaling_;
bool wait_for_trajectory_completion_;
};
Expand Down
46 changes: 39 additions & 7 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 @@ -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;
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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_);
Expand Down Expand Up @@ -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;
}
}
Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -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()))
{
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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
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/joints_allowed_start_tolerance");
}

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/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)
Expand Down

0 comments on commit 76f0009

Please sign in to comment.