Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge pull request #625 from YunfeiBai/master

Browse Source 
SDF parser
This commit is contained in:
erwincoumans
2016-05-11 19:22:43 -07:00
parent 119d47166d 7929bee128
commit 257c1425d7
32 changed files with 2713 additions and 228 deletions
Download Patch File Download Diff File Expand all files Collapse all files

414
data/kuka_iiwa/kuka_world.sdf Normal file
View File

@@ -0,0 +1,414 @@
<sdf version='1.6'>
<world name='default'>
<model name='lbr_iiwa'>
<joint name='fix_to_world' type='fixed'>
<parent>world</parent>
<child>lbr_iiwa_link_0</child>
</joint>
<link name='lbr_iiwa_link_0'>
<pose frame=''>0 0 0 0 -0 0</pose>
<inertial>
<pose frame=''>-0.1 0 0.07 0 -0 0</pose>
<mass>0.01</mass>
<inertia>
<ixx>0.05</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.06</iyy>
<iyz>0</iyz>
<izz>0.03</izz>
</inertia>
</inertial>
<collision name='lbr_iiwa_link_0_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/coarse/link_0.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='lbr_iiwa_link_0_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/link_0.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<link name='lbr_iiwa_link_1'>
<pose frame=''>0 0 0.1575 0 -0 0</pose>
<inertial>
<pose frame=''>0 -0.03 0.12 0 -0 0</pose>
<mass>4</mass>
<inertia>
<ixx>0.1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.09</iyy>
<iyz>0</iyz>
<izz>0.02</izz>
</inertia>
</inertial>
<collision name='lbr_iiwa_link_1_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/coarse/link_1.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='lbr_iiwa_link_1_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/link_1.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='lbr_iiwa_joint_1' type='revolute'>
<child>lbr_iiwa_link_1</child>
<parent>lbr_iiwa_link_0</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-2.96706</lower>
<upper>2.96706</upper>
<effort>300</effort>
<velocity>10</velocity>
</limit>
<dynamics>
<damping>0.5</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>0</use_parent_model_frame>
</axis>
</joint>
<link name='lbr_iiwa_link_2'>
<pose frame=''>0 0 0.36 1.5708 -0 -3.14159</pose>
<inertial>
<pose frame=''>0.0003 0.059 0.042 0 -0 0</pose>
<mass>4</mass>
<inertia>
<ixx>0.05</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.018</iyy>
<iyz>0</iyz>
<izz>0.044</izz>
</inertia>
</inertial>
<collision name='lbr_iiwa_link_2_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/coarse/link_2.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='lbr_iiwa_link_2_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/link_2.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='lbr_iiwa_joint_2' type='revolute'>
<child>lbr_iiwa_link_2</child>
<parent>lbr_iiwa_link_1</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-2.0944</lower>
<upper>2.0944</upper>
<effort>300</effort>
<velocity>10</velocity>
</limit>
<dynamics>
<damping>0.5</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>0</use_parent_model_frame>
</axis>
</joint>
<link name='lbr_iiwa_link_3'>
<pose frame=''>0 -0 0.5645 0 0 0</pose>
<inertial>
<pose frame=''>0 0.03 0.13 0 -0 0</pose>
<mass>3</mass>
<inertia>
<ixx>0.08</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.075</iyy>
<iyz>0</iyz>
<izz>0.01</izz>
</inertia>
</inertial>
<collision name='lbr_iiwa_link_3_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/coarse/link_3.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='lbr_iiwa_link_3_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/link_3.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='lbr_iiwa_joint_3' type='revolute'>
<child>lbr_iiwa_link_3</child>
<parent>lbr_iiwa_link_2</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-2.96706</lower>
<upper>2.96706</upper>
<effort>300</effort>
<velocity>10</velocity>
</limit>
<dynamics>
<damping>0.5</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>0</use_parent_model_frame>
</axis>
</joint>
<link name='lbr_iiwa_link_4'>
<pose frame=''>0 -0 0.78 1.5708 0 0</pose>
<inertial>
<pose frame=''>0 0.067 0.034 0 -0 0</pose>
<mass>2.7</mass>
<inertia>
<ixx>0.03</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.01</iyy>
<iyz>0</iyz>
<izz>0.029</izz>
</inertia>
</inertial>
<collision name='lbr_iiwa_link_4_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/coarse/link_4.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='lbr_iiwa_link_4_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/link_4.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='lbr_iiwa_joint_4' type='revolute'>
<child>lbr_iiwa_link_4</child>
<parent>lbr_iiwa_link_3</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-2.0944</lower>
<upper>2.0944</upper>
<effort>300</effort>
<velocity>10</velocity>
</limit>
<dynamics>
<damping>0.5</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>0</use_parent_model_frame>
</axis>
</joint>
<link name='lbr_iiwa_link_5'>
<pose frame=''>0 -0 0.9645 0 -0 -3.14159</pose>
<inertial>
<pose frame=''>0.0001 0.021 0.076 0 -0 0</pose>
<mass>1.7</mass>
<inertia>
<ixx>0.02</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.018</iyy>
<iyz>0</iyz>
<izz>0.005</izz>
</inertia>
</inertial>
<collision name='lbr_iiwa_link_5_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/coarse/link_5.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='lbr_iiwa_link_5_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/link_5.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='lbr_iiwa_joint_5' type='revolute'>
<child>lbr_iiwa_link_5</child>
<parent>lbr_iiwa_link_4</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-2.96706</lower>
<upper>2.96706</upper>
<effort>300</effort>
<velocity>10</velocity>
</limit>
<dynamics>
<damping>0.5</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>0</use_parent_model_frame>
</axis>
</joint>
<link name='lbr_iiwa_link_6'>
<pose frame=''>0 0 1.18 1.5708 -0 -3.14159</pose>
<inertial>
<pose frame=''>0 0.0006 0.0004 0 -0 0</pose>
<mass>1.8</mass>
<inertia>
<ixx>0.005</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0036</iyy>
<iyz>0</iyz>
<izz>0.0047</izz>
</inertia>
</inertial>
<collision name='lbr_iiwa_link_6_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/coarse/link_6.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='lbr_iiwa_link_6_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/link_6.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='lbr_iiwa_joint_6' type='revolute'>
<child>lbr_iiwa_link_6</child>
<parent>lbr_iiwa_link_5</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-2.0944</lower>
<upper>2.0944</upper>
<effort>300</effort>
<velocity>10</velocity>
</limit>
<dynamics>
<damping>0.5</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>0</use_parent_model_frame>
</axis>
</joint>
<link name='lbr_iiwa_link_7'>
<pose frame=''>0 0 1.261 0 0 0</pose>
<inertial>
<pose frame=''>0 0 0.02 0 -0 0</pose>
<mass>0.3</mass>
<inertia>
<ixx>0.001</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.001</iyy>
<iyz>0</iyz>
<izz>0.001</izz>
</inertia>
</inertial>
<collision name='lbr_iiwa_link_7_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/coarse/link_7.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='lbr_iiwa_link_7_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/link_7.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='lbr_iiwa_joint_7' type='revolute'>
<child>lbr_iiwa_link_7</child>
<parent>lbr_iiwa_link_6</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-3.05433</lower>
<upper>3.05433</upper>
<effort>300</effort>
<velocity>10</velocity>
</limit>
<dynamics>
<damping>0.5</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>0</use_parent_model_frame>
</axis>
</joint>
</model>
</world>
</sdf>

BIN
data/kuka_iiwa/meshes/coarse/link_0.stl Normal file
View File

Binary file not shown.

BIN
data/kuka_iiwa/meshes/coarse/link_1.stl Normal file
View File

Binary file not shown.

BIN
data/kuka_iiwa/meshes/coarse/link_2.stl Normal file
View File

Binary file not shown.

BIN
data/kuka_iiwa/meshes/coarse/link_3.stl Normal file
View File

Binary file not shown.

BIN
data/kuka_iiwa/meshes/coarse/link_4.stl Normal file
View File

Binary file not shown.

BIN
data/kuka_iiwa/meshes/coarse/link_5.stl Normal file
View File

Binary file not shown.

BIN
data/kuka_iiwa/meshes/coarse/link_6.stl Normal file
View File

Binary file not shown.

BIN
data/kuka_iiwa/meshes/coarse/link_7.stl Normal file
View File

Binary file not shown.

BIN
data/kuka_iiwa/meshes/link_0.stl Normal file
View File

Binary file not shown.

BIN
data/kuka_iiwa/meshes/link_1.stl Normal file
View File

Binary file not shown.

BIN
data/kuka_iiwa/meshes/link_2.stl Normal file
View File

Binary file not shown.

BIN
data/kuka_iiwa/meshes/link_3.stl Normal file
View File

Binary file not shown.

BIN
data/kuka_iiwa/meshes/link_4.stl Normal file
View File

Binary file not shown.

BIN
data/kuka_iiwa/meshes/link_5.stl Normal file
View File

Binary file not shown.

BIN
data/kuka_iiwa/meshes/link_6.stl Normal file
View File

Binary file not shown.

BIN
data/kuka_iiwa/meshes/link_7.stl Normal file
View File

Binary file not shown.

410
data/kuka_iiwa/model.sdf Normal file
View File

