Inverse kinematics is an active research domain in robotics since several years due to its importance in several robotics application. Among the various approaches, differential inverse kinematics is widely used due to the possibility to real-time implementation. Redundant robotic systems exhibit more degrees of freedom than those strictly required to execute a given end-effector task, in such a case, multiple tasks can be handled simultaneously in, e.g., a task-priority architecture. This paper addresses the systematic extension of the multiple tasks singularity robust solution, also known as Null-space Based Behavioral control, to the case of set-based control tasks, i.e., tasks for which a range, rather than a specific value, is assigned. This is the case for several tasks such as, for example, mechanical joint limits of robotic arm as well as obstacle avoidance for any kind of robots. Numerical validation are provided to support the solution proposed.

1. Incorporating set-based control within the
singularity-robust multiple task-priority
inverse kinematics
Gianluca Antonelli†, Signe Moe, Kristin Pettersen
†University of Cassino and Southern Lazio, Italy
http://webuser.unicas.it/lai/robotica
⊕Norwegian University of Science and Technology, Norway
http://www.ntnu.edu/amos
IROS 2014 Workshop: Real-time Motion Generation Control
Antonelli, Moe, Pettersen Chicago, 18 September 2014

2. Talk’s overview
A minimalistic review of Inverse Kinematics
Problem description
The proposed algorithm
Example
Open issues
Antonelli, Moe, Pettersen Chicago, 18 September 2014

3. A minimalistic review of Inverse Kinematics
Given a n-dimensional robotic system and
a generic m-dimensional task (e.g., the e.e. position)
= f(q) 2 Rm ˙ = J(q)q˙
An inverse mapping is required
Redundancy (n m) may be used to handle additional tasks
✛
✚
✘
✙
q˙
✛
❘
✚
✘
✙
˙
Antonelli, Moe, Pettersen Chicago, 18 September 2014

4. A minimalistic review of Inverse Kinematics
Given a n-dimensional robotic system and
a generic m-dimensional task (e.g., the e.e. position)
= f(q) 2 Rm ˙ = J(q)q˙
An inverse mapping is required
Redundancy (n m) may be used to handle additional tasks
✛
✚
✘
✗✔
✙
q˙
✛
❘
✚
✘
✙
˙
✖✕
■
Antonelli, Moe, Pettersen Chicago, 18 September 2014

5. A minimalistic review of Inverse Kinematics
Given a n-dimensional robotic system and
a generic m-dimensional task (e.g., the e.e. position)
= f(q) 2 Rm ˙ = J(q)q˙
An inverse mapping is required
Redundancy (n m) may be used to handle additional tasks
✛
✗✔
✚
✘
✗✔
✙
q˙
✛
❘
✚
✘
✙
✖✕
■
˙
a
✛✘
✚✙
˙ b
✙
✖✕
✶
Antonelli, Moe, Pettersen Chicago, 18 September 2014

6. A minimalistic review of Inverse Kinematics
˙ = Jq˙ inverted by solving proper optimization problems
pseudoinverse [Whitney(1969)]
q˙ = J†˙ = JT JJT−1
˙
null-space, gradient-based optimization [Li´egeois(1977)]
q˙ = J†˙ +N
@h
@q
task priority [Maciejewski and Klein(1985)],
[Nakamura et al.(1987)Nakamura, Hanafusa, and Yoshikawa]
a˙ a + (JbNa)† ˙ b − JbJ†
q˙ = J†
a˙ a
singularity-robust task priority [Chiaverini(1997)]
q˙ = J†
a˙ a +NaJ†
b ˙ b
Antonelli, Moe, Pettersen Chicago, 18 September 2014

7. A minimalistic review of Inverse Kinematics
˙ = Jq˙ inverted by solving proper optimization problems
pseudoinverse [Whitney(1969)]
q˙ = J†˙ = JT JJT−1
˙
null-space, gradient-based optimization [Li´egeois(1977)]
q˙ = J†˙ +N
@h
@q
task priority [Maciejewski and Klein(1985)],
[Nakamura et al.(1987)Nakamura, Hanafusa, and Yoshikawa]
a˙ a + (JbNa)† ˙ b − JbJ†
q˙ = J†
a˙ a
singularity-robust task priority [Chiaverini(1997)]
q˙ = J†
a˙ a +NaJ†
b ˙ b
Antonelli, Moe, Pettersen Chicago, 18 September 2014

