tazz 2013/8/29 1 XML 2 ROOT/robots/robotname/ 1
ROOT robotname XML robotname.xml robotname conf.xml robotname.xml Scenebuilder Scenebuilder XML robotname conf.xml robotname.xml robotname conf.xml XML 3D DLL 6 RHex 3 X Y Z XZ Y Y+ X Y Z X+ Y+ Z+ 4 XML 2
(a) (b) (c) CAD (d) STL 1: 3DCAD Scenebuilder STL OBJ 3DS STL STL STL 3DCAD STL STL robotname.xml Autodesk Inventor 2012 STL 1(a)(b) Inventor CAD 3 X+ Y+ Z+ 1(a) 1(b) z y XML 3
Scenebuilder hinge z 1(c) CAD STL [ ] [CAD ] STL 1(d) 100% body.stl leg.stl 5 XML rhex XML rhex.xml rhex conf.xml 1 rhex.xml Scenebuilder Scenebuilder 1 1 <?xml version="1.0" encoding="utf-8"?> XML 2 <namespace name="rhex"> namespace name <attr unit_length="mm"/> <attr unit_mass ="kg"/> mm kg [kg mm/s 2 ] [s] 10[N] <vmat name="vmat_body" colorname="wheat"/> <vmat name="vmat_leg" colorname="gray"/> 4
vmat visual material name colorname 256 http://en.wikipedia.org/ wiki/web_colors <pmat name="pmat_body" density="0.5[g/cmˆ3]"/> <pmat name="pmat_leg" density="1.0[g/cmˆ3]" static_friction="1.0" dynamic_friction="1.0"/> pmat physical material density static friction dynamic friction <mesh name="mesh_body" filename="body.stl"/> <mesh name="mesh_leg" filename="leg.stl"/> 3D mesh filename STL <camera name="camera" fov="0.3"/> fov <body name="body" auto_mass="true" auto_tree="true"> body body auto mass true auto tree body connector connector <connector name="c0"/> c1 c6 RHex 6 6 trn trn=" 130 0-140" [x, y, z] = [130, 0, 140] mm c camera rot rot rot="-90deg@y*-15deg@x" y 90 x 15 leg1 leg6 1 6 c0 5
<attach connector="body/c0" shape="mesh_body" vmat="vmat_body" pmat="pmat_body"/> attach connector shape vmat pmat 6 attach <hinge name="hinge_leg1" sock="body/c1" plug="leg1/c0" damper="1000000"/> hinge 1 z sock plug damper targetvel (vel 0 <gear up="hinge_leg1" down="hinge_leg2" type="pos" offset="180deg"/> hinge leg1 hinge leg2 offset 180deg hinge leg2 hinge leg1 <contact_group all_bodies="true" enable="false"/> RHex rhex conf.xml 2 Scenebuilder <rhex> desc 6
<desc> RHEX six legged robot </desc> root <root path="body"/> orientation <orientation front ="1 0 0" up="0 1 0" offset="-150mm"/> X+ Y+ Z+ front up Z+ up="0 0 1" offset 150mm assembly 7 RHex controller 8 tank 0 3 0 1 6 robots/ rhex/ 7
<?xml version="1.0" encoding="utf-8"?> <namespace name="rhex"> <attr unit_length="mm"/> <attr unit_mass ="kg"/> <vmat name="vmat_body" colorname="wheat"/> <vmat name="vmat_leg" colorname="gray"/> 1: <pmat name="pmat_body" density="0.5[g/cmˆ3]"/> <pmat name="pmat_leg" density="1.0[g/cmˆ3]" static_friction="1.0" dynamic_friction="1.0"/> <mesh name="mesh_body" filename="body.stl"/> <mesh name="mesh_leg" filename="leg.stl"/> <camera name="camera" fov="0.3"/> <body name="body" auto_mass="true" auto_tree="true"> <connector name="c0"/> <connector name="c1" trn=" 130 0-140"/> <connector name="c2" trn=" 0 0-190"/> <connector name="c3" trn="-130 0-140"/> <connector name="c4" trn=" 130 0 140"/> <connector name="c5" trn=" 0 0 190"/> <connector name="c6" trn="-130 0 140"/> <connector name="c_camera" trn="-1000 200 0" rot="-90deg@y*-15deg@x"/> </body> <body name="leg1" auto_mass="true"> <connector name="c0"/> </body> <body name="leg2" auto_mass="true"> <connector name="c0"/> </body> <body name="leg3" auto_mass="true"> <connector name="c0"/> </body> <body name="leg4" auto_mass="true"> <connector name="c0"/> </body> <body name="leg5" auto_mass="true"> <connector name="c0"/> </body> <body name="leg6" auto_mass="true"> <connector name="c0"/> </body> <attach connector="body/c0" shape="mesh_body" vmat="vmat_body" pmat="pmat_body"/> <attach connector="leg1/c0" shape="mesh_leg" vmat="vmat_leg" pmat="pmat_leg" /> <attach connector="leg2/c0" shape="mesh_leg" vmat="vmat_leg" pmat="pmat_leg" /> <attach connector="leg3/c0" shape="mesh_leg" vmat="vmat_leg" pmat="pmat_leg" /> <attach connector="leg4/c0" shape="mesh_leg" vmat="vmat_leg" pmat="pmat_leg" /> <attach connector="leg5/c0" shape="mesh_leg" vmat="vmat_leg" pmat="pmat_leg" /> <attach connector="leg6/c0" shape="mesh_leg" vmat="vmat_leg" pmat="pmat_leg" /> <attach connector="body/c_camera" camera="camera"/> <hinge name="hinge_leg1" sock="body/c1" plug="leg1/c0" damper="100000"/> <hinge name="hinge_leg2" sock="body/c2" plug="leg2/c0"/> <hinge name="hinge_leg3" sock="body/c3" plug="leg3/c0"/> <hinge name="hinge_leg4" sock="body/c4" plug="leg4/c0" damper="100000"/> <hinge name="hinge_leg5" sock="body/c5" plug="leg5/c0"/> <hinge name="hinge_leg6" sock="body/c6" plug="leg6/c0"/> <gear up="hinge_leg1" down="hinge_leg2" type="pos" offset="180deg"/> <gear up="hinge_leg1" down="hinge_leg3" type="pos" offset="0deg"/> <gear up="hinge_leg4" down="hinge_leg5" type="pos" offset="180deg"/> <gear up="hinge_leg4" down="hinge_leg6" type="pos" offset="0deg"/> <contact_group all_bodies="true" enable="false"/> </namespace> 8
2: <?xml version="1.0" encoding="utf-8"?> <rhex> <desc> RHEX six legged robot </desc> <root path="body"/> <orientation front ="1 0 0" up="0 1 0" offset="-150mm"/> <assembly> </assembly> <controllers> <controller name="tank"> <input channel="0" path="joy.left1"/> <input channel="0" path="key.left" /> <input channel="1" path="joy.right1"/> <input channel="1" path="key.right" /> <input channel="2" path="joy.up1"/> <input channel="2" path="key.up" /> <input channel="3" path="joy.down1"/> <input channel="3" path="key.down" /> <output channel="0" path="hinge_leg1.targetvel" gain="3"/> <output channel="1" path="hinge_leg4.targetvel" gain="3"/> </controller> </controllers> </rhex> rhex. xml rhex_conf. xml body. stl leg. stl rhex kmview.exe load rhex 2(a) offset 150mm step 2(b) step 1 save save unload rhex load rhex 9
(a) (b) step (c) 2: rhex run rhex:... 2(c) stop q RHex RHex 8.4 10
7 <assembly spring="..." damper="..."> <stage max_count="...">... </stage> <stage>... </stage>... </assembly> assembly spring damper / assembly stage stage max count stage <enable path="..."/> <disable path="..."/> <lock path="..." angle="..."/> <free path="..."/> enable path disable lock path targetpos angle spring damper spring damper free 0 1. load robotname 2. assmble robotname 3. run 1. 2. 1 11
3. 1 max count 4. 2 5. robotname asm.dat save robotname asm.dat 1 2 3 8 8.1 <controllers> <controller name="...">... </controller> <controller>... </controller>... </controllers> controllers controller name tank through 12
controller rhex <controller name="tank"> <input channel="0" path="joy.left1"/> <input channel="0" path="key.left" /> <input channel="1" path="joy.right1"/> <input channel="1" path="key.right" /> <input channel="2" path="joy.up1"/> <input channel="2" path="key.up" /> <input channel="3" path="joy.down1"/> <input channel="3" path="key.down" /> <output channel="0" path="hinge_leg1.targetvel" gain="3"/> <output channel="1" path="hinge_leg4.targetvel" gain="3"/> </controller> tank 8.2 <input channel="0" path="joy.left1"/> 0 <input channel="0" path="key.left"/> <output channel="0" path="hinge_leg1.targetvel" gain="3"/> 0 hinge leg1 targetvel gain 3 / <input channel="..." path="..." gain="..."/> <output channel="..." path="..." gain="..." type="..."/> channel path gain gain gain type type value diff type="value" type="diff" <output channel="0" path="hinge_leg1.targetvel" gain="3"/> type value 3 targetvel 13
<output channel="0" path="hinge_leg1.targetpos" gain="3" type="diff"/> ( 3) targetpos 8.2 tank tank H = I 0 I 1 V = I 2 I 3 O 0 = V H O 1 = V + H I i i O i i H V I 0 I 3 O 0 O 1 (O 0, O 1 ) = ( 1, 1) (O 0, O 1 ) = (0, 2) 8.3 through through O i = I i i 0 15 8.4 6 RHex RHex 3(a) 3 3(b) 14
#include <math. h> #include <windows. h> 3: extern "C"{ const double M_PI = 3. 1 4 1 5 9 2 ; const double slow_phase = 1 2 0. 0 ( M_PI / 1 8 0. 0 ) ; ///< const double slow_ratio = 0. 4 ; ///< double phase0 = 0. 0 ; double phase1 = 0. 0 ; ///< ///< double CalcAngle ( double phi ){ if ( phi < slow_phase ) return slow_ratio slow_phase + (( M_PI + slow_ratio slow_phase ) / ( M_PI + slow_phase ) ) ( phi + slow_phase ) ; if ( phi >= slow_phase ) return slow_ratio slow_phase + ( ( M_PI slow_ratio slow_phase ) / ( M_PI slow_phase ) ) ( phi slow_phase ) ; return slow_ratio phi ; } declspec ( dllexport ) void Step ( double input, double output, double dt ){ double h = input [ 0 ] input [ 1 ] ; double v = input [ 2 ] input [ 3 ] ; } } bool turn = fabs ( h ) > 0. 1 ; phase0 += ( turn? h : v ) dt ; phase1 = ( turn? phase0 : phase0 ) + M_PI ; while ( phase0 > M_PI ) phase0 = 2 M_PI ; while ( phase0 < M_PI ) phase0 += 2 M_PI ; while ( phase1 > M_PI ) phase1 = 2 M_PI ; while ( phase1 < M_PI ) phase1 += 2 M_PI ; double angle0 = CalcAngle ( phase0 ) ; double angle1 = CalcAngle ( phase1 ) ; output [ 0 ] = output [ 2 ] = angle0 ; output [ 3 ] = output [ 5 ] = angle1 ; output [ 1 ] = ( turn? angle1 : angle1 ) ; output [ 4 ] = ( turn? angle0 : angle0 ) ; BOOL APIENTRY DllMain ( HMODULE hmodule, DWORD ul_reason_for_call, LPVOID lpreserved ){ return TRUE ; } 15
(a) (b) 3: RHex DLL DLL Visual Studio DLL OK 3 RHex DLL declspec ( dllexport ) void Step ( double input, double output, double dt, int id ) ; input output dt id ID DLL input output 256 DLL rhexctrl.dll 5 spring <hinge name="hinge_leg1" sock="body/c1" plug="leg1/c0" spring="10000000" damper="1000000"/> <hinge name="hinge_leg2" sock="body/c2" plug="leg2/c0" spring="10000000" damper="1000000"/> <hinge name="hinge_leg3" sock="body/c3" plug="leg3/c0" spring="10000000" damper="1000000"/> <hinge name="hinge_leg4" sock="body/c4" plug="leg4/c0" spring="10000000" damper="1000000"/> <hinge name="hinge_leg5" sock="body/c5" plug="leg5/c0" spring="10000000" damper="1000000"/> <hinge name="hinge_leg6" sock="body/c6" plug="leg6/c0" spring="10000000" damper="1000000"/> <controller name="rhexctrl"> <input channel="0" path="joy.left1" gain="5"/> <input channel="0" path="key.left" gain="5"/> <input channel="1" path="joy.right1" gain="5"/> <input channel="1" path="key.right" gain="5"/> <input channel="2" path="joy.up1" gain="5"/> 16
<input channel="2" path="key.up" gain="5"/> <input channel="3" path="joy.down1" gain="5"/> <input channel="3" path="key.down" gain="5"/> <output channel="0" path="hinge_leg1.targetpos"/> <output channel="1" path="hinge_leg2.targetpos"/> <output channel="2" path="hinge_leg3.targetpos"/> <output channel="3" path="hinge_leg4.targetpos"/> <output channel="4" path="hinge_leg5.targetpos"/> <output channel="5" path="hinge_leg6.targetpos"/> </controller> name DLL 5 targetvel targetpos RHex 1 RHex DLL Step id 17