I'm using the TreeIkSolverVel_wdls class to implement IK for a humanoid robot. Basically the chain starts from the bottom of the right leg and goes up to the torso. From the torso onward, there are two branches, one going down from the pelvis to the left foot, and the other going from the shoulder to the tip of the right arm. My requirement was a bit specific: I needed an IK solver that could achieve the desired pose (position + orientation) for the left leg end effector, but only the desired position (any orientation) for the right arm end effector.

I created my own function for solving the position IK, similar to the CartToJnt function in TreeIkSolverPos_NR_JL. In terms of the way I constructed the tree and specified the end effectors, the first end effector is called "r_wrist" and is the right arm end effector, whereas the second one is called "l_sole" and is the left leg end effector. To make the velocity IK solver disregard orientation for the first end effector, I used the method mentioned here (http://www.orocos.org/forum/orocos/orocos-users/kdl-decoupling-ik-problem) and initialized the velocity IK solver like this

//nao_tree is the KDL::tree whereas endpoints is an std::vector<std::string> with the 2 names of the end //effectors: TreeIkSolverVel_wdls tree_vik(new KDL::TreeIkSolverVel_wdls(nao_tree, endpoints)); tree_vik.setLambda(0.01); Eigen::MatrixXd ts_weight = Eigen::MatrixXd::Identity(12, 12); ts_weight(3, 3) = 0.0; ts_weight(4, 4) = 0.0; ts_weight(5, 5) = 0.0; tree_vik.setWeightTS(ts_weight);

Then in the function which I'm using in place of Position IK solver class, the code is exactly identical to the CartToJnt function here: https://github.com/orocos/orocos_kinematics_dynamics/blob/master/orocos_kdl/src/treeiksolverpos_nr_jl.cpp#L38 except that I've added this code just below line 58:

//r_wrist is the name of the right arm end effector if (delta_twist->first == "r_wrist") {

    delta_twist->second(3) = 0.0;
    delta_twist->second(4) = 0.0;
    delta_twist->second(5) = 0.0;

}

Now the problem is that this causes the IK solver to disregard the orientation of the left leg end effector as well in some cases, and that is something I don't want. If I remove the above modifications and run everything out of the box, the solver works perfectly, but obviously then it tries to achieve the position and orientation for the right arm end effector as well. Any idea what the problem could be?

Thanks in advance.