8. A minimalistic review of Inverse Kinematics
˙ = Jq˙ inverted by solving proper optimization problems
pseudoinverse [Whitney(1969)]
q˙ = J†˙ = JT JJT−1
˙
null-space, gradient-based optimization [Li´egeois(1977)]
q˙ = J†˙ +N
@h
@q
task priority [Maciejewski and Klein(1985)],
[Nakamura et al.(1987)Nakamura, Hanafusa, and Yoshikawa]
a˙ a + (JbNa)† ˙ b − JbJ†
q˙ = J†
a˙ a
singularity-robust task priority [Chiaverini(1997)]
q˙ = J†
a˙ a +NaJ†
b ˙ b
Antonelli, Moe, Pettersen Chicago, 18 September 2014

9. A minimalistic review of Inverse Kinematics
˙ = Jq˙ inverted by solving proper optimization problems
pseudoinverse [Whitney(1969)]
q˙ = J†˙ = JT JJT−1
˙
null-space, gradient-based optimization [Li´egeois(1977)]
q˙ = J†˙ +N
@h
@q
task priority [Maciejewski and Klein(1985)],
[Nakamura et al.(1987)Nakamura, Hanafusa, and Yoshikawa]
a˙ a + (JbNa)† ˙ b − JbJ†
q˙ = J†
a˙ a
singularity-robust task priority [Chiaverini(1997)]
q˙ = J†
a˙ a +NaJ†
b ˙ b
Antonelli, Moe, Pettersen Chicago, 18 September 2014

10. A minimalistic review of Inverse Kinematics
multiple task priority [Siciliano and Slotine(1991)]
multiple singularity-robust task priority
[Mansard and Chaumette(2007)],
[Antonelli et al.(2008)Antonelli, Arrichiello, and Chiaverini], [Antonelli(2009)]
set-based multiple task priority
[Kanoun et al.(2011)Kanoun, Lamiraux, and Wieber],
[Escande et al.(2013)Escande, Mansard, and Wieber],
[Simetti et al.(2013)Simetti, Casalino, Torelli, Sperind´e, and Turetta]
set-based multiple singularity-robust task priority
this work?
Antonelli, Moe, Pettersen Chicago, 18 September 2014

11. A minimalistic review of Inverse Kinematics
multiple task priority [Siciliano and Slotine(1991)]
multiple singularity-robust task priority
[Mansard and Chaumette(2007)],
[Antonelli et al.(2008)Antonelli, Arrichiello, and Chiaverini], [Antonelli(2009)]
set-based multiple task priority
[Kanoun et al.(2011)Kanoun, Lamiraux, and Wieber],
[Escande et al.(2013)Escande, Mansard, and Wieber],
[Simetti et al.(2013)Simetti, Casalino, Torelli, Sperind´e, and Turetta]
set-based multiple singularity-robust task priority
this work?
Antonelli, Moe, Pettersen Chicago, 18 September 2014

12. A minimalistic review of Inverse Kinematics
multiple task priority [Siciliano and Slotine(1991)]
multiple singularity-robust task priority
[Mansard and Chaumette(2007)],
[Antonelli et al.(2008)Antonelli, Arrichiello, and Chiaverini], [Antonelli(2009)]
set-based multiple task priority
[Kanoun et al.(2011)Kanoun, Lamiraux, and Wieber],
[Escande et al.(2013)Escande, Mansard, and Wieber],
[Simetti et al.(2013)Simetti, Casalino, Torelli, Sperind´e, and Turetta]
set-based multiple singularity-robust task priority
this work?
Antonelli, Moe, Pettersen Chicago, 18 September 2014

13. A minimalistic review of Inverse Kinematics
multiple task priority [Siciliano and Slotine(1991)]
multiple singularity-robust task priority
[Mansard and Chaumette(2007)],
[Antonelli et al.(2008)Antonelli, Arrichiello, and Chiaverini], [Antonelli(2009)]
set-based multiple task priority
[Kanoun et al.(2011)Kanoun, Lamiraux, and Wieber],
[Escande et al.(2013)Escande, Mansard, and Wieber],
[Simetti et al.(2013)Simetti, Casalino, Torelli, Sperind´e, and Turetta]
set-based multiple singularity-robust task priority
this work?
Antonelli, Moe, Pettersen Chicago, 18 September 2014

