Ask Your Question
1

IK with KDL

asked 2013-12-10 15:30:40 -0600

tuuzdu gravatar image

updated 2013-12-10 23:05:59 -0600

Hi, everyone! I have a similar problem and I solved it like described in this topic ros-users. Program works fine, but when I try calculate joints values more different than inital values, IK solver not found. Source files: leg_kinematics.cpp hp_chainiksolverpos_nr_jl.cpp

For example. Inital frame of end-effector : 0.14208; -0.14555; -0.11145 Inital joints : 0, 0, 0

Desired frame : 0.16574, -0.1868, -0.08033

(delta_twist from hp_chainiksolverpos_nr_jl)

[ INFO] [1386694221.094697005]: delta_twist.vel 1: 0.023664 delta_twist.vel 2: -0.041252 delta_twist.vel 3: 0.031120  
[ INFO] [1386694221.094781119]: delta_twist.vel 1: 0.003682 delta_twist.vel 2: -0.006420 delta_twist.vel 3: -0.009216  
[ INFO] [1386694221.094833633]: 
Joint 1: -0.000007 
Joint 2: 0.245227 
Joint 3: 0.211637

Desired frame : 0.098897, -0.25073, 0.053264 ( joints: 0.46, 0.73, 0.84)

[ INFO] [1386694869.786644209]: 
delta_twist.vel 1: -0.043179 delta_twist.vel 2: -0.105182 delta_twist.vel 3: 0.164714  
[ INFO] [1386694869.786831916]: 
delta_twist.vel 1: 0.024520 delta_twist.vel 2: -0.157306 delta_twist.vel 3: 0.083648  
[ INFO] [1386694869.786888482]: 
delta_twist.vel 1: -0.016815 delta_twist.vel 2: -0.191048 delta_twist.vel 3: 0.084714  
[ INFO] [1386694869.786932308]: 
delta_twist.vel 1: 0.036921 delta_twist.vel 2: -0.185430 delta_twist.vel 3: 0.084714  
[ INFO] [1386694869.786992199]: 
delta_twist.vel 1: -0.014347 delta_twist.vel 2: -0.214840 delta_twist.vel 3: 0.084714  
[ INFO] [1386694869.787067596]: 
delta_twist.vel 1: 0.036921 delta_twist.vel 2: -0.185430 delta_twist.vel 3: 0.084714  
[ INFO] [1386694869.787110975]: 
delta_twist.vel 1: -0.014347 delta_twist.vel 2: -0.214840 delta_twist.vel 3: 0.084714  
[ INFO] [1386694869.787151938]: 
delta_twist.vel 1: 0.036921 delta_twist.vel 2: -0.185430 delta_twist.vel 3: 0.084714  
[ INFO] [1386694869.787192158]: 
delta_twist.vel 1: -0.014347 delta_twist.vel 2: -0.214840 delta_twist.vel 3: 0.084714  
[ INFO] [1386694869.787235519]: 
delta_twist.vel 1: 0.036921 delta_twist.vel 2: -0.185430 delta_twist.vel 3: 0.084714 
[ERROR] [1386694869.787273642]: IK not found

I can not understand what the problem is...

URDF file

edit retag flag offensive close merge delete

2 Answers

Sort by » oldest newest most voted
3

answered 2013-12-16 10:14:08 -0600

Your URDF describes a 3 DOF manipulator. However, your solver sets 6 constraints on the end effector (3 position + 3 orientation). In general, it is not possible to satisfy the 6 constraints only with 3DOF. The jacobbian you are using is 6x3. That means that it tries to satisfy the 6 constraints. That is because you get those incorrect and strange solutions.

And that is why I developed the solution described in http://ros-users.122217.n3.nabble.com/IK-with-KDL-td1867716.html?

It worked well for me reaching a 3D position with a 3DOF manipulator.

Another choice is adding 3 "fake revolute artificial joints" in the end effector.

However, the best solution should be change the jacobbian matrix to a 3x3 matrix that only considers the final position of the end effector (ignoring its final orientation). If you (or someother) develop this solution, please, share it with us :-).

edit flag offensive delete link more

Comments

But I have same solution in hp_chainiksolverpos_nr_jl.cpp: if(Equal(delta_twist.vel,Vector::Zero(),eps)) break; delta_twist.rot ignored. And I do: ik_solver_vel.setWeightTS(matrix_Mx); Last three diagonal values are 0 in matrix_Mx. Is this like your solution?)

tuuzdu gravatar imagetuuzdu ( 2013-12-17 04:57:42 -0600 )edit

The weight matrix was designed to break the numeric search if a partial solution (in position but not in the orientation)was found. It doesn't improve the numeric search itself. It still may be very hard to find even a "partial solution" if the kinematic characterization or the goal is not suitable.

Pablo Iñigo Blasco gravatar imagePablo Iñigo Blasco ( 2013-12-17 23:51:39 -0600 )edit
1

answered 2013-12-10 20:20:30 -0600

Hello,

Are you sure the end position can be reached? Also your delta_twist.vel 1 value seems to jump around, could it be that your solution is near a singular position of the Chain. Could you provide us the Chain you are using?

edit flag offensive delete link more

Comments

Yes, I'm sure, because I use rviz (with joint_state_publisher, robot_state_publisher and tf) to get values of joints and cartesian coordinates. I attached urdf file to above.

tuuzdu gravatar imagetuuzdu ( 2013-12-10 23:13:30 -0600 )edit

Anyone have ideas?

tuuzdu gravatar imagetuuzdu ( 2013-12-12 17:16:52 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2013-12-10 15:30:40 -0600

Seen: 846 times

Last updated: Dec 16 '13