Add Pilz Motion Planner(I5) Support

.gitattributes

@@ -0,0 +1,5 @@
+# .gitattributes
+
+*.so binary
+*.dll binary
+*.dylib binary

.gitignore

@@ -1,2 +1,5 @@
 *.pyc
 *.autosave
+*.dll binary
+*.so binary
+*.dylib binary

aubo_robot/aubo_driver/lib/lib32/libauborobotcontroller.so

@@ -0,0 +1 @@
+libauborobotcontroller.so.1.0.6

aubo_robot/aubo_driver/lib/lib32/libauborobotcontroller.so.1

@@ -0,0 +1 @@
+libauborobotcontroller.so.1.0.6

aubo_robot/aubo_driver/lib/lib32/libauborobotcontroller.so.1.0

@@ -0,0 +1 @@
+libauborobotcontroller.so.1.0.6

aubo_robot/aubo_driver/lib/lib64/aubocontroller/libauborobotcontroller.so

@@ -0,0 +1 @@
+libauborobotcontroller.so.1.3.1

aubo_robot/aubo_driver/lib/lib64/aubocontroller/libauborobotcontroller.so.1

@@ -0,0 +1 @@
+libauborobotcontroller.so.1.3.1

aubo_robot/aubo_driver/lib/lib64/aubocontroller/libauborobotcontroller.so.1.3

@@ -0,0 +1 @@
+libauborobotcontroller.so.1.3.1

aubo_robot/aubo_driver/lib/lib64/config/libconfig++.so

@@ -0,0 +1 @@
+libconfig++.so.11.0.2

aubo_robot/aubo_driver/lib/lib64/config/libconfig++.so.11

@@ -0,0 +1 @@
+libconfig++.so.11.0.2

aubo_robot/aubo_driver/lib/lib64/config/libconfig.so

@@ -0,0 +1 @@
+libconfig.so.11.0.2

aubo_robot/aubo_driver/lib/lib64/config/libconfig.so.11

@@ -0,0 +1 @@
+libconfig.so.11.0.2

aubo_robot/aubo_driver/lib/lib64/config/libconfig.so.9

@@ -0,0 +1 @@
+libconfig.so.9.2.0

aubo_robot/aubo_driver/lib/lib64/log4cplus/liblog4cplus-1.2.so.5

@@ -0,0 +1 @@
+liblog4cplus-1.2.so.5.1.4

aubo_robot/aubo_driver/lib/lib64/log4cplus/liblog4cplus.so

@@ -0,0 +1 @@
+liblog4cplus-1.2.so.5.1.4

aubo_robot/aubo_i5_moveit_config/config/cartesian_limits.yaml

@@ -0,0 +1,5 @@
+cartesian_limits:
+  max_trans_vel: 1
+  max_trans_acc: 2.25
+  max_trans_dec: -5
+  max_rot_vel: 1.57

aubo_robot/aubo_i5_moveit_config/config/joint_limits.yaml

@@ -5,30 +5,30 @@ joint_limits:
   foreArm_joint:
     has_velocity_limits: true
     max_velocity: 3.14
-    has_acceleration_limits: false
+    has_acceleration_limits: true
     max_acceleration: 30
   shoulder_joint:
     has_velocity_limits: true
     max_velocity: 3.14
-    has_acceleration_limits: false
+    has_acceleration_limits: true
     max_acceleration: 30
   upperArm_joint:
     has_velocity_limits: true
     max_velocity: 3.14
-    has_acceleration_limits: false
+    has_acceleration_limits: true
     max_acceleration: 30
   wrist1_joint:
     has_velocity_limits: true
     max_velocity: 2.6
-    has_acceleration_limits: false
+    has_acceleration_limits: true
     max_acceleration: 25
   wrist2_joint:
     has_velocity_limits: true
     max_velocity: 2.6
-    has_acceleration_limits: false
+    has_acceleration_limits: true
     max_acceleration: 25
   wrist3_joint:
     has_velocity_limits: true
     max_velocity: 2.6
