Enable Gazebo simulation for mycobot280 and with Moveit

mycobot_280_moveit/config/fake_controllers.yaml

@@ -1,9 +1,9 @@
 controller_list:
   - name: fake_arm_group_controller
     joints:
-      - joint2_to_joint1
-      - joint3_to_joint2
-      - joint4_to_joint3
-      - joint5_to_joint4
-      - joint6_to_joint5
-      - joint6output_to_joint6
+      - joint1
+      - joint2
+      - joint3
+      - joint4
+      - joint5
+      - joint6

mycobot_280_moveit/config/firefighter.srdf

@@ -10,33 +10,33 @@
     <!--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="arm_group">
-        <joint name="joint2_to_joint1" />
-        <joint name="joint3_to_joint2" />
-        <joint name="joint4_to_joint3" />
-        <joint name="joint5_to_joint4" />
-        <joint name="joint6_to_joint5" />
-        <joint name="joint6output_to_joint6" />
+        <joint name="joint1" />
+        <joint name="joint2" />
+        <joint name="joint3" />
+        <joint name="joint4" />
+        <joint name="joint5" />
+        <joint name="joint6" />
     </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="init_pose" group="arm_group">
-        <joint name="joint2_to_joint1" value="0" />
-        <joint name="joint3_to_joint2" value="0" />
-        <joint name="joint4_to_joint3" value="0" />
-        <joint name="joint5_to_joint4" value="0" />
-        <joint name="joint6_to_joint5" value="0" />
-        <joint name="joint6output_to_joint6" value="0" />
+        <joint name="joint1" value="0" />
+        <joint name="joint2" value="0" />
+        <joint name="joint3" value="0" />
+        <joint name="joint4" value="0" />
+        <joint name="joint5" value="0" />
+        <joint name="joint6" 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="vitual_joint" type="fixed" parent_frame="basic" child_link="joint1" />
+    <virtual_joint name="vitual_joint" type="fixed" parent_frame="basic" child_link="link1" />
     <!--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="joint1" link2="joint2" reason="Adjacent" />
-    <disable_collisions link1="joint1" link2="joint5" reason="Never" />
-    <disable_collisions link1="joint2" link2="joint3" reason="Adjacent" />
-    <disable_collisions link1="joint2" link2="joint5" reason="Never" />
-    <disable_collisions link1="joint3" link2="joint4" reason="Adjacent" />
-    <disable_collisions link1="joint3" link2="joint5" reason="Never" />
-    <disable_collisions link1="joint4" link2="joint5" reason="Adjacent" />
-    <disable_collisions link1="joint5" link2="joint6" reason="Adjacent" />
-    <disable_collisions link1="joint5" link2="joint6_flange" reason="Never" />
-    <disable_collisions link1="joint6" link2="joint6_flange" reason="Adjacent" />
+    <disable_collisions link1="link1" link2="link2" reason="Adjacent" />
+    <disable_collisions link1="link1" link2="link5" reason="Never" />
+    <disable_collisions link1="link2" link2="link3" reason="Adjacent" />
+    <disable_collisions link1="link2" link2="link5" reason="Never" />
+    <disable_collisions link1="link3" link2="link4" reason="Adjacent" />
+    <disable_collisions link1="link3" link2="link5" reason="Never" />
+    <disable_collisions link1="link4" link2="link5" reason="Adjacent" />
+    <disable_collisions link1="link5" link2="link6" reason="Adjacent" />
+    <disable_collisions link1="link5" link2="link7" reason="Never" />
+    <disable_collisions link1="link6" link2="link7" reason="Adjacent" />
 </robot>

mycobot_280_moveit/config/gazebo_controllers.yaml

@@ -0,0 +1,30 @@
+joint_state_controller:
+  type: joint_state_controller/JointStateController
+  publish_rate: 50
+arm_controller:
+  type: position_controllers/JointTrajectoryController
+  joints:
+    - joint1
+    - joint2
+    - joint3
+    - joint4
+    - joint5
+    - joint6
+  gains:
+    joint1: { p: 10, d: 0, i: 0, i_clamp: 0 }
+    joint2: { p: 10, d: 0, i: 0, i_clamp: 0 }
+    joint3: { p: 10, d: 0, i: 0, i_clamp: 0 }
+    joint4: { p: 10, d: 0, i: 0, i_clamp: 0 }
+    joint5: { p: 10, d: 0, i: 0, i_clamp: 0 }
+    joint6: { p: 10, d: 0, i: 0, i_clamp: 0 }
+
+  constraints:
+    stopped_velocity_tolerance: 0
+    joint1: {trajectory: 0.1, goal: 0}
+    joint2: {trajectory: 0.1, goal: 0}
+    joint3: {trajectory: 0.1, goal: 0}
+    joint4: {trajectory: 0.1, goal: 0}
+    joint5: {trajectory: 0.1, goal: 0}
+    joint6: {trajectory: 0.1, goal: 0}
+
+

