Correct URDF to true joint limits + Add and update documentation (#6)

* Rename openarm_bimanual_teleop to openarm_bimanual_bringup

* Update package descriptions

* Add quickstart link

* Add note to source overlay

* Add test installation step

* Add poses and update default rviz config

* Fix rev1 to match hardware

* Align rev3 with hardware joint limits

* Match hardware joint limits

* Complete with more poses

* Update README.md with moveit2 video

* Add new READMEs in openarm_bringup and openarm_bimanual_moveit_config

* Add octomap note

README.md

@@ -1,13 +1,21 @@
 # ROS2 packages for OpenArm robots
 
-- openarm_bimanual_description: humanoid upper body with two arms (urdf)
+[Quickstart](#installation)
-- openarm_bringup: [ros2_control](https://control.ros.org/humble/index.html) bringup
+
-- openarm_description: single arm (urdf)
+
+https://github.com/user-attachments/assets/90b44ef4-5cdc-4bf5-b56f-be2a5ff264b4
+
+
+
+- openarm_bimanual_description: dual arm urdf with torso and realsense head camera
+- openarm_bimanual_moveit_config: bimanual motion planning with OctoMap occupancy grid mapping
+- openarm_bimanual_bringup: setup scripts for bimanual openarm
+- openarm_bringup: setup scripts for single physical openarm
+- openarm_description: single arm urdf
 - openarm_hardware: hardware interface for ros2_control
 - openarm_moveit_config: motion planning with [moveit2](https://github.com/moveit/moveit2)
 
 
-
 ### Description Packages
 
 Each link has a visual mesh and a collision mesh, as shown in the figures below:
@@ -21,10 +29,11 @@ https://github.com/user-attachments/assets/a0f962e5-6150-49ce-b18e-9914bcb322ef
 
 ## Installation
 
-1. [Install ROS2](https://docs.ros.org/en/humble/Installation.html) (Humble with Ubuntu 22.04 is recommended)
+1. [Install ROS2 and ros-dev-tools](https://docs.ros.org/en/humble/Installation.html) (tested on Humble with Ubuntu 22.04)
-2. [Create a ROS2 workspace](https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Creating-A-Workspace/Creating-A-Workspace.html)
+2. [Create a ROS2 workspace and source the overlay](https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Creating-A-Workspace/Creating-A-Workspace.html)
 
 ```sh
+source /opt/ros/humble/setup.bash # change humble to your ROS2 distro
 mkdir -p ~/ros2_ws/src
 cd ~/ros2_ws/src
 git clone https://github.com/reazon-research/openarm_ros2.git
@@ -46,9 +55,16 @@ colcon build
 
 ```sh
 cd ~/ros2_ws
-. install/setup.bash
+source install/setup.bash
 ```
 
+5. Test the installation by launching a demo. It may be necessary to restart your computer once.
+
+```sh
+ros2 launch openarm_bimanual_moveit_config demo.launch.py
+```
+
+
 ## License
 
 All packages of `openarm_ros2` are licensed under the [BSD-3-Clause](https://opensource.org/license/bsd-3-clause).

openarm_bimanual_bringup/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.8)
-project(openarm_bimanual_teleop)
+project(openarm_bimanual_bringup)
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)

openarm_bimanual_bringup/launch/start_teleop.launch.py

@@ -1,5 +1,10 @@
 import launch
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, TimerAction, RegisterEventHandler
+from launch.actions import (
+    DeclareLaunchArgument,
+    IncludeLaunchDescription,
+    TimerAction,
+    RegisterEventHandler,
+)
 from launch.event_handlers import OnProcessStart
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, Command
 from launch_ros.actions import Node
@@ -8,14 +13,14 @@ from launch_ros.parameter_descriptions import ParameterValue
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 import pathlib
 
-def generate_launch_description():
 
+def generate_launch_description():
     pkg_share = FindPackageShare(package="openarm_bimanual_description")
 
-
+    xacro_path = (
-    xacro_path = pathlib.Path(pkg_share.find(
+        pathlib.Path(pkg_share.find("openarm_bimanual_description"))
-        "openarm_bimanual_description"
+        / "urdf/openarm_bimanual.urdf.xacro"
-    )) / "urdf/openarm_bimanual.urdf.xacro"
+    )
 
     use_sim_time = LaunchConfiguration("use_sim_time")
     use_sim_time_launch_arg = DeclareLaunchArgument(name="use_sim_time", default_value="false")
@@ -35,30 +40,28 @@ def generate_launch_description():
         launch_arguments=dict(use_sim_time=use_sim_time).items(),
     )
 
+    robot_description_content = Command(["xacro ", LaunchConfiguration("model")])
 
-    robot_description_content = Command([
+    robot_description_param = {
-    'xacro ', LaunchConfiguration("model")
+        "robot_description": ParameterValue(robot_description_content, value_type=str)
-    ])
+    }
 
-    robot_description_param = {'robot_description': ParameterValue(robot_description_content, value_type=str)}
+    controller_params = PathJoinSubstitution(
-
+        [FindPackageShare(package="openarm_bimanual_bringup"), "config", "controllers.yaml"]
-    controller_params = PathJoinSubstitution([
+    )
-        FindPackageShare(package='openarm_bimanual_teleop'),
-        'config',
-        'controllers.yaml'
-        ])
 
     controller_manager = Node(
-        package='controller_manager',
+        package="controller_manager",
-        executable='ros2_control_node',
+        executable="ros2_control_node",
-        parameters=[controller_params, robot_description_param],)
+        parameters=[controller_params, robot_description_param],
+    )
 
     delayed_controller_manager = TimerAction(period=3.0, actions=[controller_manager])
 
     joint_broadcaster_spawner = Node(
-        package='controller_manager',
+        package="controller_manager",
-        executable='spawner',
+        executable="spawner",
-        arguments=['joint_state_broad'],
+        arguments=["joint_state_broad"],
     )
 
     delayed_joint_broadcaster_spawner = RegisterEventHandler(
@@ -73,11 +76,11 @@ def generate_launch_description():
             DeclareLaunchArgument(
                 name="model",
                 default_value=str(xacro_path),
-                description="Absolute path to the robot URDF or xacro file"
+                description="Absolute path to the robot URDF or xacro file",
             ),
             use_sim_time_launch_arg,
             robot_state_publisher_node,
             delayed_controller_manager,
-            delayed_joint_broadcaster_spawner
+            delayed_joint_broadcaster_spawner,
         ]
     )

openarm_bimanual_bringup/package.xml

@@ -1,9 +1,9 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>openarm_bimanual_teleop</name>
+  <name>openarm_bimanual_bringup</name>
   <version>0.0.0</version>
-  <description>Teleoperation setup for bimanual openarm</description>
+  <description>Bringup for bimanual openarm</description>
   <maintainer email="t95zhou@uwaterloo.ca">Thomason Zhou</maintainer>
   <license>BSD-3-Clause</license>
 

openarm_bimanual_moveit_config/.setup_assistant

@@ -7,4 +7,4 @@ moveit_setup_assistant_config:
   package_settings:
     author_name: Thomason Zhou
     author_email: t95zhou@uwaterloo.ca
-    generated_timestamp: 1745222265
+    generated_timestamp: 1745486864

openarm_bimanual_moveit_config/README.md

@@ -0,0 +1,19 @@
+# MoveIt2 on Bimanual Openarms
+
+Ensure the ROS2 packages and dependencies are installed by following the instructions in `openarm_ros2/README.md`.
+
+## Physical Hardware
+1. Run `init_can.sh` from `openarm_bringup/utils`. 
+   By default, can0 is the right arm and can1 is the left arm, but this can be adjusted in the ros2_control definition in `openarm_description/urdf/openarm.ros2_control.xacro`.
+
+2. Optionally, start the head-mounted realsense camera. This enables the octomap occupancy grid for planning around obstacles.
+   
+```sh
+ros2 launch openarm_bimanual_bringup depth_camera.launch.py
+```
+
+## Launch the demo
+
+```sh
+ros2 launch openarm_bimanual_moveit_config demo.launch.py
+```

openarm_bimanual_moveit_config/config/moveit.rviz

@@ -1,51 +1,548 @@
 Panels:
   - Class: rviz_common/Displays
+    Help Height: 70
     Name: Displays
     Property Tree Widget:
       Expanded:
         - /MotionPlanning1
+        - /MotionPlanning1/Planning Request1
+      Splitter Ratio: 0.5
+    Tree Height: 160
   - Class: rviz_common/Help
     Name: Help
   - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
     Name: Views
+    Splitter Ratio: 0.5
 Visualization Manager:
+  Class: ""
   Displays:
-    - Class: rviz_default_plugins/Grid
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
       Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
       Value: true
-    - Class: moveit_rviz_plugin/MotionPlanning
+    - Acceleration_Scaling_Factor: 0.1
+      Class: moveit_rviz_plugin/MotionPlanning
+      Enabled: true
+      Move Group Namespace: ""
+      MoveIt_Allow_Approximate_IK: false
+      MoveIt_Allow_External_Program: false
+      MoveIt_Allow_Replanning: false
+      MoveIt_Allow_Sensor_Positioning: false
+      MoveIt_Planning_Attempts: 10
+      MoveIt_Planning_Time: 5
+      MoveIt_Use_Cartesian_Path: false
+      MoveIt_Use_Constraint_Aware_IK: false
+      MoveIt_Workspace:
+        Center:
+          X: 0
+          Y: 0
+          Z: 0
+        Size:
+          X: 2
+          Y: 2
+          Z: 2
       Name: MotionPlanning
       Planned Path:
+        Color Enabled: false
+        Interrupt Display: false
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          camera_accel_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          camera_accel_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          camera_bottom_screw_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          camera_color_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          camera_color_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          camera_depth_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          camera_depth_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          camera_gyro_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          camera_gyro_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          camera_infra1_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          camera_infra1_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          camera_infra2_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          camera_infra2_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          camera_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_dummy_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          left_gripper_center:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          left_link1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_link2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_link3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_link4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_link5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_link6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_link7:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_link8:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_link_left_jaw:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_link_right_jaw:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          pedestal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_dummy_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          right_gripper_center:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          right_link1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_link2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_link3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_link4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_link5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_link6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_link7:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_link8:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_link_left_jaw:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_link_right_jaw:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          world:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
         Loop Animation: true
+        Robot Alpha: 0.5
+        Robot Color: 150; 50; 150
+        Show Robot Collision: false
+        Show Robot Visual: true
+        Show Trail: false
         State Display Time: 0.05 s
+        Trail Step Size: 1
         Trajectory Topic: display_planned_path
+        Use Sim Time: false
+      Planning Metrics:
+        Payload: 1
+        Show Joint Torques: false
+        Show Manipulability: false
+        Show Manipulability Index: false
+        Show Weight Limit: false
+        TextHeight: 0.07999999821186066
+      Planning Request:
+        Colliding Link Color: 255; 0; 0
+        Goal State Alpha: 1
+        Goal State Color: 250; 128; 0
+        Interactive Marker Size: 0.15000000596046448
+        Joint Violation Color: 255; 0; 255
+        Planning Group: upper_body
+        Query Goal State: true
+        Query Start State: false
+        Show Workspace: false
+        Start State Alpha: 1
+        Start State Color: 0; 255; 0
       Planning Scene Topic: monitored_planning_scene
       Robot Description: robot_description
       Scene Geometry:
         Scene Alpha: 1
+        Scene Color: 50; 230; 50
+        Scene Display Time: 0.009999999776482582
+        Show Scene Geometry: true
+        Voxel Coloring: Z-Axis
+        Voxel Rendering: Occupied Voxels
       Scene Robot:
-        Robot Alpha: 0.5
+        Attached Body Color: 150; 50; 150
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          camera_accel_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          camera_accel_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          camera_bottom_screw_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          camera_color_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          camera_color_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          camera_depth_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          camera_depth_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          camera_gyro_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          camera_gyro_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          camera_infra1_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          camera_infra1_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          camera_infra2_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          camera_infra2_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          camera_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
             Value: true
+          left_dummy_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          left_gripper_center:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          left_link1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_link2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_link3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_link4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_link5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_link6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_link7:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_link8:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_link_left_jaw:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_link_right_jaw:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          pedestal_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_dummy_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          right_gripper_center:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          right_link1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_link2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_link3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_link4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_link5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_link6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_link7:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_link8:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_link_left_jaw:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_link_right_jaw:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          world:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+        Robot Alpha: 0.5
+        Show Robot Collision: false
+        Show Robot Visual: true
+      Value: true
+      Velocity_Scaling_Factor: 0.1
+  Enabled: true
   Global Options:
+    Background Color: 48; 48; 48
     Fixed Frame: world
+    Frame Rate: 30
+  Name: root
   Tools:
     - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
     - Class: rviz_default_plugins/MoveCamera
     - Class: rviz_default_plugins/Select
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
   Value: true
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 2.0
+      Distance: 3.150280475616455
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
       Focal Point:
-        X: -0.1
+        X: -0.10000000149011612
         Y: 0.25
-        Z: 0.30
+        Z: 0.30000001192092896
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
       Name: Current View
+      Near Clip Distance: 0.009999999776482582
       Pitch: 0.5
       Target Frame: world
-      Yaw: -0.623
+      Value: Orbit (rviz_default_plugins)
+      Yaw: -0.6230000257492065
+    Saved: ~
 Window Geometry:
+  Displays:
+    collapsed: false
   Height: 975
-  QMainWindow State: 000000ff00000000fd0000000100000000000002b400000375fc0200000005fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb000000100044006900730070006c006100790073010000003d00000123000000c900fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000166000001910000018800fffffffb0000000800480065006c0070000000029a0000006e0000006e00fffffffb0000000a0056006900650077007301000002fd000000b5000000a400ffffff000001f60000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Help:
+    collapsed: false
+  Hide Left Dock: false
+  Hide Right Dock: false
+  MotionPlanning:
+    collapsed: false
+  MotionPlanning - Trajectory Slider:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd0000000100000000000002b400000375fc0200000005fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb000000100044006900730070006c006100790073010000003d00000123000000c900fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000166000001910000017d00fffffffb0000000800480065006c0070000000029a0000006e0000006e00fffffffb0000000a0056006900650077007301000002fd000000b5000000a400ffffff000001f60000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Views:
+    collapsed: false
   Width: 1200
+  X: 120
+  Y: 27

openarm_bimanual_moveit_config/config/openarm_bimanual.srdf

@@ -125,8 +125,8 @@
         <joint name="left_left_pris1" value="0"/>
         <joint name="left_rev1" value="0.2"/>
         <joint name="left_rev2" value="0"/>
-        <joint name="left_rev3" value="0.5"/>
+        <joint name="left_rev3" value="0.3384"/>
-        <joint name="left_rev4" value="1.6"/>
+        <joint name="left_rev4" value="1.3519"/>
         <joint name="left_rev5" value="0"/>
         <joint name="left_rev6" value="0"/>
         <joint name="left_rev7" value="0"/>
@@ -139,6 +139,132 @@
         <joint name="right_rev6" value="-0.2517"/>
         <joint name="right_rev7" value="0.0053"/>
     </group_state>
+    <group_state name="hands_up" group="upper_body">
+        <joint name="left_left_pris1" value="0"/>
+        <joint name="left_rev1" value="0.1"/>
+        <joint name="left_rev2" value="0"/>
+        <joint name="left_rev3" value="0"/>
+        <joint name="left_rev4" value="2.7"/>
+        <joint name="left_rev5" value="0"/>
+        <joint name="left_rev6" value="0"/>
+        <joint name="left_rev7" value="0"/>
+        <joint name="right_left_pris1" value="0"/>
+        <joint name="right_rev1" value="-0.1"/>
+        <joint name="right_rev2" value="0"/>
+        <joint name="right_rev3" value="0"/>
+        <joint name="right_rev4" value="2.7"/>
+        <joint name="right_rev5" value="0"/>
+        <joint name="right_rev6" value="0"/>
+        <joint name="right_rev7" value="0"/>
+    </group_state>
+    <group_state name="wave_1" group="upper_body">
+        <joint name="left_left_pris1" value="0"/>
+        <joint name="left_rev1" value="0.1"/>
+        <joint name="left_rev2" value="0"/>
+        <joint name="left_rev3" value="-0.5179"/>
+        <joint name="left_rev4" value="2.4627"/>
+        <joint name="left_rev5" value="-0.0116"/>
+        <joint name="left_rev6" value="-0.5814"/>
+        <joint name="left_rev7" value="0"/>
+        <joint name="right_left_pris1" value="0"/>
+        <joint name="right_rev1" value="-0.1"/>
+        <joint name="right_rev2" value="0"/>
+        <joint name="right_rev3" value="0"/>
+        <joint name="right_rev4" value="0"/>
+        <joint name="right_rev5" value="0"/>
+        <joint name="right_rev6" value="0"/>
+        <joint name="right_rev7" value="0"/>
+    </group_state>
+    <group_state name="wave_2" group="upper_body">
+        <joint name="left_left_pris1" value="0"/>
+        <joint name="left_rev1" value="0.1"/>
+        <joint name="left_rev2" value="0"/>
+        <joint name="left_rev3" value="0.4541"/>
+        <joint name="left_rev4" value="2.4627"/>
+        <joint name="left_rev5" value="-0.0116"/>
+        <joint name="left_rev6" value="-0.5814"/>
+        <joint name="left_rev7" value="0"/>
+        <joint name="right_left_pris1" value="0"/>
+        <joint name="right_rev1" value="-0.1"/>
+        <joint name="right_rev2" value="0"/>
+        <joint name="right_rev3" value="0"/>
+        <joint name="right_rev4" value="0"/>
+        <joint name="right_rev5" value="0"/>
+        <joint name="right_rev6" value="0"/>
+        <joint name="right_rev7" value="0"/>
+    </group_state>
+    <group_state name="x" group="upper_body">
+        <joint name="left_left_pris1" value="0"/>
+        <joint name="left_rev1" value="-0.2965"/>
+        <joint name="left_rev2" value="-0.1745"/>
+        <joint name="left_rev3" value="0.4513"/>
+        <joint name="left_rev4" value="1.9092"/>
+        <joint name="left_rev5" value="0.5207"/>
+        <joint name="left_rev6" value="-0.3384"/>
+        <joint name="left_rev7" value="-0.0795"/>
+        <joint name="right_left_pris1" value="0"/>
+        <joint name="right_rev1" value="0.4715"/>
+        <joint name="right_rev2" value="-0.1224"/>
+        <joint name="right_rev3" value="-1.0298"/>
+        <joint name="right_rev4" value="2.0827"/>
+        <joint name="right_rev5" value="-0.4513"/>
+        <joint name="right_rev6" value="0"/>
+        <joint name="right_rev7" value="0"/>
+    </group_state>
+    <group_state name="swing_arm_left_forward" group="upper_body">
+        <joint name="left_left_pris1" value="0"/>
+        <joint name="left_rev1" value="-0.6147"/>
+        <joint name="left_rev2" value="-0.0356"/>
+        <joint name="left_rev3" value="0.081"/>
+        <joint name="left_rev4" value="0"/>
+        <joint name="left_rev5" value="0.5207"/>
+        <joint name="left_rev6" value="-0.3384"/>
+        <joint name="left_rev7" value="-0.0795"/>
+        <joint name="right_left_pris1" value="0"/>
+        <joint name="right_rev1" value="-0.3876"/>
+        <joint name="right_rev2" value="0.0858"/>
+        <joint name="right_rev3" value="-0.567"/>
+        <joint name="right_rev4" value="0"/>
+        <joint name="right_rev5" value="-0.4513"/>
+        <joint name="right_rev6" value="0"/>
+        <joint name="right_rev7" value="0"/>
+    </group_state>
+    <group_state name="swing_arm_right_forward" group="upper_body">
+        <joint name="left_left_pris1" value="0"/>
+        <joint name="left_rev1" value="0.2762"/>
+        <joint name="left_rev2" value="0.1553"/>
+        <joint name="left_rev3" value="0.081"/>
+        <joint name="left_rev4" value="0"/>
+        <joint name="left_rev5" value="-0.4281"/>
+        <joint name="left_rev6" value="-0.3384"/>
+        <joint name="left_rev7" value="-0.0795"/>
+        <joint name="right_left_pris1" value="0"/>
+        <joint name="right_rev1" value="0.4556"/>
+        <joint name="right_rev2" value="-0.0009"/>
+        <joint name="right_rev3" value="-0.567"/>
+        <joint name="right_rev4" value="0.0723"/>
+        <joint name="right_rev5" value="0.243"/>
+        <joint name="right_rev6" value="0"/>
+        <joint name="right_rev7" value="0"/>
+    </group_state>
+    <group_state name="siu" group="upper_body">
+        <joint name="left_left_pris1" value="0"/>
+        <joint name="left_rev1" value="0.4512"/>
+        <joint name="left_rev2" value="0.2421"/>
+        <joint name="left_rev3" value="0.081"/>
+        <joint name="left_rev4" value="0"/>
+        <joint name="left_rev5" value="-0.4281"/>
+        <joint name="left_rev6" value="-0.3384"/>
+        <joint name="left_rev7" value="-0.1326"/>
+        <joint name="right_left_pris1" value="0"/>
+        <joint name="right_rev1" value="-0.4035"/>
+        <joint name="right_rev2" value="0.19"/>
+        <joint name="right_rev3" value="-0.567"/>
+        <joint name="right_rev4" value="0.0723"/>
+        <joint name="right_rev5" value="0.243"/>
+        <joint name="right_rev6" value="0"/>
+        <joint name="right_rev7" value="0"/>
+    </group_state>
     <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
     <end_effector name="left_gripper" parent_link="left_link8" group="left_gripper" parent_group="left_side"/>
     <end_effector name="right_gripper" parent_link="right_link8" group="right_gripper" parent_group="right_side"/>

openarm_bringup/README.md

@@ -0,0 +1,9 @@
+## utils
+
+init_can.sh \<device\> \<can interface\>
+
+e.g. 
+```sh
+./init_can.sh/dev/ACM0 can0
+./init_can.sh/dev/ACM1 can1
+```

openarm_description/urdf/openarm.xacro

@@ -234,49 +234,49 @@
             <child link="${prefix}link2"/>
             <origin rpy="0 0 0" xyz="0.0 0.0 0.05325"/>
             <axis xyz="0 0 1"/>
-            <limit effort="0.0" lower="-2.0943951023931953" upper="2.0943951023931953" velocity="0.0"/>
+            <limit effort="0.0" lower="${-pi/4.0 if side=='right' and zero_pos=='arm' else -2.0943951023931953}" upper="${pi/4.0 if side=='left' and zero_pos=='arm' else 2.0943951023931953}" velocity="0.0"/>
         </joint>
         <joint name="${prefix}rev2" type="revolute">
             <parent link="${prefix}link2"/>
             <child link="${prefix}link3"/>
             <origin rpy="-1.5707963267948968 ${pi/2 if zero_pos=='up' else 0.0} 0.0" xyz="0.0 -0.02975 0.04475"/>
             <axis xyz="0 0 1"/>
-            <limit effort="0.0" lower="${-pi/2.0 if zero_pos=='up' else 0}" upper="${pi/2 if zero_pos=='up' else pi}" velocity="0.0"/>
+            <limit effort="0.0" lower="${-pi/2.0 if zero_pos=='up' else -pi/18.0}" upper="${pi/2 if zero_pos=='up' else pi * 17.0 / 18.0}" velocity="0.0"/>
         </joint>
         <joint name="${prefix}rev3" type="revolute">
             <parent link="${prefix}link3"/>
             <child link="${prefix}link4"/>
             <origin rpy="-1.5707963267949054 ${rotate} -1.562038143900564" xyz="-0.0612477 -0.000536432 0.02975"/>
             <axis xyz="0 0 -1"/>
-            <limit effort="0.0" lower="${-0.52359 if side=='right' else -3.66519}" upper="${3.66519 if side=='right' else 0.52359}" velocity="0.0"/>
+            <limit effort="0.0" lower="${-pi * 2.0 / 3.0}" upper="${pi * 2.0 / 3.0}" velocity="0.0"/>
         </joint>
         <joint name="${prefix}rev4" type="revolute">
             <parent link="${prefix}link4"/>
             <child link="${prefix}link5"/>
             <origin rpy="-0.20701608758495998 -1.5707963267948566 -2.937419385117548" xyz="0.0297547 0.0 -0.24175"/>
             <axis xyz="0 0 1"/>
-            <limit effort="0.0" lower="-0.3490658503988659" upper="2.792526803190927" velocity="0.0"/>
+            <limit effort="0.0" lower="0" upper="${pi * 5.0 / 6.0}" velocity="0.0"/>
         </joint>
         <joint name="${prefix}rev5" type="revolute">
             <parent link="${prefix}link5"/>
             <child link="${prefix}link6"/>
             <origin rpy="1.5707963267948473 -0.5569332500492129 1.556730325338251" xyz="-0.133937 0.00188408 -0.0297547"/>
             <axis xyz="0 0 -1"/>
-            <limit effort="0.0" lower="-2.0943951023931953" upper="2.0943951023931953" velocity="0.0"/>
+            <limit effort="0.0" lower="${-pi * 2.0 / 3.0}" upper="${pi * 2.0 / 3.0}" velocity="0.0"/>
         </joint>
         <joint name="${prefix}rev6" type="revolute">
             <parent link="${prefix}link6"/>
             <child link="${prefix}link7"/>
             <origin rpy="-1.5707963268024898 -1.5567303253382425 -0.5569332500461536" xyz="-0.0187648 -0.0301352 -0.12105"/>
             <axis xyz="0 0 ${reflect}"/>
-            <limit effort="0.0" lower="-1.5707963267948966" upper="1.5707963267948966" velocity="0.0"/>
+            <limit effort="0.0" lower="${-pi / 2.0}" upper="${pi / 2.0}" velocity="0.0"/>
         </joint>
         <joint name="${prefix}rev7" type="revolute">
             <parent link="${prefix}link7"/>
             <child link="${prefix}link8"/>
             <origin rpy="-1.5707963267950005 -0.00875904933536772 -0.014066001454933467" xyz="-0.000217313 -0.0154485 0.0355"/>
             <axis xyz="0 0 -1"/>
-            <limit effort="0.0" lower="-0.9599310885968813" upper="0.9599310885968813" velocity="0.0"/>
+            <limit effort="0.0" lower="${-pi * 11.0 / 36.0}" upper="${pi * 11.0 / 36.0}" velocity="0.0"/>
         </joint>
         <joint name="${prefix}left_pris1" type="prismatic">
             <parent link="${prefix}link8"/>