This library extract DH parameter from URDF only sequence link robot.
- Extract DH-Parameter from URDF
- Generate Homogeneous transformation matrix based on DH-Parameter from URDF
from urdf2dh import from_urdf
dhparams = from_urdf("exmaple.urdf")
>>> [
{"name": "joint1", "a": 0.12, "alpha": 1.507, "d": 0.340, "theta_offset": 1.517},
{"name": "joint2", "a": 1.87, "alpha": 0.0, "d": 0.000, "theta_offset": 0.0},
...,
{"name": "jointN-1", "a": 0.00, "alpha": 0.750, "d": 0.980, "theta_offset": -1.507},
]"name"キーの値はURDFで定義されているjointのname属性になります。
"a"と"d"の単位は[mm]です。URDFでは長さの単位が[m]で表示されているので注意してください。
"alpha"、"theta_offset"の単位は[rad]としています。
ここでは例としてmycobotのURDFファイル mycobot_urdf.urdf からDHパラメータを計算します。
mycobotのDHパラメータは下表のようになります。
mycobotの各座標軸とDHパラメータ
https://www.elephantrobotics.com/docs/myCobot-en/2-preparation/2-mycobot_hardware/2-mycobot_hardware.html
| Joint | alpha[rad] | a[mm] | d[mm] | theta | offset[rad] |
|---|---|---|---|---|---|
| 1 | 0 | 0 | 131.56 | theta_1 | 0 |
| 2 | PI/2 | 0 | 0 | theta_2 | -PI/2 |
| 3 | 0 | -110.4 | 0 | theta_3 | 0 |
| 4 | 0 | -96 | 66.39 | theta_4 | -PI/2 |
| 5 | PI/2 | 0 | 73.18 | theta_5 | PI/2 |
| 6 | -PI/2 | 0 | 43.6 | theta_6 | 0 |
mycobotのURDFファイルは下記のようになります。
mycobot_urdf.urdf
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="firefighter" >
<xacro:property name="width" value=".2" />
<link name="joint1">
<visual>
<geometry>
<!--- 0.0 0 -0.04 1.5708 3.14159-->
<mesh filename="package://mycobot_ros/urdf/joint1.dae"/>
</geometry>
<origin xyz = "0.0 0 0 " rpy = " 0 0 -1.5708"/>
</visual>
</link>
<link name="joint2">
<visual>
<geometry>
<mesh filename="package://mycobot_ros/urdf/joint2.dae"/>
</geometry>
<origin xyz = "0.0 0 -0.06096 " rpy = " 0 0 -1.5708"/>
</visual>
</link>
<link name="joint3">
<visual>
<geometry>
<mesh filename="package://mycobot_ros/urdf/joint3.dae"/>
</geometry>
<origin xyz = "0.0 0 0.03256 " rpy = " 0 -1.5708 0"/>
</visual>
</link>
<link name="joint4">
<visual>
<geometry>
<!--- 0.0 0 -0.04 -->
<mesh filename="package://mycobot_ros/urdf/joint4.dae"/>
</geometry>
<origin xyz = "0.0 0 0.03056 " rpy = " 0 -1.5708 0"/>
</visual>
</link>
<link name="joint5">
<visual>
<geometry>
<!--- 0.0 0 -0.04 -->
<mesh filename="package://mycobot_ros/urdf/joint5.dae"/>
</geometry>
<origin xyz = "0.0 0 -0.03356 " rpy = " 0 -1.5708 1.5708"/>
</visual>
</link>
<link name="joint6">
<visual>
<geometry>
<!--- 0.0 0 -0.04 -->
<mesh filename="package://mycobot_ros/urdf/joint6.dae"/>
</geometry>
<origin xyz = "0 0.00 -0.038 " rpy = " 0 0 0"/>
</visual>
</link>
<link name="joint6_flange">
<visual>
<geometry>
<!--- 0.0 0 -0.04 -->
<mesh filename="package://mycobot_ros/urdf/joint7.dae"/>
</geometry>
<origin xyz = "0.0 0 -0.012 " rpy = " 0 0 0"/>
</visual>
</link>
<joint name="joint2_to_joint1" type="revolute">
<axis xyz="0 0 1"/>
<limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
<parent link="joint1"/>
<child link="joint2"/>
<origin xyz= "0 0 0.13156" rpy = "0 0 0"/>
</joint>
<joint name="joint3_to_joint2" type="revolute">
<axis xyz="0 0 1"/>
<limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
<parent link="joint2"/>
<child link="joint3"/>
<origin xyz= "0 0 0" rpy = "0 1.5708 -1.5708"/>
</joint>
<joint name="joint4_to_joint3" type="revolute">
<axis xyz=" 0 0 1"/>
<limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
<parent link="joint3"/>
<child link="joint4"/>
<origin xyz= " -0.1104 0 0 " rpy = "0 0 0"/>
</joint>
<joint name="joint5_to_joint4" type="revolute">
<axis xyz=" 0 0 1"/>
<limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
<parent link="joint4"/>
<child link="joint5"/>
<origin xyz= "-0.096 0 0.06462" rpy = "0 0 -1.5708"/>
</joint>
<joint name="joint6_to_joint5" type="revolute">
<axis xyz="0 0 1"/>
<limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
<parent link="joint5"/>
<child link="joint6"/>
<origin xyz= "0 -0.07318 0" rpy = "1.5708 -1.5708 0"/>
</joint>
<joint name="joint6output_to_joint6" type="revolute">
<axis xyz="0 0 1"/>
<limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
<parent link="joint6"/>
<child link="joint6_flange"/>
<origin xyz= "0 0.0456 0" rpy = "-1.5708 0 0"/>
</joint>
</robot>実行するPythonスクリプトをexample.pyとし、次のようなフォルダ構成を想定しています。
./urdf2dh_example
─ example.py
─ mycobot_urdf.urdfexample.pyの実行結果は次のようになります。
from urdf2dh import from_urdf
print(from_urdf("mycobot_urdf.urdf"))
>>> [
{"name": "joint2_to_joint1", "a":0, "alpha": 0, "d": 131.56, "theta_offset": 0},
{"name": "joint3_to_joint2", "a":0, "alpha": 1.5707, "d": 0, "theta_offset": -1.5707},
{"name": "joint4_to_joint3", "a":-110.4,"alpha": 0, "d": 0, "theta_offset": 0},
{"name": "joint5_to_joint4", "a":-96, "alpha": 0, "d": 66.39, "theta_offset": -1.5707},
{"name": "joint6_to_joint5", "a":0, "alpha": 1.5707, "d": 73.18, "theta_offset": 1.5707},
{"name": "joint6output_to_joint6", "a":0, "alpha": -1.5707,"d": 43.6, "theta_offset": 0},
]
React
Typescript
Django
Laravel
Facebook
Microsoft
Google
Alibaba
D3
Tencent