Make a Simple Gripper - modulabs/gazebo-tutorial GitHub Wiki
Gazebo ์ค์นํ๋ฉด home ํด๋์ ~/.gazebo/models ํด๋๊ฐ ์์ฑ๋๋๋ฐ ์ฌ๊ธฐ์ ๋ชจ๋ธ ํ์ผ๋ค์ด ๊ด๋ฆฌ๋๋ค. (Model Database documentation๊ณผ SDF๋ฅผ ์ฐธ์กฐ)
์ด์ ์ฒ๋ผ Gazebo๋ฅผ ๋ฐ๋ก ์คํํด์ ๋ชจ๋ธ๋ค์ Insertํ ์ ์์ง๋ง *.world ํ์ผ์ ์ด์ฉํ๋ฉด ์ฒ์๋ถํฐ ์ํ๋ ๋ชจ๋ธ์์ Insertํ ์ฑ๋ก Gazebo๋ฅผ ์คํํ ์ ์๋ค. ๋ค์์ gripper.world ํ์ผ ์์๋ค.
world ํ์ผ์ ์ํ ๋๋ ํ ๋ฆฌ ์์ฑ
$ mkdir -p ~/.gazebo/simple_gripper_tutorial
world ํ์ผ ์์ฑ
$ gedit ~/.gazebo/simple_gripper_tutorial/gripper.world
<?xml version="1.0"?>
<sdf version="1.4">
<world name="default">
<!-- A ground plane -->
<include>
<uri>model://ground_plane</uri>
</include>
<!-- A global light source -->
<include>
<uri>model://sun</uri>
</include>
<include>
<uri>model://my_gripper</uri>
</include>
</world>
</sdf>world์์ ground_plane, sun, my_gripper ๋ชจ๋ธ์ ๋ถ๋ฌ๋ค์ด๊ณ ์๋ค. ์ฐธ๊ณ ๋ก 'model://' ๋ค์์ ์ค๋ ์ด๋ฆ์ model.config ํ์ผ๊ณผ *.sdf ํ์ผ๋ค์ด ์์ฑ๋๋ ๋๋ ํ ๋ฆฌ ์ด๋ฆ์ด๋ค.
๋จผ์ ๋ชจ๋ธ ํ์ผ ์์ฑ์ ์ํด์ ~/.gazebo/models์ ํด๋น ๋๋ ํ ๋ฆฌ๋ฅผ ์์ฑํ๋ค.
$ mkdir -p ~/.gazebo/models/my_gripper
$ cd ~/.gazebo/models/my_gripper
Gripper ๋ชจ๋ธ ํ์ผ์ model.config, simple_gripper.sdf ๋ ๊ฐ๋ฅผ ์์ฑํ๋ค. (model.config ํ์ผ์ ์ด๋ฆ์ ์์๋ก ์ง์ ์ ์๋ค!)
$ gedit ~/.gazebo/models/my_gripper/model.config
<?xml version="1.0"?>
<model>
<name>My Gripper</name>
<version>1.0</version>
<sdf version='1.4'>simple_gripper.sdf</sdf>
<author>
<name>My Name</name>
<email>[email protected]</email>
</author>
<description>
My awesome robot.
</description>
</model>$ gedit ~/.gazebo/models/my_gripper/simple_gripper.sdf
<?xml version="1.0"?>
<sdf version="1.4">
<model name="simple_gripper">
<link name="riser">
<pose>-0.15 0.0 0.5 0 0 0</pose>
<inertial>
<pose>0 0 -0.5 0 0 0</pose>
<inertia>
<ixx>0.01</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.01</iyy>
<iyz>0</iyz>
<izz>0.01</izz>
</inertia>
<mass>10.0</mass>
</inertial>
<collision name="collision">
<geometry>
<box>
<size>0.2 0.2 1.0</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.2 0.2 1.0</size>
</box>
</geometry>
<material>
<script>Gazebo/Purple</script>
</material>
</visual>
</link>
<link name="palm">
<pose>0.0 0.0 0.05 0 0 0</pose>
<inertial>
<inertia>
<ixx>0.01</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.01</iyy>
<iyz>0</iyz>
<izz>0.01</izz>
</inertia>
<mass>0.5</mass>
</inertial>
<collision name="collision">
<geometry>
<box>
<size>0.1 0.2 0.1</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.1 0.2 0.1</size>
</box>
</geometry>
<material>
<script>Gazebo/Red</script>
</material>
</visual>
</link>
<link name="left_finger">
<pose>0.1 0.2 0.05 0 0 -0.78539</pose>
<inertial>
<inertia>
<ixx>0.01</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.01</iyy>
<iyz>0</iyz>
<izz>0.01</izz>
</inertia>
<mass>0.1</mass>
</inertial>
<collision name="collision">
<geometry>
<box>
<size>0.1 0.3 0.1</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.1 0.3 0.1</size>
</box>
</geometry>
<material>
<script>Gazebo/Blue</script>
</material>
</visual>
</link>
<link name="left_finger_tip">
<pose>0.336 0.3 0.05 0 0 1.5707</pose>
<inertial>
<inertia>
<ixx>0.01</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.01</iyy>
<iyz>0</iyz>
<izz>0.01</izz>
</inertia>
<mass>0.1</mass>
</inertial>
<collision name="collision">
<geometry>
<box>
<size>0.1 0.2 0.1</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.1 0.2 0.1</size>
</box>
</geometry>
<material>
<script>Gazebo/Blue</script>
</material>
</visual>
</link>
<link name="right_finger">
<pose>0.1 -0.2 0.05 0 0 .78539</pose>
<inertial>
<inertia>
<ixx>0.01</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.01</iyy>
<iyz>0</iyz>
<izz>0.01</izz>
</inertia>
<mass>0.1</mass>
</inertial>
<collision name="collision">
<geometry>
<box>
<size>0.1 0.3 0.1</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.1 0.3 0.1</size>
</box>
</geometry>
<material>
<script>Gazebo/Green</script>
</material>
</visual>
</link>
<link name="right_finger_tip">
<pose>0.336 -0.3 0.05 0 0 1.5707</pose>
<inertial>
<inertia>
<ixx>0.01</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.01</iyy>
<iyz>0</iyz>
<izz>0.01</izz>
</inertia>
<mass>0.1</mass>
</inertial>
<collision name="collision">
<geometry>
<box>
<size>0.1 0.2 0.1</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.1 0.2 0.1</size>
</box>
</geometry>
<material>
<script>Gazebo/Green</script>
</material>
</visual>
</link>
<static>false</static>
<joint name="palm_left_finger" type="revolute">
<pose>0 -0.15 0 0 0 0</pose>
<child>left_finger</child>
<parent>palm</parent>
<axis>
<limit>
<lower>-0.4</lower>
<upper>0.4</upper>
</limit>
<xyz>0 0 1</xyz>
</axis>
</joint>
<joint name="left_finger_tip" type="revolute">
<pose>0 0.1 0 0 0 0</pose>
<child>left_finger_tip</child>
<parent>left_finger</parent>
<axis>
<limit>
<lower>-0.4</lower>
<upper>0.4</upper>
</limit>
<xyz>0 0 1</xyz>
</axis>
</joint>
<joint name="palm_right_finger" type="revolute">
<pose>0 0.15 0 0 0 0</pose>
<child>right_finger</child>
<parent>palm</parent>
<axis>
<limit>
<lower>-0.4</lower>
<upper>0.4</upper>
</limit>
<xyz>0 0 1</xyz>
</axis>
</joint>
<joint name="right_finger_tip" type="revolute">
<pose>0 0.1 0 0 0 0</pose>
<child>right_finger_tip</child>
<parent>right_finger</parent>
<axis>
<limit>
<lower>-0.4</lower>
<upper>0.4</upper>
</limit>
<xyz>0 0 1</xyz>
</axis>
</joint>
<joint name="palm_riser" type="prismatic">
<child>palm</child>
<parent>riser</parent>
<axis>
<limit>
<lower>0</lower>
<upper>0.9</upper>
</limit>
<xyz>0 0 1</xyz>
</axis>
</joint>
</model>
</sdf>model.config์ simple_gripper.sdf ํ์ผ์ ๋ํ ์ค๋ช ์ ์๋ตํ๋ค.
๋ค์์ผ๋ก world ํ์ผ์ ์คํํ๋ค.
$ gazebo ~/.gazebo/simple_gripper_tutorial/gripper.world
์ค๋ช
ํ๋๋ก ground_plane, simple_gripper ๊ฐ ๊ธฐ๋ณธ์ผ๋ก Insert๋์ด ์๋ Gazebo ํ๋ฉด์ ๋ณผ ์ ์๋ค.
๋ชจ๋ธ์ ์ฐํด๋ฆญ์ ํ๊ณ View->Joints์ View->Wireframe์ ์ ํํด์ฃผ๋ฉด ์กฐ์ธํธ๊ฐ ์ขํ๊ณ๋ก ๋์คํ๋ ์ด ๋๋ ๊ฒ์ ๋ณผ ์ ์๋ค.
๋ํ ๋ชจ๋ธ์ ์กฐ์ธํธ์ ํ์ ๊ฐํด๋ณผ ์ ์๋๋ฐ ์ค๋ฅธ์ชฝ ๊ฐ์ด๋ฐ ํ์๋ ํธ๋ค์ ์ผ์ชฝ์ผ๋ก ๋๋๊ทธ ํ๋ฉด Joints ํญ์ด ์๊ธด๋ค. ๊ทธ ๋ค์ ๋ชจ๋ธ์ ํด๋ฆญํด์ฃผ๋ฉด Force, Position, Velocity ํญ์์ ๊ฐ ์กฐ์ธํธ๋ค์ ํ์ด๋ ์์น, ์๋ ๋ฑ์ ๊ฐํด๋ณผ ์ ์๋ค.