@@ -0,0 +1,410 @@
<sdf version='1.6'>
<world name='default'>
<model name='lbr_iiwa'>
<link name='lbr_iiwa_link_0'>
<pose frame=''>0 0 0 0 -0 0</pose>
<inertial>
<pose frame=''>-0.1 0 0.07 0 -0 0</pose>
<mass>0</mass>
<inertia>
<ixx>0.05</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.06</iyy>
<iyz>0</iyz>
<izz>0.03</izz>
</inertia>
</inertial>
<collision name='lbr_iiwa_link_0_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/coarse/link_0.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='lbr_iiwa_link_0_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/link_0.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<link name='lbr_iiwa_link_1'>
<pose frame=''>0 0 0.1575 0 -0 0</pose>
<inertial>
<pose frame=''>0 -0.03 0.12 0 -0 0</pose>
<mass>4</mass>
<inertia>
<ixx>0.1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.09</iyy>
<iyz>0</iyz>
<izz>0.02</izz>
</inertia>
</inertial>
<collision name='lbr_iiwa_link_1_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/coarse/link_1.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='lbr_iiwa_link_1_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/link_1.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='lbr_iiwa_joint_1' type='revolute'>
<child>lbr_iiwa_link_1</child>
<parent>lbr_iiwa_link_0</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-2.96706</lower>
<upper>2.96706</upper>
<effort>300</effort>
<velocity>10</velocity>
</limit>
<dynamics>
<damping>0.5</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>0</use_parent_model_frame>
</axis>
</joint>
<link name='lbr_iiwa_link_2'>
<pose frame=''>0 0 0.36 1.5708 -0 -3.14159</pose>
<inertial>
<pose frame=''>0.0003 0.059 0.042 0 -0 0</pose>
<mass>4</mass>
<inertia>
<ixx>0.05</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.018</iyy>
<iyz>0</iyz>
<izz>0.044</izz>
</inertia>
</inertial>
<collision name='lbr_iiwa_link_2_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/coarse/link_2.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='lbr_iiwa_link_2_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/link_2.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='lbr_iiwa_joint_2' type='revolute'>
<child>lbr_iiwa_link_2</child>
<parent>lbr_iiwa_link_1</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-2.0944</lower>
<upper>2.0944</upper>
<effort>300</effort>
<velocity>10</velocity>
</limit>
<dynamics>
<damping>0.5</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>0</use_parent_model_frame>
</axis>
</joint>
<link name='lbr_iiwa_link_3'>
<pose frame=''>0 -0 0.5645 0 0 0</pose>
<inertial>
<pose frame=''>0 0.03 0.13 0 -0 0</pose>
<mass>3</mass>
<inertia>
<ixx>0.08</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.075</iyy>
<iyz>0</iyz>
<izz>0.01</izz>
</inertia>
</inertial>
<collision name='lbr_iiwa_link_3_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/coarse/link_3.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='lbr_iiwa_link_3_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/link_3.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='lbr_iiwa_joint_3' type='revolute'>
<child>lbr_iiwa_link_3</child>
<parent>lbr_iiwa_link_2</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-2.96706</lower>
<upper>2.96706</upper>
<effort>300</effort>
<velocity>10</velocity>
</limit>
<dynamics>
<damping>0.5</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>0</use_parent_model_frame>
</axis>
</joint>
<link name='lbr_iiwa_link_4'>
<pose frame=''>0 -0 0.78 1.5708 0 0</pose>
<inertial>
<pose frame=''>0 0.067 0.034 0 -0 0</pose>
<mass>2.7</mass>
<inertia>
<ixx>0.03</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.01</iyy>
<iyz>0</iyz>
<izz>0.029</izz>
</inertia>
</inertial>
<collision name='lbr_iiwa_link_4_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/coarse/link_4.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='lbr_iiwa_link_4_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/link_4.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='lbr_iiwa_joint_4' type='revolute'>
<child>lbr_iiwa_link_4</child>
<parent>lbr_iiwa_link_3</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-2.0944</lower>
<upper>2.0944</upper>
<effort>300</effort>
<velocity>10</velocity>
</limit>
<dynamics>
<damping>0.5</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>0</use_parent_model_frame>
</axis>
</joint>
<link name='lbr_iiwa_link_5'>
<pose frame=''>0 -0 0.9645 0 -0 -3.14159</pose>
<inertial>
<pose frame=''>0.0001 0.021 0.076 0 -0 0</pose>
<mass>1.7</mass>
<inertia>
<ixx>0.02</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.018</iyy>
<iyz>0</iyz>
<izz>0.005</izz>
</inertia>
</inertial>
<collision name='lbr_iiwa_link_5_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/coarse/link_5.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='lbr_iiwa_link_5_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/link_5.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='lbr_iiwa_joint_5' type='revolute'>
<child>lbr_iiwa_link_5</child>
<parent>lbr_iiwa_link_4</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-2.96706</lower>
<upper>2.96706</upper>
<effort>300</effort>
<velocity>10</velocity>
</limit>
<dynamics>
<damping>0.5</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>0</use_parent_model_frame>
</axis>
</joint>
<link name='lbr_iiwa_link_6'>
<pose frame=''>0 0 1.18 1.5708 -0 -3.14159</pose>
<inertial>
<pose frame=''>0 0.0006 0.0004 0 -0 0</pose>
<mass>1.8</mass>
<inertia>
<ixx>0.005</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0036</iyy>
<iyz>0</iyz>
<izz>0.0047</izz>
</inertia>
</inertial>
<collision name='lbr_iiwa_link_6_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/coarse/link_6.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='lbr_iiwa_link_6_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/link_6.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='lbr_iiwa_joint_6' type='revolute'>
<child>lbr_iiwa_link_6</child>
<parent>lbr_iiwa_link_5</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-2.0944</lower>
<upper>2.0944</upper>
<effort>300</effort>
<velocity>10</velocity>
</limit>
<dynamics>
<damping>0.5</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>0</use_parent_model_frame>
</axis>
</joint>
<link name='lbr_iiwa_link_7'>
<pose frame=''>0 0 1.261 0 0 0</pose>
<inertial>
<pose frame=''>0 0 0.02 0 -0 0</pose>
<mass>0.3</mass>
<inertia>
<ixx>0.001</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.001</iyy>
<iyz>0</iyz>
<izz>0.001</izz>
</inertia>
</inertial>
<collision name='lbr_iiwa_link_7_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/coarse/link_7.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='lbr_iiwa_link_7_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/link_7.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='lbr_iiwa_joint_7' type='revolute'>
<child>lbr_iiwa_link_7</child>
<parent>lbr_iiwa_link_6</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-3.05433</lower>
<upper>3.05433</upper>
<effort>300</effort>
<velocity>10</velocity>
</limit>
<dynamics>
<damping>0.5</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>0</use_parent_model_frame>
</axis>
</joint>
</model>
</world>
</sdf>

284
data/kuka_iiwa/model.urdf Normal file
View File

