openarm/openarm_ros2
1
0
Fork 0
Code Issues Pull Requests Actions 1 Packages Projects Releases Wiki Activity

Implement openarm ros2_control support with openarm_hardware and openarm_bringup (#2)

Browse Source 
- openarm_bringup: ros2_control bringup
- openarm_hardware: hardware interface for ros2_control
This commit is contained in:
thomason 2025-03-28 18:05:38 +09:00 committed by GitHub
parent 52f39010d7
commit 595fbe7745
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
51 changed files with 2248 additions and 1025 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
README.md
View File

@ -1,8 +1,10 @@
# ROS2 packages for OpenArm robots
- openarm_bimanual_description: urdf with pedestal torso and arm on each side
- openarm_description: urdf with gripper actuator
- openarm_bimanual_description: humanoid upper body with two arms (urdf)
- openarm_description: single arm (urdf)
- openarm_moveit_config: motion planning with [moveit2](https://github.com/moveit/moveit2)
- openarm_bringup: [ros2_control](https://control.ros.org/humble/index.html) bringup
- openarm_hardware: hardware interface for ros2_control
## Description Packages
@ -11,15 +13,10 @@ Each link has a visual mesh and a collision mesh, as shown in the figures below:
<img width="412" alt="visual meshes of openarm_bimanual_description urdf in rviz2" src="https://github.com/user-attachments/assets/9020efc3-69bc-420d-93a1-305885925638" />
<img width="383" alt="collision meshes of openarm_bimanual_description urdf in rviz2" src="https://github.com/user-attachments/assets/6f62184e-ccea-4859-9364-7c7d1b8def86" />
### TODO:
- [ ] Add results from true inertia tests to URDF
## MoveIt2 Support
https://github.com/user-attachments/assets/a0f962e5-6150-49ce-b18e-9914bcb322ef
### TODO:
- [ ] ROS 2 control packages (separate branch)
Tested with:
- [x] Rolling

576
openarm_bimanual_description/urdf/openarm_bimanual.urdf
View File

@ -1,576 +0,0 @@
<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from openarm_bimanual.urdf.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<robot name="openarm_bimanual">
<link name="world"/>
<material name="gray">
<color rgba="0.4117647058823529 0.4117647058823529 0.4117647058823529 1.0"/>
</material>
<link name="pedestal_link">
<visual>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<geometry>
<mesh filename="package://openarm_bimanual_description/meshes/pedestal_link.stl"/>
</geometry>
<material name="gray">
<color rgba="0.4117647058823529 0.4117647058823529 0.4117647058823529 1.0"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.0 0.0 0.489"/>
<geometry>
<box size="0.18 0.06 0.978000000000004"/>
</geometry>
</collision>
<inertial>
<mass value="4.850309883435878"/>
<origin rpy="0 0 0" xyz="0.0 -0.0180451 0.410247"/>
<inertia ixx="0.6232810322991779" ixy="7.916644206828546e-12" ixz="-1.094192214704477e-11" iyy="0.6253514495087933" iyz="-0.03342233248166168" izz="0.02000994027356939"/>
</inertial>
</link>
<link name="right_link1">
<visual>
<origin rpy="0 0 0" xyz="0.0 0.0 -0.0007"/>
<geometry>
<mesh filename="package://openarm_bimanual_description/meshes/link1.stl"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0289"/>
<geometry>
<box size="0.11 0.073 0.0592"/>
</geometry>
</collision>
<inertial>
<mass value="0.5769283920617254"/>
<origin rpy="0 0 0" xyz="0.0 2.52845e-05 0.0229621"/>
<inertia ixx="0.00034035167574060775" ixy="-2.623557985991217e-18" ixz="-6.093726095527621e-18" iyy="0.0004300058300010205" iyz="-5.09829482074806e-07" izz="0.0004802171432001996"/>
</inertial>
</link>
<link name="right_link2">
<visual>
<origin rpy="0 0 0" xyz="0.0 0.0 -0.05395"/>
<geometry>
<mesh filename="package://openarm_bimanual_description/meshes/link2.stl"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0371236"/>
<geometry>
<box size="0.0590000000149012 0.08200000000000002 0.0742471498637348"/>
</geometry>
</collision>
<inertial>
<mass value="0.16257504134917358"/>
<origin rpy="0 0 0" xyz="-0.000225878 -0.00183836 0.0278368"/>
<inertia ixx="0.00023903110213294374" ixy="-7.551692921096027e-08" ixz="-1.1282723362433474e-06" iyy="0.00010490798314778774" iyz="5.9079627839725245e-06" izz="0.00019737368685935776"/>
</inertial>
</link>
<link name="right_link3">
<visual>
<origin rpy="1.5707963267948966 0 0" xyz="0.0 0.0987 0.02975"/>
<geometry>
<mesh filename="package://openarm_bimanual_description/meshes/link3.stl"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin rpy="1.5707963267948966 0 0" xyz="-0.0164466 -0.00045542 0.02975"/>
<geometry>
<box size="0.08989101803441879 0.06999999999999983 0.05903276947803012"/>
</geometry>
</collision>
<inertial>
<mass value="0.4201676469910031"/>
<origin rpy="1.5707963267948966 0 0" xyz="-0.00688022 0.0 0.0282752"/>
<inertia ixx="0.00020256001126230057" ixy="6.004247875311213e-06" ixz="1.3304903057525575e-06" iyy="0.0002970624991387495" iyz="5.980157765704868e-07" izz="0.00032889994351244413"/>
</inertial>
</link>
<link name="right_link4">
<visual>
<origin rpy="3.141592653589793 -1.5620381439005742 0" xyz="-0.0986962 0.0 0.0621144"/>
<geometry>
<mesh filename="package://openarm_bimanual_description/meshes/link4.stl"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin rpy="3.141592653589793 -1.5620381439005742 0" xyz="0.000266276 -0.0125642 -0.132604"/>
<geometry>
<box size="0.27584673777150337 0.08211429171582063 0.07186573179325861"/>
</geometry>
</collision>
<inertial>
<mass value="0.819475539373447"/>
<origin rpy="3.141592653589793 -1.5620381439005742 0" xyz="0.000781326 -0.0019461 -0.132411"/>
<inertia ixx="0.00044078452033976287" ixy="6.020023694043618e-05" ixz="0.00014637459261941218" iyy="0.009208791480530413" iyz="1.5469710436444469e-06" izz="0.009170539316023695"/>
</inertial>
</link>
<link name="right_link5">
<visual>
<origin rpy="0 -0.008758182894317394 0" xyz="0.303864 0.0 -0.128451"/>
<geometry>
<mesh filename="package://openarm_bimanual_description/meshes/link5.stl"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin rpy="0 -0.008758182894316818 0" xyz="-0.0542814 0.00265291 -0.0302022"/>
<geometry>
<box size="0.16827213220625828 0.06430555048399171 0.08257859967140947"/>
</geometry>
</collision>
<inertial>
<mass value="0.4086748254352304"/>
<origin rpy="0 -0.008758182894316905 0" xyz="-0.0831891 0.00251789 -0.0290107"/>
<inertia ixx="0.00031417157425298747" ixy="-1.1391566029536723e-05" ixz="-1.883474090056124e-05" iyy="0.001221711435313989" iyz="1.46725418421127e-06" izz="0.0010755135067660488"/>
</inertial>
</link>
<link name="right_link6">
<visual>
<origin rpy="-2.1276679791326423 -1.5542266826921929 0" xyz="-0.0485412 -0.0860404 0.437784"/>
<geometry>
<mesh filename="package://openarm_bimanual_description/meshes/link6_rightarm.stl"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin rpy="-2.127667979132636 -1.5542266826921927 0" xyz="-0.00260794 -0.00312921 -0.0652641"/>
<geometry>
<box size="0.13114353004704765 0.05953478325301745 0.08817196195081879"/>
</geometry>
</collision>
<inertial>
<mass value="0.3448471958049249"/>
<origin rpy="-2.127667979132636 -1.5542266826921927 0" xyz="-0.00898536 -0.0135065 -0.0438611"/>
<inertia ixx="0.00019355980999748924" ixy="-1.0061665815148114e-06" ixz="-3.9096495715032716e-05" iyy="0.0003486912031865755" iyz="6.088060315273385e-06" izz="0.00028774975153756256"/>
</inertial>
</link>
<link name="right_link7">
<visual>
<origin rpy="0 -0.008758182894510852 0" xyz="0.558839 -0.00358671 -0.0631962"/>
<geometry>
<mesh filename="package://openarm_bimanual_description/meshes/link7_rightarm.stl"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin rpy="0 -0.008758182894511114 0" xyz="-0.000318103 0.0022839 0.0340014"/>
<geometry>
<box size="0.058327402175132874 0.04432916698753661 0.08267601622411752"/>
</geometry>
</collision>
<inertial>
<mass value="0.2782138078738053"/>
<origin rpy="0 -0.008758182894511043 0" xyz="5.99432e-05 0.0041433 0.0354274"/>
<inertia ixx="0.00010424153315648891" ixy="2.735265831785291e-07" ixz="-1.0662322382945994e-07" iyy="0.00012313550743283462" iyz="-8.604996754782856e-08" izz="9.221319958512582e-05"/>
</inertial>
</link>
<link name="right_link8">
<visual>
<origin rpy="1.5709195259578714 -0.014065461951077267 0" xyz="0.557948 0.103587 0.019724"/>
<geometry>
<mesh filename="package://openarm_bimanual_description/meshes/link8.stl"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin rpy="1.5709195259578714 -0.01406546195107726 0" xyz="-0.042694 -0.000543176 0.0110286"/>
<geometry>
<box size="0.13038799671743653 0.05180887692688926 0.1601236763440867"/>
</geometry>
</collision>
<inertial>
<mass value="0.31261452743802165"/>
<origin rpy="1.5709195259578714 -0.01406546195107726 0" xyz="-0.0607602 -0.000341696 0.00876618"/>
<inertia ixx="0.00023465661366788053" ixy="7.609048882006294e-05" ixz="3.088694121124684e-07" iyy="0.0005065459365215377" iyz="1.9022818028658623e-07" izz="0.0003737029250058136"/>
</inertial>
</link>
<link name="right_link_left_jaw">
<visual>
<origin rpy="0 -0.008758182894469432 0" xyz="0.665265 -0.00286677 -0.0209282"/>
<geometry>
<mesh filename="package://openarm_bimanual_description/meshes/left_jaw.stl"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin rpy="0 -0.0087581828944695 0" xyz="0.665265 -0.00286677 -0.0209282"/>
<geometry>
<mesh filename="package://openarm_bimanual_description/meshes/left_jaw.stl"/>
</geometry>
</collision>
<inertial>
<mass value="0.04297897856394934"/>
<origin rpy="0 -0.008758182894469429 0" xyz="-0.0187138 0.00217075 0.0159499"/>
<inertia ixx="1.1771768742932353e-05" ixy="-2.389928166589898e-06" ixz="3.999292204534159e-06" iyy="2.3837354480794824e-05" iyz="3.494431877242991e-07" izz="3.0474590214086944e-05"/>
</inertial>
</link>
<link name="right_link_right_jaw">
<visual>
<origin rpy="0 -0.008758182898001485 0" xyz="0.665265 -0.00286677 -0.175928"/>
<geometry>
<mesh filename="package://openarm_bimanual_description/meshes/right_jaw.stl"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin rpy="0 -0.008758182898001553 0" xyz="0.665265 -0.00286677 -0.175928"/>
<geometry>
<mesh filename="package://openarm_bimanual_description/meshes/right_jaw.stl"/>
</geometry>
</collision>
<inertial>
<mass value="0.042981665301134515"/>
<origin rpy="0 -0.00875818289800148 0" xyz="-0.0187844 -0.00272415 -0.0159503"/>
<inertia ixx="1.1561293087621594e-05" ixy="2.537718468008212e-06" ixz="-3.6591812257702003e-06" iyy="2.370165527871047e-05" iyz="4.848038235889027e-07" izz="3.039652946542647e-05"/>
</inertial>
</link>
<joint name="right_rev1" type="revolute">
<parent link="right_link1"/>
<child link="right_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"/>
</joint>
<joint name="right_rev2" type="revolute">
<parent link="right_link2"/>
<child link="right_link3"/>
<origin rpy="-1.5707963267948968 0 0" xyz="0.0 -0.02975 0.04475"/>
<axis xyz="0 0 1"/>
<limit effort="0.0" lower="0.0" upper="3.141592653589793" velocity="0.0"/>
</joint>
<joint name="right_rev3" type="revolute">
<parent link="right_link3"/>
<child link="right_link4"/>
<origin rpy="-1.5707963267949054 0 -1.562038143900564" xyz="-0.0612477 -0.000536432 0.02975"/>
<axis xyz="0 0 1"/>
<limit effort="0.0" lower="-3.6651914291880923" upper="0.5235987755982988" velocity="0.0"/>
</joint>
<joint name="right_rev4" type="revolute">
<parent link="right_link4"/>
<child link="right_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"/>
</joint>
<joint name="right_rev5" type="revolute">
<parent link="right_link5"/>
<child link="right_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"/>
</joint>
<joint name="right_rev6" type="revolute">
<parent link="right_link6"/>
<child link="right_link7"/>
<origin rpy="-1.5707963268024898 -1.5567303253382425 -0.5569332500461536" xyz="-0.0187648 -0.0301352 -0.12105"/>
<axis xyz="0 0 1"/>
<limit effort="0.0" lower="-1.5707963267948966" upper="1.5707963267948966" velocity="0.0"/>
</joint>
<joint name="right_rev7" type="revolute">
<parent link="right_link7"/>
<child link="right_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"/>
</joint>
<joint name="right_left_pris1" type="prismatic">
<parent link="right_link8"/>
<child link="right_link_left_jaw"/>
<origin rpy="1.570796326795101 -0.014066001454929162 0.00875904933542146" xyz="-0.1071 0.0768568 0.0132053"/>
<axis xyz="0 0 1"/>
<limit effort="0.0" lower="0.0" upper="0.0451" velocity="0.0"/>
</joint>
<joint name="right_right_pris2" type="prismatic">
<parent link="right_link8"/>
<child link="right_link_right_jaw"/>
<origin rpy="1.570796326794883 -0.014066001454927403 0.008759049336290027" xyz="-0.10571 -0.0781373 0.0132053"/>
<axis xyz="0 0 1"/>
<limit effort="0.0" lower="0.0" upper="0.0451" velocity="0.0"/>
<mimic joint="right_left_pris1" multiplier="-1.0" offset="0.0"/>
</joint>
<link name="left_link1">
<visual>
<origin rpy="0 0 0" xyz="0.0 0.0 -0.0007"/>
<geometry>
<mesh filename="package://openarm_bimanual_description/meshes/link1.stl"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0289"/>
<geometry>
<box size="0.11 0.073 0.0592"/>
</geometry>
</collision>
<inertial>
<mass value="0.5769283920617254"/>
<origin rpy="0 0 0" xyz="0.0 2.52845e-05 0.0229621"/>
<inertia ixx="0.00034035167574060775" ixy="-2.623557985991217e-18" ixz="-6.093726095527621e-18" iyy="0.0004300058300010205" iyz="-5.09829482074806e-07" izz="0.0004802171432001996"/>
</inertial>
</link>
<link name="left_link2">
<visual>
<origin rpy="0 0 0" xyz="0.0 0.0 -0.05395"/>
<geometry>
<mesh filename="package://openarm_bimanual_description/meshes/link2.stl"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0371236"/>
<geometry>
<box size="0.0590000000149012 0.08200000000000002 0.0742471498637348"/>
</geometry>
</collision>
<inertial>
<mass value="0.16257504134917358"/>
<origin rpy="0 0 0" xyz="-0.000225878 -0.00183836 0.0278368"/>
<inertia ixx="0.00023903110213294374" ixy="-7.551692921096027e-08" ixz="-1.1282723362433474e-06" iyy="0.00010490798314778774" iyz="5.9079627839725245e-06" izz="0.00019737368685935776"/>
</inertial>
</link>
<link name="left_link3">
<visual>
<origin rpy="1.5707963267948966 0 0" xyz="0.0 0.0987 0.02975"/>
<geometry>
<mesh filename="package://openarm_bimanual_description/meshes/link3.stl"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin rpy="1.5707963267948966 0 0" xyz="-0.0164466 -0.00045542 0.02975"/>
<geometry>
<box size="0.08989101803441879 0.06999999999999983 0.05903276947803012"/>
</geometry>
</collision>
<inertial>
<mass value="0.4201676469910031"/>
<origin rpy="1.5707963267948966 0 0" xyz="-0.00688022 0.0 0.0282752"/>
<inertia ixx="0.00020256001126230057" ixy="6.004247875311213e-06" ixz="1.3304903057525575e-06" iyy="0.0002970624991387495" iyz="5.980157765704868e-07" izz="0.00032889994351244413"/>
</inertial>
</link>
<link name="left_link4">
<visual>
<origin rpy="3.141592653589793 -1.5620381439005742 0" xyz="-0.0986962 0.0 0.0621144"/>
<geometry>
<mesh filename="package://openarm_bimanual_description/meshes/link4.stl"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin rpy="3.141592653589793 -1.5620381439005742 0" xyz="0.000266276 -0.0125642 -0.132604"/>
<geometry>
<box size="0.27584673777150337 0.08211429171582063 0.07186573179325861"/>
</geometry>
</collision>
<inertial>
<mass value="0.819475539373447"/>
<origin rpy="3.141592653589793 -1.5620381439005742 0" xyz="0.000781326 -0.0019461 -0.132411"/>
<inertia ixx="0.00044078452033976287" ixy="6.020023694043618e-05" ixz="0.00014637459261941218" iyy="0.009208791480530413" iyz="1.5469710436444469e-06" izz="0.009170539316023695"/>
</inertial>
</link>
<link name="left_link5">
<visual>
<origin rpy="0 -0.008758182894317394 0" xyz="0.303864 0.0 -0.128451"/>
<geometry>
<mesh filename="package://openarm_bimanual_description/meshes/link5.stl"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin rpy="0 -0.008758182894316818 0" xyz="-0.0542814 0.00265291 -0.0302022"/>
<geometry>
<box size="0.16827213220625828 0.06430555048399171 0.08257859967140947"/>
</geometry>
</collision>
<inertial>
<mass value="0.4086748254352304"/>
<origin rpy="0 -0.008758182894316905 0" xyz="-0.0831891 0.00251789 -0.0290107"/>
<inertia ixx="0.00031417157425298747" ixy="-1.1391566029536723e-05" ixz="-1.883474090056124e-05" iyy="0.001221711435313989" iyz="1.46725418421127e-06" izz="0.0010755135067660488"/>
</inertial>
</link>
<link name="left_link6">
<visual>
<origin rpy="5.269260632722435 1.5542266826921929 3.141592653589793" xyz="0.0605412 0.0878404 0.435484"/>
<geometry>
<mesh filename="package://openarm_bimanual_description/meshes/link6_leftarm.stl"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin rpy="-2.127667979132636 -1.5542266826921927 0" xyz="-0.00260794 -0.00312921 -0.0652641"/>
<geometry>
<box size="0.13114353004704765 0.05953478325301745 0.08817196195081879"/>
</geometry>
</collision>
<inertial>
<mass value="0.3448471958049249"/>
<origin rpy="-2.127667979132636 -1.5542266826921927 0" xyz="-0.00898536 -0.0135065 -0.0438611"/>
<inertia ixx="0.00019355980999748924" ixy="-1.0061665815148114e-06" ixz="-3.9096495715032716e-05" iyy="0.0003486912031865755" iyz="6.088060315273385e-06" izz="0.00028774975153756256"/>
</inertial>
</link>
<link name="left_link7">
<visual>
<origin rpy="0 3.150350836484304 0" xyz="0.558839 -0.0034132900000000002 0.1341962"/>
<geometry>
<mesh filename="package://openarm_bimanual_description/meshes/link7_leftarm.stl"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin rpy="0 -0.008758182894511114 0" xyz="-0.000318103 0.0022839 0.0340014"/>
<geometry>
<box size="0.058327402175132874 0.04432916698753661 0.08267601622411752"/>
</geometry>
</collision>
<inertial>
<mass value="0.2782138078738053"/>
<origin rpy="0 -0.008758182894511043 0" xyz="5.99432e-05 0.0041433 0.0354274"/>
<inertia ixx="0.00010424153315648891" ixy="2.735265831785291e-07" ixz="-1.0662322382945994e-07" iyy="0.00012313550743283462" iyz="-8.604996754782856e-08" izz="9.221319958512582e-05"/>
</inertial>
</link>
<link name="left_link8">
<visual>
<origin rpy="1.5709195259578714 -0.014065461951077267 0" xyz="0.557948 0.103587 0.019724"/>
<geometry>
<mesh filename="package://openarm_bimanual_description/meshes/link8.stl"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin rpy="1.5709195259578714 -0.01406546195107726 0" xyz="-0.042694 -0.000543176 0.0110286"/>
<geometry>
<box size="0.13038799671743653 0.05180887692688926 0.1601236763440867"/>
</geometry>
</collision>
<inertial>
<mass value="0.31261452743802165"/>
<origin rpy="1.5709195259578714 -0.01406546195107726 0" xyz="-0.0607602 -0.000341696 0.00876618"/>
<inertia ixx="0.00023465661366788053" ixy="7.609048882006294e-05" ixz="3.088694121124684e-07" iyy="0.0005065459365215377" iyz="1.9022818028658623e-07" izz="0.0003737029250058136"/>
</inertial>
</link>
<link name="left_link_left_jaw">
<visual>
<origin rpy="0 -0.008758182894469432 0" xyz="0.665265 -0.00286677 -0.0209282"/>
<geometry>
<mesh filename="package://openarm_bimanual_description/meshes/left_jaw.stl"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin rpy="0 -0.0087581828944695 0" xyz="0.665265 -0.00286677 -0.0209282"/>
<geometry>
<mesh filename="package://openarm_bimanual_description/meshes/left_jaw.stl"/>
</geometry>
</collision>
<inertial>
<mass value="0.04297897856394934"/>
<origin rpy="0 -0.008758182894469429 0" xyz="-0.0187138 0.00217075 0.0159499"/>
<inertia ixx="1.1771768742932353e-05" ixy="-2.389928166589898e-06" ixz="3.999292204534159e-06" iyy="2.3837354480794824e-05" iyz="3.494431877242991e-07" izz="3.0474590214086944e-05"/>
</inertial>
</link>
<link name="left_link_right_jaw">
<visual>
<origin rpy="0 -0.008758182898001485 0" xyz="0.665265 -0.00286677 -0.175928"/>
<geometry>
<mesh filename="package://openarm_bimanual_description/meshes/right_jaw.stl"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin rpy="0 -0.008758182898001553 0" xyz="0.665265 -0.00286677 -0.175928"/>
<geometry>
<mesh filename="package://openarm_bimanual_description/meshes/right_jaw.stl"/>
</geometry>
</collision>
<inertial>
<mass value="0.042981665301134515"/>
<origin rpy="0 -0.00875818289800148 0" xyz="-0.0187844 -0.00272415 -0.0159503"/>
<inertia ixx="1.1561293087621594e-05" ixy="2.537718468008212e-06" ixz="-3.6591812257702003e-06" iyy="2.370165527871047e-05" iyz="4.848038235889027e-07" izz="3.039652946542647e-05"/>
</inertial>
</link>
<joint name="left_rev1" type="revolute">
<parent link="left_link1"/>
<child link="left_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"/>
</joint>
<joint name="left_rev2" type="revolute">
<parent link="left_link2"/>
<child link="left_link3"/>
<origin rpy="-1.5707963267948968 0 0" xyz="0.0 -0.02975 0.04475"/>
<axis xyz="0 0 1"/>
<limit effort="0.0" lower="0.0" upper="3.141592653589793" velocity="0.0"/>
</joint>
<joint name="left_rev3" type="revolute">
<parent link="left_link3"/>
<child link="left_link4"/>
<origin rpy="-1.5707963267949054 3.141592653589793 -1.562038143900564" xyz="-0.0612477 -0.000536432 0.02975"/>
<axis xyz="0 0 -1"/>
<limit effort="0.0" lower="-3.6651914291880923" upper="0.5235987755982988" velocity="0.0"/>
</joint>
<joint name="left_rev4" type="revolute">
<parent link="left_link4"/>
<child link="left_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"/>
</joint>
<joint name="left_rev5" type="revolute">
<parent link="left_link5"/>
<child link="left_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"/>
</joint>
<joint name="left_rev6" type="revolute">
<parent link="left_link6"/>
<child link="left_link7"/>
<origin rpy="-1.5707963268024898 -1.5567303253382425 -0.5569332500461536" xyz="-0.0187648 -0.0301352 -0.12105"/>
<axis xyz="0 0 1"/>
<limit effort="0.0" lower="-1.5707963267948966" upper="1.5707963267948966" velocity="0.0"/>
</joint>
<joint name="left_rev7" type="revolute">
<parent link="left_link7"/>
<child link="left_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"/>
</joint>
<joint name="left_left_pris1" type="prismatic">
<parent link="left_link8"/>
<child link="left_link_left_jaw"/>
<origin rpy="1.570796326795101 -0.014066001454929162 0.00875904933542146" xyz="-0.1071 0.0768568 0.0132053"/>
<axis xyz="0 0 1"/>
<limit effort="0.0" lower="0.0" upper="0.0451" velocity="0.0"/>
</joint>
<joint name="left_right_pris2" type="prismatic">
<parent link="left_link8"/>
<child link="left_link_right_jaw"/>
<origin rpy="1.570796326794883 -0.014066001454927403 0.008759049336290027" xyz="-0.10571 -0.0781373 0.0132053"/>
<axis xyz="0 0 1"/>
<limit effort="0.0" lower="0.0" upper="0.0451" velocity="0.0"/>
<mimic joint="left_left_pris1" multiplier="-1.0" offset="0.0"/>
</joint>
<joint name="dummy_joint" type="fixed">
<parent link="world"/>
<child link="pedestal_link"/>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
</joint>
<joint name="right_fixed1" type="fixed">
<parent link="pedestal_link"/>
<child link="left_link1"/>
<origin rpy="3.0144791747751833 -1.5707963267948324 0.12689401604101067" xyz="0.09 0.0 0.893"/>
</joint>
<joint name="left_fixed1" type="fixed">
<parent link="pedestal_link"/>
<child link="right_link1"/>
<origin rpy="6.160245846096606 -1.5707963267948268 0.12293946108298068" xyz="-0.09 0.0 0.893"/>
</joint>
</robot>

30
openarm_bringup/CMakeLists.txt Normal file
View File

@ -0,0 +1,30 @@
cmake_minimum_required(VERSION 3.8)
project(openarm_bringup)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# comment the line when a copyright and license is added to all source files
set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# comment the line when this package is in a git repo and when
# a copyright and license is added to all source files
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
install(DIRECTORY launch config
DESTINATION share/${PROJECT_NAME}
)
ament_package()

25
openarm_bringup/LICENSE Normal file
View File

@ -0,0 +1,25 @@
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.

84
openarm_bringup/config/openarm_controllers.yaml Normal file
View File

@ -0,0 +1,84 @@
# Copyright (c) 2024 Stogl Robotics Consulting UG (haftungsbeschränkt)
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#
# Source of this file are templates in
# [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository.
#
# Author: Dr. Denis
#
controller_manager:
ros__parameters:
update_rate: 100 # Hz
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
forward_position_controller:
type: forward_command_controller/ForwardCommandController
forward_velocity_controller:
type: forward_command_controller/ForwardCommandController
joint_trajectory_controller:
type: joint_trajectory_controller/JointTrajectoryController
forward_position_controller:
ros__parameters:
joints:
- rev1
- rev2
- rev3
- rev4
- rev5
- rev6
- rev7
interface_name: position
forward_velocity_controller:
ros__parameters:
joints:
- rev1
- rev2
- rev3
- rev4
- rev5
- rev6
- rev7
interface_name: velocity
joint_trajectory_controller:
ros__parameters:
joints:
- rev1
- rev2
- rev3
- rev4
- rev5
- rev6
- rev7
command_interfaces:
- position
state_interfaces:
- position
state_publish_rate: 50.0 # Defaults to 50
action_monitor_rate: 50.0 # Defaults to 20
allow_partial_joints_goal: false # Defaults to false
constraints:
stopped_velocity_tolerance: 0.01 # Defaults to 0.01
goal_time: 0.0 # Defaults to 0.0 (start immediately)

58
openarm_bringup/config/test_goal_publishers_config.yaml Normal file
View File

@ -0,0 +1,58 @@
# Copyright (c) 2024 Stogl Robotics Consulting UG (haftungsbeschränkt)
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#
# Source of this file are templates in
# [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository.
#
publisher_forward_position_controller:
ros__parameters:
controller_name: "forward_position_controller"
wait_sec_between_publish: 5
goal_names: ["pos1", "pos2", "pos3", "pos4"]
pos1: [0.185, 0.185, 0.185, 0.185, 0.185, 0.185, 0.185]
pos2: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
pos3: [-0.185, -0.185, -0.185, -0.185, -0.185, -0.185, -0.185]
pos4: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
publisher_joint_trajectory_controller:
ros__parameters:
controller_name: "joint_trajectory_controller"
wait_sec_between_publish: 6
repeat_the_same_goal: 1 # useful to simulate continuous inputs
goal_time_from_start: 3.0
goal_names: ["pos1", "pos2", "pos3", "pos4"]
pos1:
positions: [0.185, 0.185, 0.185, 0.185, 0.185, 0.185, 0.185]
pos2:
positions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
pos3:
positions: [-0.185, -0.185, -0.185, -0.185, -0.185, -0.185, -0.185]
pos4:
positions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
joints:
- rev1
- rev2
- rev3
- rev4
- rev5
- rev6
- rev7

239
openarm_bringup/launch/openarm.launch.py Normal file
View File

@ -0,0 +1,239 @@
# Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt)
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#
# Source of this file are templates in
# [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository.
#
# Author: Dr. Denis
#
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, RegisterEventHandler, TimerAction
from launch.event_handlers import OnProcessExit, OnProcessStart
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
def generate_launch_description():
# Declare arguments
declared_arguments = []
declared_arguments.append(
DeclareLaunchArgument(
"runtime_config_package",
default_value="openarm_bringup",
description='Package with the controller\'s configuration in "config" folder. \
Usually the argument is not set, it enables use of a custom setup.',
)
)
declared_arguments.append(
DeclareLaunchArgument(
"controllers_file",
default_value="openarm_controllers.yaml",
description="YAML file with the controllers configuration.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"description_package",
default_value="openarm_description",
description="Description package with robot URDF/xacro files. Usually the argument \
is not set, it enables use of a custom description.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"description_file",
default_value="openarm.urdf.xacro",
description="URDF/XACRO description file with the robot.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"prefix",
default_value='""',
description="Prefix of the joint names, useful for \
multi-robot setup. If changed than also joint names in the controllers' configuration \
have to be updated.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"use_mock_hardware",
default_value="true",
description="Start robot with mock hardware mirroring command to its states.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"mock_sensor_commands",
default_value="false",
description="Enable mock command interfaces for sensors used for simple simulations. \
Used only if 'use_mock_hardware' parameter is true.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"robot_controller",
default_value="joint_trajectory_controller",
choices=["forward_position_controller", "joint_trajectory_controller"],
description="Robot controller to start.",
)
)
# Initialize Arguments
runtime_config_package = LaunchConfiguration("runtime_config_package")
controllers_file = LaunchConfiguration("controllers_file")
description_package = LaunchConfiguration("description_package")
description_file = LaunchConfiguration("description_file")
prefix = LaunchConfiguration("prefix")
use_mock_hardware = LaunchConfiguration("use_mock_hardware")
mock_sensor_commands = LaunchConfiguration("mock_sensor_commands")
robot_controller = LaunchConfiguration("robot_controller")
# Get URDF via xacro
robot_description_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[FindPackageShare(description_package), "urdf", description_file]
),
" ",
"prefix:=",
prefix,
" ",
"use_mock_hardware:=",
use_mock_hardware,
" ",
"mock_sensor_commands:=",
mock_sensor_commands,
" ",
]
)
robot_description = {"robot_description": robot_description_content}
robot_controllers = PathJoinSubstitution(
[FindPackageShare(runtime_config_package), "config", controllers_file]
)
rviz_config_file = PathJoinSubstitution(
[FindPackageShare(description_package), "rviz", "openarm.rviz"]
)
control_node = Node(
package="controller_manager",
executable="ros2_control_node",
output="both",
parameters=[robot_description, robot_controllers],
)
robot_state_pub_node = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
output="both",
parameters=[robot_description],
)
rviz_node = Node(
package="rviz2",
executable="rviz2",
name="rviz2",
output="log",
arguments=["-d", rviz_config_file],
)
joint_state_broadcaster_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
)
robot_controller_names = [robot_controller]
robot_controller_spawners = []
for controller in robot_controller_names:
robot_controller_spawners += [
Node(
package="controller_manager",
executable="spawner",
arguments=[controller, "-c", "/controller_manager"],
)
]
inactive_robot_controller_names = []
inactive_robot_controller_spawners = []
for controller in inactive_robot_controller_names:
inactive_robot_controller_spawners += [
Node(
package="controller_manager",
executable="spawner",
arguments=[controller, "-c", "/controller_manager", "--inactive"],
)
]
# Delay loading and activation of `joint_state_broadcaster` after start of ros2_control_node
delay_joint_state_broadcaster_spawner_after_ros2_control_node = RegisterEventHandler(
event_handler=OnProcessStart(
target_action=control_node,
on_start=[
TimerAction(
period=3.0,
actions=[joint_state_broadcaster_spawner],
),
],
)
)
# Delay loading and activation of robot_controller_names after `joint_state_broadcaster`
delay_robot_controller_spawners_after_joint_state_broadcaster_spawner = []
for i, controller in enumerate(robot_controller_spawners):
delay_robot_controller_spawners_after_joint_state_broadcaster_spawner += [
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=(
robot_controller_spawners[i - 1]
if i > 0
else joint_state_broadcaster_spawner
),
on_exit=[controller],
)
)
]
# Delay start of inactive_robot_controller_names after other controllers
delay_inactive_robot_controller_spawners_after_joint_state_broadcaster_spawner = []
for i, controller in enumerate(inactive_robot_controller_spawners):
delay_inactive_robot_controller_spawners_after_joint_state_broadcaster_spawner += [
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=(
inactive_robot_controller_spawners[i - 1]
if i > 0
else robot_controller_spawners[-1]
),
on_exit=[controller],
)
)
]
return LaunchDescription(
declared_arguments
+ [
control_node,
robot_state_pub_node,
rviz_node,
delay_joint_state_broadcaster_spawner_after_ros2_control_node,
]
+ delay_robot_controller_spawners_after_joint_state_broadcaster_spawner
+ delay_inactive_robot_controller_spawners_after_joint_state_broadcaster_spawner
)