14. Problem description
Example of an upper bound set-based control
j(t) max,j
in the bearing (orange) area the
task is active, the inequality arises
0 ˙ j(t) = Jj(q(t))q˙ (t)
satisfied violated
inactive active
max
S
This inequality should be considered together with remaining
set­based
and equality tasks
∗the term active is not to be intended as in the active set method for
optimization problems of inequality constraints
Antonelli, Moe, Pettersen Chicago, 18 September 2014

15. Problem description
Example of an upper bound set-based control
j(t) max,j
in the bearing (orange) area the
task is active, the inequality arises
0 ˙ j(t) = Jj(q(t))q˙ (t)
satisfied violated
inactive active
max
S
This inequality should be considered together with remaining
set­based
and equality tasks
∗the term active is not to be intended as in the active set method for
optimization problems of inequality constraints
Antonelli, Moe, Pettersen Chicago, 18 September 2014

16. Problem description
Example of an upper bound set-based control
j(t) max,j
in the bearing (orange) area the
task is active, the inequality arises
0 ˙ j(t) = Jj(q(t))q˙ (t)
satisfied violated
inactive active
max
S
This inequality should be considered together with remaining
set­based
and equality tasks
∗the term active is not to be intended as in the active set method for
optimization problems of inequality constraints
Antonelli, Moe, Pettersen Chicago, 18 September 2014

17. Set-based control
Several tasks need to be controlled in a range rather
than an exact value
mechanical joint limits
obstacle avoidance
directional sensors (e.g. camera)
manipulability
. . .
priority (hierarchy) also among set­based
tasks
Antonelli, Moe, Pettersen Chicago, 18 September 2014

18. Set-based control
Several tasks need to be controlled in a range rather
than an exact value
mechanical joint limits critical
obstacle avoidance critical
directional sensors (e.g. camera) not critical
manipulability not critical
. . .
priority (hierarchy) also among set­based
tasks
Antonelli, Moe, Pettersen Chicago, 18 September 2014

19. Set-based control
Several tasks need to be controlled in a range rather
than an exact value
mechanical joint limits
obstacle avoidance
directional sensors (e.g. camera)
manipulability
. . .
priority (hierarchy) also among set­based
tasks
Antonelli, Moe, Pettersen Chicago, 18 September 2014

20. The underlying idea
Example:
Joint 2 in its bearing area
Its inequality constraint should be taken into account
However, a-posteriori, it might arise that the overall desired
velocity satisfies the constraint simply ignoring it
(the other tasks pushes q2 away from its limit)
A-priori it is not possible to forecast which task might provide this
velocity (i.e., higher or lower priority, set-based or equality)
The solution is to compute all the 2na possible solutions† by
considering or ignoring the active set-based tasks
†na is the # of active tasks
Antonelli, Moe, Pettersen Chicago, 18 September 2014

21. The underlying idea
Example:
Joint 2 in its bearing area
Its inequality constraint should be taken into account
However, a-posteriori, it might arise that the overall desired
velocity satisfies the constraint simply ignoring it
(the other tasks pushes q2 away from its limit)
A-priori it is not possible to forecast which task might provide this
velocity (i.e., higher or lower priority, set-based or equality)
The solution is to compute all the 2na possible solutions† by
considering or ignoring the active set-based tasks
†na is the # of active tasks
Antonelli, Moe, Pettersen Chicago, 18 September 2014

22. The underlying idea
Example:
Joint 2 in its bearing area
Its inequality constraint should be taken into account
However, a-posteriori, it might arise that the overall desired
velocity satisfies the constraint simply ignoring it
(the other tasks pushes q2 away from its limit)
A-priori it is not possible to forecast which task might provide this
velocity (i.e., higher or lower priority, set-based or equality)
The solution is to compute all the 2na possible solutions† by
considering or ignoring the active set-based tasks
†na is the # of active tasks
Antonelli, Moe, Pettersen Chicago, 18 September 2014

23. The underlying idea
Example:
Joint 2 in its bearing area
Its inequality constraint should be taken into account
However, a-posteriori, it might arise that the overall desired
velocity satisfies the constraint simply ignoring it
(the other tasks pushes q2 away from its limit)
A-priori it is not possible to forecast which task might provide this
velocity (i.e., higher or lower priority, set-based or equality)
The solution is to compute all the 2na possible solutions† by
considering or ignoring the active set-based tasks
†na is the # of active tasks
Antonelli, Moe, Pettersen Chicago, 18 September 2014

