読者です 読者をやめる 読者になる 読者になる

cvl-robot's diary

研究ノート メモメモ https://github.com/dotchang/

メモ:ikfastを動かすためにsympyのインストール

(編集中)

ikfastを動かすために、sympy-0.7.1をソースからインストール

https://github.coをm/sympy/sympy/releases/tag/sympy-0.7.1

sudo apt-get install cython

sudp python setup.py install

 

sympyのバージョン確認

python `openrave-config --python-dir`/openravepy/_openravepy_/ikfast.py --help

 

旧バージョン0.6.6の削除

sudo apt-get remove python-sympy

 

$ python ikfast.py --help

Usage: ikfast.py [options]

 

IKFast: The Robot Kinematics Compiler

Software License Agreement (Lesser GPL v3).  Copyright (C) 2009-2011 Rosen

Diankov.  IKFast is distributed in the hope that it will be useful, but

WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or

FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License

for more details.  IKFast is part of OpenRAVE. This program can be used with

robots or kinbodies defined and is independent of the OpenRAVE databases.

Example usage for 7 DOF Barrett WAM where the 3rd joint is a free parameter:

python ikfast.py --robot=robots/barrettwam.robot.xml --baselink=0 --eelink=7

--savefile=ik.cpp --freeindex=2

 

Options:

  --version             show program's version number and exit

  -h, --help            show this help message and exit

  --robot=ROBOT         robot file (COLLADA or OpenRAVE XML)

  --savefile=SAVEFILE   filename where to store the generated c++ code

  --baselink=BASELINK   base link index to start extraction of ik chain

  --eelink=EELINK       end effector link index to end extraction of ik chain

  --freeindex=FREEINDICES

                        Optional joint index specifying a free parameter of

                        the manipulator. If not specified, assumes all joints

                        not solving for are free parameters. Can be specified

                        multiple times for multiple free parameters.

  --iktype=IKTYPE       The iktype to generate the ik for. Possible values

                        are: ray4d, lookat3d, translationxy2d,

                        translationxaxisangle4d, rotation3d,

                        translationzaxisangle4d, translationyaxisangle4d,

                        direction3d, transform6d,

                        translationxaxisangleznorm4d, translation3d,

                        translationxyorientation3d,

                        translationyaxisanglexnorm4d, translationdirection5d,

                        translationzaxisangleynorm4d

  --lang=LANG           The language to generate the code in (default=cpp),

                        available=(cpp)

  -d DEBUG, --debug=DEBUG

                        Debug level for python nose (smaller values allow more

                        text).

 

 

 

 

OpenRaveのモデルは.xmlか.zaeの2形式。.zaeはmanifest.xmlCollada形式の.daeの2つをzip圧縮したもの。

適当にkawada-hironx.zaeの情報を見てみる

$ openrave-robot.py --help

Usage: openrave-robot.py openrave-filename [options]

 

Queries information about OpenRAVE-loadable robots

 

Options:

  -h, --help            show this help message and exit

  --list=DOLIST         Lists the manipulators/sensors/links/joints names of

                        the robot.

  --info=DOINFO         Prints detailed information on

                        manipulators/sensors/links/joints information of a

                        robot.

  --hash=DOHASH         If set, will output hashes of the loaded body

                        depending if manipname or sensorname are set. Can be

                        one of (body,kinematics,robot)

  --manipname=MANIPNAME

                        if manipulator name is specified will return the

                        manipulator hash of the robot

  --sensorname=SENSORNAME

                        if manipulator name is specified will return the

                        sensor hash of the robot

 

cvl@cvl-desktop:~/openrave-0.8.2/src/robots$ openrave-robot.py kawada-hironx.zae --list manipulators 

rightarm

rightarm_torso

leftarm

leftarm_torso

head

head_torso

cvl@cvl-desktop:~/openrave-0.8.2/src/robots$ openrave-robot.py kawada-hironx.zae --list sensors

cvl@cvl-desktop:~/openrave-0.8.2/src/robots$ openrave-robot.py kawada-hironx.zae --list links