@@ -0,0 +1,284 @@
<?xml version="1.0" ?>
<!-- ======================================================================= -->
<!-- | This document was autogenerated by xacro from lbr_iiwa.urdf.xacro | -->
<!-- | Original xacro: https://github.com/rtkg/lbr_iiwa/archive/master.zip | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- | Changes (kohlhoff): | -->
<!-- | * Removed gazebo tags. | -->
<!-- | * Removed unused materials. | -->
<!-- | * Made mesh paths relative. | -->
<!-- | * Removed material fields from collision segments. | -->
<!-- | * Removed the self_collision_checking segment. | -->
<!-- | * Removed transmission segments, since they didn't match the | -->
<!-- | convention, will have to added back later. | -->
<!-- ======================================================================= -->
<!--LICENSE: -->
<!--Copyright (c) 2015, Robert Krug & Todor Stoyanov, AASS Research Center, -->
<!--Orebro University, Sweden -->
<!--All rights reserved. -->
<!-- -->
<!--Redistribution and use in source and binary forms, with or without -->
<!--modification, are permitted provided that the following conditions are -->
<!--met: -->
<!-- -->
<!--1. Redistributions of source code must retain the above copyright notice,-->
<!-- this list of conditions and the following disclaimer. -->
<!-- -->
<!--2. 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. -->
<!-- -->
<!--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. -->
<robot name="lbr_iiwa" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- Import Rviz colors -->
<material name="Grey">
<color rgba="0.2 0.2 0.2 1.0"/>
</material>
<material name="Orange">
<color rgba="1.0 0.423529411765 0.0392156862745 1.0"/>
</material>
<!--Import the lbr iiwa macro -->
<!--Import Transmissions -->
<!--Include Utilities -->
<!--The following macros are adapted from the LWR 4 definitions of the RCPRG - https://github.com/RCPRG-ros-pkg/lwr_robot -->
<!--Little helper macros to define the inertia matrix needed for links.-->
<!--Cuboid-->
<!--Cylinder: length is along the y-axis! -->
<!--lbr-->
<link name="lbr_iiwa_link_0">
<inertial>
<origin rpy="0 0 0" xyz="-0.1 0 0.07"/>
<!--Increase mass from 5 Kg original to provide a stable base to carry the
arm.-->
<mass value="0.0"/>
<inertia ixx="0.05" ixy="0" ixz="0" iyy="0.06" iyz="0" izz="0.03"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/link_0.stl"/>
</geometry>
<material name="Grey"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/coarse/link_0.stl"/>
</geometry>
</collision>
</link>
<!-- joint between link_0 and link_1 -->
<joint name="lbr_iiwa_joint_1" type="revolute">
<parent link="lbr_iiwa_link_0"/>
<child link="lbr_iiwa_link_1"/>
<origin rpy="0 0 0" xyz="0 0 0.1575"/>
<axis xyz="0 0 1"/>
<limit effort="300" lower="-2.96705972839" upper="2.96705972839" velocity="10"/>
<dynamics damping="0.5"/>
</joint>
<link name="lbr_iiwa_link_1">
<inertial>
<origin rpy="0 0 0" xyz="0 -0.03 0.12"/>
<mass value="4"/>
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.09" iyz="0" izz="0.02"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/link_1.stl"/>
</geometry>
<material name="Orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/coarse/link_1.stl"/>
</geometry>
</collision>
</link>
<!-- joint between link_1 and link_2 -->
<joint name="lbr_iiwa_joint_2" type="revolute">
<parent link="lbr_iiwa_link_1"/>
<child link="lbr_iiwa_link_2"/>
<origin rpy="1.57079632679 0 3.14159265359" xyz="0 0 0.2025"/>
<axis xyz="0 0 1"/>
<limit effort="300" lower="-2.09439510239" upper="2.09439510239" velocity="10"/>
<dynamics damping="0.5"/>
</joint>
<link name="lbr_iiwa_link_2">
<inertial>
<origin rpy="0 0 0" xyz="0.0003 0.059 0.042"/>
<mass value="4"/>
<inertia ixx="0.05" ixy="0" ixz="0" iyy="0.018" iyz="0" izz="0.044"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/link_2.stl"/>
</geometry>
<material name="Orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/coarse/link_2.stl"/>
</geometry>
</collision>
</link>
<!-- joint between link_2 and link_3 -->
<joint name="lbr_iiwa_joint_3" type="revolute">
<parent link="lbr_iiwa_link_2"/>
<child link="lbr_iiwa_link_3"/>
<origin rpy="1.57079632679 0 3.14159265359" xyz="0 0.2045 0"/>
<axis xyz="0 0 1"/>
<limit effort="300" lower="-2.96705972839" upper="2.96705972839" velocity="10"/>
<dynamics damping="0.5"/>
</joint>
<link name="lbr_iiwa_link_3">
<inertial>
<origin rpy="0 0 0" xyz="0 0.03 0.13"/>
<mass value="3"/>
<inertia ixx="0.08" ixy="0" ixz="0" iyy="0.075" iyz="0" izz="0.01"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/link_3.stl"/>
</geometry>
<material name="Orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/coarse/link_3.stl"/>
</geometry>
</collision>
</link>
<!-- joint between link_3 and link_4 -->
<joint name="lbr_iiwa_joint_4" type="revolute">
<parent link="lbr_iiwa_link_3"/>
<child link="lbr_iiwa_link_4"/>
<origin rpy="1.57079632679 0 0" xyz="0 0 0.2155"/>
<axis xyz="0 0 1"/>
<limit effort="300" lower="-2.09439510239" upper="2.09439510239" velocity="10"/>
<dynamics damping="0.5"/>
</joint>
<link name="lbr_iiwa_link_4">
<inertial>
<origin rpy="0 0 0" xyz="0 0.067 0.034"/>
<mass value="2.7"/>
<inertia ixx="0.03" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.029"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/link_4.stl"/>
</geometry>
<material name="Orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/coarse/link_4.stl"/>
</geometry>
</collision>
</link>
<!-- joint between link_4 and link_5 -->
<joint name="lbr_iiwa_joint_5" type="revolute">
<parent link="lbr_iiwa_link_4"/>
<child link="lbr_iiwa_link_5"/>
<origin rpy="-1.57079632679 3.14159265359 0" xyz="0 0.1845 0"/>
<axis xyz="0 0 1"/>
<limit effort="300" lower="-2.96705972839" upper="2.96705972839" velocity="10"/>
<dynamics damping="0.5"/>
</joint>
<link name="lbr_iiwa_link_5">
<inertial>
<origin rpy="0 0 0" xyz="0.0001 0.021 0.076"/>
<mass value="1.7"/>
<inertia ixx="0.02" ixy="0" ixz="0" iyy="0.018" iyz="0" izz="0.005"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/link_5.stl"/>
</geometry>
<material name="Orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/coarse/link_5.stl"/>
</geometry>
</collision>
</link>
<!-- joint between link_5 and link_6 -->
<joint name="lbr_iiwa_joint_6" type="revolute">
<parent link="lbr_iiwa_link_5"/>
<child link="lbr_iiwa_link_6"/>
<origin rpy="1.57079632679 0 0" xyz="0 0 0.2155"/>
<axis xyz="0 0 1"/>
<limit effort="300" lower="-2.09439510239" upper="2.09439510239" velocity="10"/>
<dynamics damping="0.5"/>
</joint>
<link name="lbr_iiwa_link_6">
<inertial>
<origin rpy="0 0 0" xyz="0 0.0006 0.0004"/>
<mass value="1.8"/>
<inertia ixx="0.005" ixy="0" ixz="0" iyy="0.0036" iyz="0" izz="0.0047"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/link_6.stl"/>
</geometry>
<material name="Orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/coarse/link_6.stl"/>
</geometry>
</collision>
</link>
<!-- joint between link_6 and link_7 -->
<joint name="lbr_iiwa_joint_7" type="revolute">
<parent link="lbr_iiwa_link_6"/>
<child link="lbr_iiwa_link_7"/>
<origin rpy="-1.57079632679 3.14159265359 0" xyz="0 0.081 0"/>
<axis xyz="0 0 1"/>
<limit effort="300" lower="-3.05432619099" upper="3.05432619099" velocity="10"/>
<dynamics damping="0.5"/>
</joint>
<link name="lbr_iiwa_link_7">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0.02"/>
<mass value="0.3"/>
<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/link_7.stl"/>
</geometry>
<material name="Grey"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/coarse/link_7.stl"/>
</geometry>
</collision>
</link>
</robot>

818
data/kuka_iiwa/model2.sdf Normal file
View File

