Support Moveit for real arm

mycobot_280/launch/mycobot_follow.launch

@@ -1,4 +1,7 @@
 <launch>
+	<arg name="port" default="/dev/ttyUSB0" />
+	<arg name="baud" default="115200" />
+
 	<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_urdf.urdf"/>
 	<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot.rviz" />
 
@@ -8,4 +11,10 @@
 		<rosparam param="source_list" subst_value="true">["joint_states"]</rosparam>
 	</node>
 	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+
+        <node name="mycobot_read_interface"  pkg="mycobot_280" type="follow_display.py" >
+          <param name="port" type="string" value="$(arg port)" />
+	  <param name="baud" type="int" value="$(arg baud)" />
+        </node>
+
 </launch>

mycobot_280/launch/slider_control.launch

@@ -1,6 +1,6 @@
 <launch>
-	<!-- <arg name="port" default="/dev/ttyUSB0" />
-	<arg name="baud" default="115200" /> -->
+	<arg name="port" default="/dev/ttyUSB0" />
+	<arg name="baud" default="115200" />
 
 	<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_urdf.urdf"/>
 	<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot.rviz" />
@@ -15,10 +15,10 @@
 		<!-- <rosparam param="source_list" subst_value="true">["joint_states"]</rosparam> -->
 	</node>
 	<!-- Open control script -->
-	<!-- <node name="control_slider" pkg="mycobot_280" type="slider_control.py"> 
+	<node name="control_slider" pkg="mycobot_280" type="slider_control.py"> 
 		<param name="port" type="string" value="$(arg port)" />
 		<param name="baud" type="int" value="$(arg baud)" />
-	</node> -->
+	</node>
 	<!-- Show in Rviz -->
 	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
 </launch>

mycobot_280/launch/startup.launch

@@ -0,0 +1,12 @@
+<launch>
+  <arg name="port" default="/dev/ttyUSB0" doc ="the serial port for connection" />
+  <arg name="baud" default="115200" doc ="the serial baudrate" />
+
+  <!-- Driver -->
+  <include file="$(find mycobot_communication)/launch/mycobot_interface.launch" >
+    <arg name="port" value="$(arg port)" />
+    <arg name="baud" value="$(arg baud)" />
+    <arg name="robot_type" value="mycobot" />
+  </include>
+
+</launch>

mycobot_280/scripts/follow_display.py

@@ -85,8 +85,10 @@ def talker():
             coords = [0, 0, 0, 0, 0, 0]
             rospy.loginfo("error [101]: can not get coord values")
 
-        marker_.pose.position.x = coords[1] / 1000 * -1
-        marker_.pose.position.y = coords[0] / 1000
+        # marker_.pose.position.x = coords[1] / 1000 * -1
+        # marker_.pose.position.y = coords[0] / 1000
+        marker_.pose.position.x = coords[0] / 1000
+        marker_.pose.position.y = coords[1] / 1000
         marker_.pose.position.z = coords[2] / 1000
 
         marker_.color.a = 1.0

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.launch

@@ -24,12 +24,11 @@
   </include>
 
   <!-- 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">[move_group/fake_controller_joint_states]</rosparam>
+    <rosparam param="source_list">[/joint_states]</rosparam>
   </node>
 
   <!-- Given the published joint states, publish tf for the robot links -->
@@ -38,7 +37,7 @@
   <!-- 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"/>
-    <arg name="fake_execution" value="true"/>
+    <arg name="fake_execution" value="false"/>
     <arg name="info" value="true"/>
     <arg name="debug" value="$(arg debug)"/>
   </include>

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>