Merge pull request #355 from jrgnicho/fix/exercise-4.0-moveit2-motion…

…-and-planning

Fix/exercise 4.0 moveit2 motion and planning

exercises/4.0/ros2/src/myworkcell_core/CMakeLists.txt

@@ -24,7 +24,6 @@ find_package(tf2_geometry_msgs REQUIRED)
 
 find_package(moveit_msgs REQUIRED)
 find_package(moveit_ros_planning_interface REQUIRED)
-find_package(Boost REQUIRED COMPONENTS system)
 
 # Interface generation dependencies
 find_package(geometry_msgs REQUIRED)
@@ -40,20 +39,17 @@ add_executable(vision_node src/vision_node.cpp)
 ament_target_dependencies(vision_node rclcpp fake_ar_publisher tf2_ros tf2_geometry_msgs)
 rosidl_target_interfaces(vision_node ${PROJECT_NAME} "rosidl_typesupport_cpp")
 
-# The myworkcell_node executable
+# The myworkcell_node executable (Uses MoveItCpp)
 add_executable(myworkcell_node src/myworkcell_node.cpp)
 ament_target_dependencies(myworkcell_node
   rclcpp
   moveit_msgs
   moveit_ros_planning_interface
 )
-target_link_libraries(myworkcell_node Boost::system)
 rosidl_target_interfaces(myworkcell_node ${PROJECT_NAME} "rosidl_typesupport_cpp")
 
 # Mark executables and/or libraries for installation
 install(TARGETS vision_node myworkcell_node
-    ARCHIVE DESTINATION lib/${PROJECT_NAME}
-    LIBRARY DESTINATION lib/${PROJECT_NAME}
     RUNTIME DESTINATION lib/${PROJECT_NAME}
 )
 

exercises/4.0/ros2/src/myworkcell_core/src/myworkcell_node.cpp

@@ -2,19 +2,36 @@
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <myworkcell_core/srv/localize_part.hpp>
 
-#include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit/moveit_cpp/moveit_cpp.h>
+#include <moveit/moveit_cpp/planning_component.h>
 
 class ScanNPlan : public rclcpp::Node
 {
 public:
-  ScanNPlan() : Node("scan_n_plan", rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true))
+  ScanNPlan() :
+    Node("scan_n_plan", rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true))
   {
     if (! this->has_parameter("base_frame"))
     {
       this->declare_parameter("base_frame", "world");
     }
 
     vision_client_ = this->create_client<myworkcell_core::srv::LocalizePart>("localize_part");
+
+    RCLCPP_INFO(this->get_logger(),"Scan and Plan ready");
+  }
+
+  // MoveIt setup
+  void setup()
+  {
+    // Instantiate moveit_cpp
+    moveit_cpp_ = std::make_shared<moveit_cpp::MoveItCpp>(this->shared_from_this());
+
+    // Planning component associated with a single motion group
+    planning_component_ = std::make_shared<moveit_cpp::PlanningComponent>("manipulator", moveit_cpp_);
+
+    // Parameters set on this node
+    plan_parameters_.load(this->shared_from_this());
   }
 
   void start(const std::string& base_frame)
@@ -26,7 +43,6 @@ class ScanNPlan : public rclcpp::Node
     request->base_frame = base_frame;
 
     auto future = vision_client_->async_send_request(request);
