How to get KDL solver to solve for 6D, and not 3D only.

edit

Hi all,

I am trying to use KDL to calculate the inverse kinematics of a UR5 robot. I am using something like this

constexpr std::size_t num_joints = 6;
// initial guess of joint angles 
KDL::JntArray joints_init;
joints_init.resize(num_joints);
// joints_init.data[0] = -0.5;
// joints_init.data[1] = -M_PI / 2;
// joints_init.data[2] = -M_PI / 2;
// joints_init.data[4] = M_PI / 2;
// joints_init.data[5] = 0.3;

/**********************************
 * target robot pose of tool w.r.t.
 *********************************/
KDL::Frame target_frame; // set desired end effector position

/***************************************************************************
 * pose corresponding to [0, -PI/2, -PI/2, 0, PI/2, 0] according to TF
 **************************************************************************/
KDL::Vector pos;
pos.data[0] = -0.675;
pos.data[1] = 0.1110;
pos.data[2] = 0.511;

// set desired end effector rotation
KDL::Rotation rot;
target_frame.p = pos;
target_frame.M = rot.RPY(-3.141, -1.568, -0.004);

KDL::JntArray joints_result;
joints_result.resize(num_joints);

std::cout << "Calling solver for IK!" << std::endl;
int status = ik_solver.CartToJnt(joints_init, target_frame, joints_result);

It seems that this works for the position only. I thought that the solver should solve for 6D also. Am I using this wrong? Does it only solve for position (3D) and not for the full pose(6D)?

Thanks!