46
openarm_bringup/launch/test_forward_position_controller.launch.py Normal file
View File

@ -0,0 +1,46 @@
# Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt)
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#
# Source of this file are templates in
# [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository.
#
# Author: Dr. Denis
#
from launch import LaunchDescription
from launch.substitutions import PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
def generate_launch_description():
position_goals = PathJoinSubstitution(
[FindPackageShare("openarm_bringup"), "config", "test_goal_publishers_config.yaml"]
)
return LaunchDescription(
[
Node(
package="ros2_controllers_test_nodes",
executable="publisher_forward_position_controller",
name="publisher_forward_position_controller",
parameters=[position_goals],
output={
"stdout": "screen",
"stderr": "screen",
},
)
]
)

46
openarm_bringup/launch/test_joint_trajectory_controller.launch.py Normal file
View File

@ -0,0 +1,46 @@
# Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt)
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#
# Source of this file are templates in
# [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository.
#
# Author: Dr. Denis
#
from launch import LaunchDescription
from launch.substitutions import PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
def generate_launch_description():
position_goals = PathJoinSubstitution(
[FindPackageShare("openarm_bringup"), "config", "test_goal_publishers_config.yaml"]
)
return LaunchDescription(
[
Node(
package="ros2_controllers_test_nodes",
executable="publisher_joint_trajectory_controller",
name="publisher_joint_trajectory_controller",
parameters=[position_goals],
output={
"stdout": "screen",
"stderr": "screen",
},
)
]
)