mycobot_280_moveit/config/joint_limits.yaml

@@ -2,32 +2,32 @@
 # 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:
-  joint2_to_joint1:
+  joint1:
     has_velocity_limits: false
     max_velocity: 0
     has_acceleration_limits: false
     max_acceleration: 0
-  joint3_to_joint2:
+  joint2:
     has_velocity_limits: false
     max_velocity: 0
     has_acceleration_limits: false
     max_acceleration: 0
-  joint4_to_joint3:
+  joint3:
     has_velocity_limits: false
     max_velocity: 0
     has_acceleration_limits: false
     max_acceleration: 0
-  joint5_to_joint4:
+  joint4:
     has_velocity_limits: false
     max_velocity: 0
     has_acceleration_limits: false
     max_acceleration: 0
-  joint6_to_joint5:
+  joint5:
     has_velocity_limits: false
     max_velocity: 0
     has_acceleration_limits: false
     max_acceleration: 0
-  joint6output_to_joint6:
+  joint6:
     has_velocity_limits: false
     max_velocity: 0
     has_acceleration_limits: false

mycobot_280_moveit/config/ompl_planning.yaml

@@ -146,5 +146,5 @@ arm_group:
     - LazyPRMstar
     - SPARS
     - SPARStwo
-  projection_evaluator: joints(joint2_to_joint1,joint3_to_joint2)
+  projection_evaluator: joints(joint1,joint2)
   longest_valid_segment_fraction: 0.005

mycobot_280_moveit/config/ros_controllers.yaml

@@ -9,18 +9,33 @@ generic_hw_control_loop:
 # Settings for ros_control hardware interface
 hardware_interface:
   joints:
-    - vitual_joint
-    - joint2_to_joint1
-    - joint3_to_joint2
-    - joint4_to_joint3
-    - joint5_to_joint4
-    - joint6_to_joint5
-    - joint6output_to_joint6
+    - joint1
+    - joint2
+    - joint3
+    - joint4
+    - joint5
+    - joint6
   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: arm_controller
+    action_ns: follow_joint_trajectory
+    default: True
+    type: FollowJointTrajectory
+    joints:
+      - joint1
+      - joint2
+      - joint3
+      - joint4
+      - joint5
+      - joint6
+  - name: gripper_controller
+    action_ns: follow_joint_trajectory
+    default: True
+    type: FollowJointTrajectory
+    joints:
+      joint6

mycobot_280_moveit/launch/demo_gazebo.launch

@@ -22,7 +22,7 @@
   <arg name="gazebo_gui" default="true"/>
   <arg name="paused" default="false"/>
   <!-- By default, use the urdf location provided from the package -->
-  <arg name="urdf_path" default="$(find mycobot_description)/urdf/mycobot/mycobot_urdf.urdf"/>
+  <arg name="urdf_path" default="$(find mycobot_description)/urdf/mycobot/mycobot.gazebo.xacro"/>
 
   <!-- launch the gazebo simulator and spawn the robot -->
   <include file="$(find mycobot_280_moveit)/launch/gazebo.launch" >
@@ -38,16 +38,6 @@
 
   <!-- If needed, broadcast static tf for robot root -->
 
-
-  <!-- We do not have a robot connected, so publish fake joint states -->
-  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
-    <param name="use_gui" value="$(arg use_gui)"/>
-    <rosparam param="source_list">[/joint_states]</rosparam>
-  </node>
-
-  <!-- Given the published joint states, publish tf for the robot links -->
-  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
-
   <!-- Run the main MoveIt! executable without trajectory execution (we do not have controllers configured by default) -->
   <include file="$(find mycobot_280_moveit)/launch/move_group.launch">
     <arg name="allow_trajectory_execution" value="true"/>

mycobot_280_moveit/launch/gazebo.launch

@@ -2,7 +2,8 @@
 <launch>
   <arg name="paused" default="false"/>
   <arg name="gazebo_gui" default="true"/>
-  <arg name="urdf_path" default="$(find mycobot_description)/urdf/mycobot/mycobot_urdf.urdf"/>
+  <arg name="urdf_path" default="$(find mycobot_description)/urdf/mycobot/mycobot.gazebo.xacro"/>
+  <arg name="robot_namespace" default="" />
 
   <!-- startup simulated world -->
   <include file="$(find gazebo_ros)/launch/empty_world.launch">
