moveit inverse_kinematics C++ API
I have been trying to use the inverse kinematic from the moveit package on my URDF hand model. I have used the setup "moveit setup assistance" to generate all the configuration files necessary and I have tried out the demo.launch which is generated by the config assistant and tried out some planning. All works fine.
What I am trying to do is to move the fingers of my model via the moveit C++ API. Before going into the specifics of my problem, here is the setup for the index finger:
The index has 4 Revolute joints and one fixed joint at the tip of the finger. I have created two groups, one is index and the other is index_tip as suggested in this tutorial. The last fixed joint is the which I want to use as the tip for the target of the inverse kinematics.
Eigen::Affine3d target = kinematic_state->getGlobalLinkTransform("index_dof3_joint");
target(0,3) = target(0,3) + dx;
target(1,3) = target(1,3) + dx;
target(2,3) = target(2,3) + dx;
if(kinematic_state->setFromIK(joint_model_group[INDEX],target,5,0.1))
When I run this the value returned from setFromIK is false and fails to find a solution. Now the distance between the current position and target is very small, so I don't think the solution is unfeasible. I have also set in my kinematics.yaml file "positiononlyik: True", since I only want to do position IK on the finger tip.
It does return true when the target is equal to the current and no motion is required.
There are a few things that I have not quite understood yet. In the moveit setup assistant I am told to create two groups, one for the chain an one for the end-effector. However I do not see how this relation is used in the Moveit C++ API.
Also if someone knows how to to set a parameter similar to "positiononlyik" for KDL I would be grateful.
Asked by gpldecha on 2015-03-24 10:31:02 UTC
Answers
I have resolved my problem by using KDL directly. You can use KDL::ChainIkSolverVel_wdls which allows you to set the weights of the task target (6DoF). I set the components of the orientation to 0. Another caveat to be aware of is that KDL only seems to work well when your kinematic chain has more than 6 joints.
I have found that forward kinematics always works with a Chain with 4 DoF and that other inverse kinematics solvers also work (but do not allow you to set the task space weights). However got crashes when using KDL::ChainIkSolverVel_wdls with a kinematic chain which had 4 DoF, so I hadded some dummy joints (small joint angles) so that my chain is always at least size 6 (with respect to the number of joints, fixed joints don't count).
Bellow is some pseudo code in case anybody is having the same difficulty.
KDL::ChainFkSolverPos_recursive* fk_solver_pos;//Forward position solver
KDL::ChainIkSolverVel_wdls* ik_solver_vel_wdls;
KDL::ChainIkSolverPos_NR* ik_solver_pos_NR;
ik_solver_vel_wdls = new KDL::ChainIkSolverVel_wdls(chain, 0.001, 5);
ik_solver_vel_wdls->setLambda(0.01);
Eigen::MatrixXd TS; TS.resize(6,6); TS.setIdentity();
TS(3,3) = 0; TS(4,4) = 0; TS(5,5) = 0;
ik_solver_vel_wdls->setWeightTS(TS);
KDL::JointArry q,q_out;
KDL::Frame target;
int ret = ik_solver_pos_NR->CartToJnt(q,target,q_out);
Side note: I am aware that FastIK is a possible alternative to KDL, however it requires to install OpenRave, which seems to be problematic under Ubuntu 14.04. Also the ros converter urdf_to_collada for indigo is broken and not working. There is just generally speaking a lot of hassle to get an IK solver at the moment under indigo.
Asked by gpldecha on 2015-03-27 07:29:02 UTC
Comments