24. The underlying idea
Example:
Joint 2 in its bearing area
Its inequality constraint should be taken into account
However, a-posteriori, it might arise that the overall desired
velocity satisfies the constraint simply ignoring it
(the other tasks pushes q2 away from its limit)
A-priori it is not possible to forecast which task might provide this
velocity (i.e., higher or lower priority, set-based or equality)
The solution is to compute all the 2na possible solutions† by
considering or ignoring the active set-based tasks
†na is the # of active tasks
Antonelli, Moe, Pettersen Chicago, 18 September 2014

25. An example
5 tasks in priority, mixed equality and set-based
na = 2 set-based are active
Antonelli, Moe, Pettersen Chicago, 18 September 2014

26. An example
5 tasks in priority, mixed equality and set-based
na = 2 set-based are active
★
✧
✥
decreasing priority
✦❄
Antonelli, Moe, Pettersen Chicago, 18 September 2014

27. An example
5 tasks in priority, mixed equality and set-based
na = 2 set-based are active
✓✏
each computed as if it were alone q˙ x = J†
✒✑
✓✏
✒✑
✓✏
✒✑
✓✏
✒✑
✓✏
✒✑
x˙ x,des
Antonelli, Moe, Pettersen Chicago, 18 September 2014

28. An example
5 tasks in priority, mixed equality and set-based
na = 2 set-based are active
★✥
✧✦
first step: the sole primary, equality task
(null space for next step is computed)
Antonelli, Moe, Pettersen Chicago, 18 September 2014

29. An example
5 tasks in priority, mixed equality and set-based
na = 2 set-based are active
★✥
✧✦
★✥
✧✦
second step: we build two solutions
by ignoring (left) or handling (right) the set­based
task
Antonelli, Moe, Pettersen Chicago, 18 September 2014

30. An example
5 tasks in priority, mixed equality and set-based
na = 2 set-based are active
★✥
✧✦
★✥
✧✦
equality tasks are handled in the usual way
Antonelli, Moe, Pettersen Chicago, 18 September 2014

31. An example
5 tasks in priority, mixed equality and set-based
na = 2 set-based are active
★✥
✧✦
★✥
✧✦
★✥
✧✦
★✥
✧✦
each active set­based
task doubles the possible solutions
Antonelli, Moe, Pettersen Chicago, 18 September 2014

32. An example
5 tasks in priority, mixed equality and set-based
na = 2 set-based are active
✬
✫
✩
✪
in the end: 2na possibilities from which one needs to be selected
Antonelli, Moe, Pettersen Chicago, 18 September 2014

33. A more formal description
1: Initialize Q with one null n × 1 vector
2: Initialize N with one Identity matrix
3: for i = 1 to p do
4: Compute Jx
5: Compute q˙ x,des according to eq. ˙ qx,des = J†˙ x,des + e = J† ˙ ref
6: if i==set-based then
7: Qtemp = Q
8: Ntemp = N
9: end if
10: Update Q summing to each the vector q˙ x,des pre-multiplied by the
corresponding null space projectors in N
11: Update N properly including, in each, the contribution of Jx
12: if i==set-based then
13: Q = QSQtemp
14: N = N SNtemp
15: end if
16: end for
17: return Q {it contains 2nset,a elements}
Antonelli, Moe, Pettersen Chicago, 18 September 2014

34. Selection of the output
The algorithm builds 2na solutions
The output, sent to the low level controller, is selected
1 Prune the velocities that violate highest priority set-based tasks
2 Select the largest-norm velocity
Antonelli, Moe, Pettersen Chicago, 18 September 2014

35. Selection of the output
The algorithm builds 2na solutions
The output, sent to the low level controller, is selected
1 Prune the velocities that violate highest priority set-based tasks
2 Select the largest-norm velocity
We are ignoring each set­based
task in half of the solutions
Antonelli, Moe, Pettersen Chicago, 18 September 2014

36. Selection of the output
The algorithm builds 2na solutions
The output, sent to the low level controller, is selected
1 Prune the velocities that violate highest priority set-based tasks
2 Select the largest-norm velocity
Rational: each null­space
projection filter out some velocity
component, the less conservative should be the largest­norm
Antonelli, Moe, Pettersen Chicago, 18 September 2014

