Cartesian control for $z$ vector of end effector frame to point towards a specific direction

Question

Consider a robotic manipulator with 7-DOF (all joints are revolute) that accepts joint velocities as execution commands. What I am trying to achieve is the following:

I want to calculate the velocities of the joints needed in order for the $z$ vector of the end effector frame to point towards a ball which moves on the $xy$ plane. I am asked only to change the end effectors orientation and not its position. The position and the velocity of the ball, both expressed in the end effector frame are given by a function get_state(). The time step of the simulation is $T = 0.004ms$. So, basically the procedure is like this:

• Simulate one step of the ball
• Use get_state() function to obtain ball's position and velocity expressed in the end effector frame
• Do what it needs to be done to calculate the appropriate joint velocities such that $z$ vector of end effector frame points towards the ball
• Repeat

Considering the fact that the $z$ vector of the end effector should point towards the center of the ball, the problem breaks down to the simpler one that the $z$ vector should point towards a point in space. Consider also that the forward kinematics is solved, so the homogeneous transform from the 'world_frame' to the 'end_effector_frame' is known. My thoughts for solving it are:

• At the end the $z$ vector of the "end_effector_frame" should be aligned to the unit vector $u$ in the direction from the origin of the end effector frame to the point $p$ which is the position of the ball expressed in the end effector frame and is known.
• So, basically I have to come up with a rotation matrix $R$ capable of rotating the $z$ vector onto the $u$ vector. I found a possible way of deriving this rotation matrix here.
• After having this rotation matrix $R$, I thought of expressing it as an $[ \text{angle} \ \text{axis} ]$ representation and then calculate $dw = angle \cdot axis$, which is the rotational movement the end effector has to perform in order to achieve the new orientation (maybe add also a proportional controller $K \cdot dw$).
• So, because then end effector performs only a rotational movement and not at all any translational, its total desired movement should be: $ D = [0 \ \ 0 \ \ 0 \ \ dw(1) \ \ dw(2) \ \ dw(3)]^T $.
• Final step is to compute the joint velocities by using the pseudoinverse of the manipulator's jacobian, like so: $ \dot{q} = J^{-1}(q) \cdot D $.

Basically it is cartesian control of the manipulator. First of all, my basic concern is whether this approach is correct or is it completely wrong and if it is correct, should I use the aforementioned formula (found at the link) in order to compute the proper rotation matrix or is there another way of finding this matrix that is more suitable for robotics ? Besides that would an inverse kinematics approach be more suitable ?

Finally, I am curious about the fact that the velocity of the ball is given and expressed in the end effector frame. The procedure I described above doesn't use this knowledge and I am curious if I am missing something and I should use this velocity in any certain way.

Answer 1

By imposing that the position of the end-effector $\mathbf{x} \in \mathbb{R}^3$ has to remain fixed, you're actually limiting your IK that, as a result, might struggle to achieve the task of aligning the vector $\mathbf{z} \in \mathbb{R}^3$ to point toward the target $\mathbf{p} \in \mathbb{R}^3$ when, for instance, one of the joints $\mathbf{q} \in \mathbb{R}^n$ reaches its bounds. To sum up, we are not required to stick to the same position, but rather to point to the ball.

To this end, you could try to solve a different optimization problem. Aligning the vector $\mathbf{z}$ to point toward $\mathbf{p}$ can be written as:

$$ \max_{\mathbf{q}} \{ \mathbf{z}\left(\mathbf{q}\right) \cdot \left(\mathbf{p}-\mathbf{x}\left(\mathbf{q}\right)\right) \}, $$

where $\mathbf{z}$, $\mathbf{x}$ and $\mathbf{p}$ are all expressed in the world reference frame.

The maximization of the dot product can be easily turned into the following minimization:

$$ \min_{\mathbf{q}} \{ \mathbf{z}\left(\mathbf{q}\right) \cdot \left(\mathbf{x}\left(\mathbf{q}\right)-\mathbf{p}\right) \}, $$

At this point, you can apply standard IK algorithms such as the Jacobian transposed, the pseudoinverse, the damped least-squares, and the like.

Concerning the velocity of the ball $\dot{\mathbf{p}}$, you could recruit the well-known differential law $\dot{\mathbf{p}}=\mathbf{J} \dot{\mathbf{q}}$, invert it and plug into your IK using the null-space policy.

My suggestion is to start off considering that, instead of dealing with $\mathbf{p}$ in the minimization above, you could come up with a better estimate using a look-ahead, which is in its simplest form $\tilde{\mathbf{p}} = \mathbf{p} + \dot{\mathbf{p}} \cdot \Delta t$.

Interestingly, in the past, I implemented something similar for the iCub robot using a nonlinear constrained optimization package called Ipopt. See the relative pull request and the corresponding code.