When developing software for motion planning and control of robots it is common to deal with robots that have some sort of redundancy. This means that the robot has more degrees of freedom than the task at hand. Consider for example the following cases:
- A robotic manipulator with more than 6 joints;
- A robotic finger with agonist-antagonist tendons;
- Cable-driven systems.
Common tasks with these types of robots are:
- Solving the inverse kinematics (IK) problem, i.e., for a given target pose of the end-effector, what set of joint angles with achieve that pose?
$$ \dot{p} = J \dot{q} \tag{1} $$
The IK is usually solved iteratively, by repeatedly solving \(\dot{q} = J^{\#} \dot{p}\) where \(J^{\#} \) is the Moore-Penrose pseudoinverse. - Forward dynamics and simulation: what end-effector forces will the given joint torques and accelerations result to? This is found by inversion of the dynamic equation of motion. In static conditions it simplifies to:
$$ \tau = J^T w \tag{2} $$
This is usually inverted by means of Quadratic Constrained Optimization. In the case of static equilibrium, the solution of the ID problem can be simplified to solving the following equation: \(w = {J^{\#}}^T \tau \)
Working on the control of cable-driven systems or tendon-driven robotic hands (such as agonist-antagonist fingers) often means working on the following problems:
- Given a target joint torque, find the tendon/cable forces that will achieve those values:
$$ \tau = R \mathbf{f} \tag{4}$$
where \(R \in \mathcal{R}^{M \times N}\) is the Moment Arm matrix mapping tendon forces \(\mathbf{f}\) to joint torques \(\mathbf{\tau}\) (depending on the radius of the pulleys). - Find the extensions of the tendons/cables of a robotic finger given the desired joint speed:
$$ \dot{s} = R^T \dot{q} \tag{3} $$
Because of the agonist-antagonist design a tendon-driven fingers, the Moment Arm matrix R has always got more columns than rows, making Eq. 3 underconstrained and Eq. 4 overconstrained.
| Problem name | Inverse kinematics: find the joint velocity given the end-effector velocity | Find the end-effector wrench given the joint torques and accelerations | Find the tendon/cable forces given the joint torques | Finger’s inverse kinematics: find a feasible tendon/cable speed given the desired joint velocity |
| Formulation | given \( \dot{p} = J(q) \dot{q}\), find \(\dot{q}\): such that: \(\underline{\dot{q}} \leq \dot{q} \leq \bar{\dot{q}}\) \(C \dot{p} \leq d\) | given \(\tau = J(q)^T w\), find \(w\): such that: \(\underline{\tau} \leq \tau \leq \bar{\tau}\) \(C w \leq d\) | given \(\tau = R f\), find \(f\): such that: \(\underline{\tau} \leq \tau \leq \bar{\tau}\) \(\underline{f} \leq f \leq \bar{f}\) | given \(\dot{s} = R^T \dot{q}\), find \(\dot{q}\): such that: \(\underline{\dot{s}} \leq \dot{s} \leq \bar{\dot{s}}\) \(\underline{\dot{q}} \leq \dot{q} \leq \bar{\dot{q}}\) |
| Type | Underdetermined | Overdetermined | Underdetermined | Overdetermined |
| Number of solutions | Infinite | 1 or none | Infinite | 1 or none |
All the problems above can be defined as a quadratic constrained optimisation problem where the goal is to find \(x^*\) such that:
$$ \\ A x = b \\ C x \leq d \tag{5} $$
For example:
- \(\dot{p} = J \dot{q}\) (inverse kinematics problem) can be written as \(A x = b\) where \(A = J, b = \dot{p}\) and \(x = \dot{q}\).
- \(\tau = J^T w\) (forward dynamics problem) can be written as \(A x = b\) where \(A = J^T, b = \tau\) and \(x = w\).
Proposed algorithm:
