ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

moveit inverse_kinematics C++ API

asked 2015-03-24 10:31:02 -0500

gpldecha gravatar image

updated 2015-03-24 10:34:58 -0500

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:

Hand model URDF

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;

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 "position_only_ik: 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 "position_only_ik" for KDL I would be grateful.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted

answered 2015-03-27 07:29:02 -0500

gpldecha gravatar image

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);
Eigen::MatrixXd TS;  TS.resize(6,6);   TS.setIdentity();  
TS(3,3) = 0;     TS(4,4) = 0;     TS(5,5) = 0;

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.

edit flag offensive delete link more

Question Tools



Asked: 2015-03-24 10:31:02 -0500

Seen: 1,737 times

Last updated: Mar 27 '15