0
$\begingroup$

I am developing a controller for a Franka Emika with 7 DoF. The cartesian motion is working, but I'd like to add some constraints in the joint movement range, with the values described by the manufacturer. I've read a little but I think I couldn't get the principle so well. I've got a general idea from this question:

Redundancy and Null space projection

But didn't really got how to get a function

$$ \dot{q} = k_0 \frac{\partial w(q)}{\partial q} $$

such that I can project into the Null Space and achieve this secondary task.

Some references to build better foundations and go deeper are also very appreciated.

Thank you!

$\endgroup$

1 Answer 1

2
$\begingroup$

Let's consider the definition of $w(\mathbf{q})$ from the end of the linked post:

$$ w(\mathbf{q}) = - \frac{\alpha}{n} \sum^{n}_{i=1} \left( \frac{q_{i} - \hat{q}_{i}}{\bar{l}_{i} - \underline{l}_{i}} \right)^{2} $$

where some terms have been renamed or changed - for instance $\alpha$ represents an arbitrary but constant weight to be scaled by $n$, $\bar{l}_{i}$ and $\underline{l}_{i}$ are the upper limit and lower limits of joint $i$ respectively, and $\hat{q}_{i} = \frac{\bar{l}_{i} + \underline{l}_{i}}{2}$ is the midpoint between the joint limits. Note that $\mathbf{q}$ represents the current joint configuration as a vector, and $q_{i}$ represents the $i^{\text{th}}$ joint belonging to that vector. Now, let's solve the equation $\dot{\mathbf{q}} = k_{0}\frac{\partial w(\mathbf{q})}{\partial \mathbf{q}}$ entry-wise as follows:

$$ \dot{q}_{i} = - k_{0} \frac{2\alpha}{n} \left( \frac{q_{i} - \hat{q}_{i}}{\bar{l}_{i} - \underline{l}_{i}} \right) $$

There's alot of terms here, so we can use $\alpha$ to remove the $k_{0}$ and $n$ by setting it to some value relative to $k_{0}$ and $n$, such as $\frac{\beta n}{2k_{0}}$. This gives us:

$$ \dot{q}_{i} = - \beta \left( \frac{q_{i} - \hat{q}_{i}}{\bar{l}_{i} - \underline{l}_{i}} \right) $$

Note that the only role $\alpha$ played in the original definition was to make this simplification possible. Because we are looking at a single joint, we can boil down this equation further by defining a $\gamma_{i}$ term to combine $\beta$ and the joint limits together (keeping in mind that applying this process to each joint would use the parameters of that specific joint). With $\gamma_{i} = \left( \frac{\beta}{\bar{l}_{i} - \underline{l}_{i}} \right) \gt 0$, we have:

$$ \dot{\mathbf{q}} = \left[ \begin{array}{c} - \gamma_{1} (q_{1} - \hat{q}_{1}) \\ \vdots \\ - \gamma_{n} (q_{n} - \hat{q}_{n}) \end{array} \right] $$

So, now that we have a formula for $\dot{\mathbf{q}}$, let's analyze what's happening. As we can see, if this unconstrained velocity is applied to the robot, then each joint will move towards the halfway point between its joint limits, $\hat{q}_{i}$. This is because $- \gamma_{i} (q_{i} - \hat{q}_{i})$ is negative when the value of joint $i$ is greater than this midpoint, and $- \gamma_{i} (q_{i} - \hat{q}_{i})$ is positive given the opposite case.

This concept of using a quadratic function as a objective function is very common - especially in gradient-based applications such as controls. Because a quadratic has only one minimum or maximum and the gradient along these functions can be used to iteratively compute this min/max, they make a good choice for simple and effective optimization objectives. That is essentially the idea behind how such a function is defined - if you have a desired position, velocity, acceleration, etc. profile for the motion of your robot, quadratic functions can be used to represent the "cost" or "quality" of the current state of your arm plus a well-defined way to reach the minimum of this "cost" or maximum of this "quality".

So, let's say we have an arbitrary objective function $v(\mathbf{q})$ that is quadratic and has a maximum value that is achieved by setting the joint velocities to $\frac{\partial v(\mathbf{q})}{\partial \mathbf{q}}$ (the same properties as $w(\mathbf{q})$). Note that $\frac{\partial v(\mathbf{q})}{\partial \mathbf{q}} \in \mathbb{R}^{n}$. If we now want to apply these joint velocities in a manner such that the end-effector velocity is not affected, they need to be projected onto the null-space of the end-effector Jacobian. For the following analysis, note that the pseudo-inverse of the Jacobian, $\mathbf{J}^{+}$, conceptually represents a way of solving for the smallest joint velocities that can exactly satisfy the desired end-effector velocity, if that end-effector velocity is achievable. So, given the null-space projection matrix, $\mathbf{I} - \mathbf{J}^{+} \mathbf{J}$, where $\mathbf{I}$ is the $n\times n$ identity matrix, we see that constraining the velocity $\frac{\partial v(\mathbf{q})}{\partial \mathbf{q}}$ to the null-space gives us:

$$ \dot{\mathbf{q}} = (\mathbf{I} - \mathbf{J}^{+} \mathbf{J}) \frac{\partial v(\mathbf{q})}{\partial \mathbf{q}} $$

Noting that:

$$ \dot{\mathbf{x}}_{v} = \mathbf{J} \frac{\partial v(\mathbf{q})}{\partial \mathbf{q}} $$

represents the end-effector velocities created by setting the joint velocities to $\frac{\partial v(\mathbf{q})}{\partial \mathbf{q}}$. Incorporating this term into the null-space projection gives us:

$$ \dot{\mathbf{q}} = \mathbf{I} \frac{\partial v(\mathbf{q})}{\partial \mathbf{q}} - \mathbf{J}^{+} \dot{\mathbf{x}}_{v} = \frac{\partial v(\mathbf{q})}{\partial \mathbf{q}} - \mathbf{J}^{+} \dot{\mathbf{x}}_{v} $$

So, in essence the null-space projection solves for the end-effector velocities that will occur from using an unconstrained joint velocity and then removes the joint velocities that cause this end-effector velocity. Thus, as long as the optimization objective function can be maximized/minimized without moving the end-effector, this method is effective. However, it's very important to note that this method does not guarantee that the objective function will be maximized/minimized, and even worse it cannot guarantee that it will have any effect at all (consider the case where $\frac{\partial v(\mathbf{q})}{\partial \mathbf{q}}$ is orthogonal to the null-space).

To guarantee joint limit avoidance, manually disabling the given joint from moving towards the limit once it has passed some threshold is an option. In addition, I believe model-predictive control (MPC) is another tool people use for this sort of problem, but I'm not terribly familiar with this.

As far as references go, the go-to paper here is titled “On the inverse kinematics of redundant manipulators: Characterization of the self-motion manifolds” by Joel Burdick. It's an older paper, but it lays out the fundamental concepts of redundant robot kinematics about as good as anyone will. Apart from this, there are many, many papers that should go over various techniques for trajectory optimization based on the null-space of the Jacobian.

$\endgroup$
1
  • $\begingroup$ Nice answer! Another very common tool to deal with joint bounds in literature is nonlinear constrained optimization. $\endgroup$ Mar 1, 2023 at 10:08

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.