-    has_acceleration_limits: false
+    has_acceleration_limits: true
     max_acceleration: 25

aubo_robot/aubo_i5_moveit_config/launch/move_group.launch

@@ -1,7 +1,8 @@
 <launch>
 
   <include file="$(find aubo_i5_moveit_config)/launch/planning_context.launch" />
-
+  <arg name="pipeline" default="ompl" />
+
   <!-- GDB Debug Option -->
   <arg name="debug" default="false" />
   <arg unless="$(arg debug)" name="launch_prefix" value="" />
@@ -39,10 +40,26 @@
   -->
 
   <!-- Planning Functionality -->
-  <include ns="move_group" file="$(find aubo_i5_moveit_config)/launch/planning_pipeline.launch.xml">
-    <arg name="pipeline" value="ompl" />
+
+  <group ns="move_group/planning_pipelines">
+
+  <!-- Pilz motion Planner-->
+  <include ns="pilz_industrial_motion_planner" file="$(find aubo_i5_moveit_config)/launch/planning_pipeline.launch.xml">
+    <arg name="pipeline" value="pilz_industrial_motion_planner" />
   </include>
 
+  <!-- OMPL motion Planner-->
+  <include ns="ompl" file="$(find aubo_i5_moveit_config)/launch/planning_pipeline.launch.xml">
+    <arg name="pipeline" value="ompl" />
+  </include>
+
+  <!-- for other Planners Loader -->
+  <include if="$(eval arg('pipeline') not in ['ompl', 'pilz_industrial_motion_planner'])" ns="$(arg pipeline)"
+             file="$(find aubo_i5_moveit_config)/launch/planning_pipeline.launch.xml">
+  <arg name="pipeline" value="$(arg pipeline)" />  
+  </include>
+
+  </group>  
   <!-- Trajectory Execution Functionality -->
   <include ns="move_group" file="$(find aubo_i5_moveit_config)/launch/trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
     <arg name="moveit_manage_controllers" value="true" />

aubo_robot/aubo_i5_moveit_config/launch/moveit_planning_execution.launch

@@ -9,6 +9,7 @@
   <!--  - if sim=false, a robot_ip argument is required -->
   <arg name="sim" default="true" />
   <arg name="robot_ip"/>
+  <arg name="pipeline" default="ompl" /><!-- Choose planning pipeline -->
 
   <!-- load the robot_description parameter before launching ROS-I nodes -->
   <include file="$(find aubo_i5_moveit_config)/launch/planning_context.launch" >
@@ -41,6 +42,7 @@
 
   <include file="$(find aubo_i5_moveit_config)/launch/move_group.launch">
     <arg name="publish_monitored_planning_scene" value="true" />
+    <arg name="pipeline" value="$(arg pipeline)" />
   </include>
 
   <include file="$(find aubo_i5_moveit_config)/launch/moveit_rviz.launch">

aubo_robot/aubo_i5_moveit_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml

@@ -0,0 +1,29 @@
+<launch>
+
+  <!-- Pilz Command Planner Plugin for MoveIt -->
+  <arg name="planning_plugin" value="pilz_industrial_motion_planner::CommandPlanner" />
+
+  <arg name="planning_adapters" value="
+                                       industrial_trajectory_filters/UniformSampleFilter
+                                       default_planner_request_adapters/AddTimeParameterization
+                                       default_planner_request_adapters/FixWorkspaceBounds
+                                       default_planner_request_adapters/FixStartStateBounds
+                                       default_planner_request_adapters/FixStartStateCollision
+                                       default_planner_request_adapters/FixStartStatePathConstraints
+                                       default_planner_request_adapters/ResolveConstraintFrames"/>
+
+  <arg name="start_state_max_bounds_error" value="0.1" />
+  <param name="sample_duration" value="0.005"/>
+  <param name="planning_plugin" value="$(arg planning_plugin)" />
+  <param name="request_adapters" value="$(arg planning_adapters)" />
+  <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
+  <!--<rosparam command="load" file="$(find aubo_i5_moveit_config)/config/cartesian_limits.yaml"/>-->
+
+    <!-- Define default planner (for all groups) -->
+  <param name="default_planner_config" value="PTP" />
+
+  <!-- MoveGroup capabilities to load for this pipeline, append sequence capability -->
+  <param name="capabilities" value="pilz_industrial_motion_planner/MoveGroupSequenceAction
+                                    pilz_industrial_motion_planner/MoveGroupSequenceService" />
+
+</launch>