18
openarm_bringup/package.xml Normal file
View File

@ -0,0 +1,18 @@
<?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_bringup</name>
<version>0.0.0</version>
<description>Bringup script for OpenArm</description>
<maintainer email="t95zhou@uwaterloo.ca">Thomason Zhou</maintainer>
<license>BSD-3-Clause</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

0
openarm_description/resource/openarm_description Normal file
View File

12
openarm_description/urdf/initial_positions.yaml Normal file
View File

@ -0,0 +1,12 @@
# Default initial positions for openarm's ros2_control fake system
initial_positions:
left_pris1: 0
right_pris2: 0
rev1: 0
rev2: 0
rev3: 0
rev4: 0
rev5: 0
rev6: 0
rev7: 0

46
openarm_moveit_config/config/openarm_grip.ros2_control.xacro → openarm_description/urdf/openarm.ros2_control.xacro
View File

@ -1,84 +1,96 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="openarm_grip_ros2_control" params="name initial_positions_file">
<xacro:macro name="openarm_ros2_control" params="name initial_positions_file prefix:=''">
<xacro:property name="initial_positions" value="${xacro.load_yaml(initial_positions_file)['initial_positions']}"/>
<ros2_control name="${name}" type="system">
<ros2_control name="${prefix}${name}" type="system">
<hardware>
<!-- By default, set up controllers for simulation. This won't work on real hardware -->
<plugin>mock_components/GenericSystem</plugin>
<plugin>openarm_hardware/OpenArmHW</plugin>
</hardware>
<joint name="rev1">
<joint name="${prefix}rev1">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['rev1']}</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="rev2">
<joint name="${prefix}rev2">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['rev2']}</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="rev3">
<joint name="${prefix}rev3">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['rev3']}</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="rev4">
<joint name="${prefix}rev4">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['rev4']}</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="rev5">
<joint name="${prefix}rev5">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['rev5']}</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="rev6">
<joint name="${prefix}rev6">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['rev6']}</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="rev7">
<joint name="${prefix}rev7">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['rev7']}</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="slider_left">
<joint name="${prefix}left_pris1">
<command_interface name="position"/>
<command_interface name="velocity"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['slider_left']}</param>
<param name="initial_value">${initial_positions['left_pris1']}</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
<joint name="slider_right">
<command_interface name="position"/>
<command_interface name="velocity"/>
<joint name="${prefix}right_pris2">
<state_interface name="position">
<param name="initial_value">${initial_positions['slider_right']}</param>
<param name="initial_value">${initial_positions['left_pris1']}</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
</ros2_control>

89
openarm_description/urdf/openarm.urdf
View File

@ -10,6 +10,95 @@
<!-- side right, left -->
<!-- recommended prefixes left_, right_, etc. -->
<!-- zero_pos up, arm -->
<ros2_control name="OpenArmHW" type="system">
<hardware>
<!-- By default, set up controllers for simulation. This won't work on real hardware -->
<plugin>mock_components/GenericSystem</plugin>
<plugin>openarm_hardware/OpenArmHW</plugin>
</hardware>
<joint name="rev1">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<state_interface name="position">
<param name="initial_value">0</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="rev2">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<state_interface name="position">
<param name="initial_value">0</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="rev3">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<state_interface name="position">
<param name="initial_value">0</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="rev4">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<state_interface name="position">
<param name="initial_value">0</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="rev5">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<state_interface name="position">
<param name="initial_value">0</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="rev6">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<state_interface name="position">
<param name="initial_value">0</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="rev7">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<state_interface name="position">
<param name="initial_value">0</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="left_pris1">
<command_interface name="position"/>
<state_interface name="position">
<param name="initial_value">0</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
<joint name="right_pris2">
<state_interface name="position">
<param name="initial_value">0</param>
</state_interface>
</joint>
</ros2_control>
<link name="link1">
<visual>
<origin rpy="0 0 0" xyz="0.0 0.0 -0.0007"/>

1
openarm_description/urdf/openarm.urdf.xacro
View File

@ -2,5 +2,4 @@
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="openarm">
<xacro:include filename="openarm.xacro"/>
<xacro:openarm/>
<xacro:include filename="openarm_sensors.urdf.xacro"/>
</robot>

572
openarm_description/urdf/openarm.xacro
View File