-
     if (future.wait_for(std::chrono::seconds(3)) == std::future_status::timeout)
     {
       RCLCPP_ERROR(this->get_logger(), "Failed to receive LocalizePart service response");
@@ -49,21 +65,48 @@ class ScanNPlan : public rclcpp::Node
     move_target.header.frame_id = base_frame;
     move_target.pose = response->pose;
 
-    RCLCPP_INFO(this->get_logger(), "Creating move group interface");
-    moveit::planning_interface::MoveGroupInterface move_group(
-        this->shared_from_this(),
-        "manipulator");
+    // getting current state of robot from environment
+    if (!moveit_cpp_->getPlanningSceneMonitor()->requestPlanningSceneState())
+    {
+      RCLCPP_ERROR(this->get_logger(), "Failed to get planning scene");
+      return;
+    }
+    moveit::core::RobotStatePtr start_robot_state = moveit_cpp_->getCurrentState(2.0);
+
+    // setting start robot state
+    planning_component_->setStartState(*start_robot_state);
 
-    RCLCPP_INFO(this->get_logger(), "Setting move target pose");
-    move_group.setPoseTarget(move_target);
+    // Set motion goal of end effector link
+    std::string ee_link = moveit_cpp_->getRobotModel()->getJointModelGroup(
+        planning_component_->getPlanningGroupName())->getLinkModelNames().back();
 
-    RCLCPP_INFO(this->get_logger(), "Ready to plan and move!");
-    move_group.move();
-    RCLCPP_INFO(this->get_logger(), "Move completed");
+    RCLCPP_INFO(this->get_logger(), "Setting move target pose for tip link \"%s\"", ee_link.c_str());
+    planning_component_->setGoal(move_target, ee_link);
+
+    // Now we can plan!
+    moveit_cpp::PlanningComponent::PlanSolution plan_solution = planning_component_->plan(plan_parameters_);
+    if (!plan_solution)
+    {
+      RCLCPP_ERROR(this->get_logger(),"Failed to plan");
+      return;
+    }
+
+    // If planning succeeded, execute the returned trajectory
+    bool success = moveit_cpp_->execute("manipulator", plan_solution.trajectory, true);
+    if (!success)
+    {
+      RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to execute trajectory");
+      return;
+    }
   }
 
 private:
   // Planning components
+  moveit_cpp::MoveItCppPtr moveit_cpp_;
+  moveit_cpp::PlanningComponentPtr planning_component_;
+  moveit_cpp::PlanningComponent::PlanRequestParameters plan_parameters_;
+
+  // perception interface
   rclcpp::Client<myworkcell_core::srv::LocalizePart>::SharedPtr vision_client_;
 };
 
@@ -89,6 +132,9 @@ int main(int argc, char **argv)
     }
   };
 
+  // Perform MoveIt initialization
+  app->setup();
+
   // Run our application
   app->start(base_frame);
   rclcpp::shutdown();

exercises/4.0/ros2/src/myworkcell_moveit_config/config/controllers.yaml

@@ -1,7 +1,7 @@
 controller_names:
-  - fake_joint_trajectory_controller
+  - manipulator_joint_trajectory_controller
 
-fake_joint_trajectory_controller:
+manipulator_joint_trajectory_controller:
   action_ns: follow_joint_trajectory
   type: FollowJointTrajectory
   default: true

exercises/4.0/ros2/src/myworkcell_moveit_config/config/joint_limits.yaml

@@ -9,9 +9,12 @@ joint_limits:
     max_acceleration: 0
   shoulder_lift_joint:
     has_velocity_limits: true
-    max_velocity: 3.15
-    has_acceleration_limits: false
-    max_acceleration: 0
+    max_velocity: 6.0
+    has_acceleration_limits: true
+    max_acceleration: 0.62831853
+    has_position_limits: true
+    max_position: 0.0
+    min_position: -2.0
   shoulder_pan_joint:
     has_velocity_limits: true
     max_velocity: 3.15

exercises/4.0/ros2/src/myworkcell_moveit_config/config/kinematics.yaml

@@ -1,5 +1,5 @@
 manipulator:
   kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
-  kinematics_solver_search_resolution: 0.005
-  kinematics_solver_timeout: 0.005
-  kinematics_solver_attempts: 3
+  kinematics_solver_search_resolution: 0.01
+  kinematics_solver_timeout: 0.5
+  kinematics_solver_attempts: 40

exercises/4.0/ros2/src/myworkcell_moveit_config/config/myworkcell.srdf

@@ -12,6 +12,17 @@
     <group name="manipulator">
         <chain base_link="base_link" tip_link="tool0" />
     </group>
+
+    <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
+    <group_state name="home" group="manipulator">
+        <joint name="elbow_joint" value="1.0589" />
+        <joint name="shoulder_lift_joint" value="-0.9884" />
+        <joint name="shoulder_pan_joint" value="0.706" />
+        <joint name="wrist_1_joint" value="-1.5531" />
+        <joint name="wrist_2_joint" value="-1.5531" />
+        <joint name="wrist_3_joint" value="0" />
+    </group_state>
+
     <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
     <disable_collisions link1="base_link" link2="shoulder_link" reason="Adjacent" />
     <disable_collisions link1="base_link" link2="table" reason="Adjacent" />

