HAPTIX Simulation World API with Custom World Example - modulabs/gazebo-tutorial GitHub Wiki
์ด ํํ ๋ฆฌ์ผ์ SDF๋ฅผ ์ฌ์ฉํ์ฌ ์ฌ์ฉ์ ์ง์ world๋ฅผ ๊ตฌ์ถํ๋ ๋ฐฉ๋ฒ์ ๋ณด์ฌ์ค๋ค. ๋ํ ์๋ฎฌ๋ ์ด์ world API๋ฅผ ์ฌ์ฉํ์ฌ ๊ฐ์ฒด ์ํธ ์์ฉ์ ๊ฐ์งํ๊ณ ๊ฐ์ฒด ์์์ ์กฐ์ํ๋ ๋ฐฉ๋ฒ์ ๋ํ ๊ฐ๋จํ ์์ ๋ฅผ ์ ๊ณตํ๋ค. ์ค์น ๋จ๊ณ ๋ฐ [world API ํํ ๋ฆฌ์ผ]์(https://github.com/modulabs/gdyn-gazebo-tutorial/wiki/HAPTIX-Simulation-World-API) ์ด๋ฏธ ์๋ฃํ๋ค๊ณ ๊ฐ์ ํฉ๋๋ค.
API๋ฌธ์ ์ ๋ฌธ์ ์ฌ๊ธฐ๋ฅผ ์ฐธ๊ณ ํ๋ผ. SDF ํฌ๋งท๋ฅผ ์ฌ์ฉํ์ฌ ๊ฐ์ ๋ณด world๋ฅผ ๊ตฌ์ถํ๋ ๋ฌธ์๋ ์ฌ๊ธฐ๋ฅผ ์ฐธ๊ณ ํ๋ผ.
๋จผ์ SDF๋ฅผ ์ฌ์ฉํ์ฌ world๋ฅผ ๊ตฌ์ถํ๋ค.
<?xml version="1.0" ?>
<sdf version="1.5">
<world name="default">
<plugin name="HaptixWorldPlugin" filename="libHaptixWorldPlugin.so"/>
<scene>
<shadows>0</shadows>
</scene>
<gui>
<camera name="user_camera">
<pose>0 -0.6 1.3 0 0.4 1.57</pose>
</camera>
<plugin name="HaptixGUIPlugin" filename="libHaptixGUIPlugin.so">
<default_size>25 25</default_size>
<force_min>0.001</force_min>
<force_max>20</force_max>
<color_no_contact>255 255 255 0</color_no_contact>
<color_max>255 0 0 255</color_max>
<color_min>255 227 32 255</color_min>
<hand_side>right</hand_side>
<timer_topic>~/timer_control</timer_topic>
<contacts>
<contact name="mpl::rPalm0">
<pos>150 200</pos>
<index>0</index>
<size>50 100</size>
</contact>
<contact name="mpl::rPalm1">
<index>1</index>
<size>50 100</size>
<pos>85 200</pos>
</contact>
<contact name="mpl::rHandEdge">
<pos>10 235</pos>
<index>2</index>
<size>40 70</size>
</contact>
<contact name="mpl::rHandTop">
<pos>299 55</pos>
<index>3</index>
<size>25 31</size>
</contact>
<contact name="mpl::rThProximal1">
<pos>245 240</pos>
<index>4</index>
</contact>
<contact name="mpl::rThProximal2">
<pos>275 195</pos>
<index>5</index>
</contact>
<contact name="mpl::rThDistal">
<pos>302 130</pos>
<index>6</index>
</contact>
<contact name="mpl::rIndProximal">
<pos>218 116</pos>
<index>7</index>
</contact>
<contact name="mpl::rIndMedial">
<pos>233 43</pos>
<index>8</index>
</contact>
<contact name="mpl::rIndDistal">
<pos>248 -30</pos>
<index>9</index>
</contact>
<contact name="mpl::rMidProximal">
<pos>152 98</pos>
<index>10</index>
</contact>
<contact name="mpl::rMidMedial">
<pos>155 19</pos>
<index>11</index>
</contact>
<contact name="mpl::rMidDistal">
<pos>153 -60</pos>
<index>12</index>
</contact>
<contact name="mpl::rRingProximal">
<pos>84 101</pos>
<index>13</index>
</contact>
<contact name="mpl::rRingMedial">
<pos>74 41</pos>
<index>14</index>
</contact>
<contact name="mpl::rRingDistal">
<pos>64 -19</pos>
<index>15</index>
</contact>
<contact name="mpl::rLittleProximal">
<pos>35 156</pos>
<index>16</index>
</contact>
<contact name="mpl::rLittleMedial">
<pos>14 105</pos>
<index>17</index>
</contact>
<contact name="mpl::rLittleDistal">
<pos>-7 54</pos>
<index>18</index>
</contact>
</contacts>
<task_group name="Grasp">
<task id="grasp_1" name="Block 10cm">
<enabled>1</enabled>
<icon>file://media/gui/arat/arat_icons/grasp_1.jpg</icon>
<instructions>
Instructions: Pick up the 10 cm block and place it on top of the box.
</instructions>
</task>
<task id="grasp_2" name="Block 2.5cm">
<enabled>1</enabled>
<icon>file://media/gui/arat/arat_icons/grasp_2.jpg</icon>
<instructions>
Instructions: Pick up the 2.5 cm block and place it on top of the box.
</instructions>
</task>
<task id="grasp_3" name="Block 5cm">
<enabled>1</enabled>
<initial>1</initial>
<icon>file://media/gui/arat/arat_icons/grasp_3.jpg</icon>
<instructions>
Instructions: Pick up the 5 cm block and place it on top of the box.
</instructions>
</task>
<task id="grasp_4" name="Block 7.5cm">
<enabled>1</enabled>
<icon>file://media/gui/arat/arat_icons/grasp_4.jpg</icon>
<instructions>
Instructions: Pick up the 7.5 cm block and place it on top of the box.
</instructions>
</task>
<task id="grasp_5" name="Cricket ball">
<enabled>1</enabled>
<icon>file://media/gui/arat/arat_icons/grasp_5.jpg</icon>
<instructions>
Instructions: Pick up the cricket ball and place it on top of the box, inside of the tin lid.
</instructions>
</task>
<task id="grasp_6"name="Stone">
<enabled>1</enabled>
<icon>file://media/gui/arat/arat_icons/grasp_6.jpg</icon>
<instructions>
Instructions: Pick up the stone and place it on top of the box.
</instructions>
</task>
</task_group>
<task_group name="Grip">
<task id="grip_1" name="Cups">
<enabled>1</enabled>
<icon>file://media/gui/arat/arat_icons/grip_1.jpg</icon>
<instructions>
Instructions: Pour the marble from one cup into the other.
</instructions>
</task>
<task id="grip_2" name="Tube 2.25cm">
<enabled>1</enabled>
<icon>file://media/gui/arat/arat_icons/grip_2.jpg</icon>
<instructions>
Instructions: Pick up the 2.25cm diameter tube and place it on the wooden peg.
</instructions>
</task>
<task id="grip_3" name="Tube 1cm">
<enabled>1</enabled>
<icon>file://media/gui/arat/arat_icons/grip_3.jpg</icon>
<instructions>
Instructions: Pick up the 1cm diameter tube and place it on the metal peg.
</instructions>
</task>
<task id="grip_4" name="Washer">
<enabled>1</enabled>
<icon>file://media/gui/arat/arat_icons/grip_4.jpg</icon>
<instructions>
Instructions: Pick up the washer and place it on the metal peg.
</instructions>
</task>
</task_group>
<task_group name="Pinch">
<task id="pinch_1" name="Ball bearing">
<enabled>1</enabled>
<icon>file://media/gui/arat/arat_icons/pinch_1.jpg</icon>
<instructions>
Instructions: Pick up the ball bearing and place it on top of the box, inside of the tin lid.
</instructions>
</task>
<task id="pinch_2" name="Marble">
<enabled>1</enabled>
<icon>file://media/gui/arat/arat_icons/pinch_2.jpg</icon>
<instructions>
Instructions: Pick up the marble and place it on top of the box, inside of the tin lid.
</instructions>
</task>
</task_group>
<arm_keys>
<!-- Indices 0-5 are: tx, ty, tz, rx, ry, rz -->
<arm inc_key="w" dec_key="s" index="0" increment=0.025></arm>
<arm inc_key="a" dec_key="d" index="1" increment=0.025></arm>
<arm inc_key="q" dec_key="e" index="2" increment=0.025></arm>
<arm inc_key="W" dec_key="S" index="3" increment=0.025></arm>
<arm inc_key="A" dec_key="D" index="4" increment=0.025></arm>
<arm inc_key="Q" dec_key="E" index="5" increment=0.025></arm>
</arm_keys>
<motor_keys>
<motor inc_key="z" dec_key="Z" index=0 increment=0.05></motor>
<motor inc_key="x" dec_key="X" index=1 increment=0.05></motor>
<motor inc_key="c" dec_key="C" index=2 increment=0.05></motor>
<motor inc_key="1" dec_key="!" index=3 increment=0.05></motor>
<motor inc_key="2" dec_key="@" index=4 increment=0.05></motor>
<motor inc_key="3" dec_key="#" index=5 increment=0.05></motor>
<motor inc_key="4" dec_key="$" index=6 increment=0.05></motor>
<motor inc_key="5" dec_key="%" index=7 increment=0.05></motor>
<motor inc_key="6" dec_key="^" index=8 increment=0.05></motor>
<!-- I'm having trouble parsing "&" and "&", so I'll just use
"amp" and handle it in the code. -->
<motor inc_key="7" dec_key="amp" index=9 increment=0.05></motor>
<motor inc_key="8" dec_key="*" index=10 increment=0.05></motor>
<motor inc_key="9" dec_key="(" index=11 increment=0.05></motor>
<motor inc_key="0" dec_key=")" index=12 increment=0.05></motor>
</motor_keys>
<grasp_keys>
<grasp inc_key="1" dec_key="!" name="Spherical" increment="0.015"></grasp>
<grasp inc_key="2" dec_key="@" name="Cylindrical" increment="0.015"></grasp>
<grasp inc_key="3" dec_key="#" name="FinePinch(British)" increment="0.015"></grasp>
<grasp inc_key="4" dec_key="$" name="FinePinch(American)" increment="0.015"></grasp>
<grasp inc_key="5" dec_key="%" name="ThreeFingerPinch" increment="0.015"></grasp>
<grasp inc_key="6" dec_key="^" name="Palmar(Tray)" increment="0.015"></grasp>
<grasp inc_key="7" dec_key="amp" name="Hook" increment="0.015"></grasp>
</grasp_keys>
</plugin>
<plugin name="TimerGUIPlugin" filename="libTimerGUIPlugin.so">
<size>155 80</size>
<start_stop_button>1</start_stop_button>
<topic>~/timer_control</topic>
</plugin>
</gui>
<physics type="ode">
<gravity>0.000000 0.000000 -9.810000</gravity>
<ode>
<solver>
<type>quick</type>
<iters>100</iters>
<precon_iters>0</precon_iters>
<sor>1.000000</sor>
</solver>
<constraints>
<cfm>0.000000</cfm>
<erp>0.200000</erp>
<contact_max_correcting_vel>0.000000</contact_max_correcting_vel>
<contact_surface_layer>0.00000</contact_surface_layer>
</constraints>
</ode>
<bullet>
<solver>
<type>sequential_impulse</type>
<iters>100</iters>
<sor>1.000000</sor>
</solver>
<constraints>
<cfm>0.000000</cfm>
<erp>0.200000</erp>
<split_impulse>true</split_impulse>
<split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
<contact_surface_layer>0.01000</contact_surface_layer>
</constraints>
</bullet>
<simbody>
<accuracy>0.001</accuracy>
<max_transient_velocity>0.01</max_transient_velocity>
<contact>
<stiffness>1e8</stiffness>
<dissipation>10</dissipation>
<static_friction>0.15</static_friction>
<dynamic_friction>0.1</dynamic_friction>
<viscous_friction>0.0</viscous_friction>
</contact>
</simbody>
<real_time_update_rate>1000</real_time_update_rate>
<max_step_size>0.001000</max_step_size>
</physics>
<!-- debug model for assumed screen
model in simulation frame,
used for debugging pose transforms
to check alignment of movement.
<model name="screen">
<pose>0 -0.65 1.5 0 0 0</pose>
<link name="link">
<visual name="visual">
<pose>-0.25 -0.005 -0.1 0 0 0</pose>
<geometry>
<box>
<size>0.5 0.01 0.2</size>
</box>
</geometry>
</visual>
</link>
<static>true</static>
</model>
-->
<!-- A global light source -->
<include>
<uri>model://sun</uri>
</include>
<!-- A ground plane -->
<include>
<uri>model://ground_plane</uri>
</include>
<include>
<uri>model://table</uri>
</include>
<include>
<uri>model://mpl_haptix_right_forearm</uri>
<pose>0.4 -0.7 1.2 0 0 3.1416</pose>
</include>
<model name="polhemus_source">
<pose>-.5 260 1.3 0 3.1416 1.57</pose>
<link name="link">
<inertial>
<mass>0.25</mass>
<inertia>
<ixx>0.000104167</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.000104167</iyy>
<iyz>0</iyz>
<izz>0.000104167</izz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
</link>
<static>true</static>
</model>
<plugin name="arrange_polhemus" filename="libArrangePlugin.so">
<topic_name>~/arrange_polhemus</topic_name>
<model_name>polhemus_source</model_name>
<initial_arrangement>no_polhemus</initial_arrangement>
<arrangement name="no_polhemus">
<pose model="polhemus_source">-.5 260 1.3 0 3.1416 1.57</pose>
</arrangement>
<arrangement name="have_polhemus">
<pose model="polhemus_source">-.5 0 1.3 0 3.1416 1.57</pose>
</arrangement>
</plugin>
<plugin name="hydra" filename="libHydraPlugin.so">
<pivot>0.04 0 0</pivot>
<grab>0.12 0 0</grab>
</plugin>
<model name="sphere_visual_model">
<pose>0.3 0.0 1.35 0 0 3.1416</pose>
<link name="sphere_link">
<visual name="sphere_visual">
<geometry>
<sphere>
<radius>0.03</radius>
</sphere>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Green</name>
</script>
</material>
</visual>
<collision name="sphere_visual">
<geometry>
<sphere>
<radius>0.03</radius>
</sphere>
</geometry>
</collision>
</link>
<static>1</static>
</model>
</world>
</sdf>์ด world ํ์ผ์ ๊ธฐ๋ณธ arat.world์ ๋งค์ฐ ์ ์ฌํ๋ฉฐ, ์ฃผ์ ์ฐจ์ด์ ์ sphere_visual_model์ด ์ถ๊ฐ ๋ ๊ฒ์ด๋ค:
<model name="sphere_visual_model">
<pose>0.4 0.0 1.2 0 0 3.1416</pose>
<link name="sphere_link">
<visual name="sphere_visual">
<geometry>
<sphere>
<radius>0.03</radius>
</sphere>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Green</name>
</script>
</material>
</visual>
<collision name="sphere_visual">
<geometry>
<sphere>
<radius>0.03</radius>
</sphere>
</geometry>
</collision>
</link>
<static>1</static>
</model>๋ค์์, ์๊ฐ ๊ตฌํ ๋ชจ๋ธ์ ์ ์ด ์ํ๋ฅผ ๊ฐ์งํ๋ ๊ฐ๋จํ Octave/Matlab์ ์์ฑํ๊ณ , ๋ค๋ฅธ ๋ฌผ์ฒด์ ์ ์ด ํ ๋ ๊ตฌ์ฒด์ ์์์ ๋ น์์์ ๋นจ๊ฐ์์ผ๋ก ๋ฐ๊พผ๋ค.
% test program that changes color of a sphere when it comes into contact
% with other objects (e.g. hand)
hxs_set_model_collide_mode('sphere_visual_model', 1);
pose = hxs_model_transform('sphere_visual_model');
while (true)
% note that the color vector is defined by the RGBA 4-tuple:
%
% https://s3.amazonaws.com/osrf-distributions/haptix/api/0.7.1/struct__hxsColor.html
%
% The elements are ordered such that:
% color(1) = float value for the red component
% color(2) = float value for the green component
% color(3) = float value for the blue component
% color(4) = float value for the alpha component
%
color = [0;1;0;1];
if length(hxs_contacts('sphere_visual_model')) > 0,
color = [1;0;0;1];
end,
hxs_set_model_color('sphere_visual_model', color);
% move sphere around
t = hx_read_sensors().time_stamp;
p = pose;
p.pos(1) = pose.pos(1) - 0.2*cos(1*t);
hxs_set_model_transform('sphere_visual_model', p);
sleep(0.001);
endwhileํํ ๋ฆฌ์ผ ํ์ผ์ ๋ค์ด๋ก๋ ํ๋ค.
์์ Matlab ์์ ์ ๊ฐ์ world๋ฅผ ์ฌ์ฉํ์ฌ, ์๊ฐ์ ๊ตฌํ ๋ชจ๋ธ์์ ์ ์ด ์ํ๋ฅผ ๊ฐ์งํ๋ ๊ฐ๋จํ C ์ฝ๋๋ฅผ ์์ฑํ๊ณ ๋ค๋ฅธ ๊ฐ์ฒด์ ์ ์ด ํ ๋ ๊ตฌ์ ์์์ ๋ น์์์ ๋นจ๊ฐ์์ผ๋ก ๋ณ๊ฒฝํ๋ค:
/*
* Copyright (C) 2015 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
//
// Test program that changes color of a sphere model when it comes into contact
// with other objects (e.g. hand).
//
// Green sphere model moves along table, turns red upon making contact.
//
#include <math.h>
#include <stdio.h>
#include <unistd.h>
#include <haptix/comm/haptix_sim.h>
#include <haptix/comm/haptix.h>
#define RADII 6.28318530718
int main(int argc, char **argv)
{
int t2, t1 = 0;
float reset = 0.0, time = 0.0;
hxSensor t;
hxsContactPoints contact_pts;
hxsTransform pose, p;
hxsColor color;
hxsCollideMode collide_mode, one = hxsDETECTIONONLY;
if (hx_connect(NULL, 0) != hxOK)
{
printf("hx_connect(): Request error.\n");
return -1;
}
hxs_set_model_collide_mode("sphere_visual_model", &one);
hxs_model_transform("sphere_visual_model", &pose);
p = pose;
while (1)
{
// changes sphere color upon contact
color.alpha = 1.0;
color.b = 0.0;
color.g = 1.0;
color.r = 0.0;
hxs_contacts("sphere_visual_model", &contact_pts);
if (contact_pts.contact_count > 0)
{
color.alpha = 1.0;
color.b = 0.0;
color.g = 0.0;
color.r = 1.0;
}
hxs_set_model_color("sphere_visual_model", &color);
// take simulation time (nsec)
hx_read_sensors(&t);
t2 = t.time_stamp.nsec;
// adjust time so that time_stamp reset does not also reset sphere position
if (!t2 && t1)
{
// if time_stamp reset then update 'reset tracker'
reset = time;
}
// adjust position of sphere
time = fmod(0.01*t2, RADII) + reset;
p.pos.x = pose.pos.x - 0.2*cos(time);
hxs_set_model_transform("sphere_visual_model", &p);
t1 = t2;
nanosleep(1000000);
}
return 0;
}Haptix C API ํํ ๋ฆฌ์ผ์ ์ปดํ์ผ ์น์
์ ๋์จ Haptix C API ์ฝ๋์ ๋ง์ฐฌ๊ฐ์ง๋ก custom_world.c๋ฅผ ์์ฑํ๋ค.
ํ์ผ์ ๋ค์ด๋ก๋ ํ๋ค:
์์ ์คํ์ ์ํด ํฐ๋ฏธ๋์์ ๊ฐ์ ๋ณด๋ฅผ ์์ํ๋ค:
gazebo custom_haptix.world
๋จผ์ Matlab ๋๋ Octave ์คํฌ๋ฆฝํธ๋ฅผ ์คํํ๋ ๋ฐฉ๋ฒ์ ๋ํ์ฌ world API ์์ ์น์ ์ ์ฐธ๊ณ ํ๋ค.
๋ค์์ผ๋ก Matlab ๋๋ Octave ๋ช
๋ น ํ๋กฌํํธ์์ custom_world.m ์คํฌ๋ฆฝํธ๋ฅผ ํธ์ถํ๋ฉด ์์ด ํต๊ณผ ํ ๋ ๊ตฌํ์ด ๋
น์์์ ๋นจ๊ฐ์์ผ๋ก ๋ฐ๋์ด์ผํ๋ค.
Linux ์ฌ์ฉ์๋ฅผ ์ํ ํํธ๋ก, custom_world.m ์คํ ์ ์ ์ฅํ๋ธ ๋๋ matlab ํ๋กฌํํธ์์ haptix ์คํฌ๋ฆฝํธ์ ๋ํ ๊ฒฝ๋ก๋ฅผ ์ถ๊ฐํ์ฌ๋ผ:
path('[replace with your path to install]/lib/x86_64-linux-gnu/haptix-comm/octave', path)
path('[replace with path to your custom_world.m]', path)
Haptix C API ํํ ๋ฆฌ์ผ์์ ์๋ฎฌ๋ ์ด์
์น์
์ ๋์์๋ Haptix C API ์ฝ๋์ฒ๋ผ custom_world.c๋ก๋ถํฐ ์ปดํ์ผ๋ ๋ฐ์ด๋๋ฆฌ๋ฅผ ์คํํ๋ค.