37. Numerical example: 4-link planar
pr. task type min max
1 mech. joint 1 set-based −90◦ 90◦
2 mech. joint 2 set-based −100◦ 100◦
3 mech. joint 3 set-based −90◦ 90◦
4 mech. joint 4 set-based −120◦ 120◦
5 e.e. obst. av. set-based 0.3m –
6 e.e. position equality – –
7 e.e. orientation set-based −45◦ 45◦
mech 1
mech 2
mech 3
mech 4
ee obs
ee pos
0 5 10 15 20
or.
time [s]
150
100
50
0
−50
−100
−150
0 5 10 15 20
time [s]
a
Antonelli, Moe, Pettersen Chicago, 18 September 2014

38. To do
Guarantee continuity among samples (task de-activation)
Computational analysis
Most of the computations shared among the possible solutions
Recursive formula for null spaces available
Nab...y = Nab...x hI − JyNab...x† JyNab...xi
Comparison with alternative algorithms on-going
Stability analysis
Antonelli, Moe, Pettersen Chicago, 18 September 2014

39. Bibliography I
G. Antonelli.
Stability analysis for prioritized closed-loop inverse kinematic algorithms for
redundant robotic systems.
IEEE Transactions on Robotics, 25(5):985–994, October 2009.
G. Antonelli, F. Arrichiello, and S. Chiaverini.
The Null-Space-based Behavioral control for autonomous robotic systems.
Journal of Intelligent Service Robotics, 1(1):27–39, Jan. 2008.
S. Chiaverini.
Singularity-robust task-priority redundancy resolution for real-time kinematic
control of robot manipulators.
IEEE Transactions on Robotics and Automation, 13(3):398–410, 1997.
A. Escande, N. Mansard, and P.-B. Wieber.
Hierarchical quadratic programming: Fast online humanoid-robot motion
generation.
International Journal of Robotics Research, 2013.
Antonelli, Moe, Pettersen Chicago, 18 September 2014

40. Bibliography II
O. Kanoun, F. Lamiraux, and P.B. Wieber.
Kinematic control of redundant manipulators: Generalizing the task-priority
framework to inequality task.
Robotics, IEEE Transactions on, 27(4):785–792, 2011.
A. Li´egeois.
Automatic supervisory control of the configuration and behavior of muldibody
mechanisms.
IEEE Transactions on Systems, Man and Cybernetics, 7:868–871, 1977.
A.A. Maciejewski and C.A. Klein.
Obstacle avoidance for kinematically redundant manipulators in dynamically
varying environments.
The International Journal of Robotics Research, 4(3):109–117, 1985.
N. Mansard and F. Chaumette.
Task sequencing for high-level sensor-based control.
IEEE Transactions on Robotics and Automation, 23(1):60–72, 2007.
Antonelli, Moe, Pettersen Chicago, 18 September 2014

41. Bibliography III
Y. Nakamura, H. Hanafusa, and T. Yoshikawa.
Task-priority based redundancy control of robot manipulators.
The International Journal Robotics Research, 6(2):3–15, 1987.
B. Siciliano and J.-J.E. Slotine.
A general framework for managing multiple tasks in highly redundant robotic
systems.
In Proceedings 5th International Conference on Advanced Robotics, pages
1211–1216, Pisa, I, 1991.
E. Simetti, G. Casalino, S. Torelli, A. Sperind´e, and A. Turetta.
Floating underwater manipulation: Developed control methodology and
experimental validation within the TRIDENT project.
Journal of Field Robotics, 31(3):364–385, 2013.
D.E. Whitney.
Resolved motion rate control of manipulators and human prostheses.
IEEE Transactions on Man-Machine Systems, 10(2):47–52, 1969.
Antonelli, Moe, Pettersen Chicago, 18 September 2014

42. Incorporating set-based control within the
singularity-robust multiple task-priority
inverse kinematics
Gianluca Antonelli†, Signe Moe, Kristin Pettersen
†University of Cassino and Southern Lazio, Italy
http://webuser.unicas.it/lai/robotica
⊕Norwegian University of Science and Technology, Norway
http://www.ntnu.edu/amos
IROS 2014 Workshop: Real-time Motion Generation Control
Antonelli, Moe, Pettersen Chicago, 18 September 2014