@@ -12,12 +13,16 @@
   </include>
 
   <!-- send robot urdf to param server -->
-  <param name="robot_description" textfile="$(arg urdf_path)" />
+  <param name="robot_description" command="$(find xacro)/xacro '$(arg urdf_path)' robot_namespace:='$(arg robot_namespace)'" />
 
   <!-- push robot_description to factory and spawn robot in gazebo at the origin, change x,y,z arguments to spawn in a different position -->
   <node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot -x 0 -y 0 -z 0"
     respawn="false" output="screen" />
 
-  <include file="$(find mycobot_280_moveit)/launch/ros_controllers.launch"/>
+  <!-- joint controller -->
+  <include file="$(find mycobot_280_moveit)/launch/gazebo_controllers.launch"/>
+
+  <!-- robot state publisher for tf -->
+  <node name="$(anon robot_state_publisher)" pkg="robot_state_publisher" type="robot_state_publisher" />
 
 </launch>

mycobot_280_moveit/launch/gazebo_controllers.launch

@@ -0,0 +1,11 @@
+<?xml version="1.0"?>
+<launch>
+
+  <!-- Load joint controller configurations from YAML file to parameter server -->
+  <rosparam file="$(find mycobot_280_moveit)/config/gazebo_controllers.yaml" command="load"/>
+
+  <!-- Load the controllers -->
+  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
+        output="screen" args="joint_state_controller arm_controller" />
+
+</launch>

mycobot_280_moveit/launch/moveit.rviz

@@ -74,37 +74,37 @@ Visualization Manager:
           Expand Link Details: false
           Expand Tree: false
           Link Tree Style: Links in Alphabetic Order
-          joint1:
+          link1:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          joint2:
+          link2:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          joint3:
+          link3:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          joint4:
+          link4:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          joint5:
+          link5:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          joint6:
+          link6:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          joint6_flange:
+          link7:
             Alpha: 1
             Show Axes: false
             Show Trail: false
@@ -154,37 +154,37 @@ Visualization Manager:
           Expand Link Details: false
           Expand Tree: false
           Link Tree Style: Links in Alphabetic Order
-          joint1:
+          link1:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          joint2:
+          link2:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          joint3:
+          link3:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          joint4:
+          link4:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          joint5:
+          link5:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          joint6:
+          link6:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          joint6_flange:
+          link7:
             Alpha: 1
             Show Axes: false
             Show Trail: false
@@ -198,7 +198,7 @@ Visualization Manager:
   Global Options:
     Background Color: 48; 48; 48
     Default Light: true
-    Fixed Frame: joint1
+    Fixed Frame: link1
     Frame Rate: 30
   Name: root
   Tools:
@@ -226,7 +226,7 @@ Visualization Manager:
       Name: Current View
       Near Clip Distance: 0.00999999978
       Pitch: 0.439796686
-      Target Frame: joint1
+      Target Frame: link1
       Value: XYOrbit (rviz)
       Yaw: 2.5317657
     Saved: ~

mycobot_280_moveit/launch/planning_context.launch

@@ -6,7 +6,7 @@
   <arg name="robot_description" default="robot_description"/>
 
   <!-- Load universal robot description format (URDF) -->
-  <param if="$(arg load_robot_description)" name="$(arg robot_description)" textfile="$(find mycobot_description)/urdf/mycobot/mycobot_urdf.urdf"/>
+  <param if="$(arg load_robot_description)" name="$(arg robot_description)" command="xacro gazebo_plugin:=true '$(find mycobot_description)/urdf/mycobot/mycobot.urdf.xacro'"/>
 
   <!-- The semantic description that corresponds to the URDF -->
   <param name="$(arg robot_description)_semantic" textfile="$(find mycobot_280_moveit)/config/firefighter.srdf" />

mycobot_280_moveit/launch/ros_controllers.launch

@@ -6,6 +6,6 @@
 
   <!-- Load the controllers -->
   <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
-    output="screen" args=""/>
+        output="screen" args="" />
 
 </launch>

mycobot_280_moveit/package.xml

@@ -22,7 +22,9 @@
   <run_depend>moveit_planners_ompl</run_depend>
   <run_depend>moveit_ros_visualization</run_depend>
   <run_depend>moveit_setup_assistant</run_depend>
+  <run_depend>moveit_simple_controller_manager</run_depend>
   <run_depend>joint_state_publisher</run_depend>
+  <run_depend>joint_trajectory_controller</run_depend>
   <run_depend>robot_state_publisher</run_depend>
   <run_depend>xacro</run_depend>
   <!-- This package is referenced in the warehouse launch files, but does not build out of the box at the moment. Commented the dependency until this works. -->