aubo_robot/aubo_i5_moveit_config/launch/planning_context.launch

@@ -14,6 +14,7 @@
   <!-- Load updated joint limits (override information from URDF) -->
   <group ns="$(arg robot_description)_planning">
     <rosparam command="load" file="$(find aubo_i5_moveit_config)/config/joint_limits.yaml"/>
+     <rosparam command="load" file="$(find aubo_i5_moveit_config)/config/cartesian_limits.yaml"/>
   </group>
 
   <!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace -->

aubo_robot/aubo_i5_pilz_moveit_config/.setup_assistant

@@ -0,0 +1,11 @@
+moveit_setup_assistant_config:
+  URDF:
+    package: aubo_gazebo
+    relative_path: urdf/aubo_i5.xacro
+    xacro_args: "--inorder "
+  SRDF:
+    relative_path: config/aubo_i5.srdf
+  CONFIG:
+    author_name: ZY
+    author_email: ZY@china.cn
+    generated_timestamp: 1639978740

aubo_robot/aubo_i5_pilz_moveit_config/CMakeLists.txt

@@ -0,0 +1,10 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(aubo_i5_pilz_moveit_config)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  PATTERN "setup_assistant.launch" EXCLUDE)
+install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

aubo_robot/aubo_i5_pilz_moveit_config/config/aubo_i5.srdf

@@ -0,0 +1,46 @@
+<?xml version="1.0" ?>
+<!--This does not replace URDF, and is not an extension of URDF.
+    This is a format for representing semantic information about the robot structure.
+    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
+-->
+<robot name="aubo_i5">
+    <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
+    <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
+    <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
+    <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
+    <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
+    <group name="manipulator_i5">
+        <chain base_link="base_link" tip_link="wrist3_Link" />
+    </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="zero" group="manipulator_i5">
+        <joint name="foreArm_joint" value="0" />
+        <joint name="shoulder_joint" value="0" />
+        <joint name="upperArm_joint" value="0" />
+        <joint name="wrist1_joint" value="0" />
+        <joint name="wrist2_joint" value="0" />
+        <joint name="wrist3_joint" value="0" />
+    </group_state>
+    <group_state name="home" group="manipulator_i5">
+        <joint name="foreArm_joint" value="-1.5418" />
+        <joint name="shoulder_joint" value="0" />
+        <joint name="upperArm_joint" value="0" />
+        <joint name="wrist1_joint" value="0" />
+        <joint name="wrist2_joint" value="-1.5209" />
+        <joint name="wrist3_joint" value="0" />
+    </group_state>
+    <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
+    <virtual_joint name="base_link" type="fixed" parent_frame="pedestal" child_link="base_link" />
+    <!--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="pedestal_Link" reason="Adjacent" />
+    <disable_collisions link1="base_link" link2="shoulder_Link" reason="Adjacent" />
+    <disable_collisions link1="base_link" link2="upperArm_Link" reason="Never" />
+    <disable_collisions link1="foreArm_Link" link2="upperArm_Link" reason="Adjacent" />
+    <disable_collisions link1="foreArm_Link" link2="wrist1_Link" reason="Adjacent" />
+    <disable_collisions link1="pedestal_Link" link2="shoulder_Link" reason="Never" />
+    <disable_collisions link1="shoulder_Link" link2="upperArm_Link" reason="Adjacent" />
+    <disable_collisions link1="shoulder_Link" link2="wrist1_Link" reason="Never" />
+    <disable_collisions link1="wrist1_Link" link2="wrist2_Link" reason="Adjacent" />
+    <disable_collisions link1="wrist1_Link" link2="wrist3_Link" reason="Never" />
+    <disable_collisions link1="wrist2_Link" link2="wrist3_Link" reason="Adjacent" />
+</robot>