@ -1,282 +1,290 @@
<?xml version="1.0" ?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<material name="gray">
<color rgba="${105/255} ${105/255} ${105/255} 1.0"/>
</material>
<xacro:macro name="openarm" params="side:='right' prefix:='' zero_pos='up'">
<!-- side right, left -->
<!-- recommended prefixes left_, right_, etc. -->
<!-- zero_pos up, arm -->
<xacro:property name="reflect" value="${1 if side=='right' else -1}"/>
<xacro:property name="rotate" value="${0 if side=='right' else pi}"/>
<link name="${prefix}link1">
<visual>
<origin rpy="0 0 0" xyz="0.0 0.0 -0.0007"/>
<geometry>
<mesh filename="package://openarm_description/meshes/link1.stl"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0289"/>
<geometry>
<box size="0.11 0.073 0.0592"/>
</geometry>
</collision>
<inertial>
<mass value="0.5769283920617254"/>
<origin rpy="0 0 0" xyz="0.0 2.52845e-05 0.0229621"/>
<inertia ixx="0.00034035167574060775" ixy="-2.623557985991217e-18" ixz="-6.093726095527621e-18" iyy="0.0004300058300010205" iyz="-5.09829482074806e-07" izz="0.0004802171432001996"/>
</inertial>
</link>
<link name="${prefix}link2">
<visual>
<origin rpy="0 0 0" xyz="0.0 0.0 -0.05395"/>
<geometry>
<mesh filename="package://openarm_description/meshes/link2.stl"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0371236"/>
<geometry>
<box size="0.0590000000149012 0.08200000000000002 0.0742471498637348"/>
</geometry>
</collision>
<inertial>
<mass value="0.16257504134917358"/>
<origin rpy="0 0 0" xyz="-0.000225878 -0.00183836 0.0278368"/>
<inertia ixx="0.00023903110213294374" ixy="-7.551692921096027e-08" ixz="-1.1282723362433474e-06" iyy="0.00010490798314778774" iyz="5.9079627839725245e-06" izz="0.00019737368685935776"/>
</inertial>
</link>
<link name="${prefix}link3">
<visual>
<origin rpy="${pi * 0.5} 0 0" xyz="0.0 0.0987 0.02975"/>
<geometry>
<mesh filename="package://openarm_description/meshes/link3.stl"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin rpy="${pi * 0.5} 0 0" xyz="-0.0164466 -0.00045542 0.02975"/>
<geometry>
<box size="0.08989101803441879 0.06999999999999983 0.05903276947803012"/>
</geometry>
</collision>
<inertial>
<mass value="0.4201676469910031"/>
<origin rpy="${pi * 0.5} 0 0" xyz="-0.00688022 0.0 0.0282752"/>
<inertia ixx="0.00020256001126230057" ixy="6.004247875311213e-06" ixz="1.3304903057525575e-06" iyy="0.0002970624991387495" iyz="5.980157765704868e-07" izz="0.00032889994351244413"/>
</inertial>
</link>
<link name="${prefix}link4">
<visual>
<origin rpy="${pi} -1.5620381439005742 0" xyz="-0.0986962 0.0 0.0621144"/>
<geometry>
<mesh filename="package://openarm_description/meshes/link4.stl"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin rpy="${pi} -1.5620381439005742 0" xyz="0.000266276 -0.0125642 -0.132604"/>
<geometry>
<box size="0.27584673777150337 0.08211429171582063 0.07186573179325861"/>
</geometry>
</collision>
<inertial>
<mass value="0.819475539373447"/>
<origin rpy="${pi} -1.5620381439005742 0" xyz="0.000781326 -0.0019461 -0.132411"/>
<inertia ixx="0.00044078452033976287" ixy="6.020023694043618e-05" ixz="0.00014637459261941218" iyy="0.009208791480530413" iyz="1.5469710436444469e-06" izz="0.009170539316023695"/>
</inertial>
</link>
<link name="${prefix}link5">
<visual>
<origin rpy="0 -0.008758182894317394 0" xyz="0.303864 0.0 -0.128451"/>
<geometry>
<mesh filename="package://openarm_description/meshes/link5.stl"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin rpy="0 -0.008758182894316818 0" xyz="-0.0542814 0.00265291 -0.0302022"/>
<geometry>
<box size="0.16827213220625828 0.06430555048399171 0.08257859967140947"/>
</geometry>
</collision>
<inertial>
<mass value="0.4086748254352304"/>
<origin rpy="0 -0.008758182894316905 0" xyz="-0.0831891 0.00251789 -0.0290107"/>
<inertia ixx="0.00031417157425298747" ixy="-1.1391566029536723e-05" ixz="-1.883474090056124e-05" iyy="0.001221711435313989" iyz="1.46725418421127e-06" izz="0.0010755135067660488"/>
</inertial>
</link>
<link name="${prefix}link6">
<visual>
<origin rpy="${-2.1276679791326423*reflect + rotate} ${reflect* -1.5542266826921929} ${rotate}" xyz="${reflect*-0.0485412 + (0 if side=='right' else 0.012)} ${reflect*-0.0860404 +(0 if side=='right' else 0.0018)} ${0.437784- (0 if side=='right' else 0.0023)}"/>
<geometry>
<mesh filename="package://openarm_description/meshes/${'link6_rightarm.stl' if side=='right' else 'link6_leftarm.stl'}"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin rpy="-2.127667979132636 -1.5542266826921927 0" xyz="-0.00260794 -0.00312921 -0.0652641"/>
<geometry>
<box size="0.13114353004704765 0.05953478325301745 0.08817196195081879"/>
</geometry>
</collision>
<inertial>
<mass value="0.3448471958049249"/>
<origin rpy="-2.127667979132636 -1.5542266826921927 0" xyz="-0.00898536 -0.0135065 -0.0438611"/>
<inertia ixx="0.00019355980999748924" ixy="-1.0061665815148114e-06" ixz="-3.9096495715032716e-05" iyy="0.0003486912031865755" iyz="6.088060315273385e-06" izz="0.00028774975153756256"/>
</inertial>
</link>
<link name="${prefix}link7">
<visual>
<origin rpy="0 ${reflect*-0.008758182894510852 + rotate} 0" xyz="0.558839 ${reflect*-0.00358671 + (0 if side=='right' else -0.007)} ${reflect*-0.0631962 + (0 if side=='right' else 0.071)}"/>
<geometry>
<mesh filename="package://openarm_description/meshes/${'link7_rightarm.stl' if side=='right' else 'link7_leftarm.stl'}"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin rpy="0 -0.008758182894511114 0" xyz="-0.000318103 0.0022839 0.0340014"/>
<geometry>
<box size="0.058327402175132874 0.04432916698753661 0.08267601622411752"/>
</geometry>
</collision>
<inertial>
<mass value="0.2782138078738053"/>
<origin rpy="0 -0.008758182894511043 0" xyz="5.99432e-05 0.0041433 0.0354274"/>
<inertia ixx="0.00010424153315648891" ixy="2.735265831785291e-07" ixz="-1.0662322382945994e-07" iyy="0.00012313550743283462" iyz="-8.604996754782856e-08" izz="9.221319958512582e-05"/>
</inertial>
</link>
<link name="${prefix}link8">
<visual>
<origin rpy="1.5709195259578714 -0.014065461951077267 0" xyz="0.557948 0.103587 0.019724"/>
<geometry>
<mesh filename="package://openarm_description/meshes/link8.stl"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin rpy="1.5709195259578714 -0.01406546195107726 0" xyz="-0.042694 -0.000543176 0.0110286"/>
<geometry>
<box size="0.13038799671743653 0.05180887692688926 0.1601236763440867"/>
</geometry>
</collision>
<inertial>
<mass value="0.31261452743802165"/>
<origin rpy="1.5709195259578714 -0.01406546195107726 0" xyz="-0.0607602 -0.000341696 0.00876618"/>
<inertia ixx="0.00023465661366788053" ixy="7.609048882006294e-05" ixz="3.088694121124684e-07" iyy="0.0005065459365215377" iyz="1.9022818028658623e-07" izz="0.0003737029250058136"/>
</inertial>
</link>
<link name="${prefix}link_left_jaw">
<visual>
<origin rpy="0 -0.008758182894469432 0" xyz="0.665265 -0.00286677 -0.0209282"/>
<geometry>
<mesh filename="package://openarm_description/meshes/left_jaw.stl"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin rpy="0 -0.0087581828944695 0" xyz="0.665265 -0.00286677 -0.0209282"/>
<geometry>
<mesh filename="package://openarm_description/meshes/left_jaw.stl"/>
</geometry>
</collision>
<inertial>
<mass value="0.04297897856394934"/>
<origin rpy="0 -0.008758182894469429 0" xyz="-0.0187138 0.00217075 0.0159499"/>
<inertia ixx="1.1771768742932353e-05" ixy="-2.389928166589898e-06" ixz="3.999292204534159e-06" iyy="2.3837354480794824e-05" iyz="3.494431877242991e-07" izz="3.0474590214086944e-05"/>
</inertial>
</link>
<link name="${prefix}link_right_jaw">
<visual>
<origin rpy="0 -0.008758182898001485 0" xyz="0.665265 -0.00286677 -0.175928"/>
<geometry>
<mesh filename="package://openarm_description/meshes/right_jaw.stl"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin rpy="0 -0.008758182898001553 0" xyz="0.665265 -0.00286677 -0.175928"/>
<geometry>
<mesh filename="package://openarm_description/meshes/right_jaw.stl"/>
</geometry>
</collision>
<inertial>
<mass value="0.042981665301134515"/>
<origin rpy="0 -0.00875818289800148 0" xyz="-0.0187844 -0.00272415 -0.0159503"/>
<inertia ixx="1.1561293087621594e-05" ixy="2.537718468008212e-06" ixz="-3.6591812257702003e-06" iyy="2.370165527871047e-05" iyz="4.848038235889027e-07" izz="3.039652946542647e-05"/>
</inertial>
</link>
<joint name="${prefix}rev1" type="revolute">
<parent link="${prefix}link1"/>
<child link="${prefix}link2"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.05325"/>
<axis xyz="0 0 ${reflect*1}"/>
<limit effort="0.0" lower="-2.0943951023931953" upper="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" 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.0}" upper="${pi/2 if zero_pos=='up' else pi}" 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 ${reflect*1}"/>
<limit effort="0.0" lower="-3.6651914291880923" upper="0.5235987755982988" 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"/>
</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"/>
</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 1"/>
<limit effort="0.0" lower="-1.5707963267948966" upper="1.5707963267948966" 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"/>
</joint>
<joint name="${prefix}left_pris1" type="prismatic">
<parent link="${prefix}link8"/>
<child link="${prefix}link_left_jaw"/>
<origin rpy="1.570796326795101 -0.014066001454929162 0.00875904933542146" xyz="-0.1071 0.0768568 0.0132053"/>
<axis xyz="0 0 1"/>
<limit effort="0.0" lower="0.0" upper="0.0451" velocity="0.0"/>
</joint>
<joint name="${prefix}right_pris2" type="prismatic">
<parent link="${prefix}link8"/>
<child link="${prefix}link_right_jaw"/>
<origin rpy="1.570796326794883 -0.014066001454927403 0.008759049336290027" xyz="-0.10571 -0.0781373 0.0132053"/>
<axis xyz="0 0 1"/>
<limit effort="0.0" lower="0.0" upper="0.0451" velocity="0.0"/>
<mimic joint="${prefix}left_pris1" multiplier="-1.0" offset="0.0"/>
</joint>
</xacro:macro>
</robot>
<?xml version="1.0" ?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<material name="gray">
<color rgba="${105/255} ${105/255} ${105/255} 1.0"/>
</material>
<xacro:macro name="openarm" params="side:='right' prefix:='' zero_pos='up'">
<!-- side right, left -->
<!-- recommended prefixes left_, right_, etc. -->
<!-- zero_pos up, arm -->
<xacro:property name="reflect" value="${1 if side=='right' else -1}"/>
<xacro:property name="rotate" value="${0 if side=='right' else pi}"/>
<xacro:include filename="openarm_sensors.urdf.xacro"/>
<xacro:arg name="initial_positions_file" default="initial_positions.yaml" />
<xacro:include filename="openarm.ros2_control.xacro" />
<xacro:openarm_ros2_control name="OpenArmHW" prefix="${prefix}" initial_positions_file="$(arg initial_positions_file)"/>
<link name="${prefix}link1">
<visual>
<origin rpy="0 0 0" xyz="0.0 0.0 -0.0007"/>
<geometry>
<mesh filename="package://openarm_description/meshes/link1.stl"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0289"/>
<geometry>
<box size="0.11 0.073 0.0592"/>
</geometry>
</collision>
<inertial>
<mass value="0.5769283920617254"/>
<origin rpy="0 0 0" xyz="0.0 2.52845e-05 0.0229621"/>
<inertia ixx="0.00034035167574060775" ixy="-2.623557985991217e-18" ixz="-6.093726095527621e-18" iyy="0.0004300058300010205" iyz="-5.09829482074806e-07" izz="0.0004802171432001996"/>
</inertial>
</link>
<link name="${prefix}link2">
<visual>
<origin rpy="0 0 0" xyz="0.0 0.0 -0.05395"/>
<geometry>
<mesh filename="package://openarm_description/meshes/link2.stl"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0371236"/>
<geometry>
<box size="0.0590000000149012 0.08200000000000002 0.0742471498637348"/>
</geometry>
</collision>
<inertial>
<mass value="0.16257504134917358"/>
<origin rpy="0 0 0" xyz="-0.000225878 -0.00183836 0.0278368"/>
<inertia ixx="0.00023903110213294374" ixy="-7.551692921096027e-08" ixz="-1.1282723362433474e-06" iyy="0.00010490798314778774" iyz="5.9079627839725245e-06" izz="0.00019737368685935776"/>
</inertial>
</link>
<link name="${prefix}link3">
<visual>
<origin rpy="${pi * 0.5} 0 0" xyz="0.0 0.0987 0.02975"/>
<geometry>
<mesh filename="package://openarm_description/meshes/link3.stl"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin rpy="${pi * 0.5} 0 0" xyz="-0.0164466 -0.00045542 0.02975"/>
<geometry>
<box size="0.08989101803441879 0.06999999999999983 0.05903276947803012"/>
</geometry>
</collision>
<inertial>
<mass value="0.4201676469910031"/>
<origin rpy="${pi * 0.5} 0 0" xyz="-0.00688022 0.0 0.0282752"/>
<inertia ixx="0.00020256001126230057" ixy="6.004247875311213e-06" ixz="1.3304903057525575e-06" iyy="0.0002970624991387495" iyz="5.980157765704868e-07" izz="0.00032889994351244413"/>
</inertial>
</link>
<link name="${prefix}link4">
<visual>
<origin rpy="${pi} -1.5620381439005742 0" xyz="-0.0986962 0.0 0.0621144"/>
<geometry>
<mesh filename="package://openarm_description/meshes/link4.stl"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin rpy="${pi} -1.5620381439005742 0" xyz="0.000266276 -0.0125642 -0.132604"/>
<geometry>
<box size="0.27584673777150337 0.08211429171582063 0.07186573179325861"/>
</geometry>
</collision>
<inertial>
<mass value="0.819475539373447"/>
<origin rpy="${pi} -1.5620381439005742 0" xyz="0.000781326 -0.0019461 -0.132411"/>
<inertia ixx="0.00044078452033976287" ixy="6.020023694043618e-05" ixz="0.00014637459261941218" iyy="0.009208791480530413" iyz="1.5469710436444469e-06" izz="0.009170539316023695"/>
</inertial>
</link>
<link name="${prefix}link5">
<visual>
<origin rpy="0 -0.008758182894317394 0" xyz="0.303864 0.0 -0.128451"/>
<geometry>
<mesh filename="package://openarm_description/meshes/link5.stl"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin rpy="0 -0.008758182894316818 0" xyz="-0.0542814 0.00265291 -0.0302022"/>
<geometry>
<box size="0.16827213220625828 0.06430555048399171 0.08257859967140947"/>
</geometry>
</collision>
<inertial>
<mass value="0.4086748254352304"/>
<origin rpy="0 -0.008758182894316905 0" xyz="-0.0831891 0.00251789 -0.0290107"/>
<inertia ixx="0.00031417157425298747" ixy="-1.1391566029536723e-05" ixz="-1.883474090056124e-05" iyy="0.001221711435313989" iyz="1.46725418421127e-06" izz="0.0010755135067660488"/>
</inertial>
</link>
<link name="${prefix}link6">
<visual>
<origin rpy="${-2.1276679791326423*reflect + rotate} ${reflect* -1.5542266826921929} ${rotate}" xyz="${reflect*-0.0485412 + (0 if side=='right' else 0.012)} ${reflect*-0.0860404 +(0 if side=='right' else 0.0018)} ${0.437784- (0 if side=='right' else 0.0023)}"/>
<geometry>
<mesh filename="package://openarm_description/meshes/${'link6_rightarm.stl' if side=='right' else 'link6_leftarm.stl'}"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin rpy="-2.127667979132636 -1.5542266826921927 0" xyz="-0.00260794 -0.00312921 -0.0652641"/>
<geometry>
<box size="0.13114353004704765 0.05953478325301745 0.08817196195081879"/>
</geometry>
</collision>
<inertial>
<mass value="0.3448471958049249"/>
<origin rpy="-2.127667979132636 -1.5542266826921927 0" xyz="-0.00898536 -0.0135065 -0.0438611"/>
<inertia ixx="0.00019355980999748924" ixy="-1.0061665815148114e-06" ixz="-3.9096495715032716e-05" iyy="0.0003486912031865755" iyz="6.088060315273385e-06" izz="0.00028774975153756256"/>
</inertial>
</link>
<link name="${prefix}link7">
<visual>
<origin rpy="0 ${reflect*-0.008758182894510852 + rotate} 0" xyz="0.558839 ${reflect*-0.00358671 + (0 if side=='right' else -0.007)} ${reflect*-0.0631962 + (0 if side=='right' else 0.071)}"/>
<geometry>
<mesh filename="package://openarm_description/meshes/${'link7_rightarm.stl' if side=='right' else 'link7_leftarm.stl'}"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin rpy="0 -0.008758182894511114 0" xyz="-0.000318103 0.0022839 0.0340014"/>
<geometry>
<box size="0.058327402175132874 0.04432916698753661 0.08267601622411752"/>
</geometry>
</collision>
<inertial>
<mass value="0.2782138078738053"/>
<origin rpy="0 -0.008758182894511043 0" xyz="5.99432e-05 0.0041433 0.0354274"/>
<inertia ixx="0.00010424153315648891" ixy="2.735265831785291e-07" ixz="-1.0662322382945994e-07" iyy="0.00012313550743283462" iyz="-8.604996754782856e-08" izz="9.221319958512582e-05"/>
</inertial>
</link>
<link name="${prefix}link8">
<visual>
<origin rpy="1.5709195259578714 -0.014065461951077267 0" xyz="0.557948 0.103587 0.019724"/>
<geometry>
<mesh filename="package://openarm_description/meshes/link8.stl"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin rpy="1.5709195259578714 -0.01406546195107726 0" xyz="-0.042694 -0.000543176 0.0110286"/>
<geometry>
<box size="0.13038799671743653 0.05180887692688926 0.1601236763440867"/>
</geometry>
</collision>
<inertial>
<mass value="0.31261452743802165"/>
<origin rpy="1.5709195259578714 -0.01406546195107726 0" xyz="-0.0607602 -0.000341696 0.00876618"/>
<inertia ixx="0.00023465661366788053" ixy="7.609048882006294e-05" ixz="3.088694121124684e-07" iyy="0.0005065459365215377" iyz="1.9022818028658623e-07" izz="0.0003737029250058136"/>
</inertial>
</link>
<link name="${prefix}link_left_jaw">
<visual>
<origin rpy="0 -0.008758182894469432 0" xyz="0.665265 -0.00286677 -0.0209282"/>
<geometry>
<mesh filename="package://openarm_description/meshes/left_jaw.stl"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin rpy="0 -0.0087581828944695 0" xyz="0.665265 -0.00286677 -0.0209282"/>
<geometry>
<mesh filename="package://openarm_description/meshes/left_jaw.stl"/>
</geometry>
</collision>
<inertial>
<mass value="0.04297897856394934"/>
<origin rpy="0 -0.008758182894469429 0" xyz="-0.0187138 0.00217075 0.0159499"/>
<inertia ixx="1.1771768742932353e-05" ixy="-2.389928166589898e-06" ixz="3.999292204534159e-06" iyy="2.3837354480794824e-05" iyz="3.494431877242991e-07" izz="3.0474590214086944e-05"/>
</inertial>
</link>
<link name="${prefix}link_right_jaw">
<visual>
<origin rpy="0 -0.008758182898001485 0" xyz="0.665265 -0.00286677 -0.175928"/>
<geometry>
<mesh filename="package://openarm_description/meshes/right_jaw.stl"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin rpy="0 -0.008758182898001553 0" xyz="0.665265 -0.00286677 -0.175928"/>
<geometry>
<mesh filename="package://openarm_description/meshes/right_jaw.stl"/>
</geometry>
</collision>
<inertial>
<mass value="0.042981665301134515"/>
<origin rpy="0 -0.00875818289800148 0" xyz="-0.0187844 -0.00272415 -0.0159503"/>
<inertia ixx="1.1561293087621594e-05" ixy="2.537718468008212e-06" ixz="-3.6591812257702003e-06" iyy="2.370165527871047e-05" iyz="4.848038235889027e-07" izz="3.039652946542647e-05"/>
</inertial>
</link>
<joint name="${prefix}rev1" type="revolute">
<parent link="${prefix}link1"/>
<child link="${prefix}link2"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.05325"/>
<axis xyz="0 0 ${reflect*1}"/>
<limit effort="0.0" lower="-2.0943951023931953" upper="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" 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.0}" upper="${pi/2 if zero_pos=='up' else pi}" 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 ${reflect*1}"/>
<limit effort="0.0" lower="-3.6651914291880923" upper="0.5235987755982988" 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"/>
</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"/>
</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 1"/>
<limit effort="0.0" lower="-1.5707963267948966" upper="1.5707963267948966" 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"/>
</joint>
<joint name="${prefix}left_pris1" type="prismatic">
<parent link="${prefix}link8"/>
<child link="${prefix}link_left_jaw"/>
<origin rpy="1.570796326795101 -0.014066001454929162 0.00875904933542146" xyz="-0.1071 0.0768568 0.0132053"/>
<axis xyz="0 0 1"/>
<limit effort="0.0" lower="0.0" upper="0.0451" velocity="0.0"/>
</joint>
<joint name="${prefix}right_pris2" type="prismatic">
<parent link="${prefix}link8"/>
<child link="${prefix}link_right_jaw"/>
<origin rpy="1.570796326794883 -0.014066001454927403 0.008759049336290027" xyz="-0.10571 -0.0781373 0.0132053"/>
<axis xyz="0 0 1"/>
<limit effort="0.0" lower="0.0" upper="0.0451" velocity="0.0"/>
<mimic joint="${prefix}left_pris1" multiplier="-1.0" offset="0.0"/>
</joint>
</xacro:macro>
</robot>

