validate trajectory before execution (#63)

* validate trajectory before execution

* addressed review comments

* moved validateTrajectory to TrajectoryExecutionManager

* addressed @davetcoleman's comments

* make allowed_start_tolerance dynamically configurable

* addressed @v4hn's comments

* moved validate to executeThread

* allow_start_tolerance == 0 disables trajectory validation

* moved validate to execute()

* add validation test

* increased default value for allowed_start_tolerance to 0.01

moveit_ros/move_group/src/move_group_context.cpp

@@ -50,7 +50,8 @@ move_group::MoveGroupContext::MoveGroupContext(const planning_scene_monitor::Pla
 
   if (allow_trajectory_execution_)
   {
-    trajectory_execution_manager_.reset(new trajectory_execution_manager::TrajectoryExecutionManager(planning_scene_monitor_->getRobotModel()));
+    trajectory_execution_manager_.reset(new trajectory_execution_manager::TrajectoryExecutionManager(planning_scene_monitor_->getRobotModel(),
+                                                                                                     planning_scene_monitor_->getStateMonitor()));
     plan_execution_.reset(new plan_execution::PlanExecution(planning_scene_monitor_, trajectory_execution_manager_));
     plan_with_sensing_.reset(new plan_execution::PlanWithSensing(trajectory_execution_manager_));
     if (debug)

moveit_ros/planning/plan_execution/src/plan_execution.cpp

@@ -77,7 +77,8 @@ plan_execution::PlanExecution::PlanExecution(const planning_scene_monitor::Plann
   trajectory_execution_manager_(trajectory_execution)
 {
   if (!trajectory_execution_manager_)
-    trajectory_execution_manager_.reset(new trajectory_execution_manager::TrajectoryExecutionManager(planning_scene_monitor_->getRobotModel()));
+    trajectory_execution_manager_.reset(new trajectory_execution_manager::TrajectoryExecutionManager(planning_scene_monitor_->getRobotModel(),
+                                                                                                     planning_scene_monitor_->getStateMonitor()));
 
   default_max_replan_attempts_ = 5;
 

moveit_ros/planning/trajectory_execution_manager/CMakeLists.txt

@@ -1,7 +1,7 @@
 set(MOVEIT_LIB_NAME moveit_trajectory_execution_manager)
 
 add_library(${MOVEIT_LIB_NAME} src/trajectory_execution_manager.cpp)
-target_link_libraries(${MOVEIT_LIB_NAME} moveit_robot_model_loader ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+target_link_libraries(${MOVEIT_LIB_NAME} moveit_planning_scene_monitor moveit_robot_model_loader ${catkin_LIBRARIES} ${Boost_LIBRARIES})
 add_dependencies(${MOVEIT_LIB_NAME} ${moveit_ros_planning_EXPORTED_TARGETS}) # don't build until necessary msgs are finish
 
 install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION lib)

moveit_ros/planning/trajectory_execution_manager/cfg/TrajectoryExecutionDynamicReconfigure.cfg

@@ -7,5 +7,6 @@ gen.add("execution_duration_monitoring", bool_t, 1, "Monitor the execution durat
 gen.add("allowed_execution_duration_scaling", double_t, 2, "Accept durations that take a little more time than specified", 1.1, 1, 10)
 gen.add("allowed_goal_duration_margin", double_t, 3, "Allow more than the expected execution time before triggering a trajectory cancel (applied after scaling)", 0.5, 0.1, 5)
 gen.add("execution_velocity_scaling", double_t, 4, "Multiplicative factor for execution speed", 1, 0.1, 10)
+gen.add("allowed_start_tolerance", double_t, 5, "Allowed joint-value tolerance for validation of trajectory's start point against current robot state", 0.01, 0);
 
 exit(gen.generate(PACKAGE, PACKAGE, "TrajectoryExecutionDynamicReconfigure"))

moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h

@@ -39,6 +39,7 @@
 
 #include <moveit/macros/class_forward.h>
 #include <moveit/robot_model/robot_model.h>
+#include <moveit/planning_scene_monitor/current_state_monitor.h>
 #include <moveit_msgs/RobotTrajectory.h>
 #include <sensor_msgs/JointState.h>
 #include <std_msgs/String.h>
@@ -79,10 +80,10 @@ class TrajectoryExecutionManager
   };
 
   /// Load the controller manager plugin, start listening for events on a topic.
-  TrajectoryExecutionManager(const robot_model::RobotModelConstPtr &kmodel);
+  TrajectoryExecutionManager(const robot_model::RobotModelConstPtr &kmodel, const planning_scene_monitor::CurrentStateMonitorPtr &csm);
 
   /// Load the controller manager plugin, start listening for events on a topic.
-  TrajectoryExecutionManager(const robot_model::RobotModelConstPtr &kmodel, bool manage_controllers);
+  TrajectoryExecutionManager(const robot_model::RobotModelConstPtr &kmodel, const planning_scene_monitor::CurrentStateMonitorPtr &csm, bool manage_controllers);
 
   /// Destructor. Cancels all running trajectories (if any)
   ~TrajectoryExecutionManager();
@@ -204,6 +205,9 @@ class TrajectoryExecutionManager
   /// By default, this is 1.0
   void setExecutionVelocityScaling(double scaling);
 
+  /// Set joint-value tolerance for validating trajectory's start point against current robot state
+  void setAllowedStartTolerance(double tolerance);
+
 private:
 
   struct ControllerInformation
@@ -226,6 +230,8 @@ class TrajectoryExecutionManager
 
   void reloadControllerInformation();
 
+  /// Validate first point of trajectory matches current robot state
+  bool validate(const TrajectoryExecutionContext &context) const;
   bool configure(TrajectoryExecutionContext &context, const moveit_msgs::RobotTrajectory &trajectory, const std::vector<std::string> &controllers);
 
   void updateControllersState(const ros::Duration &age);
@@ -251,6 +257,7 @@ class TrajectoryExecutionManager
   void receiveEvent(const std_msgs::StringConstPtr &event);
 
   robot_model::RobotModelConstPtr robot_model_;
+  planning_scene_monitor::CurrentStateMonitorPtr csm_;
   ros::NodeHandle node_handle_;
   ros::NodeHandle root_node_handle_;
   ros::Subscriber event_topic_subscriber_;
@@ -295,6 +302,7 @@ class TrajectoryExecutionManager
   bool execution_duration_monitoring_;
   double allowed_execution_duration_scaling_;
   double allowed_goal_duration_margin_;
+  double allowed_start_tolerance_; // joint tolerance for validate(): radians for revolute joints
   double execution_velocity_scaling_;
 };
 

moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp

@@ -35,6 +35,7 @@
 /* Author: Ioan Sucan */
 
 #include <moveit/trajectory_execution_manager/trajectory_execution_manager.h>
+#include <moveit/robot_state/robot_state.h>
 #include <moveit_ros_planning/TrajectoryExecutionDynamicReconfigureConfig.h>
 #include <dynamic_reconfigure/server.h>
 
@@ -67,23 +68,27 @@ class TrajectoryExecutionManager::DynamicReconfigureImpl
     owner_->setAllowedExecutionDurationScaling(config.allowed_execution_duration_scaling);
     owner_->setAllowedGoalDurationMargin(config.allowed_goal_duration_margin);
     owner_->setExecutionVelocityScaling(config.execution_velocity_scaling);
+    owner_->setAllowedStartTolerance(config.allowed_start_tolerance);
   }
 
   TrajectoryExecutionManager *owner_;
   dynamic_reconfigure::Server<TrajectoryExecutionDynamicReconfigureConfig> dynamic_reconfigure_server_;
 };
 
-TrajectoryExecutionManager::TrajectoryExecutionManager(const robot_model::RobotModelConstPtr &kmodel) :
-  robot_model_(kmodel), node_handle_("~")
+TrajectoryExecutionManager::TrajectoryExecutionManager(const robot_model::RobotModelConstPtr &kmodel,
+                                                       const planning_scene_monitor::CurrentStateMonitorPtr &csm) :
+  robot_model_(kmodel), csm_(csm), node_handle_("~")
 {
   if (!node_handle_.getParam("moveit_manage_controllers", manage_controllers_))
     manage_controllers_ = false;
 
   initialize();
 }
 
-TrajectoryExecutionManager::TrajectoryExecutionManager(const robot_model::RobotModelConstPtr &kmodel, bool manage_controllers) :
-  robot_model_(kmodel), node_handle_("~"), manage_controllers_(manage_controllers)
+TrajectoryExecutionManager::TrajectoryExecutionManager(const robot_model::RobotModelConstPtr &kmodel,
+                                                       const planning_scene_monitor::CurrentStateMonitorPtr &csm,
+                                                       bool manage_controllers) :
+  robot_model_(kmodel), csm_(csm), node_handle_("~"), manage_controllers_(manage_controllers)
 {
   initialize();
 }
@@ -109,6 +114,7 @@ void TrajectoryExecutionManager::initialize()
   run_continuous_execution_thread_ = true;
   execution_duration_monitoring_ = true;
   execution_velocity_scaling_ = 1.0;
+  allowed_start_tolerance_ = 0.01;
 
   // TODO: Reading from old param location should be removed in L-turtle. Handled by DynamicReconfigure.
   if (node_handle_.getParam("allowed_execution_duration_scaling", allowed_execution_duration_scaling_))
@@ -191,6 +197,11 @@ void TrajectoryExecutionManager::setExecutionVelocityScaling(double scaling)
   execution_velocity_scaling_ = scaling;
 }
 
+void TrajectoryExecutionManager::setAllowedStartTolerance(double tolerance)
+{
+  allowed_start_tolerance_ = tolerance;
+}
+
 bool TrajectoryExecutionManager::isManagingControllers() const
 {
   return manage_controllers_;
@@ -859,6 +870,54 @@ bool TrajectoryExecutionManager::distributeTrajectory(const moveit_msgs::RobotTr
   return true;
 }
 
+bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext &context) const
+{
+  if (allowed_start_tolerance_ == 0) // skip validation on this magic number
+    return true;
+
+  ROS_DEBUG_NAMED("traj_execution", "Validating trajectory with allowed_start_tolerance %g", allowed_start_tolerance_);
+
+  robot_state::RobotStatePtr current_state;
+  if (!csm_->waitForCurrentState(1.0) || !(current_state = csm_->getCurrentState()))
+  {
+    ROS_WARN_NAMED("traj_execution", "Failed to validate trajectory: couldn't receive full current joint state within 1s");
+    return false;
+  }
+
+  for (auto& trajectory : context.trajectory_parts_)
+  {
+    const std::vector<double> &positions = trajectory.joint_trajectory.points.front().positions;
+    const std::vector<std::string> &joint_names = trajectory.joint_trajectory.joint_names;
+    const std::size_t n = joint_names.size();
+    if (positions.size() != n)
+    {
+      ROS_ERROR_NAMED("traj_execution", "Wrong trajectory: #joints: %zu != #positions: %zu", n, positions.size());
+      return false;
+    }
+
+    for (std::size_t i = 0; i < n; ++i)
+    {
+      const robot_model::JointModel *jm = current_state->getJointModel(joint_names[i]);
+      if (!jm)
+      {
+        ROS_ERROR_STREAM_NAMED("traj_execution", "Unknown joint in trajectory: " << joint_names[i]);
+        return false;
+      }
+      // TODO: check multi-DoF joints ?
+      if (fabs(current_state->getJointPositions(jm)[0] - positions[i]) > allowed_start_tolerance_)
+      {
+        ROS_ERROR_NAMED("traj_execution",
+                        "\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(), positions[i], current_state->getJointPositions(jm)[0]);
+        return false;
+      }
+    }
+  }
+  return true;
+}
+
 bool TrajectoryExecutionManager::configure(TrajectoryExecutionContext &context, const moveit_msgs::RobotTrajectory &trajectory, const std::vector<std::string> &controllers)
 {
   if (trajectory.multi_dof_joint_trajectory.points.empty() &&  trajectory.joint_trajectory.points.empty())
@@ -1016,8 +1075,18 @@ void TrajectoryExecutionManager::execute(const ExecutionCompleteCallback &callba
 void TrajectoryExecutionManager::execute(const ExecutionCompleteCallback &callback, const PathSegmentCompleteCallback &part_callback, bool auto_clear)
 {
   stopExecution(false);
-  execution_complete_ = false;
+
+  // check whether first trajectory starts at current robot state
+  if (trajectories_.size() && !validate(*trajectories_.front()))
+  {
+    last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
+    if (auto_clear)
+      clear();
+    return;
+  }
+
   // start the execution thread
+  execution_complete_ = false;
   execution_thread_.reset(new boost::thread(&TrajectoryExecutionManager::executeThread, this, callback, part_callback, auto_clear));
 }
 
@@ -1071,7 +1140,7 @@ void TrajectoryExecutionManager::executeThread(const ExecutionCompleteCallback &
     return;
   }
 
-  ROS_DEBUG_NAMED("traj_execution","Starting trajectory execution ...");
+  ROS_DEBUG_NAMED("traj_execution", "Starting trajectory execution ...");
   // assume everything will be OK
   last_execution_status_ = moveit_controller_manager::ExecutionStatus::SUCCEEDED;
 

moveit_ros/planning/trajectory_execution_manager/test/test_app.cpp

@@ -36,6 +36,7 @@
 
 #include <moveit/trajectory_execution_manager/trajectory_execution_manager.h>
 #include <moveit/robot_model_loader/robot_model_loader.h>
+#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
 
 int main(int argc, char **argv)
 {
@@ -44,8 +45,9 @@ int main(int argc, char **argv)
   ros::AsyncSpinner spinner(1);
   spinner.start();
 
-  robot_model_loader::RobotModelLoader rml;
-  trajectory_execution_manager::TrajectoryExecutionManager tem(rml.getModel(), true);
+  robot_model_loader::RobotModelLoaderPtr rml(new robot_model_loader::RobotModelLoader);
+  planning_scene_monitor::PlanningSceneMonitor psm(rml);
+  trajectory_execution_manager::TrajectoryExecutionManager tem(rml->getModel(), psm.getStateMonitor(), true);
 
   std::cout << "1:\n";
   if (!tem.ensureActiveControllersForJoints(std::vector<std::string>(1, "basej")))

moveit_ros/planning_interface/test/python_move_group.py

@@ -4,6 +4,7 @@
 import numpy as np
 import rospy
 import rostest
+import os
 
 from moveit_ros_planning_interface._moveit_move_group_interface import MoveGroup
 
@@ -35,9 +36,32 @@ def test_target_setting(self):
         self.check_target_setting([0.3] * n, {name: 0.3 for name in self.group.get_active_joints()})
         self.check_target_setting([0.5] + [0.3]*(n-1), "joint_1", 0.5)
 
+    def plan(self, target):
+        self.group.set_joint_value_target(target)
+        return self.group.compute_plan()
+
+    def test_validation(self):
+        current = np.asarray(self.group.get_current_joint_values())
+
+        plan1 = self.plan(current + 0.2)
+        plan2 = self.plan(current + 0.2)
+
+        # first plan should execute
+        self.assertTrue(self.group.execute(plan1))
+
+        # second plan should be invalid now (due to modified start point) and rejected
+        self.assertFalse(self.group.execute(plan2))
+
+        # newly planned trajectory should execute again
+        plan3 = self.plan(current)
+        self.assertTrue(self.group.execute(plan3))
+
 
 if __name__ == '__main__':
     PKGNAME = 'moveit_ros_planning_interface'
     NODENAME = 'moveit_test_python_move_group'
     rospy.init_node(NODENAME)
     rostest.rosrun(PKGNAME, NODENAME, PythonMoveGroupTest)
+
+    # suppress cleanup segfault
+    os._exit(0)

moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajectory_execution.launch.xml

@@ -10,6 +10,8 @@
   <param name="trajectory_execution/allowed_execution_duration_scaling" value="1.2"/> <!-- default 1.2 -->
   <!-- Allow more than the expected execution time before triggering a trajectory cancel (applied after scaling) -->
   <param name="trajectory_execution/allowed_goal_duration_margin" value="0.5"/> <!-- default 0.5 -->
+  <!-- Allowed joint-value tolerance for validation that trajectory's first point matches current robot state -->
+  <param name="trajectory_execution/allowed_start_tolerance" value="0.01"/> <!-- default 0.01 -->
 
   <!-- Load the robot specific controller manager; this sets the moveit_controller_manager ROS parameter -->
   <arg name="moveit_controller_manager" default="[ROBOT_NAME]" />