# Inverse Kinematics problem formulation (optimization)

Note:Q is a diagonal posituve semi-definite matrix

Why is the IK formulation like that? It looks like it's trying to find the optimum dq/dt that will minimize the magnitude dq/dt. How is that giving IK solution? and isn't the optimum value be dq/dt=0?

EDIT:

• Can you post more pics of the text following the question to maybe provide clearer context of the problem? – Petch Puttichai Nov 11 '17 at 5:16
• I've added picture – mon Nov 12 '17 at 7:06
• I have edited my answer. You can see if that helps. – Petch Puttichai Nov 12 '17 at 9:58

The formulation is typical for redundant robots, in which there are an infinite number of joint velocity vectors that could satisfy the $\dot{r}_{t}$ goal. In the version you cite, the $Q$ matrix would allow you to weight the different joint velocities in order to create an optimal solution that matches that $Q$. Other formulations of this approach specifically call out the null space of the manipulator Jacobian to be used for satisfying the optimization criteria.

Also, that last equation is inverted to solve for joint velocities, which makes this an approach to inverse kinematics. With nonsquare Jacobians, pseudoinverses are used for this.

• It is worth mentioning that you would use a weighted pseudo inverse, with $Q$ as weight. I believe that this would require that the rank of $Q$ is at least equal to the dimension of $\dot{r}_t$. – fibonatic Nov 10 '17 at 5:29
• but could you answer why it's trying to minimize q dot? – mon Nov 10 '17 at 7:43
• It is not trying to minimize $\dot{q}$. It is minimizing $\dot{q}^TQ \dot{q}$, subject to the constraint that the joint velocities must achieve the end effector velocities, as mapped by $J$. – SteveO Nov 10 '17 at 7:47
• Think of it this way: If $Q$ is the identity matrix, then this algorithm will minimize the least-squared norm of all of the joint velocities while still achieving the end effector velocities. However, if you want to further reduce the velocities of the larger motors, maybe you could make the diagonal elements of $Q$ something like (10, 10, 5, 3, 1, 1), to try to make the smaller (less encumbered) motors move more to achieve the desired trajectory. – SteveO Nov 10 '17 at 7:52
• yes, I understood it's tring to minimize qdot with different weighting for each joint while obeying relation of J qdot=rdot. I just said it roughly, why it wants to minimize qdot? – mon Nov 11 '17 at 5:18

Now that I have investigated the paper in question, here is my new answer.

It turns out that the paper actually discusses task-based motion generation. That is, given a task (end-effector velocity trajectory) $\dot{r}(t)$, find a robot input (joint velocity trajectory) $\dot{q}(t)$ that realizes the task. The formulated optimization problem is happening at a time instant on the trajectory.

So if the Jacobian is square and full-rank, you wouldn't have to worry about this optimization problem since there can be only one $\dot{q}$ corresponding to a given $\dot{r}$. However, when the robot is redundant, there can be infinitely many $\dot{q}$ satisfying $J\dot{q} = \dot{r}$. Now that you have choices, you can do fancier things.

In this case, the minimization objective is $\Vert \dot{q}^TQ\dot{q} \Vert$ to minimize the magnitude of the joint velocity. The reason is simple: one wouldn't want the joint velocity to just go unreasonably high.

Why is the IK formulation like that?

I think it is just that this formulation is mentioned as an example that leads to the main content of the paper. IK problems don't have to be formulated this way. There are other formulations out there.

How is that giving IK solution?

$\dot{r}$ implicitly specifies the next end-effector pose. A solution $\dot{q}$ implicitly specifies the next IK solution to reach the specified pose.

isn't the optimum value be dq/dt=0?

Without the equality constraint, yes. However, there is this inequality constraint. So $\dot{q}$ can be non-zero.

The formulation is general, i.e., it can be used with any types of robots.

I think it may be easier to consider the discrete-time analogy of the problem.

Given an initial point $q$ (i.e., a robot configuration), this optimization problem is actually trying to pull $q$ towards some point $q^\ast$ that is an IK solution to your problem. What is effectively steering the point $q$ towards a solution is the difference $\Delta r$ between your desired end-effector position and your current end-effector position. However, what you control is actually the joint values. They are related by

$$J\Delta q = \Delta r.$$

You can then compute the joint velocity that satisfies the above equation (given the current $\Delta r$). And since $\Delta q$ is essentially the difference of joint values between consecutive time steps, $\Delta q = q[t + \Delta t] - q[t]$, you will obtain a joint value for the next time step.

That's why this is the constraint that is optimization problem is subject to.

At the end, the system should stop moving when it reaches an IK solution. This means $\Delta q$ should be zero. That's why the minimization objective is to minimize a norm of $\dot{q}$.

I think the positive semi-definite matrix $Q$ in the objective function will control how each of the joint values converges to an IK solution. For example, if $Q$ is an identity matrix, it means you place equal importance to each DOF (that is, you do not have any bias in favor of some DOFs to converge faster than the others).

• I don’t think the system should stop moving. Rather, the system should meet the end effector velocities with the minimum value of the optimization criterion. This is true whether in continuous or discrete time scenarios. – SteveO Nov 10 '17 at 4:20
• By the way, I like your description of steering the end effector toward its target. – SteveO Nov 10 '17 at 4:54
• @SteveO Well, I think ideally the system should stop moving once an IK solution is found, i.e., a local minimum is reached, since at such a solution, $\dot{r} = 0$. Of course, for a redundant robot, there exists some non-zero $\dot{q}$ that satisfies the relation as well (null space-related issue that you discuss in your answer). But anyway, the norm will be minimized when $\dot{q} = 0$. – Petch Puttichai Nov 10 '17 at 8:57
• Thanks for the description. However, I still don’t understand. When it reaches the IK solution, the end effector should be moving at $\dot{r}$, right? – SteveO Nov 11 '17 at 0:45
• Why should the end-effector still be moving when it reaches an IK solution? – Petch Puttichai Nov 11 '17 at 1:54