64
openarm_hardware/CMakeLists.txt Normal file
View File

@ -0,0 +1,64 @@
cmake_minimum_required(VERSION 3.8)
project(openarm_hardware)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
find_package(hardware_interface REQUIRED)
find_package(pluginlib REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_lifecycle REQUIRED)
add_library(${PROJECT_NAME} SHARED
src/openarm_hardware.cpp
)
target_include_directories(${PROJECT_NAME}
PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
ament_target_dependencies(${PROJECT_NAME}
hardware_interface
pluginlib
rclcpp
rclcpp_lifecycle
)
ament_export_libraries(${PROJECT_NAME})
pluginlib_export_plugin_description_file(hardware_interface openarm_hardware.xml)
install(DIRECTORY include/
DESTINATION include
)
install(FILES
openarm_hardware.xml
DESTINATION share/${PROJECT_NAME}
)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
find_package(ament_cmake_gmock REQUIRED)
ament_add_gmock(test_openarm_hardware
test/test_openarm_hardware.cpp
)
target_link_libraries(test_openarm_hardware
${PROJECT_NAME}
)
ament_target_dependencies(test_openarm_hardware
hardware_interface
)
set(ament_cmake_copyright_FOUND TRUE)
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()

25
openarm_hardware/LICENSE Normal file
View File

@ -0,0 +1,25 @@
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.

26
openarm_hardware/include/openarm_hardware/canbus.hpp Normal file
View File

@ -0,0 +1,26 @@
#ifndef CANBUS_H
#define CANBUS_H
#include <iostream>
#include <cstring>
#include <unistd.h>
#include <sys/ioctl.h>
#include <net/if.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#include <array>
#include <fcntl.h>
class CANBus {
public:
explicit CANBus(const std::string& interface);
~CANBus();
bool send(uint16_t motor_id, const std::array<uint8_t, 8>& data);
struct can_frame recv();
private:
int sock_;
};
#endif // CANBUS_H

94
openarm_hardware/include/openarm_hardware/motor.hpp Normal file
View File

@ -0,0 +1,94 @@
#ifndef MOTOR_H
#define MOTOR_H
#include <cstdint>
#include <map>
#include <vector>
#include <iostream>
#include <array>
#include <algorithm>
#include <cstring>
#include <stdio.h>
enum class DM_Motor_Type : uint8_t {
DM4310 = 0,
DM4310_48V,
DM4340,
DM4340_48V,
DM6006,
DM8006,
DM8009,
DM10010L,
DM10010,
DMH3510,
DMH6215,
DMG6220,
COUNT
};
enum class DM_variable : uint8_t {
UV_Value = 0, KT_Value, OT_Value, OC_Value, ACC, DEC, MAX_SPD, MST_ID,
ESC_ID, TIMEOUT, CTRL_MODE, Damp, Inertia, hw_ver, sw_ver, SN, NPP, Rs,
LS, Flux, Gr, PMAX, VMAX, TMAX, I_BW, KP_ASR, KI_ASR, KP_APR, KI_APR,
OV_Value, GREF, Deta, V_BW, IQ_c1, VL_c1, can_br, sub_ver,
u_off = 50, v_off, k1, k2, m_off, dir,
p_m = 80, xout,
COUNT
};
enum class Control_Type : uint8_t {
MIT = 1,
POS_VEL,
VEL,
Torque_Pos,
COUNT
};
class Motor {
public:
Motor() = default;
Motor(DM_Motor_Type motorType, uint16_t slaveID, uint16_t masterID);
void recv_data(double q, double dq, double tau, int tmos, int trotor);
double getPosition() const;
double getVelocity() const;
double getTorque() const;
int getParam(int RID) const;
uint16_t SlaveID;
uint16_t MasterID;
bool isEnable;
Control_Type NowControlMode;
DM_Motor_Type MotorType;
int getStateTmos() const;
int getStateTrotor() const;
double getGoalPosition() const;
double getGoalTau() const;
void setGoalPosition(double pos);
void setGoalTau(double tau);
void setStateTmos(int tmos);
void setStateTrotor(int trotor);
private:
double Pd, Vd;
double goal_position, goal_tau;
double state_q, state_dq, state_tau;
int state_tmos, state_trotor;
std::map<int, int> temp_param_dict;
};
double LIMIT_MIN_MAX(double x, double min, double max);
uint16_t double_to_uint(double x, double x_min, double x_max, int bits);
double uint_to_double(uint16_t x, double min, double max, int bits);
std::array<uint8_t, 4> double_to_uint8s(double value);
std::array<uint8_t, 4> data_to_uint8s(uint32_t value);
uint32_t uint8s_to_uint32(uint8_t byte1, uint8_t byte2, uint8_t byte3, uint8_t byte4);
double uint8s_to_double(uint8_t byte1, uint8_t byte2, uint8_t byte3, uint8_t byte4);
bool is_in_ranges(int number);
void print_hex(const std::vector<uint8_t>& data);
template <typename T>
T get_enum_by_index(int index);
#endif // MOTOR_H

47
openarm_hardware/include/openarm_hardware/motor_control.hpp Normal file
View File

@ -0,0 +1,47 @@
#ifndef MOTOR_CONTROL_H
#define MOTOR_CONTROL_H
#include <iostream>
#include <vector>
#include <map>
#include <array>
#include <cstring>
#include <unistd.h>
#include "motor.hpp"
#include "canbus.hpp"
#include <atomic>
#include <functional>
class MotorControl {
public:
explicit MotorControl(CANBus& canbus);
void controlMIT(Motor& motor, double kp, double kd, double q, double dq, double tau);
void controlMIT2(Motor& motor, double kp, double kd, double q, double dq, double tau);
void enable(Motor& motor);
void disable(Motor& motor);
void set_zero_position(Motor& motor);
void recv();
void sendData(uint16_t motor_id, const std::array<uint8_t,8>& data);
bool addMotor(Motor& motor);
void recv_set_param_data();
private:
CANBus& canbus_;
std::map<uint16_t, Motor*> motors_map;
static constexpr double Limit_Param[12][3] = {
{12.5, 30, 10}, {12.5, 50, 10}, {12.5, 8, 28}, {12.5, 10, 28},
{12.5, 45, 20}, {12.5, 45, 40}, {12.5, 45, 54}, {12.5, 25, 200},
{12.5, 20, 200}, {12.5, 280, 1}, {12.5, 45, 10}, {12.5, 45, 10}
};
void processPacket(const can_frame& frame);
void controlCmd(Motor &motor, uint8_t cmd );
void readRIDParam(Motor& motor, DM_variable RID);
void writeMotorParam(Motor& motor, DM_variable RID, double value);
};
#endif // MOTOR_CONTROL_H

95
openarm_hardware/include/openarm_hardware/openarm_hardware.hpp Normal file
View File

@ -0,0 +1,95 @@
// Copyright (c) 2025, Reazon Holdings, Inc.
// Copyright (c) 2025, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef OPENARM_HARDWARE__OPENARM_HARDWARE_HPP_
#define OPENARM_HARDWARE__OPENARM_HARDWARE_HPP_
#include <string>
#include <vector>
#include <memory>
#include "openarm_hardware/visibility_control.h"
#include "hardware_interface/system_interface.hpp"
#include "hardware_interface/handle.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp_lifecycle/state.hpp"
#include "canbus.hpp"
#include "motor.hpp"
#include "motor_control.hpp"
namespace openarm_hardware
{
const std::vector<DM_Motor_Type> MOTORS_TYPES = {DM_Motor_Type::DM4340, DM_Motor_Type::DM4340, DM_Motor_Type::DM4340, DM_Motor_Type::DM4340, DM_Motor_Type::DM4310, DM_Motor_Type::DM4310, DM_Motor_Type::DM4310};
const std::vector<uint16_t> CAN_DEVICE_IDS = {0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07};
const std::vector<uint16_t> CAN_MASTER_IDS = {0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17};
const std::vector<bool> MOTOR_WITH_TORQUE = {true,true,true,true,true,true,true};
const Control_Type CONTROL_MODE = Control_Type::MIT;
const double DEFAULT_KP = 1.0;
const double DEFAULT_KD = 0.0;
class OpenArmHW : public hardware_interface::SystemInterface
{
public:
OpenArmHW();
TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
hardware_interface::CallbackReturn on_init(
const hardware_interface::HardwareInfo & info) override;
TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
hardware_interface::CallbackReturn on_configure(
const rclcpp_lifecycle::State & previous_state) override;
TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
hardware_interface::CallbackReturn on_activate(
const rclcpp_lifecycle::State & previous_state) override;
TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
hardware_interface::CallbackReturn on_deactivate(
const rclcpp_lifecycle::State & previous_state) override;
TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
hardware_interface::return_type read(
const rclcpp::Time & time, const rclcpp::Duration & period) override;
TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
hardware_interface::return_type write(
const rclcpp::Time & time, const rclcpp::Duration & period) override;
private:
std::unique_ptr<CANBus> canbus_;
MotorControl motor_control_;
std::vector<double> pos_commands_;
std::vector<double> pos_states_;
std::vector<double> vel_commands_;
std::vector<double> vel_states_;
std::vector<double> tau_ff_commands_;
std::vector<double> tau_states_;
std::vector<std::unique_ptr<Motor>> motors_;
};
} // namespace openarm_hardware
#endif // OPENARM_HARDWARE__OPENARM_HARDWARE_HPP_

50
openarm_hardware/include/openarm_hardware/visibility_control.h Normal file
View File

@ -0,0 +1,50 @@
// Copyright (c) 2025, Reazon Holdings, Inc.
// Copyright (c) 2025, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef OPENARM_HARDWARE__VISIBILITY_CONTROL_H_
#define OPENARM_HARDWARE__VISIBILITY_CONTROL_H_
// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
// https://gcc.gnu.org/wiki/Visibility
#if defined _WIN32 || defined __CYGWIN__
#ifdef __GNUC__
#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((dllexport))
#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __attribute__((dllimport))
#else
#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __declspec(dllexport)
#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __declspec(dllimport)
#endif
#ifdef TEMPLATES__ROS2_CONTROL__VISIBILITY_BUILDING_DLL
#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT
#else
#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT
#endif
#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL
#else
#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((visibility("default")))
#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT
#if __GNUC__ >= 4
#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC __attribute__((visibility("default")))
#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL __attribute__((visibility("hidden")))
#else
#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL
#endif
#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE
#endif
#endif // OPENARM_HARDWARE__VISIBILITY_CONTROL_H_

9
openarm_hardware/openarm_hardware.xml Normal file
View File

@ -0,0 +1,9 @@
<library path="openarm_hardware">
<class name="openarm_hardware/OpenArmHW"
type="openarm_hardware::OpenArmHW"
base_class_type="hardware_interface::SystemInterface">
<description>
ros2_control hardware interface.
</description>
</class>
</library>

18
openarm_hardware/package.xml Normal file
View File

@ -0,0 +1,18 @@
<?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_hardware</name>
<version>0.0.0</version>
<description>Hardware interface for OpenArm</description>
<maintainer email="t95zhou@uwaterloo.ca">Thomason Zhou</maintainer>
<license>BSD-3-Clause</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

20
openarm_hardware/setup.bash Executable file
View File

@ -0,0 +1,20 @@
#!/bin/bash
CAN_INTERFACE=can0
for DEVICE in /dev/ttyACM*; do
if [ -e "$DEVICE" ]; then
echo "Using device: $DEVICE"
break
fi
done
if [ -z "$DEVICE" ]; then
echo "No /dev/ttyACM* device found."
exit 1
fi
sudo pkill -f slcand
sudo slcand -o -c -s8 "$DEVICE" "$CAN_INTERFACE"
sudo ip link set "$CAN_INTERFACE" up type can bitrate 1000000
ip link show "$CAN_INTERFACE"

68
openarm_hardware/src/canbus.cpp Normal file
View File

@ -0,0 +1,68 @@
#include "canbus.hpp"
#include <poll.h>
CANBus::CANBus(const std::string& interface) {
struct ifreq ifr;
struct sockaddr_can addr;
sock_ = socket(PF_CAN, SOCK_RAW, CAN_RAW);
if (sock_ < 0) {
perror("Error while opening CAN socket");
exit(EXIT_FAILURE);
}
std::strncpy(ifr.ifr_name, interface.c_str(), IFNAMSIZ);
if (ioctl(sock_, SIOCGIFINDEX, &ifr) < 0) {
perror("Error getting interface index");
exit(EXIT_FAILURE);
}
std::memset(&addr, 0, sizeof(addr));
addr.can_family = AF_CAN;
addr.can_ifindex = ifr.ifr_ifindex;
if (bind(sock_, (struct sockaddr*)&addr, sizeof(addr)) < 0) {
perror("Error in CAN socket bind");
exit(EXIT_FAILURE);
}
}
CANBus::~CANBus() {
close(sock_);
}
bool CANBus::send(uint16_t motor_id, const std::array<uint8_t, 8>& data) {
struct can_frame frame;
std::memset(&frame, 0, sizeof(frame));
frame.can_id = motor_id;
frame.can_dlc = data.size();
std::copy(data.begin(), data.end(), frame.data);
if (write(sock_, &frame, sizeof(frame)) != sizeof(frame)) {
perror("Error sending CAN frame");
return false;
} else {
// std::cout << "Sent CAN frame to motor ID " << motor_id << std::endl;
return true;
}
}
struct can_frame CANBus::recv() {
struct can_frame frame;
std::memset(&frame, 0, sizeof(frame));
// change socket into non blocking mode
// int flags = fcntl(sock_, F_GETFL, 0);
// fcntl(sock_, F_SETFL, flags | O_NONBLOCK);
int nbytes = read(sock_, &frame, sizeof(struct can_frame));
if (nbytes < 0) {
perror("CAN read error");
} else {
// std::cout << "Received CAN frame from motor ID " << frame.can_id << std::endl;
}
return frame;
}

120
openarm_hardware/src/motor.cpp Normal file
View File

@ -0,0 +1,120 @@
#include "motor.hpp"
Motor::Motor(DM_Motor_Type motorType, uint16_t slaveID, uint16_t masterID)
: MotorType(motorType), SlaveID(slaveID), MasterID(masterID),
Pd(0.0), Vd(0.0), goal_position(0.0), goal_tau(0.0),
state_q(0.0), state_dq(0.0), state_tau(0.0),
state_tmos(0), state_trotor(0),
isEnable(false), NowControlMode(Control_Type::MIT) {}
void Motor::recv_data(double q, double dq, double tau, int tmos, int trotor) {
state_q = q;
state_dq = dq;
state_tau = tau;
state_tmos = tmos;
state_trotor = trotor;
}
double Motor::getPosition() const { return state_q; }
double Motor::getVelocity() const { return state_dq; }
double Motor::getTorque() const { return state_tau; }
double Motor::getGoalPosition() const {
return goal_position;
}
void Motor::setGoalPosition(double pos) {
goal_position = pos;
}
double Motor::getGoalTau() const {
return goal_tau;
}
void Motor::setGoalTau(double tau) {
goal_tau = tau;
}
int Motor::getStateTmos() const {
return state_tmos;
}
void Motor::setStateTmos(int tmos) {
state_tmos = tmos;
}
int Motor::getStateTrotor() const {
return state_trotor;
}
void Motor::setStateTrotor(int trotor) {
state_trotor = trotor;
}
int Motor::getParam(int RID) const {
auto it = temp_param_dict.find(RID);
return (it != temp_param_dict.end()) ? it->second : -1;
}
double LIMIT_MIN_MAX(double x, double min, double max) {
return std::max(min, std::min(x, max));
}
uint16_t double_to_uint(double x, double x_min, double x_max, int bits) {
x = LIMIT_MIN_MAX(x, x_min, x_max);
double span = x_max - x_min;
double data_norm = (x - x_min) / span;
return static_cast<uint16_t>(data_norm * ((1 << bits) - 1));
}
double uint_to_double(uint16_t x, double min, double max, int bits) {
double span = max - min;
double data_norm = static_cast<double>(x) / ((1 << bits) - 1);
return data_norm * span + min;
}
std::array<uint8_t, 4> double_to_uint8s(double value) {
std::array<uint8_t, 4> bytes;
std::memcpy(bytes.data(), &value, sizeof(double));
return bytes;
}
std::array<uint8_t, 4> data_to_uint8s(uint32_t value) {
std::array<uint8_t, 4> bytes;
std::memcpy(bytes.data(), &value, sizeof(uint32_t));
return bytes;
}
uint32_t uint8s_to_uint32(uint8_t byte1, uint8_t byte2, uint8_t byte3, uint8_t byte4) {
uint32_t value;
uint8_t bytes[4] = {byte1, byte2, byte3, byte4};
std::memcpy(&value, bytes, sizeof(uint32_t));
return value;
}
double uint8s_to_double(uint8_t byte1, uint8_t byte2, uint8_t byte3, uint8_t byte4) {
double value;
uint8_t bytes[4] = {byte1, byte2, byte3, byte4};
std::memcpy(&value, bytes, sizeof(double));
return value;
}
bool is_in_ranges(int number) {
return (7 <= number && number <= 10) || (13 <= number && number <= 16) || (35 <= number && number <= 36);
}
void print_hex(const std::vector<uint8_t>& data) {
for (auto byte : data) {
std::cout << std::hex << std::uppercase << (int)byte << " ";
}
std::cout << std::dec << std::endl;
}
template <typename T>
T get_enum_by_index(int index) {
if (index >= 0 && index < static_cast<int>(T::COUNT)) {
return static_cast<T>(index);
}
return static_cast<T>(-1);
}

163
openarm_hardware/src/motor_control.cpp Normal file
View File

@ -0,0 +1,163 @@
#include "motor_control.hpp"
#include "motor.hpp"
MotorControl::MotorControl(CANBus& canbus) : canbus_(canbus) {}
bool MotorControl::addMotor(Motor& motor) {
motors_map[motor.SlaveID] = &motor;
if (motor.MasterID != 0) {
motors_map[motor.MasterID] = &motor;
}
return true;
}
void MotorControl::enable(Motor& motor) {
controlCmd(motor, 0xFC);
sleep(0.3);
}
void MotorControl::disable(Motor& motor) {
controlCmd(motor, 0xFD);
sleep(0.3);
}
void MotorControl::set_zero_position(Motor& motor){
controlCmd(motor, 0xFE);
sleep(0.3);
recv();
}
void MotorControl::controlMIT(Motor& motor, double kp, double kd, double q, double dq, double tau) {
controlMIT2(motor, kp, kd, q, dq, tau);
recv();
}
void MotorControl::controlMIT2(Motor& motor, double kp, double kd, double q, double dq, double tau) {
if (motors_map.find(motor.SlaveID) == motors_map.end()) {
std::cerr << "controlMIT ERROR: Motor ID not found" << std::endl;
return;
}
uint16_t kp_uint = double_to_uint(kp, 0, 500, 12);
uint16_t kd_uint = double_to_uint(kd, 0, 5, 12);
int motor_index = static_cast<int>(motor.MotorType);
double Q_MAX = Limit_Param[motor_index][0];
double DQ_MAX = Limit_Param[motor_index][1];
double TAU_MAX = Limit_Param[motor_index][2];
uint16_t q_uint = double_to_uint(q, -Q_MAX, Q_MAX, 16);
uint16_t dq_uint = double_to_uint(dq, -DQ_MAX, DQ_MAX, 12);
uint16_t tau_uint = double_to_uint(tau, -TAU_MAX, TAU_MAX, 12);
std::array<uint8_t, 8> data = {
static_cast<uint8_t>((q_uint >> 8) & 0xFF),
static_cast<uint8_t>(q_uint & 0xFF),
static_cast<uint8_t>(dq_uint >> 4),
static_cast<uint8_t>(((dq_uint & 0xF) << 4) | ((kp_uint >> 8) & 0xF)),
static_cast<uint8_t>(kp_uint & 0xFF),
static_cast<uint8_t>(kd_uint >> 4),
static_cast<uint8_t>(((kd_uint & 0xF) << 4) | ((tau_uint >> 8) & 0xF)),
static_cast<uint8_t>(tau_uint & 0xFF)
};
sendData(motor.SlaveID, data);
}
void MotorControl::sendData(uint16_t motor_id, const std::array<uint8_t, 8>& data) {
canbus_.send(motor_id, data);
}
void MotorControl::recv() {
can_frame frame = canbus_.recv();
processPacket(frame);
}
void MotorControl::processPacket(const can_frame& frame) {
uint16_t motorID = frame.data[0];
uint8_t cmd = 0x11;
if (cmd == 0x11) {
if (motorID != 0x00) {
auto it = motors_map.find(motorID);
if (it != motors_map.end() && it->second) {
Motor* motor = it->second;
uint16_t q_uint = (frame.data[1] << 8) | frame.data[2];
uint16_t dq_uint = (frame.data[3] << 4) | (frame.data[4] >> 4);
uint16_t tau_uint = ((frame.data[4] & 0xf) << 8) | frame.data[5];
int t_mos = frame.data[6];
int t_rotor = frame.data[7];
double Q_MAX = Limit_Param[static_cast<int>(motor->MotorType)][0];
double DQ_MAX = Limit_Param[static_cast<int>(motor->MotorType)][1];
double TAU_MAX = Limit_Param[static_cast<int>(motor->MotorType)][2];
double recv_q = uint_to_double(q_uint, -Q_MAX, Q_MAX, 16);
double recv_dq = uint_to_double(dq_uint, -DQ_MAX, DQ_MAX, 12);
double recv_tau = uint_to_double(tau_uint, -TAU_MAX, TAU_MAX, 12);
motor->recv_data(recv_q, recv_dq, recv_tau, t_mos, t_rotor);
}
} else {
uint16_t MasterID = frame.data[0] & 0x0F;
auto it = motors_map.find(MasterID);
if (it != motors_map.end() && it->second) {
Motor* motor = it->second;
uint16_t q_uint = (frame.data[1] << 8) | frame.data[2];
uint16_t dq_uint = (frame.data[3] << 4) | (frame.data[4] >> 4);
uint16_t tau_uint = ((frame.data[4] & 0xf) << 8) | frame.data[5];
int t_mos = frame.data[6];
int t_rotor = frame.data[7];
double Q_MAX = Limit_Param[static_cast<int>(motor->MotorType)][0];
double DQ_MAX = Limit_Param[static_cast<int>(motor->MotorType)][1];
double TAU_MAX = Limit_Param[static_cast<int>(motor->MotorType)][2];
double recv_q = uint_to_double(q_uint, -Q_MAX, Q_MAX, 16);
double recv_dq = uint_to_double(dq_uint, -DQ_MAX, DQ_MAX, 12);
double recv_tau = uint_to_double(tau_uint, -TAU_MAX, TAU_MAX, 12);
motor->recv_data(recv_q, recv_dq, recv_tau, t_mos, t_rotor);
}
}
}
}
void MotorControl::controlCmd(Motor& motor, uint8_t cmd) {
std::array<uint8_t, 8> data_buf = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, cmd};
sendData(motor.SlaveID, data_buf);
}
void MotorControl::readRIDParam(Motor& motor, DM_variable RID) {
std::array<uint8_t, 8> data = {
static_cast<uint8_t>(motor.SlaveID & 0xFF),
static_cast<uint8_t>((motor.SlaveID >> 8) & 0xFF),
0x33,
static_cast<uint8_t>(RID),
0x00, 0x00, 0x00, 0x00
};
canbus_.send(0x7FF, data);
}
void MotorControl::writeMotorParam(Motor& motor, DM_variable RID, double value) {
std::array<uint8_t, 8> data = {
static_cast<uint8_t>(motor.SlaveID & 0xFF),
static_cast<uint8_t>((motor.SlaveID >> 8) & 0xFF),
0x55,
static_cast<uint8_t>(RID)
};
if (is_in_ranges(static_cast<int>(RID))) {
auto intData = data_to_uint8s(static_cast<uint32_t>(value));
std::copy(intData.begin(), intData.end(), data.begin() + 4);
} else {
auto doubleData = double_to_uint8s(value);
std::copy(doubleData.begin(), doubleData.end(), data.begin() + 4);
}
canbus_.send(0x7FF, data);
}

