Fix robot limit loading and LIN planner

src/custom_xarm_moveit_config/config/lite6/cartesian_limits.yaml

@@ -0,0 +1,5 @@
+cartesian_limits:
+  max_trans_vel: 2.0
+  max_trans_acc: 3.00
+  max_trans_dec: -5.0
+  max_rot_vel: 1.57

src/custom_xarm_moveit_config/config/lite6/joint_limits.yaml

@@ -4,31 +4,31 @@
 joint_limits:
   joint1:
     has_velocity_limits: true
-    max_velocity: 2.14
+    max_velocity: 3.14
     has_acceleration_limits: true
-    max_acceleration: 1.0
+    max_acceleration: 20.0
   joint2:
     has_velocity_limits: true
-    max_velocity: 2.14
+    max_velocity: 3.14
     has_acceleration_limits: true
-    max_acceleration: 1.0
+    max_acceleration: 20.0
   joint3:
     has_velocity_limits: true
-    max_velocity: 2.14
+    max_velocity: 3.14
     has_acceleration_limits: true
-    max_acceleration: 1.0
+    max_acceleration: 20.0
   joint4:
     has_velocity_limits: true
-    max_velocity: 2.14
+    max_velocity: 3.14
     has_acceleration_limits: true
-    max_acceleration: 1.0
+    max_acceleration: 20.0
   joint5:
     has_velocity_limits: true
-    max_velocity: 2.14
+    max_velocity: 3.14
     has_acceleration_limits: true
-    max_acceleration: 1.0
+    max_acceleration: 20.0
   joint6:
     has_velocity_limits: true
-    max_velocity: 2.14
+    max_velocity: 3.14
     has_acceleration_limits: true
-    max_acceleration: 1.0
+    max_acceleration: 20.0

src/custom_xarm_moveit_config/launch/lib/robot_moveit_config_lib.py

@@ -65,19 +65,22 @@ def add_prefix_to_moveit_params(controllers_yaml=None, ompl_planning_yaml=None,
 
 
 def get_xarm_robot_description_parameters(
-    xacro_urdf_file=PathJoinSubstitution([FindPackageShare('xarm_description'), 'urdf', 'xarm_device.urdf.xacro']),
+    xacro_urdf_file=PathJoinSubstitution([FindPackageShare('custom_xarm_description'), 'urdf', 'xarm_device.urdf.xacro']),
-    xacro_srdf_file=PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'srdf', 'xarm.srdf.xacro']),
+    xacro_srdf_file=PathJoinSubstitution([FindPackageShare('custom_xarm_moveit_config'), 'srdf', 'xarm.srdf.xacro']),
     urdf_arguments={},
     srdf_arguments={},
     arguments={}):
     urdf_arguments['ros2_control_plugin'] = urdf_arguments.get('ros2_control_plugin', 'uf_robot_hardware/UFRobotSystemHardware')
-    moveit_config_package_name = 'xarm_moveit_config'
+    moveit_config_package_name = 'custom_xarm_moveit_config'
     xarm_type = arguments.get('xarm_type', None)
     
     # xarm_description/launch/lib/robot_description_lib.py
-    mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory('xarm_description'), 'launch', 'lib', 'robot_description_lib.py'))
+    mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory('custom_xarm_description'), 'launch', 'lib', 'robot_description_lib.py'))
     get_xacro_file_content = getattr(mod, 'get_xacro_file_content')
 