@@ -0,0 +1,818 @@
<sdf version='1.6'>
<world name='default'>
<model name='lbr_iiwa'>
<link name='lbr_iiwa_link_0'>
<pose frame=''>0 0 0 0 -0 0</pose>
<inertial>
<pose frame=''>-0.1 0 0.07 0 -0 0</pose>
<mass>0</mass>
<inertia>
<ixx>0.05</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.06</iyy>
<iyz>0</iyz>
<izz>0.03</izz>
</inertia>
</inertial>
<collision name='lbr_iiwa_link_0_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/coarse/link_0.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='lbr_iiwa_link_0_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/link_0.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<link name='lbr_iiwa_link_1'>
<pose frame=''>0 0 0.1575 0 -0 0</pose>
<inertial>
<pose frame=''>0 -0.03 0.12 0 -0 0</pose>
<mass>4</mass>
<inertia>
<ixx>0.1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.09</iyy>
<iyz>0</iyz>
<izz>0.02</izz>
</inertia>
</inertial>
<collision name='lbr_iiwa_link_1_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/coarse/link_1.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='lbr_iiwa_link_1_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/link_1.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='lbr_iiwa_joint_1' type='revolute'>
<child>lbr_iiwa_link_1</child>
<parent>lbr_iiwa_link_0</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-2.96706</lower>
<upper>2.96706</upper>
<effort>300</effort>
<velocity>10</velocity>
</limit>
<dynamics>
<damping>0.5</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>0</use_parent_model_frame>
</axis>
</joint>
<link name='lbr_iiwa_link_2'>
<pose frame=''>0 0 0.36 1.5708 -0 -3.14159</pose>
<inertial>
<pose frame=''>0.0003 0.059 0.042 0 -0 0</pose>
<mass>4</mass>
<inertia>
<ixx>0.05</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.018</iyy>
<iyz>0</iyz>
<izz>0.044</izz>
</inertia>
</inertial>
<collision name='lbr_iiwa_link_2_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/coarse/link_2.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='lbr_iiwa_link_2_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/link_2.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='lbr_iiwa_joint_2' type='revolute'>
<child>lbr_iiwa_link_2</child>
<parent>lbr_iiwa_link_1</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-2.0944</lower>
<upper>2.0944</upper>
<effort>300</effort>
<velocity>10</velocity>
</limit>
<dynamics>
<damping>0.5</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>0</use_parent_model_frame>
</axis>
</joint>
<link name='lbr_iiwa_link_3'>
<pose frame=''>0 -0 0.5645 0 0 0</pose>
<inertial>
<pose frame=''>0 0.03 0.13 0 -0 0</pose>
<mass>3</mass>
<inertia>
<ixx>0.08</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.075</iyy>
<iyz>0</iyz>
<izz>0.01</izz>
</inertia>
</inertial>
<collision name='lbr_iiwa_link_3_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/coarse/link_3.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='lbr_iiwa_link_3_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/link_3.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='lbr_iiwa_joint_3' type='revolute'>
<child>lbr_iiwa_link_3</child>
<parent>lbr_iiwa_link_2</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-2.96706</lower>
<upper>2.96706</upper>
<effort>300</effort>
<velocity>10</velocity>
</limit>
<dynamics>
<damping>0.5</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>0</use_parent_model_frame>
</axis>
</joint>
<link name='lbr_iiwa_link_4'>
<pose frame=''>0 -0 0.78 1.5708 0 0</pose>
<inertial>
<pose frame=''>0 0.067 0.034 0 -0 0</pose>
<mass>2.7</mass>
<inertia>
<ixx>0.03</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.01</iyy>
<iyz>0</iyz>
<izz>0.029</izz>
</inertia>
</inertial>
<collision name='lbr_iiwa_link_4_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/coarse/link_4.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='lbr_iiwa_link_4_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/link_4.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='lbr_iiwa_joint_4' type='revolute'>
<child>lbr_iiwa_link_4</child>
<parent>lbr_iiwa_link_3</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-2.0944</lower>
<upper>2.0944</upper>
<effort>300</effort>
<velocity>10</velocity>
</limit>
<dynamics>
<damping>0.5</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>0</use_parent_model_frame>
</axis>
</joint>
<link name='lbr_iiwa_link_5'>
<pose frame=''>0 -0 0.9645 0 -0 -3.14159</pose>
<inertial>
<pose frame=''>0.0001 0.021 0.076 0 -0 0</pose>
<mass>1.7</mass>
<inertia>
<ixx>0.02</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.018</iyy>
<iyz>0</iyz>
<izz>0.005</izz>
</inertia>
</inertial>
<collision name='lbr_iiwa_link_5_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/coarse/link_5.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='lbr_iiwa_link_5_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/link_5.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='lbr_iiwa_joint_5' type='revolute'>
<child>lbr_iiwa_link_5</child>
<parent>lbr_iiwa_link_4</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-2.96706</lower>
<upper>2.96706</upper>
<effort>300</effort>
<velocity>10</velocity>
</limit>
<dynamics>
<damping>0.5</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>0</use_parent_model_frame>
</axis>
</joint>
<link name='lbr_iiwa_link_6'>
<pose frame=''>0 0 1.18 1.5708 -0 -3.14159</pose>
<inertial>
<pose frame=''>0 0.0006 0.0004 0 -0 0</pose>
<mass>1.8</mass>
<inertia>
<ixx>0.005</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0036</iyy>
<iyz>0</iyz>
<izz>0.0047</izz>
</inertia>
</inertial>
<collision name='lbr_iiwa_link_6_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/coarse/link_6.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='lbr_iiwa_link_6_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/link_6.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='lbr_iiwa_joint_6' type='revolute'>
<child>lbr_iiwa_link_6</child>
<parent>lbr_iiwa_link_5</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-2.0944</lower>
<upper>2.0944</upper>
<effort>300</effort>
<velocity>10</velocity>
</limit>
<dynamics>
<damping>0.5</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>0</use_parent_model_frame>
</axis>
</joint>
<link name='lbr_iiwa_link_7'>
<pose frame=''>0 0 1.261 0 0 0</pose>
<inertial>
<pose frame=''>0 0 0.02 0 -0 0</pose>
<mass>0.3</mass>
<inertia>
<ixx>0.001</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.001</iyy>
<iyz>0</iyz>
<izz>0.001</izz>
</inertia>
</inertial>
<collision name='lbr_iiwa_link_7_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/coarse/link_7.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='lbr_iiwa_link_7_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/link_7.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='lbr_iiwa_joint_7' type='revolute'>
<child>lbr_iiwa_link_7</child>
<parent>lbr_iiwa_link_6</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-3.05433</lower>
<upper>3.05433</upper>
<effort>300</effort>
<velocity>10</velocity>
</limit>
<dynamics>
<damping>0.5</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>0</use_parent_model_frame>
</axis>
</joint>
</model>
<model name='lbr_iiwa'>
<pose frame=''>2 2 0 0 -0 0</pose>
<link name='lbr_iiwa_link_0'>
<pose frame=''>0 0 0 0 -0 0</pose>
<inertial>
<pose frame=''>-0.1 0 0.07 0 -0 0</pose>
<mass>0</mass>
<inertia>
<ixx>0.05</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.06</iyy>
<iyz>0</iyz>
<izz>0.03</izz>
</inertia>
</inertial>
<collision name='lbr_iiwa_link_0_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/coarse/link_0.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='lbr_iiwa_link_0_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/link_0.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<link name='lbr_iiwa_link_1'>
<pose frame=''>0 0 0.1575 0 -0 0</pose>
<inertial>
<pose frame=''>0 -0.03 0.12 0 -0 0</pose>
<mass>4</mass>
<inertia>
<ixx>0.1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.09</iyy>
<iyz>0</iyz>
<izz>0.02</izz>
</inertia>
</inertial>
<collision name='lbr_iiwa_link_1_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/coarse/link_1.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='lbr_iiwa_link_1_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/link_1.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='lbr_iiwa_joint_1' type='revolute'>
<child>lbr_iiwa_link_1</child>
<parent>lbr_iiwa_link_0</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-2.96706</lower>
<upper>2.96706</upper>
<effort>300</effort>
<velocity>10</velocity>
</limit>
<dynamics>
<damping>0.5</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>0</use_parent_model_frame>
</axis>
</joint>
<link name='lbr_iiwa_link_2'>
<pose frame=''>0 0 0.36 1.5708 -0 -3.14159</pose>
<inertial>
<pose frame=''>0.0003 0.059 0.042 0 -0 0</pose>
<mass>4</mass>
<inertia>
<ixx>0.05</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.018</iyy>
<iyz>0</iyz>
<izz>0.044</izz>
</inertia>
</inertial>
<collision name='lbr_iiwa_link_2_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/coarse/link_2.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='lbr_iiwa_link_2_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/link_2.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='lbr_iiwa_joint_2' type='revolute'>
<child>lbr_iiwa_link_2</child>
<parent>lbr_iiwa_link_1</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-2.0944</lower>
<upper>2.0944</upper>
<effort>300</effort>
<velocity>10</velocity>
</limit>
<dynamics>
<damping>0.5</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>0</use_parent_model_frame>
</axis>
</joint>
<link name='lbr_iiwa_link_3'>
<pose frame=''>0 -0 0.5645 0 0 0</pose>
<inertial>
<pose frame=''>0 0.03 0.13 0 -0 0</pose>
<mass>3</mass>
<inertia>
<ixx>0.08</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.075</iyy>
<iyz>0</iyz>
<izz>0.01</izz>
</inertia>
</inertial>
<collision name='lbr_iiwa_link_3_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/coarse/link_3.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='lbr_iiwa_link_3_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/link_3.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='lbr_iiwa_joint_3' type='revolute'>
<child>lbr_iiwa_link_3</child>
<parent>lbr_iiwa_link_2</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-2.96706</lower>
<upper>2.96706</upper>
<effort>300</effort>
<velocity>10</velocity>
</limit>
<dynamics>
<damping>0.5</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>0</use_parent_model_frame>
</axis>
</joint>
<link name='lbr_iiwa_link_4'>
<pose frame=''>0 -0 0.78 1.5708 0 0</pose>
<inertial>
<pose frame=''>0 0.067 0.034 0 -0 0</pose>
<mass>2.7</mass>
<inertia>
<ixx>0.03</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.01</iyy>
<iyz>0</iyz>
<izz>0.029</izz>
</inertia>
</inertial>
<collision name='lbr_iiwa_link_4_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/coarse/link_4.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='lbr_iiwa_link_4_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/link_4.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='lbr_iiwa_joint_4' type='revolute'>
<child>lbr_iiwa_link_4</child>
<parent>lbr_iiwa_link_3</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-2.0944</lower>
<upper>2.0944</upper>
<effort>300</effort>
<velocity>10</velocity>
</limit>
<dynamics>
<damping>0.5</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>0</use_parent_model_frame>
</axis>
</joint>
<link name='lbr_iiwa_link_5'>
<pose frame=''>0 -0 0.9645 0 -0 -3.14159</pose>
<inertial>
<pose frame=''>0.0001 0.021 0.076 0 -0 0</pose>
<mass>1.7</mass>
<inertia>
<ixx>0.02</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.018</iyy>
<iyz>0</iyz>
<izz>0.005</izz>
</inertia>
</inertial>
<collision name='lbr_iiwa_link_5_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/coarse/link_5.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='lbr_iiwa_link_5_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/link_5.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='lbr_iiwa_joint_5' type='revolute'>
<child>lbr_iiwa_link_5</child>
<parent>lbr_iiwa_link_4</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-2.96706</lower>
<upper>2.96706</upper>
<effort>300</effort>
<velocity>10</velocity>
</limit>
<dynamics>
<damping>0.5</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>0</use_parent_model_frame>
</axis>
</joint>
<link name='lbr_iiwa_link_6'>
<pose frame=''>0 0 1.18 1.5708 -0 -3.14159</pose>
<inertial>
<pose frame=''>0 0.0006 0.0004 0 -0 0</pose>
<mass>1.8</mass>
<inertia>
<ixx>0.005</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0036</iyy>
<iyz>0</iyz>
<izz>0.0047</izz>
</inertia>
</inertial>
<collision name='lbr_iiwa_link_6_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/coarse/link_6.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='lbr_iiwa_link_6_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/link_6.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='lbr_iiwa_joint_6' type='revolute'>
<child>lbr_iiwa_link_6</child>
<parent>lbr_iiwa_link_5</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-2.0944</lower>
<upper>2.0944</upper>
<effort>300</effort>
<velocity>10</velocity>
</limit>
<dynamics>
<damping>0.5</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>0</use_parent_model_frame>
</axis>
</joint>
<link name='lbr_iiwa_link_7'>
<pose frame=''>0 0 1.261 0 0 0</pose>
<inertial>
<pose frame=''>0 0 0.02 0 -0 0</pose>
<mass>0.3</mass>
<inertia>
<ixx>0.001</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.001</iyy>
<iyz>0</iyz>
<izz>0.001</izz>
</inertia>
</inertial>
<collision name='lbr_iiwa_link_7_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/coarse/link_7.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='lbr_iiwa_link_7_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/link_7.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='lbr_iiwa_joint_7' type='revolute'>
<child>lbr_iiwa_link_7</child>
<parent>lbr_iiwa_link_6</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-3.05433</lower>
<upper>3.05433</upper>
<effort>300</effort>
<velocity>10</velocity>
</limit>
<dynamics>
<damping>0.5</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>0</use_parent_model_frame>
</axis>
</joint>
</model>
</world>
</sdf>

285
data/kuka_iiwa/model_for_sdf.urdf Normal file
View File