138
openarm_hardware/src/openarm_hardware.cpp Normal file
View File

@ -0,0 +1,138 @@
// Copyright (c) 2025, Reazon Holdings, Inc.
// Copyright (c) 2025, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <limits>
#include <vector>
#include "openarm_hardware/openarm_hardware.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "rclcpp/rclcpp.hpp"
namespace openarm_hardware
{
static const std::string& can_device_name = "can0";
OpenArmHW::OpenArmHW():
canbus_(std::make_unique<CANBus>(can_device_name)),
motor_control_(MotorControl(*canbus_)) {
for(size_t i = 0; i < MOTORS_TYPES.size(); ++i){
motors_[i] = std::make_unique<Motor>(MOTORS_TYPES[i], CAN_DEVICE_IDS[i], CAN_MASTER_IDS[i]);
}
for(const auto& motor: motors_){
motor_control_.addMotor(*motor);
}
}
hardware_interface::CallbackReturn OpenArmHW::on_init(
const hardware_interface::HardwareInfo & info)
{
if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS)
{
return CallbackReturn::ERROR;
}
pos_states_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
pos_commands_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
vel_states_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
vel_commands_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
tau_states_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
tau_ff_commands_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
return CallbackReturn::SUCCESS;
}
hardware_interface::CallbackReturn OpenArmHW::on_configure(
const rclcpp_lifecycle::State & /*previous_state*/)
{
// zero position or calibrate to pose
return CallbackReturn::SUCCESS;
}
std::vector<hardware_interface::StateInterface> OpenArmHW::export_state_interfaces()
{
std::vector<hardware_interface::StateInterface> state_interfaces;
for (size_t i = 0; i < info_.joints.size(); ++i)
{
state_interfaces.emplace_back(hardware_interface::StateInterface(info_.joints[i].name, hardware_interface::HW_IF_POSITION, &pos_states_[i]));
state_interfaces.emplace_back(hardware_interface::StateInterface(info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &vel_states_[i]));
state_interfaces.emplace_back(hardware_interface::StateInterface(info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &tau_states_[i]));
}
return state_interfaces;
}
std::vector<hardware_interface::CommandInterface> OpenArmHW::export_command_interfaces()
{
std::vector<hardware_interface::CommandInterface> command_interfaces;
for (size_t i = 0; i < info_.joints.size(); ++i)
{
command_interfaces.emplace_back(hardware_interface::CommandInterface(info_.joints[i].name, hardware_interface::HW_IF_POSITION, &pos_commands_[i]));
command_interfaces.emplace_back(hardware_interface::CommandInterface(info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &vel_commands_[i]));
command_interfaces.emplace_back(hardware_interface::CommandInterface(info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &tau_ff_commands_[i]));
}
return command_interfaces;
}
hardware_interface::CallbackReturn OpenArmHW::on_activate(
const rclcpp_lifecycle::State & /*previous_state*/)
{
for(const auto& motor: motors_){
motor_control_.enable(*motor);
}
read(rclcpp::Time(0), rclcpp::Duration(0, 0));
return CallbackReturn::SUCCESS;
}
hardware_interface::CallbackReturn OpenArmHW::on_deactivate(
const rclcpp_lifecycle::State & /*previous_state*/)
{
for(const auto& motor: motors_){
motor_control_.disable(*motor);
}
return CallbackReturn::SUCCESS;
}
hardware_interface::return_type OpenArmHW::read(
const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
{
for(size_t i = 0; i < motors_.size(); ++i){
pos_states_[i] = motors_[i]->getPosition();
vel_states_[i] = motors_[i]->getVelocity();
tau_states_[i] = motors_[i]->getTorque();
}
return hardware_interface::return_type::OK;
}
hardware_interface::return_type OpenArmHW::write(
const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
{
for(size_t i = 0; i < motors_.size(); ++i){
motor_control_.controlMIT(*motors_[i], DEFAULT_KP, DEFAULT_KD, pos_commands_[i], vel_commands_[i], tau_ff_commands_[i]);
}
return hardware_interface::return_type::OK;
}
} // namespace openarm_hardware
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(
openarm_hardware::OpenArmHW, hardware_interface::SystemInterface)

131
openarm_hardware/test/test_openarm_hardware.cpp Normal file
View File

@ -0,0 +1,131 @@
// Copyright (c) 2025, Reazon Holdings, Inc.
// Copyright (c) 2025, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gmock/gmock.h>
#include <string>
#include "hardware_interface/resource_manager.hpp"
#include "ros2_control_test_assets/components_urdfs.hpp"
#include "ros2_control_test_assets/descriptions.hpp"
class TestOpenArmHW : public ::testing::Test
{
protected:
void SetUp() override
{
openarm_hardware_7dof_ =
R"(
<ros2_control name="OpenArmHW7DOF" type="system">
<hardware>
<!-- By default, set up controllers for simulation. This won't work on real hardware -->
<plugin>mock_components/GenericSystem</plugin>
<plugin>openarm_hardware/OpenArmHW</plugin>
</hardware>
<joint name="rev1">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<state_interface name="position">
<param name="initial_value">0</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="rev2">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<state_interface name="position">
<param name="initial_value">0</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="rev3">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<state_interface name="position">
<param name="initial_value">0</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="rev4">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<state_interface name="position">
<param name="initial_value">0</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="rev5">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<state_interface name="position">
<param name="initial_value">0</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="rev6">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<state_interface name="position">
<param name="initial_value">0</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="rev7">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<state_interface name="position">
<param name="initial_value">0</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="left_pris1">
<command_interface name="position"/>
<state_interface name="position">
<param name="initial_value">0</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
<joint name="right_pris2">
<state_interface name="position">
<param name="initial_value">0</param>
</state_interface>
</joint>
</ros2_control>
)";
}
std::string openarm_hardware_7dof_;
};
TEST_F(TestOpenArmHW, load_openarm_hardware_7dof)
{
auto urdf = ros2_control_test_assets::urdf_head + openarm_hardware_7dof_ +
ros2_control_test_assets::urdf_tail;
ASSERT_NO_THROW(hardware_interface::ResourceManager rm(urdf));
}

