Manages compiled inverse kinematics files for robots using ikfast.
Running:
openrave.py --database inversekinematics
First set the active manipulator, and then instantiate the InverseKinematicsModel class specifying the iktype and free indices.
robot.SetActiveManipulator(...)
ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(robot,iktype=IkParameterizationType.Transform6D)
if not ikmodel.load():
ikmodel.autogenerate()
The supported types are defined by IkParameterizationType and are propagated throughout the entire OpenRAVE framework. All solve methods take in a IkParameterization structure, which handles each IK type’s serialization, distances metrics, derivatives, and transformation.
To show the manipulator and IK results do:
openrave.py --database inversekinematics --robot=robots/pr2-beta-static.zae --manipname=leftarm --show
It is also possible to test the IK on a scene:
openrave.py --database inversekinematics --robot=data/pr2test1.env.xml --manipname=leftarm --show
This database generator uses IKFast: The Robot Kinematics Compiler to generate optimized and stable analytic inverse kinematics solvers for any robot manipulator. The manipulator’s arm joints are used for obtaining the joints to solve for. The user can specify the IK type (Rotation, Translation, Full 6D, Ray 4D, etc), the free joints of the kinematics, and the precision. For example, generating the right arm 6D IK for the PR2 robot where the free joint is the first joint and the free increment is 0.01 radians is:
openrave.py --database inversekinematics --robot=robots/pr2-beta-static.zae --manipname=rightarm --freejoint=r_shoulder_pan_joint --freeinc=0.01
Generating the 3d rotation IK for the stage below is:
openrave.py --database inversekinematics --robot=robots/rotation_stage.robot.xml --iktype=rotation3d
Generating the ray inverse kinematics for the 4 degrees of freedom barrett wam is:
openrave.py --database inversekinematics --robot=robots/barrettwam4.robot.xml --iktype=ray4d
openrave.py --database inversekinematics --robot=robots/pr2-beta-static.zae --iktype=ray4d --manipname=rightarm_camera
openrave.py --database inversekinematics --robot=robots/neuronics-katana.zae --iktype=translationdirection5d --manipname=arm
The filename that the code is saved in can be retrieved by
openrave.py --database inversekinematics --robot=robots/neuronics-katana.zae --iktype=translationdirection5d --manipname=arm --getfilename
Every IK solver should be tested with the robot using --iktests=XXX. However, calling inversekinematics will always re-generate the IK, even if one already exists. In order to just run tests, it is possible to specify the --usecached option to prevent re-generation and specifically test:
openrave.py --database inversekinematics --robot=robots/barrettwam.robot.xml --usecached --iktests=100
This will give the success rate along with information whether the IK gives a wrong results or fails to find a solution.
If there are a lot of free joints in the IK solver, then their discretization can greatly affect whether solutions are found or not. In this case, it is advisable to reduce the discretization threshold by using the --freeinc option.
It is possible to use the auto-generation process through c++ by loading the IKFast problem and calling LoadIKFastSolver command.
ikfastloader.cpp - example for loading IK in C++.
Usage: openrave.py --database inversekinematics [options] Uses ikfast to compute the closed-form inverse kinematics equations of a robot manipulator, generates a C++ file, and compiles this file into a shared object which can then be loaded by OpenRAVE. Options: -h, --help show this help message and exit --freejoint=FREEJOINTS Optional joint name specifying a free parameter of the manipulator. The value of a free joint is known at runtime, but not known at IK generation time. If nothing specified, assumes all joints not solving for are free parameters. Can be specified multiple times for multiple free parameters. --precision=PRECISION The precision to compute the inverse kinematics in, (default=8). --usecached If set, will always try to use the cached ik c++ file, instead of generating a new one. --freeinc=FREEINC The discretization value of freejoints. --numiktests=IKTESTS, --iktests=IKTESTS Will test the ik solver and return the success rate. IKTESTS can be an integer to specify number of random tests, it can also be a filename to specify the joint values of the manipulator to test. The formst of the filename is #numiktests [dof values]* --perftiming=PERFTIMING Number of IK calls for measuring the internal ikfast solver. --outputlang=OUTPUTLANG If specified, will output the generated code in that language (ie --outputlang=cpp). -i, --ipython if true will drop into the ipython interpreter right before ikfast is called --iktype=IKTYPE The ik type to build the solver current types are: Transform6D, Rotation3D, Translation3D, Direction3D, Ray4D, Lookat3D, TranslationDirection5D, TranslationXY2D, TranslationXYOrientation3D, TranslationLocalGlobal6D, TranslationXAxisAngle4D, TranslationYAxisAngle4D, TranslationZAxisAngle4D, TranslationXAxisAngleZNorm4D, TranslationYAxisAngleXNorm4D, TranslationZAxisAngleYNorm4D, CustomDataBit OpenRAVE Environment Options: --loadplugin=_LOADPLUGINS List all plugins and the interfaces they provide. --collision=_COLLISION Default collision checker to use --physics=_PHYSICS physics engine to use (default=none) --viewer=_VIEWER viewer to use (default=qtcoin) --server=_SERVER server to use (default=None). --serverport=_SERVERPORT port to load server on (default=4765). --module=_MODULES module to load, can specify multiple modules. Two arguments are required: "name" "args". -l _LEVEL, --level=_LEVEL, --log_level=_LEVEL Debug level, one of (fatal,error,warn,info,debug,verbose,verifyplans) --testmode if set, will run the program in a finite amount of time and spend computation time validating results. Used for testing OpenRAVE Database Generator General Options: --show Graphically shows the built model --getfilename If set, will return the final database filename where all data is stored --gethas If set, will exit with 0 if datafile is generated and up to date, otherwise will return a 1. This will require loading the model and checking versions, so might be a little slow. --robot=ROBOT OpenRAVE robot to load (default=robots/barrettsegway.robot.xml) --numthreads=NUMTHREADS number of threads to compute the database with (default=1) --manipname=MANIPNAME The name of the manipulator on the robot to use
ベースクラス: openravepy.databases.DatabaseGenerator
Generates analytical inverse-kinematics solutions, compiles them into a shared object/DLL, and sets the robot’s iksolver. Only generates the models for the robot’s active manipulator. To generate IK models for each manipulator in the robot, mulitple InverseKinematicsModel classes have to be created.
パラメタ: |
|
---|
When ‘entered’ will hide all the non-arm links in order to facilitate visiblity of the gripper
Returns a default set of free indices if the robot has more joints than required by the IK. In the futrue, this function will contain heuristics in order to select the best indices candidates.
Returns a list of delta increments appropriate for each free index
OPENRAVE_API IkSolverBasePtr RaveCreateIkSolver(EnvironmentBasePtr penv, const std::string & name)
OPENRAVE_API ModuleBasePtr RaveCreateModule(EnvironmentBasePtr penv, const std::string & name)
OPENRAVE_API void RaveDestroy()
Destroys the entire OpenRAVE state and all loaded environments.
This functions should be always called before program shutdown in order to assure all resources are relased appropriately.
OPENRAVE_API std::string RaveFindDatabaseFile(const std::string & filename, bool bRead = true )
Searches for a filename in the database and returns a full path/URL to it.
- Parameters
- filename -
- the relative filename in the database
- bRead -
- if true will only return a file if it exists. If false, will return the filename of the first valid database directory.
- Return
- a non-empty string if a file could be found.
Returns the compiler version that openravepy_int was compiled with