exercises/4.0/ros2/src/myworkcell_moveit_config/config/ompl_planning.yaml

@@ -27,10 +27,10 @@ planner_configs:
   RRT:
     type: geometric::RRT
     range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
-    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
+    goal_bias: 0.01  # When close to goal select goal, with this probability? default: 0.05
   RRTConnect:
     type: geometric::RRTConnect
-    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    range: 0.01  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
   RRTstar:
     type: geometric::RRTstar
     range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
@@ -121,7 +121,7 @@ planner_configs:
     dense_delta_fraction: 0.001  # delta fraction for interface detection. default: 0.001
     max_failures: 5000  # maximum consecutive failure limit. default: 5000
 manipulator:
-  default_planner_config: RRT
+  default_planner_config: RRTConnect
   planner_configs:
     - SBL
     - EST
@@ -147,12 +147,12 @@ manipulator:
     - SPARS
     - SPARStwo
   projection_evaluator: joints(shoulder_pan_joint,shoulder_lift_joint)
-  longest_valid_segment_fraction: "0.005"
-
+  longest_valid_segment_fraction: 0.01
 planning_plugin: 'ompl_interface/OMPLPlanner'
 request_adapters: >-
-  default_planner_request_adapters/AddTimeOptimalParameterization
-  default_planner_request_adapters/FixWorkspaceBounds
-  default_planner_request_adapters/FixStartStateBounds
-  default_planner_request_adapters/FixStartStateCollision
-  default_planner_request_adapters/FixStartStatePathConstraints
+    default_planner_request_adapters/AddTimeOptimalParameterization
+    default_planner_request_adapters/FixWorkspaceBounds
+    default_planner_request_adapters/FixStartStateBounds
+    default_planner_request_adapters/FixStartStateCollision
+    default_planner_request_adapters/FixStartStatePathConstraints
+start_state_max_bounds_error: 0.1

exercises/4.0/ros2/src/myworkcell_moveit_config/config/ros_controllers.yaml

@@ -1,35 +1,33 @@
-# MoveIt-specific simulation settings
-moveit_sim_hw_interface:
-  joint_model_group: controllers_initial_group_
-  joint_model_group_pose: controllers_initial_pose_
-# Settings for ros_control control loop
-generic_hw_control_loop:
-  loop_hz: 300
-  cycle_time_error_threshold: 0.01
-# Settings for ros_control hardware interface
-hardware_interface:
-  joints:
-    - shoulder_pan_joint
-    - shoulder_lift_joint
-    - elbow_joint
-    - wrist_1_joint
-    - wrist_2_joint
-    - wrist_3_joint
-  sim_control_mode: 1  # 0: position, 1: velocity
-# Publish all joint states
-# Creates the /joint_states topic necessary in ROS
-joint_state_controller:
-  type: joint_state_controller/JointStateController
-  publish_rate: 50
-controller_list:
-  - name: manipulator_controller
-    action_ns: follow_joint_trajectory
-    default: True
-    type: FollowJointTrajectory
+controller_manager:
+  ros__parameters:
+    update_rate: 600  # Hz
+    manipulator_joint_trajectory_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+    joint_state_controller:
+      type: joint_state_controller/JointStateController
+
+# parameters for each controller listed under controller manager
+manipulator_joint_trajectory_controller:
+  ros__parameters:
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
     joints:
-      - shoulder_pan_joint
-      - shoulder_lift_joint
       - elbow_joint
+      - shoulder_lift_joint
+      - shoulder_pan_joint
       - wrist_1_joint
       - wrist_2_joint
-      - wrist_3_joint
+      - wrist_3_joint
+    state_publish_rate: 100.0
+    action_monitor_rate: 20.0
+    allow_partial_joints_goal: false
+    constraints:
+      stopped_velocity_tolerance: 0.0
+      goal_time: 0.0
+
+joint_state_controller:
+  ros__parameters:
+    type: joint_state_controller/JointStateController