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

parse root transformation and test loading two robots

Browse Source
This commit is contained in:
yunfeibai
2016-05-11 15:52:50 -07:00
parent 06a2669b32
commit 7929bee128
12 changed files with 879 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
data/kuka_iiwa/model.sdf
View File

@@ -88,7 +88,7 @@
</axis> </axis>
</joint> </joint>
<link name='lbr_iiwa_link_2'> <link name='lbr_iiwa_link_2'>
<pose frame=''>0 0 0.2025 1.57079632679 0 3.14159265359</pose> <pose frame=''>0 0 0.36 1.5708 -0 -3.14159</pose>
<inertial> <inertial>
<pose frame=''>0.0003 0.059 0.042 0 -0 0</pose> <pose frame=''>0.0003 0.059 0.042 0 -0 0</pose>
<mass>4</mass> <mass>4</mass>
@@ -141,7 +141,7 @@
</axis> </axis>
</joint> </joint>
<link name='lbr_iiwa_link_3'> <link name='lbr_iiwa_link_3'>
<pose frame=''>0 0.2045 0 1.57079632679 0 3.14159265359</pose> <pose frame=''>0 -0 0.5645 0 0 0</pose>
<inertial> <inertial>
<pose frame=''>0 0.03 0.13 0 -0 0</pose> <pose frame=''>0 0.03 0.13 0 -0 0</pose>
<mass>3</mass> <mass>3</mass>
@@ -194,7 +194,7 @@
</axis> </axis>
</joint> </joint>
<link name='lbr_iiwa_link_4'> <link name='lbr_iiwa_link_4'>
<pose frame=''>0 0 0.2155 1.57079632679 0 0</pose> <pose frame=''>0 -0 0.78 1.5708 0 0</pose>
<inertial> <inertial>
<pose frame=''>0 0.067 0.034 0 -0 0</pose> <pose frame=''>0 0.067 0.034 0 -0 0</pose>
<mass>2.7</mass> <mass>2.7</mass>
@@ -247,7 +247,7 @@
</axis> </axis>
</joint> </joint>
<link name='lbr_iiwa_link_5'> <link name='lbr_iiwa_link_5'>
<pose frame=''>0 0.1845 0 -1.57079632679 3.14159265359 0</pose> <pose frame=''>0 -0 0.9645 0 -0 -3.14159</pose>
<inertial> <inertial>
<pose frame=''>0.0001 0.021 0.076 0 -0 0</pose> <pose frame=''>0.0001 0.021 0.076 0 -0 0</pose>
<mass>1.7</mass> <mass>1.7</mass>
@@ -300,7 +300,7 @@
</axis> </axis>
</joint> </joint>
<link name='lbr_iiwa_link_6'> <link name='lbr_iiwa_link_6'>
<pose frame=''>0 0 0.2155 1.57079632679 0 0</pose> <pose frame=''>0 0 1.18 1.5708 -0 -3.14159</pose>
<inertial> <inertial>
<pose frame=''>0 0.0006 0.0004 0 -0 0</pose> <pose frame=''>0 0.0006 0.0004 0 -0 0</pose>
<mass>1.8</mass> <mass>1.8</mass>
@@ -353,7 +353,7 @@
</axis> </axis>
</joint> </joint>
<link name='lbr_iiwa_link_7'> <link name='lbr_iiwa_link_7'>
<pose frame=''>0 0.081 0 -1.57079632679 3.14159265359 0</pose> <pose frame=''>0 0 1.261 0 0 0</pose>
<inertial> <inertial>
<pose frame=''>0 0 0.02 0 -0 0</pose> <pose frame=''>0 0 0.02 0 -0 0</pose>
<mass>0.3</mass> <mass>0.3</mass>

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>