@@ -0,0 +1,285 @@
<?xml version="1.0" ?>
<!-- ======================================================================= -->
<!-- | This document was autogenerated by xacro from lbr_iiwa.urdf.xacro | -->
<!-- | Original xacro: https://github.com/rtkg/lbr_iiwa/archive/master.zip | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- | Changes (kohlhoff): | -->
<!-- | * Removed gazebo tags. | -->
<!-- | * Removed unused materials. | -->
<!-- | * Made mesh paths relative. | -->
<!-- | * Removed material fields from collision segments. | -->
<!-- | * Removed the self_collision_checking segment. | -->
<!-- | * Removed transmission segments, since they didn't match the | -->
<!-- | convention, will have to added back later. | -->
<!-- ======================================================================= -->
<!--LICENSE: -->
<!--Copyright (c) 2015, Robert Krug & Todor Stoyanov, AASS Research Center, -->
<!--Orebro University, Sweden -->
<!--All rights reserved. -->
<!-- -->
<!--Redistribution and use in source and binary forms, with or without -->
<!--modification, are permitted provided that the following conditions are -->
<!--met: -->
<!-- -->
<!--1. Redistributions of source code must retain the above copyright notice,-->
<!-- this list of conditions and the following disclaimer. -->
<!-- -->
<!--2. 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. -->
<!-- -->
<!--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. -->
<robot name="lbr_iiwa" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- Import Rviz colors -->
<material name="Grey">
<color rgba="0.2 0.2 0.2 1.0"/>
</material>
<material name="Orange">
<color rgba="1.0 0.423529411765 0.0392156862745 1.0"/>
</material>
<!--Import the lbr iiwa macro -->
<!--Import Transmissions -->
<!--Include Utilities -->
<!--The following macros are adapted from the LWR 4 definitions of the RCPRG - https://github.com/RCPRG-ros-pkg/lwr_robot -->
<!--Little helper macros to define the inertia matrix needed for links.-->
<!--Cuboid-->
<!--Cylinder: length is along the y-axis! -->
<!--lbr-->
<link name="lbr_iiwa_link_0">
<inertial>
<origin rpy="0 0 0" xyz="-0.1 0 0.07"/>
<!--Increase mass from 5 Kg original to provide a stable base to carry the
arm.-->
<mass value="0.01"/>
<inertia ixx="0.05" ixy="0" ixz="0" iyy="0.06" iyz="0" izz="0.03"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/link_0.stl"/>
</geometry>
<material name="Grey"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/coarse/link_0.stl"/>
</geometry>
</collision>
</link>
<!-- joint between link_0 and link_1 -->
<joint name="lbr_iiwa_joint_1" type="revolute">
<parent link="lbr_iiwa_link_0"/>
<child link="lbr_iiwa_link_1"/>
<origin rpy="0 0 0" xyz="0 0 0.1575"/>
<axis xyz="0 0 1"/>
<limit effort="300" lower="-2.96705972839" upper="2.96705972839" velocity="10"/>
<dynamics damping="0.5"/>
</joint>
<link name="lbr_iiwa_link_1">
<inertial>
<origin rpy="0 0 0" xyz="0 -0.03 0.12"/>
<mass value="4"/>
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.09" iyz="0" izz="0.02"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/link_1.stl"/>
</geometry>
<material name="Orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/coarse/link_1.stl"/>
</geometry>
</collision>
</link>
<!-- joint between link_1 and link_2 -->
<joint name="lbr_iiwa_joint_2" type="revolute">
<parent link="lbr_iiwa_link_1"/>
<child link="lbr_iiwa_link_2"/>
<origin rpy="1.57079632679 0 3.14159265359" xyz="0 0 0.2025"/>
<axis xyz="0 0 1"/>
<limit effort="300" lower="-2.09439510239" upper="2.09439510239" velocity="10"/>
<dynamics damping="0.5"/>
</joint>
<link name="lbr_iiwa_link_2">
<inertial>
<origin rpy="0 0 0" xyz="0.0003 0.059 0.042"/>
<mass value="4"/>
<inertia ixx="0.05" ixy="0" ixz="0" iyy="0.018" iyz="0" izz="0.044"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/link_2.stl"/>
</geometry>
<material name="Orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/coarse/link_2.stl"/>
</geometry>
</collision>
</link>
<!-- joint between link_2 and link_3 -->
<joint name="lbr_iiwa_joint_3" type="revolute">
<parent link="lbr_iiwa_link_2"/>
<child link="lbr_iiwa_link_3"/>
<origin rpy="1.57079632679 0 3.14159265359" xyz="0 0.2045 0"/>
<axis xyz="0 0 1"/>
<limit effort="300" lower="-2.96705972839" upper="2.96705972839" velocity="10"/>
<dynamics damping="0.5"/>
</joint>
<link name="lbr_iiwa_link_3">
<inertial>
<origin rpy="0 0 0" xyz="0 0.03 0.13"/>
<mass value="3"/>
<inertia ixx="0.08" ixy="0" ixz="0" iyy="0.075" iyz="0" izz="0.01"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/link_3.stl"/>
</geometry>
<material name="Orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/coarse/link_3.stl"/>
</geometry>
</collision>
</link>
<!-- joint between link_3 and link_4 -->
<joint name="lbr_iiwa_joint_4" type="revolute">
<parent link="lbr_iiwa_link_3"/>
<child link="lbr_iiwa_link_4"/>
<origin rpy="1.57079632679 0 0" xyz="0 0 0.2155"/>
<axis xyz="0 0 1"/>
<limit effort="300" lower="-2.09439510239" upper="2.09439510239" velocity="10"/>
<dynamics damping="0.5"/>
</joint>
<link name="lbr_iiwa_link_4">
<inertial>
<origin rpy="0 0 0" xyz="0 0.067 0.034"/>
<mass value="2.7"/>
<inertia ixx="0.03" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.029"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/link_4.stl"/>
</geometry>
<material name="Orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/coarse/link_4.stl"/>
</geometry>
</collision>
</link>
<!-- joint between link_4 and link_5 -->
<joint name="lbr_iiwa_joint_5" type="revolute">
<parent link="lbr_iiwa_link_4"/>
<child link="lbr_iiwa_link_5"/>
<origin rpy="-1.57079632679 3.14159265359 0" xyz="0 0.1845 0"/>
<axis xyz="0 0 1"/>
<limit effort="300" lower="-2.96705972839" upper="2.96705972839" velocity="10"/>
<dynamics damping="0.5"/>
</joint>
<link name="lbr_iiwa_link_5">
<inertial>
<origin rpy="0 0 0" xyz="0.0001 0.021 0.076"/>
<mass value="1.7"/>
<inertia ixx="0.02" ixy="0" ixz="0" iyy="0.018" iyz="0" izz="0.005"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/link_5.stl"/>
</geometry>
<material name="Orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/coarse/link_5.stl"/>
</geometry>
</collision>
</link>
<!-- joint between link_5 and link_6 -->
<joint name="lbr_iiwa_joint_6" type="revolute">
<parent link="lbr_iiwa_link_5"/>
<child link="lbr_iiwa_link_6"/>
<origin rpy="1.57079632679 0 0" xyz="0 0 0.2155"/>
<axis xyz="0 0 1"/>
<limit effort="300" lower="-2.09439510239" upper="2.09439510239" velocity="10"/>
<dynamics damping="0.5"/>
</joint>
<link name="lbr_iiwa_link_6">
<inertial>
<origin rpy="0 0 0" xyz="0 0.0006 0.0004"/>
<mass value="1.8"/>
<inertia ixx="0.005" ixy="0" ixz="0" iyy="0.0036" iyz="0" izz="0.0047"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/link_6.stl"/>
</geometry>
<material name="Orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/coarse/link_6.stl"/>
</geometry>
</collision>
</link>
<!-- joint between link_6 and link_7 -->
<joint name="lbr_iiwa_joint_7" type="revolute">
<parent link="lbr_iiwa_link_6"/>
<child link="lbr_iiwa_link_7"/>
<origin rpy="-1.57079632679 3.14159265359 0" xyz="0 0.081 0"/>
<axis xyz="0 0 1"/>
<limit effort="300" lower="-3.05432619099" upper="3.05432619099" velocity="10"/>
<dynamics damping="0.5"/>
</joint>
<link name="lbr_iiwa_link_7">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0.02"/>
<mass value="0.3"/>
<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/link_7.stl"/>
</geometry>
<material name="Grey"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/coarse/link_7.stl"/>
</geometry>
</collision>
</link>
</robot>

9
examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp
View File

@@ -214,10 +214,8 @@ void ImportSDFSetup::initPhysics()
//u2b.printTree();
btTransform identityTrans;
identityTrans.setIdentity();
btTransform rootTrans;
rootTrans.setIdentity();
for (int m =0; m<u2b.getNumModels();m++)
{
@@ -232,7 +230,8 @@ void ImportSDFSetup::initPhysics()
b3Printf("urdf root link index = %d\n",rootLinkIndex);
MyMultiBodyCreator creation(m_guiHelper);
ConvertURDF2Bullet(u2b,creation, identityTrans,m_dynamicsWorld,m_useMultiBody,u2b.getPathPrefix());
u2b.getRootTransformInWorld(rootTrans);
ConvertURDF2Bullet(u2b,creation, rootTrans,m_dynamicsWorld,m_useMultiBody,u2b.getPathPrefix(),true);
mb = creation.getBulletMultiBody();
}

10
examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
View File

@@ -296,7 +296,7 @@ void BulletURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVect
}
}
bool BulletURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const
bool BulletURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const
{
jointLowerLimit = 0.f;
jointUpperLimit = 0.f;
@@ -308,7 +308,7 @@ bool BulletURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joi
if (linkPtr)
{
UrdfLink* link = *linkPtr;
linkTransformInWorld = link->m_linkTransformInWorld;
if (link->m_parentJoint)
{
@@ -333,7 +333,11 @@ bool BulletURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joi
}
bool BulletURDFImporter::getRootTransformInWorld(btTransform& rootTransformInWorld) const
{
rootTransformInWorld = m_data->m_urdfParser.getModel().m_rootTransformInWorld;
return true;
}
void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut)
{

4
examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
View File

@@ -40,7 +40,9 @@ public:
virtual void getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const;
virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const;
virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const;
virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const;
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const;

6
examples/Importers/ImportURDFDemo/ROSURDFImporter.cpp
View File

@@ -223,7 +223,7 @@ void ROSURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVector3
}
}
bool ROSURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const
bool ROSURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const
{
jointLowerLimit = 0.f;
jointUpperLimit = 0.f;
@@ -281,7 +281,9 @@ bool ROSURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint,
}
}
bool ROSURDFImporter::getRootTransformInWorld(btTransform& rootTransformInWorld) const
{
}
void ROSconvertURDFToVisualShape(const Visual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut)
{

4
examples/Importers/ImportURDFDemo/ROSURDFImporter.h
View File

@@ -33,7 +33,9 @@ public:
virtual void getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const;
virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit,btScalar& jointDamping, btScalar& jointFriction) const;
virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit,btScalar& jointDamping, btScalar& jointFriction) const;
virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const;
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const;

