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

test load kuka arm sdf

Browse Source
This commit is contained in:
yunfeibai
2016-05-11 13:43:50 -07:00
parent 1bebcc6d9a
commit 06a2669b32
7 changed files with 1519 additions and 191 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>

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.2025 1.57079632679 0 3.14159265359</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.2045 0 1.57079632679 0 3.14159265359</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.2155 1.57079632679 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.1845 0 -1.57079632679 3.14159265359 0</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 0.2155 1.57079632679 0 0</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.081 0 -1.57079632679 3.14159265359 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>

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

@@ -131,7 +131,7 @@ ImportSDFSetup::ImportSDFSetup(struct GUIHelperInterface* helper, int option, co
if (gFileNameArray.size()==0) if (gFileNameArray.size()==0)
{ {
gFileNameArray.push_back("two_cubes.sdf"); gFileNameArray.push_back("kuka_iiwa/model.sdf");
} }
@@ -291,7 +291,7 @@ void ImportSDFSetup::stepSimulation(float deltaTime)
} }
//the maximal coordinates/iterative MLCP solver requires a smallish timestep to converge //the maximal coordinates/iterative MLCP solver requires a smallish timestep to converge
m_dynamicsWorld->stepSimulation(deltaTime,10,1./240.); //m_dynamicsWorld->stepSimulation(deltaTime,10,1./240.);
} }
} }

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

@@ -201,9 +201,6 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
bool hasParentJoint = u2b.getJointInfo(urdfLinkIndex, parent2joint, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit, jointDamping, jointFriction); bool hasParentJoint = u2b.getJointInfo(urdfLinkIndex, parent2joint, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit, jointDamping, jointFriction);
linkTransformInWorldSpace =parentTransformInWorldSpace*parent2joint;
int graphicsIndex = u2b.convertLinkVisualShapes(urdfLinkIndex,pathPrefix,localInertialFrame); int graphicsIndex = u2b.convertLinkVisualShapes(urdfLinkIndex,pathPrefix,localInertialFrame);
btCompoundShape* compoundShape = u2b.convertLinkCollisionShapes(urdfLinkIndex,pathPrefix,localInertialFrame); btCompoundShape* compoundShape = u2b.convertLinkCollisionShapes(urdfLinkIndex,pathPrefix,localInertialFrame);

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

