Inverse kinematics: how to specify TCP constraints in an iterative algorithm?

Question

I am trying to write some simple code to perform IK for a 6 DoF redundant robot using the Jacobian pseudo-inverse method. I can solve IK for a desired pose using the iterative method, and I want to now focus on applying constraints to the solution. Specifically I'm interested in

1. Keep the end effector orientation constant as the robot moves from initial to final pose
2. Avoid obstacles in the workspace

I've read about how the redundancy/null space of the Jacobian can be exploited to cause internal motions that satisfy desired constraints, while still executing the trajectory, but I am having trouble implementing this as an algorithm. For instance, my simple iterative algorithm looks like

error = pose_desired - pose_current;
q_vel = pinv(J)*error(:,k);
q = q + q_vel;

where $q$ is 'pushed' towards the right solution, updated until the error is minimized. But for additional constraints, the equation (Siciliano, Bruno, et al. Robotics: modelling, planning and control) specifies

$$ \dot{q} = J^\dagger*v_e - (I-J^\dagger J)\dot{q_0} \\ \dot{q_0} = k_0*(\frac{\partial w(q)}{\partial q})^T $$

where $w$ is supposed to be a term that minimizes/maximizes a chosen constraint. I don't understand the real world algorithmic implementation of this 'term' in the context of my desired constraints: so if I want to keep the orientation of my end effector constant, how can I define the parameters $w$, $q_0$ etc.? I can sense that the partial derivative signifies that it is representing the difference between how the current configuration and a future configuration affect my constraint, and can encourage 'good' choices, but not more than that.

Answer 1

If you want to keep orientation constant, the simplest approach is to, well.. do exactly that. Do not solve for the tartget point and let the joint move along a seemingly arbitratry cartesian path (but infact well defined in joint space).

Solve the inverse kinematics problem for a number of points (depending on your robots precision maybe every milimeter) along a line or any other path shape you need for obstacle avoidance. The line (or other shape) gives you the TCP positions and the orientation, as you require it, should be set constant. You will get joint coordinates for each point on a line. If you feed these squentially to the motors your robot will execute the desired trajectory with the desired orientation. No null space needed.

If you do not want to complicate things with additional shapes and IK solving, you can also constrain orientation in your method easily:

Instead of planning the whole path in one shot like you specified:

error = pose_desired - pose_current; 
q_vel = pinv(J)*error(:,k); 
q = q+ q_vel;

Break it down to 3 seperate parts

1 Set the constat orientation you require during the path execution

error = postition_current_with_const_orientation - pose_current;
q_vel = pinv(J)*error(:,k); 
q = q+ q_vel;

If the error as an RMS value is below a threshold value you can proceed to the next step

2 Move to the target position (not pose) with the constant orientation

error = postition_target_with_const_orientation - pose_current;
q_vel = pinv(J)*error(:,k); 
q = q+ q_vel;

If the error as an RMS value is below a threshold value you can proceed to the next step

3 Orient the TCP for the target orientation to attain the target pose

error = pose_target - pose_current;
q_vel = pinv(J)*error(:,k); 
q = q+ q_vel;

If the error as an RMS value is below a threshold value you can proceed to the next task/motion

EDIT: Please note that I am not sure why you used error(:,k) and not just error but I assumed it has a reason and used it as you did.

Answer 2

The pseudoinverse (Moore-Penrose inverse) provides the solution which is minimum in a least-squares sense. It is rather similar to the Simplex method in that sense. You need some type of optimization such as this to coordinate the extra axes of motion while posing the end effector at the desired position and orientation.

To also optimize a run-time constraint, you take advantage of the null space of J. That is your term multiplying $\dot{q_0}.$ Frankly it doesn't really matter what $\omega$ is, in terms of the position and orientation of the end effector. The idea is that any constraint $\omega$ only results in self-motion of the manipulator, which does not change the end effector's pose.

Look up John Ballieul's work for excellent descriptions of the use of the null space of $J$ for these purposes.