Fix planning failure resulting in crash

src/lite6_controller/launch/lite6_real.launch.py

@@ -171,6 +171,11 @@ def launch_setup(context, *args, **kwargs):
         if joint_limits_yaml and gripper_joint_limits_yaml:
             joint_limits_yaml['joint_limits'].update(gripper_joint_limits_yaml['joint_limits'])
 
+    # FIX acceleration limits
+    for i in range(1,7):
+        joint_limits_yaml['joint_limits']['joint{}'.format(i)]['has_acceleration_limits'] = True
+        joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 0.5
+
     add_prefix_to_moveit_params = getattr(mod, 'add_prefix_to_moveit_params')
     add_prefix_to_moveit_params(
         controllers_yaml=controllers_yaml, ompl_planning_yaml=ompl_planning_yaml,
@@ -222,6 +227,16 @@ def launch_setup(context, *args, **kwargs):
         # },
     }
 
+
+    pilz_planning_pipeline_config = {
+        'move_group': {
+            'planning_plugin': 'pilz_industrial_motion_planner/CommandPlanner',
+            '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_config": "PTP",
+            "capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService",
+        }
+    }
+
     # Start the actual move_group node/action server
     move_group_node = Node(
         package='moveit_ros_move_group',
@@ -229,7 +244,8 @@ def launch_setup(context, *args, **kwargs):
         output='screen',
         parameters=[
             robot_description_parameters,
-            ompl_planning_pipeline_config,
+            #ompl_planning_pipeline_config,
+            pilz_planning_pipeline_config,
             trajectory_execution,
             plan_execution,
             moveit_controllers,

src/lite6_controller/src/lite6_controller.cpp

@@ -170,12 +170,12 @@ public:
   //
 
   // Set limits for A4 paper
-  float xlim_lower = 0.12;
+  float xlim_lower = 0.20;
-  float xlim_upper = 0.2;
+  float xlim_upper = 0.40;
-  float ylim_lower = -0.2;
+  float ylim_lower = -0.20;
-  float ylim_upper = 0.2;
+  float ylim_upper = 0.20;
-  float zlim_lower = 0.1;
+  float zlim_lower = 0.145;
-  float zlim_upper = 0.2;
+  float zlim_upper = 0.16;
 
   /**
    * Function that translates an input value with a given range to a value within another range.
@@ -194,8 +194,11 @@ public:
   geometry_msgs::msg::PoseStamped translatePose(geometry_msgs::msg::PoseStamped pose)
   {
     // TODO support paper angle
-    pose.pose.position.x = translate(pose.pose.position.x, 0, 1, xlim_lower, xlim_upper);
+    auto x = pose.pose.position.x;
-    pose.pose.position.y = translate(pose.pose.position.y, 0, 1, ylim_lower, ylim_upper);
+    auto y = pose.pose.position.y;
+    // X and Y are swapped in xArm space
+    pose.pose.position.x = translate(y, 0, 1, xlim_lower, xlim_upper);
+    pose.pose.position.y = translate(x, 0, 1, ylim_lower, ylim_upper);
     pose.pose.position.z = translate(pose.pose.position.z, 0, 1, zlim_lower, zlim_upper);
 
     return pose;
@@ -211,10 +214,12 @@ public:
   /**
    * Creates a trajectory for a pose and appends it to a given trajectory
    */
-  void addPoseToTrajectory(geometry_msgs::msg::PoseStamped pose, moveit_msgs::msg::RobotTrajectory *trajectory)
+  bool addPoseToTrajectory(geometry_msgs::msg::PoseStamped pose, moveit_msgs::msg::RobotTrajectory *trajectory)
   {
     pose = translatePose(pose);
-    move_group.setPoseTarget(pose);
+    //move_group.setPoseTarget(pose);
+    move_group.setApproximateJointValueTarget(pose, "link_eef");
+
     //moveit_msgs::msg::RobotTrajectory trajectory;
     //move_group.setPlanningPipelineId("PTP");
     move_group.setPlannerId("PTP");
@@ -227,11 +232,13 @@ public:
     bool success = (move_group.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);
     RCLCPP_INFO(this->get_logger(), "Plan (pose goal) %s", success ? "SUCCEEDED" : "FAILED");
 
+    if (success)
+    {
       robot_trajectory::RobotTrajectory next_trajectory(move_group.getCurrentState()->getRobotModel(), move_group.getName());
       next_trajectory.setRobotTrajectoryMsg(*move_group.getCurrentState(), plan.trajectory_);
 
       // append trajectory, with time step of 2.0, not skipping any points
-    previous_trajectory.append(next_trajectory, 2.0, 0);
+      previous_trajectory.append(next_trajectory, 0.01, 0);
       *trajectory = moveit_msgs::msg::RobotTrajectory();
       previous_trajectory.getRobotTrajectoryMsg(*trajectory);
 
@@ -241,7 +248,9 @@ public:
       //                                    plan.trajectory_.joint_trajectory.points.end());
       //trajectory->joint_trajectory.joint_names = plan.trajectory_.joint_trajectory.joint_names;
 
+    }
     move_group.clearPoseTarget();
+    return success;
   }
 
   /**
@@ -286,7 +295,7 @@ public:
       //RCLCPP_INFO(this->get_logger(), "Planning trajectory");
 
       // Append next pose to trajectory
-      addPoseToTrajectory(p, &multi_trajectory);
+      if (!addPoseToTrajectory(p, &multi_trajectory)) continue;
 
       // set move_group start state to final pose of trajectory
       //RCLCPP_INFO(this->get_logger(), "setRobotTrajectoryMsg");