32
examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
View File

@@ -143,7 +143,7 @@ void InitURDF2BulletCache(const URDFImporterInterface& u2b, URDF2BulletCachedDat
}
void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, URDF2BulletCachedData& cache, int urdfLinkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix)
void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, URDF2BulletCachedData& cache, int urdfLinkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix, bool useSDF = false)
{
//b3Printf("start converting/extracting data from URDF interface\n");
@@ -199,11 +199,16 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
btScalar jointFriction;
bool hasParentJoint = u2b.getJointInfo(urdfLinkIndex, parent2joint, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit, jointDamping, jointFriction);
linkTransformInWorldSpace =parentTransformInWorldSpace*parent2joint;
bool hasParentJoint = u2b.getJointInfo(urdfLinkIndex, parent2joint, linkTransformInWorldSpace, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit, jointDamping, jointFriction);
if (useSDF)
{
parent2joint =parentTransformInWorldSpace.inverse()*linkTransformInWorldSpace;
}
else
{
linkTransformInWorldSpace =parentTransformInWorldSpace*parent2joint;
}
int graphicsIndex = u2b.convertLinkVisualShapes(urdfLinkIndex,pathPrefix,localInertialFrame);
btCompoundShape* compoundShape = u2b.convertLinkCollisionShapes(urdfLinkIndex,pathPrefix,localInertialFrame);
@@ -219,11 +224,10 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a);
}
*/
//btVector3 localInertiaDiagonal(0, 0, 0);
//if (mass)
//{
// shape->calculateLocalInertia(mass, localInertiaDiagonal);
//}
if (mass)
{
compoundShape->calculateLocalInertia(mass, localInertiaDiagonal);
}
btRigidBody* linkRigidBody = 0;
btTransform inertialFrameInWorldSpace = linkTransformInWorldSpace*localInertialFrame;
@@ -404,18 +408,18 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
{
int urdfChildLinkIndex = urdfChildIndices[i];
ConvertURDF2BulletInternal(u2b,creation, cache,urdfChildLinkIndex,linkTransformInWorldSpace,world1,createMultiBody,pathPrefix);
ConvertURDF2BulletInternal(u2b,creation, cache,urdfChildLinkIndex,linkTransformInWorldSpace,world1,createMultiBody,pathPrefix,useSDF);
}
}
void ConvertURDF2Bullet(const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, const btTransform& rootTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix)
void ConvertURDF2Bullet(const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, const btTransform& rootTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix, bool useSDF = false)
{
URDF2BulletCachedData cache;
InitURDF2BulletCache(u2b,cache);
int urdfLinkIndex = u2b.getRootLinkIndex();
ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex,rootTransformInWorldSpace,world1,createMultiBody,pathPrefix);
ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex,rootTransformInWorldSpace,world1,createMultiBody,pathPrefix,useSDF);
if (world1 && cache.m_bulletMultiBody)
{

3
examples/Importers/ImportURDFDemo/URDF2Bullet.h
View File

@@ -20,7 +20,8 @@ void ConvertURDF2Bullet(const URDFImporterInterface& u2b,
const btTransform& rootTransformInWorldSpace,
btMultiBodyDynamicsWorld* world,
bool createMultiBody,
const char* pathPrefix);
const char* pathPrefix,
bool useSDF = false);
#endif //_URDF2BULLET_H

4
examples/Importers/ImportURDFDemo/URDFImporterInterface.h
View File

@@ -37,7 +37,9 @@ public:
///fill an array of child link indices for this link, btAlignedObjectArray behaves like a std::vector so just use push_back and resize(0) if needed
virtual void getLinkChildIndices(int urdfLinkIndex, btAlignedObjectArray<int>& childLinkIndices) const =0;
virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const =0;
virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const =0;
virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const =0;
///quick hack: need to rethink the API/dependencies of this
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const = 0;

599
examples/Importers/ImportURDFDemo/UrdfParser.cpp
View File