WAIST

CHEST_JOINT0_Link

HEAD_JOINT0_Link

HEAD_JOINT1_Link

RARM_JOINT0_Link

RARM_JOINT1_Link

RARM_JOINT2_Link

RARM_JOINT3_Link

RARM_JOINT4_Link

RARM_JOINT5_Link

RHAND_JOINT0_Link

RHAND_JOINT1_Link

RHAND_JOINT2_Link

RHAND_JOINT3_Link

LARM_JOINT0_Link

LARM_JOINT1_Link

LARM_JOINT2_Link

LARM_JOINT3_Link

LARM_JOINT4_Link

LARM_JOINT5_Link

LHAND_JOINT0_Link

LHAND_JOINT1_Link

LHAND_JOINT2_Link

LHAND_JOINT3_Link

cvl@cvl-desktop:~/openrave-0.8.2/src/robots$ openrave-robot.py kawada-hironx.zae --list joints

CHEST_JOINT0

HEAD_JOINT0

HEAD_JOINT1

RARM_JOINT0

RARM_JOINT1

RARM_JOINT2

RARM_JOINT3

RARM_JOINT4

RARM_JOINT5

RHAND_JOINT0

RHAND_JOINT1

RHAND_JOINT2

RHAND_JOINT3

LARM_JOINT0

LARM_JOINT1

LARM_JOINT2

LARM_JOINT3

LARM_JOINT4

LARM_JOINT5

LHAND_JOINT0

LHAND_JOINT1

LHAND_JOINT2

LHAND_JOINT3

cvl@cvl-desktop:~/openrave-0.8.2/src/robots$ openrave-robot.py kawada-hironx.zae --info manipulators

name           base              end              arm-dof gripper-dof arm                 gripper    

-----------------------------------------------------------------------------------------------------

rightarm       CHEST_JOINT0_Link RARM_JOINT5_Link 6       4           3,4,5,6,7,8         9,10,11,12 

rightarm_torso WAIST             RARM_JOINT5_Link 7       4           0,3,4,5,6,7,8       9,10,11,12 

leftarm        CHEST_JOINT0_Link LARM_JOINT5_Link 6       4           13,14,15,16,17,18   19,20,21,22

leftarm_torso  WAIST             LARM_JOINT5_Link 7       4           0,13,14,15,16,17,18 19,20,21,22

head           CHEST_JOINT0_Link HEAD_JOINT1_Link 2       0           1,2                            

head_torso     WAIST             HEAD_JOINT1_Link 3       0           0,1,2                          

-----------------------------------------------------------------------------------------------------

name           base              end              arm-dof gripper-dof arm                 gripper    

cvl@cvl-desktop:~/openrave-0.8.2/src/robots$ openrave-robot.py kawada-hironx.zae --info sensors

name type link

--------------

name type link

cvl@cvl-desktop:~/openrave-0.8.2/src/robots$ openrave-robot.py kawada-hironx.zae --info links

name              index parents          

-----------------------------------------

WAIST             0                      

CHEST_JOINT0_Link 1     WAIST            

HEAD_JOINT0_Link  2     CHEST_JOINT0_Link

HEAD_JOINT1_Link  3     HEAD_JOINT0_Link 

RARM_JOINT0_Link  4     CHEST_JOINT0_Link

RARM_JOINT1_Link  5     RARM_JOINT0_Link 

RARM_JOINT2_Link  6     RARM_JOINT1_Link 

RARM_JOINT3_Link  7     RARM_JOINT2_Link 

RARM_JOINT4_Link  8     RARM_JOINT3_Link 

RARM_JOINT5_Link  9     RARM_JOINT4_Link 

RHAND_JOINT0_Link 10    RARM_JOINT5_Link 

RHAND_JOINT1_Link 11    RHAND_JOINT0_Link

RHAND_JOINT2_Link 12    RARM_JOINT5_Link 

RHAND_JOINT3_Link 13    RHAND_JOINT2_Link