aubo_robot/aubo_i5_pilz_moveit_config/config/cartesian_limits.yaml

@@ -0,0 +1,5 @@
+cartesian_limits:
+  max_trans_vel: 1
+  max_trans_acc: 2.25
+  max_trans_dec: -5
+  max_rot_vel: 1.57

aubo_robot/aubo_i5_pilz_moveit_config/config/chomp_planning.yaml

@@ -0,0 +1,18 @@
+planning_time_limit: 10.0
+max_iterations: 200
+max_iterations_after_collision_free: 5
+smoothness_cost_weight: 0.1
+obstacle_cost_weight: 1.0
+learning_rate: 0.01
+smoothness_cost_velocity: 0.0
+smoothness_cost_acceleration: 1.0
+smoothness_cost_jerk: 0.0
+ridge_factor: 0.01
+use_pseudo_inverse: false
+pseudo_inverse_ridge_factor: 1e-4
+joint_update_limit: 0.1
+collision_clearence: 0.2
+collision_threshold: 0.07
+use_stochastic_descent: true
+enable_failure_recovery: true
+max_recovery_attempts: 5

aubo_robot/aubo_i5_pilz_moveit_config/config/controllers.yaml

@@ -0,0 +1,13 @@
+controller_list:
+  - name: aubo_i5_controller
+    action_ns: follow_joint_trajectory
+    type: FollowJointTrajectory
+    default: true
+    joints:
+      - shoulder_joint
+      - upperArm_joint
+      - foreArm_joint
+      - wrist1_joint
+      - wrist2_joint
+      - wrist3_joint
+

aubo_robot/aubo_i5_pilz_moveit_config/config/fake_controllers.yaml

@@ -0,0 +1,9 @@
+controller_list:
+  - name: fake_manipulator_i5_controller
+    joints:
+      - shoulder_joint
+      - upperArm_joint
+      - foreArm_joint
+      - wrist1_joint
+      - wrist2_joint
+      - wrist3_joint

aubo_robot/aubo_i5_pilz_moveit_config/config/joint_limits.yaml

@@ -0,0 +1,34 @@
+# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
+# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
+# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
+joint_limits:
+  foreArm_joint:
+    has_velocity_limits: true
+    max_velocity: 3.14
+    has_acceleration_limits: true
+    max_acceleration: 30
+  shoulder_joint:
+    has_velocity_limits: true
+    max_velocity: 3.14
+    has_acceleration_limits: true
+    max_acceleration: 30
+  upperArm_joint:
+    has_velocity_limits: true
+    max_velocity: 3.14
+    has_acceleration_limits: true
+    max_acceleration: 30
+  wrist1_joint:
+    has_velocity_limits: true
+    max_velocity: 2.6
+    has_acceleration_limits: true
+    max_acceleration: 25
+  wrist2_joint:
+    has_velocity_limits: true
+    max_velocity: 2.6
+    has_acceleration_limits: true
+    max_acceleration: 25
+  wrist3_joint:
+    has_velocity_limits: true
+    max_velocity: 2.6
+    has_acceleration_limits: true
+    max_acceleration: 25

aubo_robot/aubo_i5_pilz_moveit_config/config/joint_names.yaml

@@ -0,0 +1,3 @@
+controller_joint_names: [shoulder_joint, upperArm_joint, foreArm_joint, wrist1_joint, wrist2_joint, wrist3_joint]
+robot_name: aubo_i5
+

aubo_robot/aubo_i5_pilz_moveit_config/config/kinematics.yaml

@@ -0,0 +1,4 @@
+manipulator_i5:
+  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+  kinematics_solver_search_resolution: 0.005
+  kinematics_solver_timeout: 0.005