10
openarm_moveit_config/.setup_assistant
View File

@ -1,20 +1,22 @@
moveit_setup_assistant_config:
urdf:
package: openarm_description
relative_path: urdf/openarm_grip.urdf
relative_path: urdf/openarm.urdf
srdf:
relative_path: config/openarm_grip.srdf
relative_path: config/openarm.srdf
package_settings:
author_name: Thomason Zhou
author_email: t95zhou@uwaterloo.ca
generated_timestamp: 1738914276
generated_timestamp: 1741767765
control_xacro:
command:
- position
- velocity
- effort
state:
- position
- velocity
- effort
modified_urdf:
xacros:
- control_xacro
@ -22,6 +24,8 @@ moveit_setup_assistant_config:
command:
- position
- velocity
- effort
state:
- position
- velocity
- effort

8
openarm_moveit_config/config/initial_positions.yaml
View File

@ -1,12 +1,12 @@
# Default initial positions for openarm_grip's ros2_control fake system
# Default initial positions for openarm's ros2_control fake system
initial_positions:
slider_left: 0
slider_right: 0
left_pris1: 0
right_pris2: 0
rev1: 0
rev2: 0
rev3: 0
rev4: 0
rev5: 0
rev6: 0
rev7: 0
rev7: 0

73
openarm_moveit_config/config/joint_limits.yaml
View File

@ -8,48 +8,53 @@ default_acceleration_scaling_factor: 0.1
# 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:
slider_left:
has_velocity_limits: true
max_velocity: 100.0
left_pris1:
has_velocity_limits: false
max_velocity: 0
has_acceleration_limits: false
max_acceleration: 0.0
slider_right:
has_velocity_limits: true
max_velocity: 100.0
max_acceleration: 0
right_pris2:
has_velocity_limits: false
max_velocity: 0
has_acceleration_limits: false
max_acceleration: 0.0
max_acceleration: 0
rev1:
has_velocity_limits: true
max_velocity: 0.5
has_acceleration_limits: true
max_acceleration: 0.5
has_velocity_limits: false
max_velocity: 0
has_acceleration_limits: false
max_acceleration: 0
rev2:
has_velocity_limits: true
max_velocity: 0.5
has_acceleration_limits: true
max_acceleration: 0.5
has_velocity_limits: false
max_velocity: 0
has_acceleration_limits: false
max_acceleration: 0
rev3:
has_velocity_limits: true
max_velocity: 0.5
has_acceleration_limits: true
max_acceleration: 0.5
has_velocity_limits: false
max_velocity: 0
has_acceleration_limits: false
max_acceleration: 0
rev4:
has_velocity_limits: true
max_velocity: 0.5
has_acceleration_limits: true
max_acceleration: 0.5
has_velocity_limits: false
max_velocity: 0
has_acceleration_limits: false
max_acceleration: 0
rev5:
has_velocity_limits: true
max_velocity: 0.5
has_acceleration_limits: true
max_acceleration: 0.5
has_velocity_limits: false
max_velocity: 0
has_acceleration_limits: false
max_acceleration: 0
rev6:
has_velocity_limits: true
max_velocity: 0.5
has_acceleration_limits: true
max_acceleration: 0.5
has_velocity_limits: false
max_velocity: 0
has_acceleration_limits: false
max_acceleration: 0
rev7:
has_velocity_limits: false
max_velocity: 0.0
max_velocity: 0
has_acceleration_limits: false
max_acceleration: 0.0
max_acceleration: 0
right_pris2:
has_velocity_limits: false
max_velocity: 0
has_acceleration_limits: false
max_acceleration: 0

8
openarm_moveit_config/config/moveit.rviz
View File

@ -19,10 +19,6 @@ Visualization Manager:
Loop Animation: true
State Display Time: 0.05 s
Trajectory Topic: display_planned_path
Planning Request:
Interactive Marker Size: 0.1
Planning Group: openarm_arm
Query Goal State: true
Planning Scene Topic: monitored_planning_scene
Robot Description: robot_description
Scene Geometry:
@ -31,7 +27,7 @@ Visualization Manager:
Robot Alpha: 0.5
Value: true
Global Options:
Fixed Frame: world
Fixed Frame: link1
Tools:
- Class: rviz_default_plugins/Interact
- Class: rviz_default_plugins/MoveCamera
@ -47,7 +43,7 @@ Visualization Manager:
Z: 0.30
Name: Current View
Pitch: 0.5
Target Frame: world
Target Frame: link1
Yaw: -0.623
Window Geometry:
Height: 975

13
openarm_moveit_config/config/moveit_controllers.yaml
View File

@ -16,13 +16,10 @@ moveit_simple_controller_manager:
- rev4
- rev5
- rev6
action_ns: follow_joint_trajectory
default: true
gripper_controller:
type: FollowJointTrajectory
joints:
- rev7
- slider_left
- slider_right
action_ns: follow_joint_trajectory
gripper_controller:
type: GripperCommand
joints:
- left_pris1
action_ns: gripper_cmd
default: true

38
openarm_moveit_config/config/ompl_planning.yaml Normal file
View File