@@ -77,7 +77,7 @@ static bool parseVector4(btVector4& vec4, const std::string& vector_str)
return true; 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(); vec3.setZero();
btArray<std::string> pieces; 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())); rgba.push_back(urdfLexicalCast<double>(pieces[i].c_str()));
} }
} }
if (rgba.size() != 3) if (rgba.size() < 3)
{ {
logger->reportWarning("Couldn't parse vector3"); logger->reportWarning("Couldn't parse vector3");
return false; return false;
} }
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]); vec3.setValue(rgba[0],rgba[1],rgba[2]);
}
return true; return true;
} }
bool UrdfParser::parseMaterial(UrdfMaterial& material, TiXmlElement *config, ErrorLogger* logger) bool UrdfParser::parseMaterial(UrdfMaterial& material, TiXmlElement *config, ErrorLogger* logger)
{ {
@@ -143,10 +149,15 @@ 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(); tr.setIdentity();
if (parseSDF)
{
parseVector3(tr.getOrigin(),std::string(xml->GetText()),logger);
}
else
{ {
const char* xyz_str = xml->Attribute("xyz"); const char* xyz_str = xml->Attribute("xyz");
if (xyz_str) if (xyz_str)
@@ -155,6 +166,31 @@ bool parseTransform(btTransform& tr, TiXmlElement* xml, ErrorLogger* 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"); const char* rpy_str = xml->Attribute("rpy");
if (rpy_str != NULL) if (rpy_str != NULL)
@@ -184,6 +220,7 @@ bool parseTransform(btTransform& tr, TiXmlElement* xml, ErrorLogger* logger)
} }
return true; return true;
} }
bool UrdfParser::parseInertia(UrdfInertia& inertia, TiXmlElement* config, ErrorLogger* logger) bool UrdfParser::parseInertia(UrdfInertia& inertia, TiXmlElement* config, ErrorLogger* logger)
{ {
inertia.m_linkLocalFrame.setIdentity(); inertia.m_linkLocalFrame.setIdentity();
@@ -333,6 +370,23 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
else if (type_name == "mesh") else if (type_name == "mesh")
{ {
geom.m_type = URDF_GEOM_MESH; geom.m_type = URDF_GEOM_MESH;
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")) { if (!shape->Attribute("filename")) {
logger->reportError("Mesh must contain a filename attribute"); logger->reportError("Mesh must contain a filename attribute");
return false; return false;
@@ -347,7 +401,7 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
{ {
geom.m_meshScale.setValue(1,1,1); geom.m_meshScale.setValue(1,1,1);
} }
}
} }
else else
{ {
@@ -474,6 +528,17 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
} }
link.m_name = linkName; link.m_name = linkName;
if (m_parseSDF) {
TiXmlElement* pose = config->FirstChildElement("pose");
if (0==pose)
{
link.m_parentLinktoLinkTransform.setIdentity();
}
else
{
parseTransform(link.m_parentLinktoLinkTransform, pose,logger,m_parseSDF);
}
}
// Inertial (optional) // Inertial (optional)
TiXmlElement *i = config->FirstChildElement("inertial"); TiXmlElement *i = config->FirstChildElement("inertial");
@@ -552,6 +617,30 @@ bool UrdfParser::parseJointLimits(UrdfJoint& joint, TiXmlElement* config, ErrorL
joint.m_effortLimit = 0.f; joint.m_effortLimit = 0.f;
joint.m_velocityLimit = 0.f; joint.m_velocityLimit = 0.f;
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"); const char* lower_str = config->Attribute("lower");
if (lower_str) if (lower_str)
{ {
@@ -578,9 +667,59 @@ bool UrdfParser::parseJointLimits(UrdfJoint& joint, TiXmlElement* config, ErrorL
{ {
joint.m_velocityLimit = urdfLexicalCast<double>(velocity_str); joint.m_velocityLimit = urdfLexicalCast<double>(velocity_str);
} }
}
return true; 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) bool UrdfParser::parseJoint(UrdfJoint& joint, TiXmlElement *config, ErrorLogger* logger)
{ {
@@ -610,6 +749,12 @@ bool UrdfParser::parseJoint(UrdfJoint& joint, TiXmlElement *config, ErrorLogger*
// Get Parent Link // Get Parent Link
TiXmlElement *parent_xml = config->FirstChildElement("parent"); TiXmlElement *parent_xml = config->FirstChildElement("parent");
if (parent_xml) if (parent_xml)
{
if (m_parseSDF)
{
joint.m_parentLinkName = std::string(parent_xml->GetText());
}
else
{ {
const char *pname = parent_xml->Attribute("link"); const char *pname = parent_xml->Attribute("link");
if (!pname) if (!pname)
@@ -623,10 +768,17 @@ bool UrdfParser::parseJoint(UrdfJoint& joint, TiXmlElement *config, ErrorLogger*
joint.m_parentLinkName = std::string(pname); joint.m_parentLinkName = std::string(pname);
} }
} }
}
// Get Child Link // Get Child Link
TiXmlElement *child_xml = config->FirstChildElement("child"); TiXmlElement *child_xml = config->FirstChildElement("child");
if (child_xml) if (child_xml)
{
if (m_parseSDF)
{
joint.m_childLinkName = std::string(child_xml->GetText());
}
else
{ {
const char *pname = child_xml->Attribute("link"); const char *pname = child_xml->Attribute("link");
if (!pname) if (!pname)
@@ -640,6 +792,7 @@ bool UrdfParser::parseJoint(UrdfJoint& joint, TiXmlElement *config, ErrorLogger*
joint.m_childLinkName = std::string(pname); joint.m_childLinkName = std::string(pname);
} }
} }
}
// Get Joint type // Get Joint type
const char* type_char = config->Attribute("type"); const char* type_char = config->Attribute("type");
@@ -672,6 +825,68 @@ bool UrdfParser::parseJoint(UrdfJoint& joint, TiXmlElement *config, ErrorLogger*
return false; 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 // Get Joint Axis
if (joint.m_type != URDFFloatingJoint && joint.m_type != URDFFixedJoint) if (joint.m_type != URDFFloatingJoint && joint.m_type != URDFFixedJoint)
{ {
@@ -749,6 +964,7 @@ bool UrdfParser::parseJoint(UrdfJoint& joint, TiXmlElement *config, ErrorLogger*
return false; return false;
} }
} }
}
return true; return true;
} }
@@ -802,6 +1018,10 @@ bool UrdfParser::initTreeAndRoot(UrdfModel& model, ErrorLogger* logger)
parentLink->m_childLinks.push_back(childLink); parentLink->m_childLinks.push_back(childLink);
parentLinkTree.insert(childLink->m_name.c_str(),parentLink->m_name.c_str()); parentLinkTree.insert(childLink->m_name.c_str(),parentLink->m_name.c_str());
if (m_parseSDF) {
joint->m_parentLinkToJointTransform = childLink->m_parentLinktoLinkTransform;
}
} }
} }

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

@@ -88,6 +88,7 @@ struct UrdfLink
{ {
std::string m_name; std::string m_name;
UrdfInertia m_inertia; UrdfInertia m_inertia;
btTransform m_parentLinktoLinkTransform;
btArray<UrdfVisual> m_visualArray; btArray<UrdfVisual> m_visualArray;
btArray<UrdfCollision> m_collisionArray; btArray<UrdfCollision> m_collisionArray;
UrdfLink* m_parentLink; UrdfLink* m_parentLink;
@@ -155,6 +156,7 @@ protected:
bool initTreeAndRoot(UrdfModel& model, ErrorLogger* logger); bool initTreeAndRoot(UrdfModel& model, ErrorLogger* logger);
bool parseMaterial(UrdfMaterial& material, class TiXmlElement *config, ErrorLogger* logger); bool parseMaterial(UrdfMaterial& material, class TiXmlElement *config, ErrorLogger* logger);
bool parseJointLimits(UrdfJoint& joint, 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 parseJoint(UrdfJoint& link, TiXmlElement *config, ErrorLogger* logger);
bool parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *config, ErrorLogger* logger); bool parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *config, ErrorLogger* logger);