Add test_controller ros2 package (#19)

icub-tech-iit · Nov 24, 2023 · 944835a · 944835a
2 parents e65b4ed + 66378bd
commit 944835a
Show file tree

Hide file tree

Showing 27 changed files with 3,581 additions and 129 deletions.
diff --git a/.gitignore b/.gitignore
@@ -1,5 +1,5 @@
 .vscode/
-*.setup_assistant
+*/.setup_assistant
 log/
 install/
 build/
diff --git a/README.md b/README.md
@@ -135,7 +135,7 @@ It contains the same information described in the previous paragraph, but custom
 
 ## Use case
 
-As described in the previous section, this repository contains two demos that shows the possibility to control a yarp-based robot within the MoveIt2 framework in the Cartesian space. First of all, make sure that `yarpserver` is running on your machine. Then, you can start launching a basic simulation:
+As described in the previous section, this repository contains two demos that show the possibility to control a yarp-based robot within the MoveIt2 framework in the Cartesian space. First of all, make sure that `yarpserver` is running on your machine. Then, you can start launching a basic simulation:
 
 ```shell
 # Build the packages within your ros2 workspace
@@ -179,7 +179,7 @@ source install/setup.bash
 export YARP_ROBOT_NAME="icub"
 
 # For the grasping demo
-ros2 launch grasp_moveit grasp_demo.launch.py
+ros2 launch robot_moveit grasp_demo.launch.py
 ```
 
 If you want to see iCub performing a circle movement, instead of the last line, you can run:
@@ -191,5 +191,6 @@ ros2 launch robot_moveit circle_demo.launch.py
 and follow the instructions on the third shell you opened.
 You should have something like this:
 
-<video src="assets/circle.mp4" controls title="circle_demo">
-</video>
+<div align="center">
+  <video src="assets/circle.mp4" width="1000">
+</div>
diff --git a/assets/icub_root_link.png b/assets/icub_root_link.png
diff --git a/assets/manipulation1_tollerance.png b/assets/manipulation1_tollerance.png
diff --git a/assets/solve_type.png b/assets/solve_type.png
diff --git a/assets/workspace_estimation.png b/assets/workspace_estimation.png
diff --git a/ergocub_moveit_config/.setup_assistant b/ergocub_moveit_config/.setup_assistant
@@ -7,4 +7,19 @@ moveit_setup_assistant_config:
   package_settings:
     author_name: Martina Gloria
     author_email: martina.gloria@iit.it
-    generated_timestamp: 1695734298
+    generated_timestamp: 1699889574
+  control_xacro:
+    command:
+      - position
+    state:
+      - position
+      - velocity
+  modified_urdf:
+    xacros:
+      - control_xacro
+  control_xacro:
+    command:
+      - position
+    state:
+      - position
+      - velocity
diff --git a/ergocub_moveit_config/config/ergocub.ros2_control.xacro b/ergocub_moveit_config/config/ergocub.ros2_control.xacro
@@ -274,7 +274,7 @@
                 <state_interface name="velocity"/>
             </joint>
         </ros2_control>
-        <ros2_control name="left_arm_torso_${name}" type="system">
+        <ros2_control name="right_arm_torso_${name}" type="system">
           <hardware>
               <plugin>robot_controller/RobotController</plugin>
               <param name="node_name">ros2_cb_node</param>
@@ -301,52 +301,52 @@
             </state_interface>
             <state_interface name="velocity"/>
         </joint>
-          <joint name="l_shoulder_pitch">
+          <joint name="r_shoulder_pitch">
             <command_interface name="position"/>
             <state_interface name="position">
-              <param name="initial_value">${initial_positions['l_shoulder_pitch']}</param>
+              <param name="initial_value">${initial_positions['r_shoulder_pitch']}</param>
             </state_interface>
             <state_interface name="velocity"/>
           </joint>
-          <joint name="l_shoulder_roll">
+          <joint name="r_shoulder_roll">
               <command_interface name="position"/>
               <state_interface name="position">
-                <param name="initial_value">${initial_positions['l_shoulder_roll']}</param>
+                <param name="initial_value">${initial_positions['r_shoulder_roll']}</param>
               </state_interface>
               <state_interface name="velocity"/>
           </joint>
-          <joint name="l_shoulder_yaw">
+          <joint name="r_shoulder_yaw">
               <command_interface name="position"/>
               <state_interface name="position">
-                <param name="initial_value">${initial_positions['l_shoulder_yaw']}</param>
+                <param name="initial_value">${initial_positions['r_shoulder_yaw']}</param>
               </state_interface>
               <state_interface name="velocity"/>
           </joint>
-          <joint name="l_elbow">
+          <joint name="r_elbow">
               <command_interface name="position"/>
               <state_interface name="position">
-                <param name="initial_value">${initial_positions['l_elbow']}</param>
+                <param name="initial_value">${initial_positions['r_elbow']}</param>
               </state_interface>
               <state_interface name="velocity"/>
           </joint>
-          <joint name="l_wrist_yaw">
+          <joint name="r_wrist_yaw">
               <command_interface name="position"/>
               <state_interface name="position">
-                <param name="initial_value">${initial_positions['l_wrist_yaw']}</param>
+                <param name="initial_value">${initial_positions['r_wrist_yaw']}</param>
               </state_interface>
               <state_interface name="velocity"/>
           </joint>
-          <joint name="l_wrist_roll">
+          <joint name="r_wrist_roll">
               <command_interface name="position"/>
               <state_interface name="position">
-                <param name="initial_value">${initial_positions['l_wrist_roll']}</param>
+                <param name="initial_value">${initial_positions['r_wrist_roll']}</param>
               </state_interface>
               <state_interface name="velocity"/>
           </joint>
-          <joint name="l_wrist_pitch">
+          <joint name="r_wrist_pitch">
               <command_interface name="position"/>
               <state_interface name="position">
-                <param name="initial_value">${initial_positions['l_wrist_pitch']}</param>
+                <param name="initial_value">${initial_positions['r_wrist_pitch']}</param>
               </state_interface>
               <state_interface name="velocity"/>
           </joint>

diff --git a/ergocub_moveit_config/config/ergocub.srdf b/ergocub_moveit_config/config/ergocub.srdf
@@ -59,47 +59,12 @@
         <joint name="torso_yaw"/>
         <chain base_link="torso_1" tip_link="chest"/>
     </group>
-    <group name="left_arm_torso">
-        <link name="torso_1"/>
-        <link name="torso_2"/>
-        <link name="chest"/>
-        <link name="l_shoulder_1"/>
-        <link name="l_shoulder_2"/>
-        <link name="l_shoulder_3"/>
-        <link name="l_upperarm"/>
-        <link name="l_forearm"/>
-        <link name="l_wrist_1"/>
-        <link name="l_wrist_2"/>
-        <link name="l_hand_palm"/>
-        <joint name="torso_roll"/>
-        <joint name="torso_pitch"/>
-        <joint name="torso_yaw"/>
-        <joint name="l_shoulder_pitch"/>
-        <joint name="l_shoulder_roll"/>
-        <joint name="l_shoulder_yaw"/>
-        <joint name="l_elbow"/>
-        <joint name="l_wrist_yaw"/>
-        <joint name="l_wrist_roll"/>
-        <joint name="l_wrist_pitch"/> 
-        <chain base_link="torso_1" tip_link="l_hand_palm"/> 
+    <group name="right_arm_torso">
+        <group name="right_arm"/>
         <group name="torso"/>
-        <group name="left_arm"/>
     </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="hi" group="left_arm_torso">
-        <joint name="l_elbow" value="0.2184"/>
-        <joint name="l_shoulder_pitch" value="-2.967"/>
-        <joint name="l_shoulder_roll" value="0.677"/>
-        <joint name="l_shoulder_yaw" value="1.3962"/>
-        <joint name="l_wrist_pitch" value="0"/>
-        <joint name="l_wrist_roll" value="0"/>
-        <joint name="l_wrist_yaw" value="0"/>
-        <joint name="torso_pitch" value="0"/>
-        <joint name="torso_roll" value="0"/>
-        <joint name="torso_yaw" value="0"/>
-    </group_state>
     <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
-    <end_effector name="left_hand_palm" parent_group="left_arm_torso" parent_link="l_wrist_2" group="left_arm"/>
+    <end_effector name="right_hand" parent_link="r_hand_palm" group="right_arm" parent_group="right_arm_torso"/>
     <!--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="virtual_joint" type="fixed" parent_frame="world" child_link="root_link"/>
     <!--PASSIVE JOINT: Purpose: this element is used to mark joints that are not actuated-->

diff --git a/ergocub_moveit_config/config/initial_positions.yaml b/ergocub_moveit_config/config/initial_positions.yaml
@@ -7,22 +7,10 @@ initial_positions:
   l_hip_pitch: 0
   l_hip_roll: 0
   l_hip_yaw: 0
-  l_index_add: 0
-  l_index_dist: 0
-  l_index_prox: 0
   l_knee: 0
-  l_middle_dist: 0
-  l_middle_prox: 0
-  l_pinkie_dist: 0
-  l_pinkie_prox: 0
-  l_ring_dist: 0
-  l_ring_prox: 0
   l_shoulder_pitch: 0
   l_shoulder_roll: 0
   l_shoulder_yaw: 0
-  l_thumb_add: 0
-  l_thumb_dist: 0
-  l_thumb_prox: 0
   l_wrist_pitch: 0
   l_wrist_roll: 0
   l_wrist_yaw: 0
@@ -35,25 +23,13 @@ initial_positions:
   r_hip_pitch: 0
   r_hip_roll: 0
   r_hip_yaw: 0
-  r_index_add: 0
-  r_index_dist: 0
-  r_index_prox: 0
   r_knee: 0
-  r_middle_dist: 0
-  r_middle_prox: 0
-  r_pinkie_dist: 0
-  r_pinkie_prox: 0
-  r_ring_dist: 0
-  r_ring_prox: 0
   r_shoulder_pitch: 0
   r_shoulder_roll: 0
   r_shoulder_yaw: 0
-  r_thumb_add: 0
-  r_thumb_dist: 0
-  r_thumb_prox: 0
   r_wrist_pitch: 0
   r_wrist_roll: 0
   r_wrist_yaw: 0
   torso_pitch: 0
   torso_roll: 0
-  torso_yaw: 0
+  torso_yaw: 0
diff --git a/ergocub_moveit_config/config/kinematics.yaml b/ergocub_moveit_config/config/kinematics.yaml
@@ -6,15 +6,15 @@ left_arm:
   kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
   kinematics_solver_search_resolution: 0.0050000000000000001
   kinematics_solver_timeout: 0.0050000000000000001
-left_arm_torso:
+left_leg:
   kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
   kinematics_solver_search_resolution: 0.0050000000000000001
   kinematics_solver_timeout: 0.0050000000000000001
-left_leg:
+right_arm:
   kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
   kinematics_solver_search_resolution: 0.0050000000000000001
   kinematics_solver_timeout: 0.0050000000000000001
-right_arm:
+right_arm_torso:
   kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
   kinematics_solver_search_resolution: 0.0050000000000000001
   kinematics_solver_timeout: 0.0050000000000000001