Fix motor names (#255)

* Fix motor names * fix motion * integrate robot hardware
memristor · Mar 9, 2023 · 4920f78 · 4920f78
1 parent 12e6eaf
commit 4920f78
Show file tree

Hide file tree

Showing 10 changed files with 142 additions and 19 deletions.
diff --git a/mep3_behavior/strategies/big_strategy_demo.xml b/mep3_behavior/strategies/big_strategy_demo.xml
@@ -8,7 +8,7 @@
       <AddObstacle label="initial"
                    polygon="0.0;0.0|-0.2;0.0|0.0;-0.2"/>
       <RemoveObstacle label="initial"/>
-      <JointPosition instance="arm_joint"
+      <JointPosition instance="m6"
                      max_velocity="90"
                      position="-90"
                      tolerance="0.3"/>

diff --git a/mep3_bringup/mep3_bringup/launch_utils.py b/mep3_bringup/mep3_bringup/launch_utils.py
@@ -30,7 +30,7 @@ def get_controller_spawners(controller_params_file):
                 '--controller-manager-timeout',
                 '50',
                 '--controller-manager',
-                f'{namespace}/controller_manager',
+                'controller_manager',
             ],
             namespace=namespace)
         )

diff --git a/mep3_controllers/src/joint_position_controller.cpp b/mep3_controllers/src/joint_position_controller.cpp
@@ -105,7 +105,7 @@ namespace mep3_controllers
 
     controller_interface::return_type JointPositionController::update(const rclcpp::Time &time, const rclcpp::Duration & /* period */)
     {
-        // ros2 action send_goal /big/joint_position_command/arm_joint mep3_msgs/action/JointPositionCommand "{ position: -1.57 }"
+        // ros2 action send_goal /big/joint_position_command/m6 mep3_msgs/action/JointPositionCommand "{ position: -1.57 }"
 
         for (std::shared_ptr<Joint> joint : joints_)
         {

diff --git a/mep3_hardware/launch/hardware_launch.py b/mep3_hardware/launch/hardware_launch.py
@@ -95,7 +95,6 @@ def launch_setup(context, *args, **kwargs):
         socketcan_bridge,
         cinch_driver,
         lidar_rplidar,
-        pumps_driver,
         lcd_driver,
     ] + get_controller_spawners(controller_params_file)
 

diff --git a/mep3_hardware/resource/big_controllers.yaml b/mep3_hardware/resource/big_controllers.yaml
@@ -15,7 +15,12 @@ big:
   joint_position_controller:
     ros__parameters:
       joints:
-        - arm_joint
+        - m5
+        - m6
+        - m7
+        - m8
+        - m9
+        - m10
 
   pump_controller:
     ros__parameters:

diff --git a/mep3_hardware/resource/big_description.urdf b/mep3_hardware/resource/big_description.urdf
@@ -22,4 +22,77 @@
             <command_interface name="velocity" />
         </joint>
     </ros2_control>
+    <ros2_control name="gpio" type="system">
+        <hardware>
+            <plugin>mep3_hardware::CanGpioHardwareInterface</plugin>
+            <param name="interface_name">can0</param>
+            <param name="can_id">0x00006C01</param>
+            <param name="can_mask">0x1FFFFFF8</param>
+        </hardware>
+        <gpio name="pump1_pump">
+            <param name="index">0</param>
+            <command_interface name="output"/>
+        </gpio>
+        <gpio name="pump1_valve">
+            <param name="index">6</param>
+            <command_interface name="output"/>
+        </gpio>
+    </ros2_control>
+    <ros2_control name="dynamixels" type="system">
+        <hardware>
+            <plugin>dynamixel_hardware/DynamixelHardware</plugin>
+            <param name="usb_port">/dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A50285BI-if00-port0</param>
+            <param name="baud_rate">115200</param>
+            <param name="use_dummy">false</param>
+            <param name="offset">2.618</param>
+        </hardware>
+        <joint name="m7">
+            <param name="id">3</param>
+            <command_interface name="position"/>
+            <command_interface name="velocity"/>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+        <joint name="m5">
+            <param name="id">7</param>
+            <command_interface name="position"/>
+            <command_interface name="velocity"/>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+        <joint name="m9">
+            <param name="id">8</param>
+            <command_interface name="position"/>
+            <command_interface name="velocity"/>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+        <joint name="m10">
+            <param name="id">14</param>
+            <command_interface name="position"/>
+            <command_interface name="velocity"/>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+        <joint name="m8">
+            <param name="id">9</param>
+            <command_interface name="position"/>
+            <command_interface name="velocity"/>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+        <joint name="m6">
+            <param name="id">55</param>
+            <command_interface name="position"/>
+            <command_interface name="velocity"/>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+    </ros2_control>
 </robot>
diff --git a/mep3_hardware/test/dynamixels_and_pump/controllers.yaml b/mep3_hardware/test/dynamixels_and_pump/controllers.yaml
@@ -14,9 +14,9 @@ pump_controller:
 joint_position_controller:
   ros__parameters:
     joints:
-      - joint_cherry_vertical
-      - joint_left_right
-      - joint_cake_vertical
-      - joint_cake_left_gripper
-      - joint_cake_right_gripper
-      - joint_cherry_gripper
+      - m5
+      - m6
+      - m7
+      - m8
+      - m9
+      - m10
diff --git a/mep3_hardware/test/dynamixels_and_pump/description.urdf b/mep3_hardware/test/dynamixels_and_pump/description.urdf
@@ -24,47 +24,47 @@
             <param name="use_dummy">false</param>
             <param name="offset">2.618</param>
         </hardware>
-        <joint name="joint_cherry_gripper">
+        <joint name="m7">
             <param name="id">3</param>
             <command_interface name="position"/>
             <command_interface name="velocity"/>
             <state_interface name="position"/>
             <state_interface name="velocity"/>
             <state_interface name="effort"/>
         </joint>
-        <joint name="joint_cherry_vertical">
+        <joint name="m5">
             <param name="id">7</param>
             <command_interface name="position"/>
             <command_interface name="velocity"/>
             <state_interface name="position"/>
             <state_interface name="velocity"/>
             <state_interface name="effort"/>
         </joint>
-        <joint name="joint_cake_left_gripper">
+        <joint name="m9">
             <param name="id">8</param>
             <command_interface name="position"/>
             <command_interface name="velocity"/>
             <state_interface name="position"/>
             <state_interface name="velocity"/>
             <state_interface name="effort"/>
         </joint>
-        <joint name="joint_cake_right_gripper">
+        <joint name="m10">
             <param name="id">14</param>
             <command_interface name="position"/>
             <command_interface name="velocity"/>
             <state_interface name="position"/>
             <state_interface name="velocity"/>
             <state_interface name="effort"/>
         </joint>
-        <joint name="joint_cake_vertical">
+        <joint name="m8">
             <param name="id">9</param>
             <command_interface name="position"/>
             <command_interface name="velocity"/>
             <state_interface name="position"/>
             <state_interface name="velocity"/>
             <state_interface name="effort"/>
         </joint>
-        <joint name="joint_left_right">
+        <joint name="m6">
             <param name="id">55</param>
             <command_interface name="position"/>
             <command_interface name="velocity"/>

diff --git a/mep3_simulation/resource/big_description.urdf b/mep3_simulation/resource/big_description.urdf
@@ -31,7 +31,7 @@
             <command_interface name="velocity" />
             <param name="sensor">right_encoder</param>
         </joint>
-        <joint name="arm_joint">
+        <joint name="m6">
             <command_interface name="position" />
             <command_interface name="velocity" />
             <state_interface name="position" />
@@ -48,4 +48,50 @@
             <command_interface name="output" />
         </gpio>
     </ros2_control>
+    <ros2_control name="dynamixels" type="system">
+        <hardware>
+            <plugin>dynamixel_hardware/DynamixelHardware</plugin>
+            <param name="use_dummy">true</param>
+        </hardware>
+        <joint name="m7">
+            <param name="id">3</param>
+            <command_interface name="position"/>
+            <command_interface name="velocity"/>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+        <joint name="m5">
+            <param name="id">7</param>
+            <command_interface name="position"/>
+            <command_interface name="velocity"/>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+        <joint name="m9">
+            <param name="id">8</param>
+            <command_interface name="position"/>
+            <command_interface name="velocity"/>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+        <joint name="m10">
+            <param name="id">14</param>
+            <command_interface name="position"/>
+            <command_interface name="velocity"/>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+        <joint name="m8">
+            <param name="id">9</param>
+            <command_interface name="position"/>
+            <command_interface name="velocity"/>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+    </ros2_control>
 </robot>
diff --git a/mep3_simulation/webots_data/worlds/eurobot.wbt b/mep3_simulation/webots_data/worlds/eurobot.wbt
@@ -450,7 +450,7 @@ DEF ROBOT_BIG GenericRobot {
           }
           device [
             RotationalMotor {
-              name "arm_joint"
+              name "m6"
               controlPID 1000 0 0
               minPosition -3.14
               maxTorque 1000