@@ -77,7 +77,7 @@ static bool parseVector4(btVector4& vec4, const std::string& vector_str)
return true;
}
static bool parseVector3(btVector3& vec3, const std::string& vector_str, ErrorLogger* logger)
static bool parseVector3(btVector3& vec3, const std::string& vector_str, ErrorLogger* logger, bool lastThree = false)
{
vec3.setZero();
btArray<std::string> pieces;
@@ -90,16 +90,22 @@ static bool parseVector3(btVector3& vec3, const std::string& vector_str, ErrorLo
rgba.push_back(urdfLexicalCast<double>(pieces[i].c_str()));
}
}
if (rgba.size() != 3)
if (rgba.size() < 3)
{
logger->reportWarning("Couldn't parse vector3");
return false;
}
vec3.setValue(rgba[0],rgba[1],rgba[2]);
if (lastThree) {
vec3.setValue(rgba[rgba.size()-3], rgba[rgba.size()-2], rgba[rgba.size()-1]);
}
else
{
vec3.setValue(rgba[0],rgba[1],rgba[2]);
}
return true;
}
bool UrdfParser::parseMaterial(UrdfMaterial& material, TiXmlElement *config, ErrorLogger* logger)
{
@@ -143,47 +149,78 @@ bool UrdfParser::parseMaterial(UrdfMaterial& material, TiXmlElement *config, Err
}
bool parseTransform(btTransform& tr, TiXmlElement* xml, ErrorLogger* logger)
bool parseTransform(btTransform& tr, TiXmlElement* xml, ErrorLogger* logger, bool parseSDF = false)
{
tr.setIdentity();
{
const char* xyz_str = xml->Attribute("xyz");
if (xyz_str)
{
parseVector3(tr.getOrigin(),std::string(xyz_str),logger);
}
}
{
const char* rpy_str = xml->Attribute("rpy");
if (rpy_str != NULL)
{
btVector3 rpy;
if (parseVector3(rpy,std::string(rpy_str),logger))
{
double phi, the, psi;
double roll = rpy[0];
double pitch = rpy[1];
double yaw = rpy[2];
phi = roll / 2.0;
the = pitch / 2.0;
psi = yaw / 2.0;
btQuaternion orn(
sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi),
cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi),
cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi),
cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi));
orn.normalize();
tr.setRotation(orn);
}
}
}
return true;
tr.setIdentity();
if (parseSDF)
{
parseVector3(tr.getOrigin(),std::string(xml->GetText()),logger);
}
else
{
const char* xyz_str = xml->Attribute("xyz");
if (xyz_str)
{
parseVector3(tr.getOrigin(),std::string(xyz_str),logger);
}
}
if (parseSDF)
{
btVector3 rpy;
if (parseVector3(rpy,std::string(xml->GetText()),logger,true))
{
double phi, the, psi;
double roll = rpy[0];
double pitch = rpy[1];
double yaw = rpy[2];
phi = roll / 2.0;
the = pitch / 2.0;
psi = yaw / 2.0;
btQuaternion orn(
sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi),
cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi),
cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi),
cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi));
orn.normalize();
tr.setRotation(orn);
}
}
else
{
const char* rpy_str = xml->Attribute("rpy");
if (rpy_str != NULL)
{
btVector3 rpy;
if (parseVector3(rpy,std::string(rpy_str),logger))
{
double phi, the, psi;
double roll = rpy[0];
double pitch = rpy[1];
double yaw = rpy[2];
phi = roll / 2.0;
the = pitch / 2.0;
psi = yaw / 2.0;
btQuaternion orn(
sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi),
cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi),
cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi),
cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi));
orn.normalize();
tr.setRotation(orn);
}
}
}
return true;
}
bool UrdfParser::parseInertia(UrdfInertia& inertia, TiXmlElement* config, ErrorLogger* logger)
{
inertia.m_linkLocalFrame.setIdentity();
@@ -333,21 +370,38 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
else if (type_name == "mesh")
{
geom.m_type = URDF_GEOM_MESH;
if (!shape->Attribute("filename")) {
logger->reportError("Mesh must contain a filename attribute");
return false;
}
geom.m_meshFileName = shape->Attribute("filename");
if (shape->Attribute("scale"))
{
parseVector3(geom.m_meshScale,shape->Attribute("scale"),logger);
} else
{
geom.m_meshScale.setValue(1,1,1);
}
if (m_parseSDF)
{
TiXmlElement* scale = shape->FirstChildElement("scale");
if (0==scale)
{
geom.m_meshScale.setValue(1,1,1);
}
else
{
parseVector3(geom.m_meshScale,scale->GetText(),logger);
}
TiXmlElement* filename = shape->FirstChildElement("uri");
geom.m_meshFileName = filename->GetText();
}
else
{
if (!shape->Attribute("filename")) {
logger->reportError("Mesh must contain a filename attribute");
return false;
}
geom.m_meshFileName = shape->Attribute("filename");
if (shape->Attribute("scale"))
{
parseVector3(geom.m_meshScale,shape->Attribute("scale"),logger);
} else
{
geom.m_meshScale.setValue(1,1,1);
}
}
}
else
{
@@ -473,7 +527,18 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
return false;
}
link.m_name = linkName;
if (m_parseSDF) {
TiXmlElement* pose = config->FirstChildElement("pose");
if (0==pose)
{
link.m_linkTransformInWorld.setIdentity();
}
else
{
parseTransform(link.m_linkTransformInWorld, pose,logger,m_parseSDF);
}
}
// Inertial (optional)
TiXmlElement *i = config->FirstChildElement("inertial");
@@ -552,35 +617,109 @@ bool UrdfParser::parseJointLimits(UrdfJoint& joint, TiXmlElement* config, ErrorL
joint.m_effortLimit = 0.f;
joint.m_velocityLimit = 0.f;
const char* lower_str = config->Attribute("lower");
if (lower_str)
{
joint.m_lowerLimit = urdfLexicalCast<double>(lower_str);
}
const char* upper_str = config->Attribute("upper");
if (upper_str)
{
joint.m_upperLimit = urdfLexicalCast<double>(upper_str);
}
// Get joint effort limit
const char* effort_str = config->Attribute("effort");
if (effort_str)
{
joint.m_effortLimit = urdfLexicalCast<double>(effort_str);
}
// Get joint velocity limit
const char* velocity_str = config->Attribute("velocity");
if (velocity_str)
{
joint.m_velocityLimit = urdfLexicalCast<double>(velocity_str);
}
if (m_parseSDF)
{
TiXmlElement *lower_xml = config->FirstChildElement("lower");
if (lower_xml) {
joint.m_lowerLimit = urdfLexicalCast<double>(lower_xml->GetText());
}
TiXmlElement *upper_xml = config->FirstChildElement("upper");
if (upper_xml) {
joint.m_upperLimit = urdfLexicalCast<double>(upper_xml->GetText());
}
TiXmlElement *effort_xml = config->FirstChildElement("effort");
if (effort_xml) {
joint.m_effortLimit = urdfLexicalCast<double>(effort_xml->GetText());
}
TiXmlElement *velocity_xml = config->FirstChildElement("velocity");
if (velocity_xml) {
joint.m_velocityLimit = urdfLexicalCast<double>(velocity_xml->GetText());
}
}
else
{
const char* lower_str = config->Attribute("lower");
if (lower_str)
{
joint.m_lowerLimit = urdfLexicalCast<double>(lower_str);
}
const char* upper_str = config->Attribute("upper");
if (upper_str)
{
joint.m_upperLimit = urdfLexicalCast<double>(upper_str);
}
// Get joint effort limit
const char* effort_str = config->Attribute("effort");
if (effort_str)
{
joint.m_effortLimit = urdfLexicalCast<double>(effort_str);
}
// Get joint velocity limit
const char* velocity_str = config->Attribute("velocity");
if (velocity_str)
{
joint.m_velocityLimit = urdfLexicalCast<double>(velocity_str);
}
}
return true;
}
bool UrdfParser::parseJointDynamics(UrdfJoint& joint, TiXmlElement* config, ErrorLogger* logger)
{
joint.m_jointDamping = 0;
joint.m_jointFriction = 0;
if (m_parseSDF) {
TiXmlElement *damping_xml = config->FirstChildElement("damping");
if (damping_xml) {
joint.m_jointDamping = urdfLexicalCast<double>(damping_xml->GetText());
}
TiXmlElement *friction_xml = config->FirstChildElement("friction");
if (friction_xml) {
joint.m_jointFriction = urdfLexicalCast<double>(friction_xml->GetText());
}
if (damping_xml == NULL && friction_xml == NULL)
{
logger->reportError("joint dynamics element specified with no damping and no friction");
return false;
}
}
else
{
// Get joint damping
const char* damping_str = config->Attribute("damping");
if (damping_str)
{
joint.m_jointDamping = urdfLexicalCast<double>(damping_str);
}
// Get joint friction
const char* friction_str = config->Attribute("friction");
if (friction_str)
{
joint.m_jointFriction = urdfLexicalCast<double>(friction_str);
}
if (damping_str == NULL && friction_str == NULL)
{
logger->reportError("joint dynamics element specified with no damping and no friction");
return false;
}
}
return true;
}
bool UrdfParser::parseJoint(UrdfJoint& joint, TiXmlElement *config, ErrorLogger* logger)
{
@@ -611,34 +750,48 @@ bool UrdfParser::parseJoint(UrdfJoint& joint, TiXmlElement *config, ErrorLogger*
TiXmlElement *parent_xml = config->FirstChildElement("parent");
if (parent_xml)
{
const char *pname = parent_xml->Attribute("link");
if (!pname)
{
logger->reportError("no parent link name specified for Joint link. this might be the root?");
logger->reportError(joint.m_name.c_str());
return false;
}
else
{
joint.m_parentLinkName = std::string(pname);
}
if (m_parseSDF)
{
joint.m_parentLinkName = std::string(parent_xml->GetText());
}
else
{
const char *pname = parent_xml->Attribute("link");
if (!pname)
{
logger->reportError("no parent link name specified for Joint link. this might be the root?");
logger->reportError(joint.m_name.c_str());
return false;
}
else
{
joint.m_parentLinkName = std::string(pname);
}
}
}
// Get Child Link
TiXmlElement *child_xml = config->FirstChildElement("child");
if (child_xml)
{
const char *pname = child_xml->Attribute("link");
if (!pname)
{
logger->reportError("no child link name specified for Joint link [%s].");
logger->reportError(joint.m_name.c_str());
return false;
}
else
{
joint.m_childLinkName = std::string(pname);
}
if (m_parseSDF)
{
joint.m_childLinkName = std::string(child_xml->GetText());
}
else
{
const char *pname = child_xml->Attribute("link");
if (!pname)
{
logger->reportError("no child link name specified for Joint link [%s].");
logger->reportError(joint.m_name.c_str());
return false;
}
else
{
joint.m_childLinkName = std::string(pname);
}
}
}
// Get Joint type
@@ -672,83 +825,146 @@ bool UrdfParser::parseJoint(UrdfJoint& joint, TiXmlElement *config, ErrorLogger*
return false;
}
// Get Joint Axis
if (joint.m_type != URDFFloatingJoint && joint.m_type != URDFFixedJoint)
{
// axis
TiXmlElement *axis_xml = config->FirstChildElement("axis");
if (!axis_xml){
logger->reportWarning("urdfdom: no axis elemement for Joint, defaulting to (1,0,0) axis");
logger->reportWarning(joint.m_name.c_str());
joint.m_localJointAxis.setValue(1,0,0);
}
else{
if (axis_xml->Attribute("xyz"))
{
if (!parseVector3(joint.m_localJointAxis,axis_xml->Attribute("xyz"),logger))
{
logger->reportError("Malformed axis element:");
logger->reportError(joint.m_name.c_str());
logger->reportError(" for joint:");
logger->reportError(axis_xml->Attribute("xyz"));
return false;
}
}
}
}
// Get limit
TiXmlElement *limit_xml = config->FirstChildElement("limit");
if (limit_xml)
{
if (!parseJointLimits(joint, limit_xml,logger))
{
logger->reportError("Could not parse limit element for joint:");
logger->reportError(joint.m_name.c_str());
return false;
}
}
else if (joint.m_type == URDFRevoluteJoint)
{
logger->reportError("Joint is of type REVOLUTE but it does not specify limits");
logger->reportError(joint.m_name.c_str());
return false;
}
else if (joint.m_type == URDFPrismaticJoint)
{
logger->reportError("Joint is of type PRISMATIC without limits");
logger->reportError( joint.m_name.c_str());
return false;
}
joint.m_jointDamping = 0;
joint.m_jointFriction = 0;
// Get Dynamics
TiXmlElement *prop_xml = config->FirstChildElement("dynamics");
if (prop_xml)
{
// Get joint damping
const char* damping_str = prop_xml->Attribute("damping");
if (damping_str)
{
joint.m_jointDamping = urdfLexicalCast<double>(damping_str);
}
// Get joint friction
const char* friction_str = prop_xml->Attribute("friction");
if (friction_str)
{
joint.m_jointFriction = urdfLexicalCast<double>(friction_str);
}
if (damping_str == NULL && friction_str == NULL)
{
logger->reportError("joint dynamics element specified with no damping and no friction");
return false;
}
}
if (m_parseSDF)
{
if (joint.m_type != URDFFloatingJoint && joint.m_type != URDFFixedJoint)
{
// axis
TiXmlElement *axis_xml = config->FirstChildElement("axis");
if (!axis_xml){
logger->reportWarning("urdfdom: no axis elemement for Joint, defaulting to (1,0,0) axis");
logger->reportWarning(joint.m_name.c_str());
joint.m_localJointAxis.setValue(1,0,0);
}
else{
TiXmlElement *xyz_xml = axis_xml->FirstChildElement("xyz");
if (xyz_xml) {
if (!parseVector3(joint.m_localJointAxis,std::string(xyz_xml->GetText()),logger))
{
logger->reportError("Malformed axis element:");
logger->reportError(joint.m_name.c_str());
logger->reportError(" for joint:");
logger->reportError(xyz_xml->GetText());
return false;
}
}
TiXmlElement *limit_xml = axis_xml->FirstChildElement("limit");
if (limit_xml)
{
if (!parseJointLimits(joint, limit_xml,logger))
{
logger->reportError("Could not parse limit element for joint:");
logger->reportError(joint.m_name.c_str());
return false;
}
}
else if (joint.m_type == URDFRevoluteJoint)
{
logger->reportError("Joint is of type REVOLUTE but it does not specify limits");
logger->reportError(joint.m_name.c_str());
return false;
}
else if (joint.m_type == URDFPrismaticJoint)
{
logger->reportError("Joint is of type PRISMATIC without limits");
logger->reportError( joint.m_name.c_str());
return false;
}
TiXmlElement *prop_xml = config->FirstChildElement("dynamics");
if (prop_xml)
{
if (!parseJointDynamics(joint, prop_xml,logger))
{
logger->reportError("Could not parse dynamics element for joint:");
logger->reportError(joint.m_name.c_str());
return false;
}
}
}
}
}
else
{
// Get Joint Axis
if (joint.m_type != URDFFloatingJoint && joint.m_type != URDFFixedJoint)
{
// axis
TiXmlElement *axis_xml = config->FirstChildElement("axis");
if (!axis_xml){
logger->reportWarning("urdfdom: no axis elemement for Joint, defaulting to (1,0,0) axis");
logger->reportWarning(joint.m_name.c_str());
joint.m_localJointAxis.setValue(1,0,0);
}
else{
if (axis_xml->Attribute("xyz"))
{
if (!parseVector3(joint.m_localJointAxis,axis_xml->Attribute("xyz"),logger))
{
logger->reportError("Malformed axis element:");
logger->reportError(joint.m_name.c_str());
logger->reportError(" for joint:");
logger->reportError(axis_xml->Attribute("xyz"));
return false;
}
}
}
}
// Get limit
TiXmlElement *limit_xml = config->FirstChildElement("limit");
if (limit_xml)
{
if (!parseJointLimits(joint, limit_xml,logger))
{
logger->reportError("Could not parse limit element for joint:");
logger->reportError(joint.m_name.c_str());
return false;
}
}
else if (joint.m_type == URDFRevoluteJoint)
{
logger->reportError("Joint is of type REVOLUTE but it does not specify limits");
logger->reportError(joint.m_name.c_str());
return false;
}
else if (joint.m_type == URDFPrismaticJoint)
{
logger->reportError("Joint is of type PRISMATIC without limits");
logger->reportError( joint.m_name.c_str());
return false;
}
joint.m_jointDamping = 0;
joint.m_jointFriction = 0;
// Get Dynamics
TiXmlElement *prop_xml = config->FirstChildElement("dynamics");
if (prop_xml)
{
// Get joint damping
const char* damping_str = prop_xml->Attribute("damping");
if (damping_str)
{
joint.m_jointDamping = urdfLexicalCast<double>(damping_str);
}
// Get joint friction
const char* friction_str = prop_xml->Attribute("friction");
if (friction_str)
{
joint.m_jointFriction = urdfLexicalCast<double>(friction_str);
}
if (damping_str == NULL && friction_str == NULL)
{
logger->reportError("joint dynamics element specified with no damping and no friction");
return false;
}
}
}
return true;
}
@@ -801,7 +1017,6 @@ bool UrdfParser::initTreeAndRoot(UrdfModel& model, ErrorLogger* logger)
parentLink->m_childJoints.push_back(joint);
parentLink->m_childLinks.push_back(childLink);
parentLinkTree.insert(childLink->m_name.c_str(),parentLink->m_name.c_str());
}
}
@@ -1035,8 +1250,16 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
}
localModel->m_name = name;
TiXmlElement* pose_xml = robot_xml->FirstChildElement("pose");
if (0==pose_xml)
{
localModel->m_rootTransformInWorld.setIdentity();
}
else
{
parseTransform(localModel->m_rootTransformInWorld,pose_xml,logger,m_parseSDF);
}
// Get all Material elements
for (TiXmlElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material"))
{

3
examples/Importers/ImportURDFDemo/UrdfParser.h
View File

@@ -88,6 +88,7 @@ struct UrdfLink
{
std::string m_name;
UrdfInertia m_inertia;
btTransform m_linkTransformInWorld;
btArray<UrdfVisual> m_visualArray;
btArray<UrdfCollision> m_collisionArray;
UrdfLink* m_parentLink;
@@ -127,6 +128,7 @@ struct UrdfJoint
struct UrdfModel
{
std::string m_name;
btTransform m_rootTransformInWorld;
btHashMap<btHashString, UrdfMaterial*> m_materials;
btHashMap<btHashString, UrdfLink*> m_links;
btHashMap<btHashString, UrdfJoint*> m_joints;
@@ -155,6 +157,7 @@ protected:
bool initTreeAndRoot(UrdfModel& model, ErrorLogger* logger);
bool parseMaterial(UrdfMaterial& material, class TiXmlElement *config, ErrorLogger* logger);
bool parseJointLimits(UrdfJoint& joint, TiXmlElement* config, ErrorLogger* logger);
bool parseJointDynamics(UrdfJoint& joint, TiXmlElement* config, ErrorLogger* logger);
bool parseJoint(UrdfJoint& link, TiXmlElement *config, ErrorLogger* logger);
bool parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *config, ErrorLogger* logger);

56
examples/SharedMemory/PhysicsClientExample.cpp
View File

@@ -16,7 +16,9 @@ struct MyMotorInfo2
{
btScalar m_velTarget;
btScalar m_maxForce;
btScalar m_posTarget;
int m_uIndex;
int m_qIndex;
};
@@ -39,7 +41,8 @@ protected:
public:
//@todo, add accessor methods
MyMotorInfo2 m_motorTargetVelocities[MAX_NUM_MOTORS];
// MyMotorInfo2 m_motorTargetVelocities[MAX_NUM_MOTORS];
MyMotorInfo2 m_motorTargetPositions[MAX_NUM_MOTORS];
int m_numMotors;
@@ -136,10 +139,17 @@ public:
{
for (int i=0;i<m_numMotors;i++)
{
btScalar targetVel = m_motorTargetVelocities[i].m_velTarget;
int uIndex = m_motorTargetVelocities[i].m_uIndex;
b3JointControlSetDesiredVelocity(commandHandle, uIndex,targetVel);
// btScalar targetVel = m_motorTargetVelocities[i].m_velTarget;
// int uIndex = m_motorTargetVelocities[i].m_uIndex;
// b3JointControlSetDesiredVelocity(commandHandle, uIndex,targetVel);
btScalar targetPos = m_motorTargetPositions[i].m_posTarget;
int qIndex = m_motorTargetPositions[i].m_qIndex;
int uIndex = m_motorTargetPositions[i].m_uIndex;
b3JointControlSetDesiredPosition(commandHandle, qIndex, targetPos);
b3JointControlSetKp(commandHandle, uIndex, 0.1);
b3JointControlSetKd(commandHandle, uIndex, 0.0);
b3JointControlSetMaximumForce(commandHandle,uIndex,1000);
}
}
@@ -195,7 +205,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
case CMD_LOAD_URDF:
{
b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "kuka_lwr/kuka.urdf");
b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model.urdf");
//setting the initial position, orientation and other arguments are optional
double startPosX = 0;
@@ -235,7 +245,15 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
if (m_selectedBody>=0)
{
b3SharedMemoryCommandHandle commandHandle = b3RequestActualStateCommandInit(m_physicsClientHandle,m_selectedBody);
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_physicsClientHandle, commandHandle);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
int numJoints = b3GetNumJoints(m_physicsClientHandle, m_selectedBody);
for (int i = 0; i < numJoints; ++i) {
struct b3JointSensorState sensorState;
b3GetJointState(m_physicsClientHandle, statusHandle, i, &sensorState);
b3Printf("Joint %d: %f", i, sensorState.m_jointMotorTorque);
}
}
break;
};
@@ -308,7 +326,8 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
}
case CMD_SEND_DESIRED_STATE:
{
b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, CONTROL_MODE_VELOCITY);
// b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, CONTROL_MODE_VELOCITY);
b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, CONTROL_MODE_POSITION_VELOCITY_PD);
prepareControlCommand(command);
b3SubmitClientCommand(m_physicsClientHandle, command);
break;
@@ -330,6 +349,12 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
break;
}
case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: {
b3SharedMemoryCommandHandle commandHandle = b3InitPhysicsParamCommand(m_physicsClientHandle);
b3PhysicsParamSetGravity(commandHandle, 0.0, 0.0, -9.8);
b3SubmitClientCommandAndWaitStatus(m_physicsClientHandle, commandHandle);
break;
}
default:
{
b3Error("Unknown buttonId");
@@ -387,6 +412,7 @@ void PhysicsClientExample::createButtons()
createButton("Create Cylinder Body",CMD_CREATE_RIGID_BODY,isTrigger);
createButton("Reset Simulation",CMD_RESET_SIMULATION,isTrigger);
createButton("Initialize Pose",CMD_INIT_POSE, isTrigger);
createButton("Set gravity", CMD_SEND_PHYSICS_SIMULATION_PARAMETERS, isTrigger);
if (m_physicsClientHandle && m_selectedBody>=0)
@@ -403,14 +429,20 @@ void PhysicsClientExample::createButtons()
if (m_numMotors<MAX_NUM_MOTORS)
{
char motorName[1024];
sprintf(motorName,"%s q'", info.m_jointName);
MyMotorInfo2* motorInfo = &m_motorTargetVelocities[m_numMotors];
sprintf(motorName,"%s q", info.m_jointName);
// MyMotorInfo2* motorInfo = &m_motorTargetVelocities[m_numMotors];
MyMotorInfo2* motorInfo = &m_motorTargetPositions[m_numMotors];
motorInfo->m_velTarget = 0.f;
motorInfo->m_posTarget = 0.f;
motorInfo->m_uIndex = info.m_uIndex;
motorInfo->m_qIndex = info.m_qIndex;
SliderParams slider(motorName,&motorInfo->m_velTarget);
slider.m_minVal=-4;
slider.m_maxVal=4;
// SliderParams slider(motorName,&motorInfo->m_velTarget);
// slider.m_minVal=-4;
// slider.m_maxVal=4;
SliderParams slider(motorName,&motorInfo->m_posTarget);
slider.m_minVal=-4;
slider.m_maxVal=4;
if (m_guiHelper && m_guiHelper->getParameterInterface())
{
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);