@ -0,0 +1,38 @@
openarm_moveit_config:
default_planner_config: RRTConnect
planner_configs:
- AnytimePathShortening
- SBL
- EST
- LBKPIECE
- BKPIECE
- KPIECE
- RRT
- RRTConnect
- RRTstar
- TRRT
- PRM
- PRMstar
- FMT
- BFMT
- PDST
- STRIDE
- BiTRRT
- LBTRRT
- BiEST
- ProjEST
- LazyPRM
- LazyPRMstar
- SPARS
- SPARStwo
projection_evaluator: joints(shoulder_pan_joint,shoulder_lift_joint)
longest_valid_segment_fraction: 0.005
planning_plugin: 'ompl_interface/OMPLPlanner'
request_adapters: >-
default_planner_request_adapters/AddTimeOptimalParameterization
default_planner_request_adapters/FixWorkspaceBounds
default_planner_request_adapters/FixStartStateBounds
default_planner_request_adapters/FixStartStateCollision
default_planner_request_adapters/FixStartStatePathConstraints
start_state_max_bounds_error: 0.1

89
openarm_moveit_config/config/openarm.srdf Normal file
View File

@ -0,0 +1,89 @@
<?xml version="1.0" encoding="UTF-8"?>
<!--This does not replace URDF, and is not an extension of URDF.
This is a format for representing semantic information about the robot structure.
A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
-->
<robot name="openarm">
<!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
<!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
<!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
<!--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="openarm_arm">
<link name="link1"/>
<link name="link2"/>
<link name="link3"/>
<link name="link4"/>
<link name="link5"/>
<link name="link6"/>
<link name="link7"/>
<link name="link8"/>
<joint name="rev1"/>
<joint name="rev2"/>
<joint name="rev3"/>
<joint name="rev4"/>
<joint name="rev5"/>
<joint name="rev6"/>
<joint name="rev7"/>
<chain base_link="link1" tip_link="link8"/>
</group>
<group name="gripper">
<link name="link_left_jaw"/>
<link name="link_right_jaw"/>
<joint name="left_pris1"/>
<joint name="right_pris2"/>
</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="zero_pos_vertical" group="openarm_arm">
<joint name="rev1" value="0"/>
<joint name="rev2" value="0"/>
<joint name="rev3" value="0"/>
<joint name="rev4" value="0"/>
<joint name="rev5" value="0"/>
<joint name="rev6" value="0"/>
<joint name="rev7" value="0"/>
</group_state>
<group_state name="horizontal_bent_elbow" group="openarm_arm">
<joint name="rev1" value="0"/>
<joint name="rev2" value="-1.5707"/>
<joint name="rev3" value="-1.5707"/>
<joint name="rev4" value="2.4974"/>
<joint name="rev5" value="0"/>
<joint name="rev6" value="0"/>
<joint name="rev7" value="0"/>
</group_state>
<!--END EFFECTOR: Purpose: Represent information about an end effector.-->
<end_effector name="gripper" parent_link="link8" group="gripper" parent_group="openarm_arm"/>
<!--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="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="link1" link2="link2" reason="Adjacent"/>
<disable_collisions link1="link1" link2="link3" reason="Never"/>
<disable_collisions link1="link1" link2="link5" reason="Never"/>
<disable_collisions link1="link2" link2="link3" reason="Adjacent"/>
<disable_collisions link1="link2" link2="link4" reason="Never"/>
<disable_collisions link1="link2" link2="link5" reason="Never"/>
<disable_collisions link1="link2" link2="link6" reason="Never"/>
<disable_collisions link1="link2" link2="link7" 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="link4" link2="link7" reason="Never"/>
<disable_collisions link1="link4" link2="link_left_jaw" reason="Never"/>
<disable_collisions link1="link4" link2="link_right_jaw" reason="Never"/>
<disable_collisions link1="link5" link2="link6" reason="Adjacent"/>
<disable_collisions link1="link5" link2="link7" reason="Never"/>
<disable_collisions link1="link5" link2="link8" reason="Never"/>
<disable_collisions link1="link5" link2="link_left_jaw" reason="Never"/>
<disable_collisions link1="link5" link2="link_right_jaw" reason="Never"/>
<disable_collisions link1="link6" link2="link7" reason="Adjacent"/>
<disable_collisions link1="link6" link2="link8" reason="Default"/>
<disable_collisions link1="link6" link2="link_left_jaw" reason="Never"/>
<disable_collisions link1="link6" link2="link_right_jaw" reason="Never"/>
<disable_collisions link1="link7" link2="link8" reason="Adjacent"/>
<disable_collisions link1="link7" link2="link_left_jaw" reason="Never"/>
<disable_collisions link1="link7" link2="link_right_jaw" reason="Never"/>
<disable_collisions link1="link8" link2="link_left_jaw" reason="Adjacent"/>
<disable_collisions link1="link8" link2="link_right_jaw" reason="Adjacent"/>
<disable_collisions link1="link_left_jaw" link2="link_right_jaw" reason="Never"/>
</robot>

5
openarm_moveit_config/config/openarm.urdf.xacro Normal file
View File

@ -0,0 +1,5 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="openarm">
<!-- Import openarm urdf file -->
<xacro:include filename="$(find openarm_description)/urdf/openarm.urdf" />
</robot>

55
openarm_moveit_config/config/openarm_grip.srdf
View File

@ -1,55 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<!--This does not replace URDF, and is not an extension of URDF.
This is a format for representing semantic information about the robot structure.
A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
-->
<robot name="openarm_grip">
<!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
<!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
<!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
<!--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="openarm_arm">
<joint name="world_joint"/>
<joint name="rev1"/>
<joint name="rev2"/>
<joint name="rev3"/>
<joint name="rev4"/>
<joint name="rev5"/>
<joint name="rev6"/>
</group>
<group name="gripper">
<link name="grip_attach_1"/>
<link name="left_jaw_1"/>
<link name="right_jaw_1"/>
</group>
<!--END EFFECTOR: Purpose: Represent information about an end effector.-->
<end_effector name="gripper" parent_link="J7_v1_1" group="gripper"/>
<!--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="world_to_dummy" type="fixed" parent_frame="world" child_link="dummy"/>
<!--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="J2_v1_1" link2="J3_v1_1" reason="Adjacent"/>
<disable_collisions link1="J2_v1_1" link2="J4_v1_1" reason="Never"/>
<disable_collisions link1="J2_v1_1" link2="J5_v1_1" reason="Never"/>
<disable_collisions link1="J2_v1_1" link2="J6_v1_1" reason="Never"/>
<disable_collisions link1="J2_v1_1" link2="J7_v1_1" reason="Never"/>
<disable_collisions link1="J2_v1_1" link2="base_link" reason="Adjacent"/>
<disable_collisions link1="J3_v1_1" link2="J4_v1_1" reason="Adjacent"/>
<disable_collisions link1="J3_v1_1" link2="J5_v1_1" reason="Never"/>
<disable_collisions link1="J3_v1_1" link2="J6_v1_1" reason="Never"/>
<disable_collisions link1="J3_v1_1" link2="J7_v1_1" reason="Never"/>
<disable_collisions link1="J3_v1_1" link2="base_link" reason="Never"/>
<disable_collisions link1="J4_v1_1" link2="J5_v1_1" reason="Adjacent"/>
<disable_collisions link1="J4_v1_1" link2="J7_v1_1" reason="Never"/>
<disable_collisions link1="J4_v1_1" link2="base_link" reason="Never"/>
<disable_collisions link1="J5_v1_1" link2="J6_v1_1" reason="Adjacent"/>
<disable_collisions link1="J5_v1_1" link2="J7_v1_1" reason="Never"/>
<disable_collisions link1="J5_v1_1" link2="base_link" reason="Never"/>
<disable_collisions link1="J6_v1_1" link2="J7_v1_1" reason="Adjacent"/>
<disable_collisions link1="J7_v1_1" link2="grip_attach_1" reason="Adjacent"/>
<disable_collisions link1="J7_v1_1" link2="left_jaw_1" reason="Never"/>
<disable_collisions link1="J7_v1_1" link2="right_jaw_1" reason="Never"/>
<disable_collisions link1="grip_attach_1" link2="left_jaw_1" reason="Adjacent"/>
<disable_collisions link1="grip_attach_1" link2="right_jaw_1" reason="Adjacent"/>
<disable_collisions link1="left_jaw_1" link2="right_jaw_1" reason="Never"/>
</robot>

14
openarm_moveit_config/config/openarm_grip.urdf.xacro
View File

@ -1,14 +0,0 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="openarm_grip">
<xacro:arg name="initial_positions_file" default="initial_positions.yaml" />
<!-- Import openarm_grip urdf file -->
<xacro:include filename="$(find openarm_description)/urdf/openarm_grip.urdf" />
<!-- Import control_xacro -->
<xacro:include filename="openarm_grip.ros2_control.xacro" />
<xacro:openarm_grip_ros2_control name="FakeSystem" initial_positions_file="$(arg initial_positions_file)"/>
</robot>

19
openarm_moveit_config/config/ros2_controllers.yaml
View File

@ -3,17 +3,20 @@ controller_manager:
ros__parameters:
update_rate: 100 # Hz
openarm_arm_controller:
type: joint_trajectory_controller/JointTrajectoryController
gripper_controller:
type: position_controllers/GripperActionController
openarm_arm_controller:
type: joint_trajectory_controller/JointTrajectoryController
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
gripper_controller:
ros__parameters:
joint: left_pris1
openarm_arm_controller:
ros__parameters:
joints:
@ -23,13 +26,13 @@ openarm_arm_controller:
- rev4
- rev5
- rev6
- rev7
command_interfaces:
- position
- velocity
- effort
state_interfaces:
- position
- velocity
allow_nonzero_velocity_at_trajectory_end: true
gripper_controller:
ros__parameters:
joint: rev7
- effort
allow_nonzero_velocity_at_trajectory_end: false

2
openarm_moveit_config/launch/demo.launch.py
View File

@ -3,5 +3,5 @@ from moveit_configs_utils.launches import generate_demo_launch
def generate_launch_description():
moveit_config = MoveItConfigsBuilder("openarm_grip", package_name="openarm_moveit_config").to_moveit_configs()
moveit_config = MoveItConfigsBuilder("openarm", package_name="openarm_moveit_config").to_moveit_configs()
return generate_demo_launch(moveit_config)

2
openarm_moveit_config/launch/move_group.launch.py
View File

@ -3,5 +3,5 @@ from moveit_configs_utils.launches import generate_move_group_launch
def generate_launch_description():
moveit_config = MoveItConfigsBuilder("openarm_grip", package_name="openarm_moveit_config").to_moveit_configs()
moveit_config = MoveItConfigsBuilder("openarm", package_name="openarm_moveit_config").to_moveit_configs()
return generate_move_group_launch(moveit_config)

2
openarm_moveit_config/launch/moveit_rviz.launch.py
View File

@ -3,5 +3,5 @@ from moveit_configs_utils.launches import generate_moveit_rviz_launch
def generate_launch_description():
moveit_config = MoveItConfigsBuilder("openarm_grip", package_name="openarm_moveit_config").to_moveit_configs()
moveit_config = MoveItConfigsBuilder("openarm", package_name="openarm_moveit_config").to_moveit_configs()
return generate_moveit_rviz_launch(moveit_config)

2
openarm_moveit_config/launch/rsp.launch.py
View File

@ -3,5 +3,5 @@ from moveit_configs_utils.launches import generate_rsp_launch
def generate_launch_description():
moveit_config = MoveItConfigsBuilder("openarm_grip", package_name="openarm_moveit_config").to_moveit_configs()
moveit_config = MoveItConfigsBuilder("openarm", package_name="openarm_moveit_config").to_moveit_configs()
return generate_rsp_launch(moveit_config)

2
openarm_moveit_config/launch/setup_assistant.launch.py
View File

@ -3,5 +3,5 @@ from moveit_configs_utils.launches import generate_setup_assistant_launch
def generate_launch_description():
moveit_config = MoveItConfigsBuilder("openarm_grip", package_name="openarm_moveit_config").to_moveit_configs()
moveit_config = MoveItConfigsBuilder("openarm", package_name="openarm_moveit_config").to_moveit_configs()
return generate_setup_assistant_launch(moveit_config)

2
openarm_moveit_config/launch/spawn_controllers.launch.py
View File

@ -3,5 +3,5 @@ from moveit_configs_utils.launches import generate_spawn_controllers_launch
def generate_launch_description():
moveit_config = MoveItConfigsBuilder("openarm_grip", package_name="openarm_moveit_config").to_moveit_configs()
moveit_config = MoveItConfigsBuilder("openarm", package_name="openarm_moveit_config").to_moveit_configs()
return generate_spawn_controllers_launch(moveit_config)

2
openarm_moveit_config/launch/static_virtual_joint_tfs.launch.py
View File

@ -3,5 +3,5 @@ from moveit_configs_utils.launches import generate_static_virtual_joint_tfs_laun
def generate_launch_description():
moveit_config = MoveItConfigsBuilder("openarm_grip", package_name="openarm_moveit_config").to_moveit_configs()
moveit_config = MoveItConfigsBuilder("openarm", package_name="openarm_moveit_config").to_moveit_configs()
return generate_static_virtual_joint_tfs_launch(moveit_config)

2
openarm_moveit_config/launch/warehouse_db.launch.py
View File

@ -3,5 +3,5 @@ from moveit_configs_utils.launches import generate_warehouse_db_launch
def generate_launch_description():
moveit_config = MoveItConfigsBuilder("openarm_grip", package_name="openarm_moveit_config").to_moveit_configs()
moveit_config = MoveItConfigsBuilder("openarm", package_name="openarm_moveit_config").to_moveit_configs()
return generate_warehouse_db_launch(moveit_config)

4
openarm_moveit_config/package.xml
View File

@ -4,7 +4,7 @@
<name>openarm_moveit_config</name>
<version>0.3.0</version>
<description>
Configuration and launch files for using OpenArm with the MoveIt Motion Planning Framework
An automatically generated package with all the configuration and launch files for using the openarm with the MoveIt Motion Planning Framework
</description>
<maintainer email="t95zhou@uwaterloo.ca">Thomason Zhou</maintainer>
@ -47,6 +47,6 @@
<export>
<build_type>ament_cmake</build_type>
<build_type>ament_cmake</build_type>
</export>
</package>