LARM_JOINT0_Link  14    CHEST_JOINT0_Link

LARM_JOINT1_Link  15    LARM_JOINT0_Link 

LARM_JOINT2_Link  16    LARM_JOINT1_Link 

LARM_JOINT3_Link  17    LARM_JOINT2_Link 

LARM_JOINT4_Link  18    LARM_JOINT3_Link 

LARM_JOINT5_Link  19    LARM_JOINT4_Link 

LHAND_JOINT0_Link 20    LARM_JOINT5_Link 

LHAND_JOINT1_Link 21    LHAND_JOINT0_Link

LHAND_JOINT2_Link 22    LARM_JOINT5_Link 

LHAND_JOINT3_Link 23    LHAND_JOINT2_Link

-----------------------------------------

name              index parents          

cvl@cvl-desktop:~/openrave-0.8.2/src/robots$ openrave-robot.py kawada-hironx.zae --info joints

name         joint_index dof_index parent_link       child_link        mimic

----------------------------------------------------------------------------

CHEST_JOINT0 0           0         WAIST             CHEST_JOINT0_Link      

HEAD_JOINT0  1           1         CHEST_JOINT0_Link HEAD_JOINT0_Link       

HEAD_JOINT1  2           2         HEAD_JOINT0_Link  HEAD_JOINT1_Link       

RARM_JOINT0  3           3         CHEST_JOINT0_Link RARM_JOINT0_Link       

RARM_JOINT1  4           4         RARM_JOINT0_Link  RARM_JOINT1_Link       

RARM_JOINT2  5           5         RARM_JOINT1_Link  RARM_JOINT2_Link       

RARM_JOINT3  6           6         RARM_JOINT2_Link  RARM_JOINT3_Link       

RARM_JOINT4  7           7         RARM_JOINT3_Link  RARM_JOINT4_Link       

RARM_JOINT5  8           8         RARM_JOINT4_Link  RARM_JOINT5_Link       

RHAND_JOINT0 9           9         RARM_JOINT5_Link  RHAND_JOINT0_Link      

RHAND_JOINT1 10          10        RHAND_JOINT0_Link RHAND_JOINT1_Link      

RHAND_JOINT2 11          11        RARM_JOINT5_Link  RHAND_JOINT2_Link      

RHAND_JOINT3 12          12        RHAND_JOINT2_Link RHAND_JOINT3_Link      

LARM_JOINT0  13          13        CHEST_JOINT0_Link LARM_JOINT0_Link       

LARM_JOINT1  14          14        LARM_JOINT0_Link  LARM_JOINT1_Link       

LARM_JOINT2  15          15        LARM_JOINT1_Link  LARM_JOINT2_Link       

LARM_JOINT3  16          16        LARM_JOINT2_Link  LARM_JOINT3_Link       

LARM_JOINT4  17          17        LARM_JOINT3_Link  LARM_JOINT4_Link       

LARM_JOINT5  18          18        LARM_JOINT4_Link  LARM_JOINT5_Link       

LHAND_JOINT0 19          19        LARM_JOINT5_Link  LHAND_JOINT0_Link      

LHAND_JOINT1 20          20        LHAND_JOINT0_Link LHAND_JOINT1_Link      

LHAND_JOINT2 21          21        LARM_JOINT5_Link  LHAND_JOINT2_Link      

LHAND_JOINT3 22          22        LHAND_JOINT2_Link LHAND_JOINT3_Link      

----------------------------------------------------------------------------

name         joint_index dof_index parent_link       child_link        mimic

 

試しにkawada-hironx.zaeのモデルを用いて、ikfastに掛けて首関節角度を求めるソースコードを出力してみましょう。

 python `openrave-config --python-dir`/openravepy/_openravepy_/ikfast.py --robot=robots/kawada-hironx.zae --baselink=0 --eelink=3 --savefile=ik.cpp --freeindex=1 --iktype=lookat3d

 

正しく指定できていれば、ちょっと時間はかかりますがc++ソースコードを吐き出してくれます。