Cart Controller help!

edit

Hi!

I am making a cart controller. The idea is using a delta of time, iterate a given number of times to converge to the desired position. So the set parameters are dt (delta time) and N (iterations)

This is the made steps

1. Get the desired task positions
2. Get the actual joint positions
3. Calculate jacobian for actual joint position
4. Calculate actual task position for the joint position
5. Calculate the error (for position and orientation)
6. Divide the error by delta time to get velocity in task space.
7. Using the jacobian and the velocity in task space, calculate the velocity in joint space.
8. Using the velocity in joint space and the delta of time, calculate the necessary (new) joint positions.
9. Iterate from 3 to 8 a number of times.

My variables are:

KDL::JntArray q_ (actual joint positions)

KDL::Frame x_des (the desired task position)

KDL::Frame x_s; (Task positions calculations)

KDL::JntArray q_ (joint positions calculations)

KDL::Jacobian J_ (jacobian)

KDL::Twist xerr_ (Task space error)

KDL::Twist xdot_s (Task space velocity)

Now this is the algoritm.

...

q_s=q_;

k=0;

while(!k>iter){

k++;

jnt_to_jac_solver_->JntToJac(q_s, J_); //Calculate jacobian for joint position

jnt_to_pose_solver_->JntToCart(q_s, x_s); //Calculate task position for the joint position

xerr_.vel = x_des.p-x_s.p; //Calculate the error for position

xerr_.rot = 0.5 * (x_des.M.UnitX() * x_s.M.UnitX() + x_des.M.UnitY() * x_s.M.UnitY() + x_des.M.UnitZ() * x_s.M.UnitZ()); //Calculate the error for orientation

xdot_s.vel = xerr_.vel/dt; //Divide the error by dt to get xdot_s

xdot_s.rot = xerr_.rot/dt; //Divide the error by dt to get xdot_s

qdot_s=f(xdot_s,J_); //Then using xdot_s, and the jacobian, I calculate qdot_s

q_s=q_s+dt*qdot_s; //New joint position

The problem is that using that it doesn't perform well. I have check that it works when I don't use the error of rotation to calculate xdot_s (that happens when I comment the Bold line in my code). So I'm pretty sure I am not calculating well the error in rotation (and so the velocity in task space).

Could someone of you please help me?

Thank you so much.