13
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("kuka_iiwa/model.sdf"); gFileNameArray.push_back("two_cubes.sdf");
} }
@@ -214,10 +214,8 @@ void ImportSDFSetup::initPhysics()
//u2b.printTree(); //u2b.printTree();
btTransform identityTrans; btTransform rootTrans;
identityTrans.setIdentity(); rootTrans.setIdentity();
for (int m =0; m<u2b.getNumModels();m++) for (int m =0; m<u2b.getNumModels();m++)
{ {
@@ -232,7 +230,8 @@ void ImportSDFSetup::initPhysics()
b3Printf("urdf root link index = %d\n",rootLinkIndex); b3Printf("urdf root link index = %d\n",rootLinkIndex);
MyMultiBodyCreator creation(m_guiHelper); 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(); mb = creation.getBulletMultiBody();
} }
@@ -291,7 +290,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.);
} }
} }

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; jointLowerLimit = 0.f;
jointUpperLimit = 0.f; jointUpperLimit = 0.f;
@@ -308,7 +308,7 @@ bool BulletURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joi
if (linkPtr) if (linkPtr)
{ {
UrdfLink* link = *linkPtr; UrdfLink* link = *linkPtr;
linkTransformInWorld = link->m_linkTransformInWorld;
if (link->m_parentJoint) 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) 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 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; 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; jointLowerLimit = 0.f;
jointUpperLimit = 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) 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 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; virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const;

18
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"); //b3Printf("start converting/extracting data from URDF interface\n");
@@ -199,7 +199,15 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
btScalar jointFriction; btScalar jointFriction;
bool hasParentJoint = u2b.getJointInfo(urdfLinkIndex, parent2joint, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit, jointDamping, jointFriction); 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); int graphicsIndex = u2b.convertLinkVisualShapes(urdfLinkIndex,pathPrefix,localInertialFrame);
@@ -400,18 +408,18 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
{ {
int urdfChildLinkIndex = urdfChildIndices[i]; 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; URDF2BulletCachedData cache;
InitURDF2BulletCache(u2b,cache); InitURDF2BulletCache(u2b,cache);
int urdfLinkIndex = u2b.getRootLinkIndex(); 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) 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, const btTransform& rootTransformInWorldSpace,
btMultiBodyDynamicsWorld* world, btMultiBodyDynamicsWorld* world,
bool createMultiBody, bool createMultiBody,
const char* pathPrefix); const char* pathPrefix,
bool useSDF = false);
#endif //_URDF2BULLET_H #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 ///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 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 ///quick hack: need to rethink the API/dependencies of this
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const = 0; virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const = 0;

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

@@ -532,11 +532,11 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
TiXmlElement* pose = config->FirstChildElement("pose"); TiXmlElement* pose = config->FirstChildElement("pose");
if (0==pose) if (0==pose)
{ {
link.m_parentLinktoLinkTransform.setIdentity(); link.m_linkTransformInWorld.setIdentity();
} }
else else
{ {
parseTransform(link.m_parentLinktoLinkTransform, pose,logger,m_parseSDF); parseTransform(link.m_linkTransformInWorld, pose,logger,m_parseSDF);
} }
} }
@@ -1017,11 +1017,6 @@ bool UrdfParser::initTreeAndRoot(UrdfModel& model, ErrorLogger* logger)
parentLink->m_childJoints.push_back(joint); parentLink->m_childJoints.push_back(joint);
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;
}
} }
} }
@@ -1255,8 +1250,16 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
} }
localModel->m_name = name; 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 // Get all Material elements
for (TiXmlElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material")) 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,7 +88,7 @@ struct UrdfLink
{ {
std::string m_name; std::string m_name;
UrdfInertia m_inertia; UrdfInertia m_inertia;
btTransform m_parentLinktoLinkTransform; btTransform m_linkTransformInWorld;
btArray<UrdfVisual> m_visualArray; btArray<UrdfVisual> m_visualArray;
btArray<UrdfCollision> m_collisionArray; btArray<UrdfCollision> m_collisionArray;
UrdfLink* m_parentLink; UrdfLink* m_parentLink;
@@ -128,6 +128,7 @@ struct UrdfJoint
struct UrdfModel struct UrdfModel
{ {
std::string m_name; std::string m_name;
btTransform m_rootTransformInWorld;
btHashMap<btHashString, UrdfMaterial*> m_materials; btHashMap<btHashString, UrdfMaterial*> m_materials;
btHashMap<btHashString, UrdfLink*> m_links; btHashMap<btHashString, UrdfLink*> m_links;
btHashMap<btHashString, UrdfJoint*> m_joints; btHashMap<btHashString, UrdfJoint*> m_joints;