+    joint_limits = load_yaml(moveit_config_package_name, 'config', xarm_type, 'joint_limits.yaml')
+    cartesian_limits = load_yaml(moveit_config_package_name, 'config', xarm_type, 'cartesian_limits.yaml')
+    joint_limits['cartesian_limits'] = cartesian_limits['cartesian_limits']
     return {
         'robot_description': get_xacro_file_content(
             xacro_file=xacro_urdf_file, 
@@ -87,6 +90,6 @@ def get_xarm_robot_description_parameters(
             xacro_file=xacro_srdf_file,
             arguments=srdf_arguments
         ),
-        'robot_description_planning': load_yaml(moveit_config_package_name, 'config', xarm_type, 'joint_limits.yaml'),
+        'robot_description_planning': joint_limits,
         'robot_description_kinematics': load_yaml(moveit_config_package_name, 'config', xarm_type, 'kinematics.yaml')
     }

src/lite6_controller/CMakeLists.txt

@@ -44,16 +44,6 @@ ament_target_dependencies(lite6_controller
   "robot_controller"
   "moveit_ros_planning_interface"
   "robot_interfaces")
-add_executable(lite6_trajectory_executor src/lite6_trajectory_executor.cpp)
-ament_target_dependencies(lite6_trajectory_executor
-  "rclcpp"
-  "moveit"
-  "rclcpp_action"
-  "Eigen3"
-  "pilz_industrial_motion_planner"
-  "robot_controller"
-  "moveit_ros_planning_interface"
-  "robot_interfaces")
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
@@ -62,7 +52,6 @@ endif()
 
 install(TARGETS
   lite6_controller
-  lite6_trajectory_executor
   DESTINATION lib/${PROJECT_NAME})
 
 install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})

src/lite6_controller/launch/lite6_gazebo.launch.py

@@ -191,17 +191,6 @@ def launch_setup(context, *args, **kwargs):
                 {"use_sim_time": use_sim_time},
             ],
         ),
-        Node(
-            package="lite6_controller",
-            executable="lite6_trajectory_executor",
-            output="log",
-            arguments=["--ros-args", "--log-level", log_level],
-            parameters=[
-                robot_description,
-                robot_description_semantic,
-                {"use_sim_time": use_sim_time},
-            ],
-        ),
         Node(
             package="virtual_drawing_surface",
             executable="drawing_surface.py",
@@ -274,10 +263,14 @@ def launch_setup(context, *args, **kwargs):
     kinematics_yaml = robot_description_parameters['robot_description_kinematics']
     joint_limits_yaml = robot_description_parameters.get('robot_description_planning', None)
 
+    #cartesian_limits = load_yaml(moveit_config_package_name, 'config', xarm_type, 'cartesian_limits.yaml')
+    #robot_description_parameters['robot_description_planning']['cartesian_limits'] = cartesian_limits['cartesian_limits']
+    #input(joint_limits_yaml)
+
     # FIX acceleration limits
-    for i in range(1,7):
+    #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)]['has_acceleration_limits'] = True
-        joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 1.0
+    #    joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 1.0
 
     kinematics_yaml['kinematics_solver'] = 'kdl_kinematics_plugin/KDLKinematicsPlugin'
     #kinematics_yaml['kinematics_solver'] = 'lma_kinematics_plugin/LMAKinematicsPlugin'
@@ -311,12 +304,8 @@ def launch_setup(context, *args, **kwargs):
         prefix=prefix.perform(context))
 
 
-    robot_description_parameters['cartesian_limits'] = {}
+    #cartesian_limits = load_yaml(moveit_config_package_name, 'config', xarm_type, 'cartesian_limits.yaml')
-    robot_description_parameters['cartesian_limits']['max_trans_vel'] = 1
+    #robot_description_parameters['robot_description_planning']['cartesian_limits'] = cartesian_limits['cartesian_limits']
-    robot_description_parameters['cartesian_limits']['max_trans_acc'] = 2.25
-    robot_description_parameters['cartesian_limits']['max_trans_dec'] = -5
-    robot_description_parameters['cartesian_limits']['max_rot_vel'] =  1.57
-
 
     # Planning pipeline
     # https://github.com/AndrejOrsula/panda_moveit2_config/blob/master/launch/move_group_fake_control.launch.py

src/lite6_controller/launch/lite6_real.launch.py

@@ -75,7 +75,7 @@ def launch_setup(context, *args, **kwargs):
     # robot description launch
     # xarm_description/launch/_robot_description.launch.py
     robot_description_launch = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_description'), 'launch', '_robot_description.launch.py'])),
+        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('custom_xarm_description'), 'launch', '_robot_description.launch.py'])),
         launch_arguments={
             'prefix': prefix,
             'hw_ns': hw_ns,
@@ -105,12 +105,12 @@ def launch_setup(context, *args, **kwargs):
 
     # robot_description_parameters
     # xarm_moveit_config/launch/lib/robot_moveit_config_lib.py
-    moveit_config_package_name = 'xarm_moveit_config'
+    moveit_config_package_name = 'custom_xarm_moveit_config'
     mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory(moveit_config_package_name), 'launch', 'lib', 'robot_moveit_config_lib.py'))
     get_xarm_robot_description_parameters = getattr(mod, 'get_xarm_robot_description_parameters')
     robot_description_parameters = get_xarm_robot_description_parameters(
-        xacro_urdf_file=PathJoinSubstitution([FindPackageShare('xarm_description'), 'urdf', 'xarm_device.urdf.xacro']),
+        xacro_urdf_file=PathJoinSubstitution([FindPackageShare('custom_xarm_description'), 'urdf', 'xarm_device.urdf.xacro']),
-        xacro_srdf_file=PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'srdf', 'xarm.srdf.xacro']),
+        xacro_srdf_file=PathJoinSubstitution([FindPackageShare('custom_xarm_moveit_config'), 'srdf', 'xarm.srdf.xacro']),
         urdf_arguments={
             'prefix': prefix,
             'hw_ns': hw_ns.perform(context).strip('/'),
@@ -154,17 +154,12 @@ def launch_setup(context, *args, **kwargs):
     ompl_planning_yaml = load_yaml(moveit_config_package_name, 'config', xarm_type, 'ompl_planning.yaml')
     kinematics_yaml = robot_description_parameters['robot_description_kinematics']
 
-    robot_description_parameters['cartesian_limits'] = {}
+    kinematics_yaml['kinematics_solver'] = 'kdl_kinematics_plugin/KDLKinematicsPlugin'
-    robot_description_parameters['cartesian_limits']['max_trans_vel'] = 1
+    kinematics_yaml['kinematics_solver_search_resolution'] = 0.005
-    robot_description_parameters['cartesian_limits']['max_trans_acc'] = 2.25
+    kinematics_yaml['kinematics_solver_timeout'] = 0.005
-    robot_description_parameters['cartesian_limits']['max_trans_dec'] = -5
+    kinematics_yaml['kinematics_solver_attempts'] = 3
-    robot_description_parameters['cartesian_limits']['max_rot_vel'] =  1.57
-
-    #kinematics_yaml['kinematics_solver'] = 'kdl_kinematics_plugin/KDLKinematicsPlugin'
-    #kinematics_yaml['kinematics_solver_search_resolution'] = 0.005
-    #kinematics_yaml['kinematics_solver_timeout'] = 0.005
-    #kinematics_yaml['kinematics_solver_attempts'] = 3
     kinematics_yaml['kinematics_solver_timeout'] = 10.0
+
     kinematics_yaml['kinematics_solver_attempts'] = 10
 
     joint_limits_yaml = robot_description_parameters.get('robot_description_planning', None)
@@ -186,11 +181,11 @@ def launch_setup(context, *args, **kwargs):
             joint_limits_yaml['joint_limits'].update(gripper_joint_limits_yaml['joint_limits'])
 
     # FIX acceleration limits
-    for i in range(1,7):
+    #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)]['has_acceleration_limits'] = True
-        #joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 1.5
+    #    #joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 1.5
-        #joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 2.5
+    #    #joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 2.5
-        joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 3.0
+    #    joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 3.0
 
     add_prefix_to_moveit_params = getattr(mod, 'add_prefix_to_moveit_params')
     add_prefix_to_moveit_params(
@@ -249,7 +244,8 @@ def launch_setup(context, *args, **kwargs):
             '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""",
             'request_adapters': """default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
-            "default_planner_config": "PTP",
+            #"default_planner_config": "PTP",
+            "default_planner_config": "LIN",
         }
     }
 
@@ -264,6 +260,7 @@ def launch_setup(context, *args, **kwargs):
         output='screen',
         parameters=[
             robot_description_parameters,
+            #cartesian_limits,
             #ompl_planning_pipeline_config,
             pilz_planning_pipeline_config,
             move_group_capabilities,

src/lite6_controller/launch/lite6_real_no_gui.launch.py

@@ -75,7 +75,7 @@ def launch_setup(context, *args, **kwargs):
     # robot description launch
     # xarm_description/launch/_robot_description.launch.py
     robot_description_launch = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_description'), 'launch', '_robot_description.launch.py'])),
+        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('custom_xarm_description'), 'launch', '_robot_description.launch.py'])),
         launch_arguments={
             'prefix': prefix,
             'hw_ns': hw_ns,
@@ -105,12 +105,12 @@ def launch_setup(context, *args, **kwargs):
 
     # robot_description_parameters
     # xarm_moveit_config/launch/lib/robot_moveit_config_lib.py
-    moveit_config_package_name = 'xarm_moveit_config'
+    moveit_config_package_name = 'custom_xarm_moveit_config'
     mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory(moveit_config_package_name), 'launch', 'lib', 'robot_moveit_config_lib.py'))
     get_xarm_robot_description_parameters = getattr(mod, 'get_xarm_robot_description_parameters')
     robot_description_parameters = get_xarm_robot_description_parameters(
-        xacro_urdf_file=PathJoinSubstitution([FindPackageShare('xarm_description'), 'urdf', 'xarm_device.urdf.xacro']),
+        xacro_urdf_file=PathJoinSubstitution([FindPackageShare('custom_xarm_description'), 'urdf', 'xarm_device.urdf.xacro']),
-        xacro_srdf_file=PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'srdf', 'xarm.srdf.xacro']),
+        xacro_srdf_file=PathJoinSubstitution([FindPackageShare('custom_xarm_moveit_config'), 'srdf', 'xarm.srdf.xacro']),
         urdf_arguments={
             'prefix': prefix,
             'hw_ns': hw_ns.perform(context).strip('/'),
@@ -156,9 +156,9 @@ def launch_setup(context, *args, **kwargs):
     joint_limits_yaml = robot_description_parameters.get('robot_description_planning', None)
 
     # FIX acceleration limits
-    for i in range(1,7):
+    #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)]['has_acceleration_limits'] = True
-        joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 1.0
+    #    joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 1.0
 
     if add_gripper.perform(context) in ('True', 'true'):
         gripper_controllers_yaml = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type.perform(context)), '{}.yaml'.format(controllers_name.perform(context)))
@@ -182,11 +182,8 @@ def launch_setup(context, *args, **kwargs):
         kinematics_yaml=kinematics_yaml, joint_limits_yaml=joint_limits_yaml,
         prefix=prefix.perform(context))
 
-    robot_description_parameters['cartesian_limits'] = {}
+    #cartesian_limits = load_yaml(moveit_config_package_name, 'config', xarm_type, 'cartesian_limits.yaml')
-    robot_description_parameters['cartesian_limits']['max_trans_vel'] = 1
+    #robot_description_parameters['robot_description_planning']['cartesian_limits'] = cartesian_limits['cartesian_limits']
-    robot_description_parameters['cartesian_limits']['max_trans_acc'] = 2.25
-    robot_description_parameters['cartesian_limits']['max_trans_dec'] = -5
-    robot_description_parameters['cartesian_limits']['max_rot_vel'] =  1.57
 
     # Planning Configuration
     ompl_planning_pipeline_config = {

src/lite6_controller/src/lite6_controller.cpp

@@ -72,8 +72,6 @@ public:
   rclcpp::TimerBase::SharedPtr trajectory_timer_;
   bool busy = false;
 
-  rclcpp::Publisher<moveit_msgs::msg::RobotTrajectory>::SharedPtr trajectory_pub;
-
   /**
    * TODO Use instead of MoveGroupInterface
    * https://industrial-training-master.readthedocs.io/en/foxy/_source/session4/ros2/0-Motion-Planning-CPP.html
@@ -126,9 +124,6 @@ public:
     //this->move_group.setMaxVelocityScalingFactor(1.0);
     this->move_group.setMaxVelocityScalingFactor(0.8);
 
-
-    trajectory_pub = this->create_publisher<moveit_msgs::msg::RobotTrajectory>("lite6_trajectory", 100);
-
     // Subscribe to target pose
     //target_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>("/target_pose", rclcpp::QoS(1), std::bind(&MoveItFollowTarget::target_pose_callback, this, std::placeholders::_1));
 
@@ -238,13 +233,15 @@ public:
       moveit_msgs::msg::MotionSequenceItem msi = moveit_msgs::msg::MotionSequenceItem();
 
       planning_interface::MotionPlanRequest mpr = planning_interface::MotionPlanRequest();
-      mpr.planner_id = "PTP";
+      //mpr.planner_id = "PTP";
-      //mpr.planner_id = "LIN";
+      mpr.planner_id = "LIN";
       mpr.group_name = move_group.getName();
-      mpr.max_velocity_scaling_factor = 1.0;
+      //mpr.max_velocity_scaling_factor = 1.0;
-      mpr.max_acceleration_scaling_factor = 0.98;
+      mpr.max_velocity_scaling_factor = 0.7;
+      mpr.max_acceleration_scaling_factor = 0.4;
+      //mpr.max_acceleration_scaling_factor = 0.1;
       mpr.allowed_planning_time = 10;
-      mpr.max_cartesian_speed = 2; // m/s
+      mpr.max_cartesian_speed = 3; // m/s
       //mpr.goal_constraints.position_constraints.header.frame_id = "world";
 
       // A tolerance of 0.01 m is specified in position
@@ -446,8 +443,7 @@ public:
       RCLCPP_INFO(this->get_logger(), "Got %ld trajectories", ts.size());
       RCLCPP_INFO(this->get_logger(), "Adding result to motion queue");
 
-      //trajectory_queue.push(ts[0]);
+      trajectory_queue.push(ts[0]);
-      this->trajectory_pub->publish(ts[0]);
 
       // Set move_group_state to the last state of planned trajectory (planning of next trajectory starts there)
       robot_trajectory::RobotTrajectory rt(move_group.getRobotModel());
@@ -477,27 +473,6 @@ public:
   }
 };
 
-class TrajectoryExecutor : public rclcpp::Node
-{
-  public:
-    //trajectory_pub = this->create_publisher<moveit_msgs::msg::RobotTrajectory>("lite6_trajectory", 10);
-    rclcpp::Subscription<moveit_msgs::msg::RobotTrajectory>::SharedPtr subscription;
-    moveit::planning_interface::MoveGroupInterface move_group;
-    TrajectoryExecutor()
-      : Node("lite6_trajectory_executor"),
-        move_group(std::shared_ptr<rclcpp::Node>(std::move(this)), MOVE_GROUP)
-    {
-      subscription = this->create_subscription<moveit_msgs::msg::RobotTrajectory>(
-        "lite6_trajectory", 100, std::bind(&TrajectoryExecutor::trajectory_callback, this, std::placeholders::_1));
-    }
-    void trajectory_callback(const moveit_msgs::msg::RobotTrajectory msg)
-    {
-      RCLCPP_INFO(this->get_logger(), "Received trajectory, executing");
-      move_group.execute(msg);
-      RCLCPP_INFO(this->get_logger(), "Finished executing trajectory");
-    }
-};
-
 /**
  * Starts lite6_controller
  */
@@ -518,7 +493,6 @@ int main(int argc, char ** argv)
   //rclcpp::executors::SingleThreadedExecutor executor;
   rclcpp::executors::MultiThreadedExecutor executor;
   executor.add_node(lite6);
-  //executor.add_node(trajectory_executor);
   executor.spin();
 
   rclcpp::shutdown();