Embed Size (px)
DESCRIPTION
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.
Citation preview
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, Italyhttp://webuser.unicas.it/lai/robotica
⊕Norwegian University of Science and Technology, Norwayhttp://www.ntnu.edu/amos
IROS 2014 Workshop: Real-time Motion Generation & Control
Antonelli, Moe, Pettersen Chicago, 18 September 2014
Talk’s overview
A minimalistic review of Inverse Kinematics
Problem description
The proposed algorithm
Example
Open issues
Antonelli, Moe, Pettersen Chicago, 18 September 2014
A minimalistic review of Inverse Kinematics
Given a n-dimensional robotic system anda generic m-dimensional task (e.g., the e.e. position)
σ = f(q) ∈ 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
A minimalistic review of Inverse Kinematics
Given a n-dimensional robotic system anda generic m-dimensional task (e.g., the e.e. position)
σ = f(q) ∈ 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
A minimalistic review of Inverse Kinematics
Given a n-dimensional robotic system anda generic m-dimensional task (e.g., the e.e. position)
σ = f(q) ∈ 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
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 [Liegeois(1977)]
q = J†σ +N∂h
∂q
task priority [Maciejewski and Klein(1985)],
[Nakamura et al.(1987)Nakamura, Hanafusa, and Yoshikawa]
q = J†aσa + (JbNa)
†(
σb − JbJ†aσa
)
singularity-robust task priority [Chiaverini(1997)]
q = J†aσa +NaJ
†
bσb
Antonelli, Moe, Pettersen Chicago, 18 September 2014
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 [Liegeois(1977)]
q = J†σ +N∂h
∂q
task priority [Maciejewski and Klein(1985)],
[Nakamura et al.(1987)Nakamura, Hanafusa, and Yoshikawa]
q = J†aσa + (JbNa)
†(
σb − JbJ†aσa
)
singularity-robust task priority [Chiaverini(1997)]
q = J†aσa +NaJ
†
bσb
Antonelli, Moe, Pettersen Chicago, 18 September 2014
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 [Liegeois(1977)]
q = J†σ +N∂h
∂q
task priority [Maciejewski and Klein(1985)],
[Nakamura et al.(1987)Nakamura, Hanafusa, and Yoshikawa]
q = J†aσa + (JbNa)
†(
σb − JbJ†aσa
)
singularity-robust task priority [Chiaverini(1997)]
q = J†aσa +NaJ
†
bσb
Antonelli, Moe, Pettersen Chicago, 18 September 2014
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 [Liegeois(1977)]
q = J†σ +N∂h
∂q
task priority [Maciejewski and Klein(1985)],
[Nakamura et al.(1987)Nakamura, Hanafusa, and Yoshikawa]
q = J†aσa + (JbNa)
†(
σb − JbJ†aσa
)
singularity-robust task priority [Chiaverini(1997)]
q = J†aσa +NaJ
†
bσb
Antonelli, Moe, Pettersen Chicago, 18 September 2014
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, Sperinde, and Turetta]
set-based multiple singularity-robust task prioritythis work?
Antonelli, Moe, Pettersen Chicago, 18 September 2014
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, Sperinde, and Turetta]
set-based multiple singularity-robust task prioritythis work?
Antonelli, Moe, Pettersen Chicago, 18 September 2014
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, Sperinde, and Turetta]
set-based multiple singularity-robust task prioritythis work?
Antonelli, Moe, Pettersen Chicago, 18 September 2014
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, Sperinde, and Turetta]
set-based multiple singularity-robust task prioritythis work?
Antonelli, Moe, Pettersen Chicago, 18 September 2014
Problem description
Example of an upper bound set-based control ∗
σj(t) ≤ σmax,j
in the bearing (orange) area thetask is active, the inequality arises
0 ≥ σj(t) = J j(q(t))q(t)
satisfied violated
inactive active
S
σε σmax
This inequality should be considered together with remaining
setbased and equality tasks
∗the term active is not to be intended as in the active set method foroptimization problems of inequality constraints
Antonelli, Moe, Pettersen Chicago, 18 September 2014
Problem description
Example of an upper bound set-based control ∗
σj(t) ≤ σmax,j
in the bearing (orange) area thetask is active, the inequality arises
0 ≥ σj(t) = J j(q(t))q(t)
satisfied violated
inactive active
S
σε σmax
This inequality should be considered together with remaining
setbased and equality tasks
∗the term active is not to be intended as in the active set method foroptimization problems of inequality constraints
Antonelli, Moe, Pettersen Chicago, 18 September 2014
Problem description
Example of an upper bound set-based control ∗
σj(t) ≤ σmax,j
in the bearing (orange) area thetask is active, the inequality arises
0 ≥ σj(t) = J j(q(t))q(t)
satisfied violated
inactive active
S
σε σmax
This inequality should be considered together with remaining
setbased and equality tasks
∗the term active is not to be intended as in the active set method foroptimization problems of inequality constraints
Antonelli, Moe, Pettersen Chicago, 18 September 2014
Set-based control
Several tasks need to be controlled in a range ratherthan an exact value
mechanical joint limits
obstacle avoidance
directional sensors (e.g. camera)
manipulability
. . .
priority (hierarchy) also among setbased tasks
Antonelli, Moe, Pettersen Chicago, 18 September 2014
Set-based control
Several tasks need to be controlled in a range ratherthan an exact value
mechanical joint limits critical
obstacle avoidance critical
directional sensors (e.g. camera) not critical
manipulability not critical
. . .
priority (hierarchy) also among setbased tasks
Antonelli, Moe, Pettersen Chicago, 18 September 2014
Set-based control
Several tasks need to be controlled in a range ratherthan an exact value
mechanical joint limits
obstacle avoidance
directional sensors (e.g. camera)
manipulability
. . .
priority (hierarchy) also among setbased tasks
Antonelli, Moe, Pettersen Chicago, 18 September 2014
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 desiredvelocity 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 thisvelocity (i.e., higher or lower priority, set-based or equality)
The solution is to compute all the 2na possible solutions† byconsidering or ignoring the active set-based tasks
†na is the # of active tasksAntonelli, Moe, Pettersen Chicago, 18 September 2014
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 desiredvelocity 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 thisvelocity (i.e., higher or lower priority, set-based or equality)
The solution is to compute all the 2na possible solutions† byconsidering or ignoring the active set-based tasks
†na is the # of active tasksAntonelli, Moe, Pettersen Chicago, 18 September 2014
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 desiredvelocity 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 thisvelocity (i.e., higher or lower priority, set-based or equality)
The solution is to compute all the 2na possible solutions† byconsidering or ignoring the active set-based tasks
†na is the # of active tasksAntonelli, Moe, Pettersen Chicago, 18 September 2014
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 desiredvelocity 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 thisvelocity (i.e., higher or lower priority, set-based or equality)
The solution is to compute all the 2na possible solutions† byconsidering or ignoring the active set-based tasks
†na is the # of active tasksAntonelli, Moe, Pettersen Chicago, 18 September 2014
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 desiredvelocity 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 thisvelocity (i.e., higher or lower priority, set-based or equality)
The solution is to compute all the 2na possible solutions† byconsidering or ignoring the active set-based tasks
†na is the # of active tasksAntonelli, Moe, Pettersen Chicago, 18 September 2014
An example
5 tasks in priority, mixed equality and set-basedna = 2 set-based are active
Antonelli, Moe, Pettersen Chicago, 18 September 2014
An example
5 tasks in priority, mixed equality and set-basedna = 2 set-based are active★
✧
✥
✦❄
decreasing priority
Antonelli, Moe, Pettersen Chicago, 18 September 2014
An example
5 tasks in priority, mixed equality and set-basedna = 2 set-based are active
✒✑✓✏✒✑✓✏✒✑✓✏✒✑✓✏✒✑✓✏
each computed as if it were alone qx = J†xσx,des
Antonelli, Moe, Pettersen Chicago, 18 September 2014
An example
5 tasks in priority, mixed equality and set-basedna = 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
An example
5 tasks in priority, mixed equality and set-basedna = 2 set-based are active
✧✦★✥
✧✦★✥
second step: we build two solutions
by ignoring (left) or handling (right) the setbased task
Antonelli, Moe, Pettersen Chicago, 18 September 2014
An example
5 tasks in priority, mixed equality and set-basedna = 2 set-based are active
✧✦★✥
✧✦★✥
equality tasks are handled in the usual way
Antonelli, Moe, Pettersen Chicago, 18 September 2014
An example
5 tasks in priority, mixed equality and set-basedna = 2 set-based are active
✧✦★✥
✧✦★✥
✧✦★✥
✧✦★✥
each active setbased task doubles the possible solutionsAntonelli, Moe, Pettersen Chicago, 18 September 2014
An example
5 tasks in priority, mixed equality and set-basedna = 2 set-based are active
✬
✫
✩
✪in the end: 2na possibilities from which one needs to be selected
Antonelli, Moe, Pettersen Chicago, 18 September 2014
A more formal description
1: Initialize Q with one null n× 1 vector2: Initialize N with one Identity matrix3: for i = 1 to p do
4: Compute Jx5: Compute qx,des according to eq. qx,des = J†
(σx,des +Λσ
)= J†σref
6: if i==set-based then
7: Qtemp = Q8: Ntemp = N9: end if
10: Update Q summing to each the vector qx,des pre-multiplied by the
corresponding null space projectors in N11: Update N properly including, in each, the contribution of Jx12: if i==set-based then
13: Q = Q⋃
Qtemp
14: N = N⋃
Ntemp
15: end if
16: end for
17: return Q {it contains 2nset,a elements}
Antonelli, Moe, Pettersen Chicago, 18 September 2014
Selection of the output
The algorithm builds 2na solutionsThe 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
Selection of the output
The algorithm builds 2na solutionsThe 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 setbased task in half of the solutions
Antonelli, Moe, Pettersen Chicago, 18 September 2014
Selection of the output
The algorithm builds 2na solutionsThe 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 nullspace projection filter out some velocity
component, the less conservative should be the largestnorm
Antonelli, Moe, Pettersen Chicago, 18 September 2014
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◦
0 5 10 15 20
or.
ee pos
ee obs
mech 4
mech 3
mech 2
mech 1
time [s]0 5 10 15 20
−150
−100
−50
0
50
100
150
time [s]
a
Antonelli, Moe, Pettersen Chicago, 18 September 2014
To do
Guarantee continuity among samples (task de-activation)
Computational analysis
Most of the computations shared among the possible solutionsRecursive formula for null spaces available
Nab...y = Nab...x
[
I −(
JyNab...x)† (
JyNab...x)
]
Comparison with alternative algorithms on-going
Stability analysis
Antonelli, Moe, Pettersen Chicago, 18 September 2014
Bibliography I
G. Antonelli.
Stability analysis for prioritized closed-loop inverse kinematic algorithms forredundant 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 kinematiccontrol 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 motiongeneration.
International Journal of Robotics Research, 2013.
Antonelli, Moe, Pettersen Chicago, 18 September 2014
Bibliography II
O. Kanoun, F. Lamiraux, and P.B. Wieber.
Kinematic control of redundant manipulators: Generalizing the task-priorityframework to inequality task.
Robotics, IEEE Transactions on, 27(4):785–792, 2011.
A. Liegeois.
Automatic supervisory control of the configuration and behavior of muldibodymechanisms.
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 dynamicallyvarying 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
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 roboticsystems.
In Proceedings 5th International Conference on Advanced Robotics, pages1211–1216, Pisa, I, 1991.
E. Simetti, G. Casalino, S. Torelli, A. Sperinde, and A. Turetta.
Floating underwater manipulation: Developed control methodology andexperimental 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
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, Italyhttp://webuser.unicas.it/lai/robotica
⊕Norwegian University of Science and Technology, Norwayhttp://www.ntnu.edu/amos
IROS 2014 Workshop: Real-time Motion Generation & Control
Antonelli, Moe, Pettersen Chicago, 18 September 2014
