133 lines
4.1 KiB
XML
133 lines
4.1 KiB
XML
<?xml version="1.0" ?>
|
|
<sdf version="1.5">
|
|
<world name="default">
|
|
<include>
|
|
<uri>model://ground_plane</uri>
|
|
</include>
|
|
<include>
|
|
<uri>model://sun</uri>
|
|
</include>
|
|
<!-- to demonstrate force torque, we'll construct a model with
|
|
two bodies stacked vertically, with a x-revolute joint connecting
|
|
them. The joint has 90 degree limit. We'll test force
|
|
torque readings and characterize them. -->
|
|
<model name="model_1">
|
|
<link name="link_1">
|
|
<inertial>
|
|
<pose>0 0 0.5 0 0 0</pose>
|
|
<inertia>
|
|
<ixx>0.100000</ixx>
|
|
<ixy>0.000000</ixy>
|
|
<ixz>0.000000</ixz>
|
|
<iyy>0.100000</iyy>
|
|
<iyz>0.000000</iyz>
|
|
<izz>0.100000</izz>
|
|
</inertia>
|
|
<mass>10.000000</mass>
|
|
</inertial>
|
|
<visual name="visual_sphere">
|
|
<pose>0 0 0.5 0 0 0</pose>
|
|
<geometry>
|
|
<sphere>
|
|
<radius>0.100000</radius>
|
|
</sphere>
|
|
</geometry>
|
|
</visual>
|
|
<collision name="collision_sphere">
|
|
<pose>0 0 0.5 0 0 0</pose>
|
|
<max_contacts>250</max_contacts>
|
|
<geometry>
|
|
<sphere>
|
|
<radius>0.100000</radius>
|
|
</sphere>
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
<link name="link_2">
|
|
<pose>0 0 1.5 0 0 0</pose>
|
|
<inertial>
|
|
<pose>0 0 0.5 0 0 0</pose>
|
|
<inertia>
|
|
<ixx>0.100000</ixx>
|
|
<ixy>0.000000</ixy>
|
|
<ixz>0.000000</ixz>
|
|
<iyy>0.100000</iyy>
|
|
<iyz>0.000000</iyz>
|
|
<izz>0.100000</izz>
|
|
</inertia>
|
|
<mass>10.000000</mass>
|
|
</inertial>
|
|
<visual name="visual_box">
|
|
<pose>0 0 0.5 0 0 0</pose>
|
|
<geometry>
|
|
<box>
|
|
<size>0.1 0.2 0.4</size>
|
|
</box>
|
|
</geometry>
|
|
</visual>
|
|
<collision name="collision_box">
|
|
<pose>0 0 0.5 0 0 0</pose>
|
|
<geometry>
|
|
<box>
|
|
<size>0.1 0.2 0.4</size>
|
|
</box>
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
<joint name="joint_01" type="revolute">
|
|
<parent>world</parent>
|
|
<child>link_1</child>
|
|
<!-- joint at origin of link_1 inertial frame -->
|
|
<!-- moement arm from link_1 inertial frame to joint_01 is 0m -->
|
|
<pose>0 0 0.5 0 0 0</pose>
|
|
<axis>
|
|
<limit>
|
|
<lower>-1.57079</lower>
|
|
<upper>1.57079</upper>
|
|
<effort>1000.000000</effort>
|
|
<velocity>1000.000000</velocity>
|
|
</limit>
|
|
<dynamics>
|
|
<damping>0.000000</damping>
|
|
<friction>0.000000</friction>
|
|
</dynamics>
|
|
<xyz>1.000000 0.000000 0.000000</xyz>
|
|
<use_parent_model_frame>true</use_parent_model_frame>
|
|
</axis>
|
|
<sensor name="force_torque" type="force_torque">
|
|
<always_on>true</always_on>
|
|
<visualize>true</visualize>
|
|
<update_rate>30</update_rate>
|
|
</sensor>
|
|
</joint>
|
|
<joint name="joint_12" type="revolute">
|
|
<parent>link_1</parent>
|
|
<child>link_2</child>
|
|
<!-- joint_1 at origin of link_2 link frame -->
|
|
<!-- moement arm from link_2 inertial frame to joint_01 is 2m -->
|
|
<!-- moement arm from link_2 inertial frame to joint_12 is 0.5m -->
|
|
<pose>0 0 0 0 0 0</pose>
|
|
<axis>
|
|
<limit>
|
|
<lower>-0.0</lower>
|
|
<upper>0.0</upper>
|
|
<effort>1000.000000</effort>
|
|
<velocity>1000.000000</velocity>
|
|
</limit>
|
|
<dynamics>
|
|
<damping>0.000000</damping>
|
|
<friction>0.000000</friction>
|
|
</dynamics>
|
|
<xyz>0.000000 0.000000 1.000000</xyz>
|
|
<use_parent_model_frame>true</use_parent_model_frame>
|
|
</axis>
|
|
<sensor name="force_torque" type="force_torque">
|
|
<always_on>true</always_on>
|
|
<visualize>true</visualize>
|
|
<update_rate>30</update_rate>
|
|
</sensor>
|
|
</joint>
|
|
</model>
|
|
</world>
|
|
</sdf>
|