Dynamic Coupling Map: Acceleration Space Analysis for Underactuated

Robots

Ziad Zamzami and Faiz Ben Amar

Sorbonne Universites, UPMC Paris VI, Institut Syst`emes Intelligents et de Robotique,

ISIR - CNRS UMR 7222, Boite courrier 173, 4 Place JUSSIEU, 75252, Paris, France

Keywords:

Underactuated Robotics, Trajectory Optimization, Dynamic Coupling, Passive Dynamics, Acceleration

Space, Dynamic Capability Visualization.

Abstract:

Swing-up and throwing tasks for underactuated manipulators are examples of dynamic motions that exhibit

highly nonlinear coupling dynamics. One of the key ingredients for such complex behaviors is motion coordi-

nation to exploit their passive dynamics. Despite the existence of powerful tools such as nonlinear trajectory

optimization, they are usually treated as blackboxes that provide local optimal trajectories. We introduce the

Dynamical Coupling Map (DCM), a novel graphical technique, to help gain insight into the output trajectory

of the optimization and analyze the capability of underactuated robots. The DCM analysis is demonstrated

on the swing up motion of a simpliﬁed model of a gymnast on high bar. The DCM shows in a graphical

and intuitive way the pivotal role of exploiting the nonlinear inertial forces to reach the unstable equilibrium

conﬁguration while taking into account the torque bounds constraints. In this paper, we present the DCM

as a posteriori analysis of a local optimal trajectory, found by employing the direct collocation trajectory

optimization framework.

1 INTRODUCTION

Humans and animals are capable of overcoming com-

plex terrain challenges with graceful and agile move-

ments. One of the key ingredients for such complex

behaviors is motion coordination to exploit natural

dynamics. Sports performers coordinate their action

in many different ways to achieve their goals. Coor-

dination is a key feature from the graceful, precise ac-

tion of an ice dancer to the explosive, physical power

of a triple jumper. Lizard coordinate its tail swing to

stabilize its dynamic motion over rough terrain (Libby

et al., 2012). Cheetah can rapidly accelerate and ma-

neuver during the pursuit of its prey by the coordinat-

ing of the motion of its tail (Patel and Braae, 2013).

Understanding and emulating these motions is the one

of the long-standing grand challenges in robotics and

biomechanics,with possible applications in rehabili-

tation, sport, search-and-rescue, environmental moni-

toring and security.

Unlike conventional fully-actuated manipulators

where the natural dynamics can be suppressed by

classical feedback control techniques, a large and di-

verse array of dynamical systems falls under the class

of underactuated systems. The unactuated nature

gives rise to many interesting control problems which

require fundamental non-linear approaches (Spong,

1998). Underactuated systems are deﬁned as mechan-

ical systems in which the dimension of the conﬁgu-

ration space exceeds that of the control input space,

i.e. fewer control inputs than the available degrees of

freedom. Such restriction can be the result of 1)an ac-

tuator failure (Arai et al., 1998); 2) intentional choice

of a minimalist design which leads to a less costly,

lighter and more compact machine; 3) rigid robots

with elastic joints or ﬂexible links (Spong, 1998); 4)

or even imposed by the nature of the system itself

such as non-prehensile manipulation (Lynch and Ma-

son, 1996) and recently extended to legged robots as

ﬂoating base systems (Wieber Pierre-Brice, Tedrake

Russ, 2015).

Synthesizing motion behavior for such underac-

tuated systems is quite challenging. Underactuation

impose constraints on their dynamics that restrict the

family of trajectories their conﬁgurations and acceler-

ations can follow. These constraints are second-order

nonholonomic constraints (Wieber, 2005)(Nagarajan

et al., 2013). Moreover, cannot be fully feedback

linearized (Spong, 1995). Underacuated system can

only be controlled indirectly either through contact

forces with the environment or through inertial forces

which rises from the nonlinear inertial coupling of the

548

Zamzami, Z. and Amar, F.

Dynamic Coupling Map: Acceleration Space Analysis for Underactuated Robots.

DOI: 10.5220/0006012405480557

In Proceedings of the 13th International Conference on Informatics in Control, Automation and Robotics (ICINCO 2016) - Volume 2, pages 548-557

ISBN: 978-989-758-198-4

Copyright

c

2016 by SCITEPRESS – Science and Technology Publications, Lda. All rights reserved

articulated system (Tamegaya et al., 2008), in this pa-

per we focus on the latter.

(Hauser and Murray, 1990) introduced a canoni-

cal model of underactuated systems known as the Ac-

robot (for acrobatic robot) introduced by (Hauser and

Murray, 1990). It can be considered is a highly sim-

pliﬁed model of a human gymnast on the high bar,

where the underactuated ﬁrst joint models the gym-

nast’s hands on the bar, and the actuated second joint

models the gymnast’s hips. The objective is to swing

up the Acrobot from the downward stable equilib-

rium point to the upright unstable equilibrium point

and balance it about the upright vertical. This motion

control problem is usually decomposed into two sub-

problems; ﬁrst, the swing-up motion control (

˚

Astr¨om

and Furuta, 2000) (Spong, 1995) (Xin, 2013), once

the robot’s conﬁguration is brought up to the neigh-

borhood of the unstable equilibrium point, switching

to a stabilization controller is necessary(Olfati-Saber,

2001). Several variants of the Acrobot have been pro-

posed either increasing the number of degrees of free-

doms (Kurazume and Hasegawa, 2002) (Shkolnik and

Tedrake, 2008) (Ibuki et al., 2015) or by varying the

placement of the actuators (Luca and Oriolo, 2002).

For the sake of illustration, we focus in this paper,

on an extended version of the acrobat called the gym-

nast introduced by (Xin and Kaneda, 2007b) arguing

the importance of a third active joint representing the

shoulder.

During the last two decades, many researchers

have investigated the approach of passivity or energy-

based control of underactuated mechanical systems.

(Lam and Davison, 2006) showed that the coupling

becomes more complex when the number of links

increases, and its control problem becomes more

challenging. (Xin and Kaneda, 2007a) noted that

energy based techniques are difﬁcult to apply for

higher dimension robots.

1.1 Approach and Contribution

In this work, we employ nonlinear trajectory opti-

mization for the swing-up motion generation and con-

trol. Second-order Nonholonomic and torque con-

straints ﬁt naturally into the direct formulation of tra-

jectory optimization. Furthermore, there exists strong

evidence that humans solve task-level and motor-level

challenges though optimization processes. A good

overview of optimal control in sensorimotor systems

is given by (Todorov, 2004).

Trajectory optimization approach is becoming in-

creasingly attractive with the advent of computational

power and the recent advances in nonlinear optimiza-

Figure 1: Simpliﬁed model of a gymnast on a high bar.

Figure 2: 3-link Gymnast model of underactuated mechan-

ical system with passive joint at the base.

tion (Posa et al., 2013)(Hauser, 2014). Given an ini-

tial trajectory that may be non-optimal or even non-

feasible, trajectory optimization methods can often

quickly converge to a high-quality, locally-optimal

solution.

We analyze the output trajectory using a novel

graphical technique that we call ”Dynamic Coupling

Map (DCM)” that help gain insight into the motion

of the system. Our ultimate goal for studying such

systems is to understand problems of dynamic loco-

motion in both biological systems and in robotic sys-

tems.

The rest of the paper is structured as follow. Sec-

tion (2) describes the Lagrangian dynamics of under-

actuated mechanical systems. A presentation of the

trajectory optimization formulation is given in section

(3). In section (4), we derive the dynamic coupling el-

lipsoid in both the unactuated acceleration space and

the task space. The results of the numerical simula-

tion and a discussion on the beneﬁts of the Dynamic

Dynamic Coupling Map: Acceleration Space Analysis for Underactuated Robots

549

Coupling Map as a graphical analysis tool to gain in-

sight to the full acceleration capabilities of the under-

actuated system is given in(5). A discussion on possi-

ble extensions completes the paper.

2 LANGRANGIAN DYNAMICS

OF UNDERACTUATED

MECHANICAL SYSTEMS

Consider a dynamic system deﬁned on a conﬁguration

manifold Q . let (q, ˙q) = (q

1

,. .. ,q

n

, ˙q

1

,. .. , ˙q

n

) denote

local coordinates on the tangent bundle T Q . We refer

to q, ˙q, and ¨q as the vectors of generalized coordinates,

generalized velocities, and generalized accelerations,

respectively. Let the system possess a control input

space of dimension m < n, where u ∈ R

m

denote the

vector of the control variables. Without loss of gen-

erality, we decompose the set of generalized coordi-

nates q = (q

1

,. .. ,q

n

) ∈ R

n

into q = (q

p

,q

a

) where

q

p

∈ R

n−m

, q

a

∈ R

m

denote the passive degrees of

freedom and the actuated degrees of freedom.

q =

q

p

(n−m×1)

q

a

(m×1)

∈ ℜ

n

(1)

We can then write the Euler-Lagrange equation of

motion in a canonical form as

M

pp

M

pa

M

T

pa

M

aa

¨q

p

¨q

a

+

N

p

(q, ˙q)

N

a

(q, ˙q)

=

0

τ

(2)

where the joint space inertia matrix is decomposed

into:

• M

pp

: represents The inertia matrix for the system

seen at the passive articulating subsystem

• M

aa

: represents The inertia matrix of the active

articulating subsystem

• M

pa

: The inertial coupling term between the ac-

tive and passive articulating subsystems

• ¨q

p

is the second derivative of the ﬂoating base’s

pose (position and orientation) w.r.t time. While

¨q

q

is the m-actuated joints acceleration.

• τ: The vector of the joint torques of the articulated

system, assuming that articulated system is fully

actuated.

• N

a

(q, ˙q): is the generalized bias force, it follows

that it is the value that τ would have to take in

order to produce zero joint acceleration, assum-

ing that all the other joint accelerations are zero.

Generalized bias force is a vector of force terms

that account for Coriolis and centrifugal forces,

gravity. We assume that there exist no external

wrenches, for the sake of the clarity of presenta-

tion.

• N

p

(q, ˙q): is the generalized bias force acting on

the passive subsystem.

3 TRAJECTORY OPTIMIZATION

A trajectory optimization problem seeks to ﬁnd the

a trajectory for some dynamical system that satisﬁes

some set of equality and inequality constraints con-

straints while minimizing some cost functional.

An abstraction of the Lagrangian dynamics in Eq.(2)

can be written as a set of differential equations

˙x(t) = f(x(t),u(t)) (3)

where x =

q

˙q

∈ R

2n

represent the system states, as-

suming the number of generalized coordinates equal

to the number of generalized velocities. The input

control vector u ∈R

m

and the transition function f()

deﬁnes the system evolution in time. Trajectory op-

timization problem seeks to ﬁnd a ﬁnite-time input

trajectory u(t),∀ ∈ [0,t

f

], such that a given criteria is

minimized,

G = φ(t

0

,x

0

,t

f

,x

f

) +

t

f

Z

t

0

g(t, x, u) dt (4)

where φ() and g() are the intermediate and ﬁnal

cost function. The objectivefunction can be written in

terms of quantities evaluated at the ends of the phases

only, in that case it is referred to as the Mayer form.

If the objective function only involves an integral it is

referred to as a problem of Lagrange, and when both

terms are present it is called a problem of Bolza.

The optimization may be subject to a set of bound-

ary constraints

b

min

< b(t

0

,x

0

) < b

max

(5)

b

min

< b(t

f

,x

f

) < b

max

(6)

In addition, Dynamic constraints, including the sec-

ond order non-holonomic constraints, can be added

as path constraints

c

min

< c(t, x(t), u(t)) < c

max

(7)

Bounds on state variables and input control limits are

added as

x

min

≤ x(t) ≤ x

max

(8)

u

min

≤ u(t) ≤ u

max

(9)

The result of the optimization is an optimal trajec-

tory given by [x

∗

(t),u

∗

(t)]

ICINCO 2016 - 13th International Conference on Informatics in Control, Automation and Robotics

550

To compute a feasible motion plan we transcribe

the differential equation of the robot dynamics to al-

gebraic equations and solve them through NonLinear

Optimisation (NLP). (Betts, 2010) showed that that

NLP can be interpreted as discrete approximations of

the continuous optimal control problem. There are

mature NLP solvers used in robotics; SNOPT (Gill

et al., 2002) which is based on Sequential Quadratic

Programming (SQP). The second solver is iPOPT

(Wachter and Biegler, 2006) using the Interior Point

Method. However, The complete description of the

solvers and the algorithms used lies outside of the

scope of this paper.

We employ direct method for transcribing and

solving the optimal control problem. The solution to

an optimal control problem via direct methods scales

well to high-dimensional systems. In direct collo-

cation, the optimization searches over a set of deci-

sion variables z, comprised by a vector of the states

and control trajectories discretized over time at cer-

tain points or nodes, i.e., z = [xk, uk], for k = 1,...,N,

where N represents the total number of nodes. Finally

The NLP can be formulated as.

minimize

z

G(z) (10)

subject to : l ≤

z

c(z)

Az

≤ u (11)

In, direct collocation the input is represented as a

piecewise-linear. function of time, and the state is

piecewise-cubic. The values of the state and con-

trol at each knot point are the decision variables.

The slope of state is prescribed by the dynamics at

each knot point. The collocation points are the mid-

vpoints of each cubic segment. The slope of the cu-

bic at the collocation point is constrained to match

the system dynamics at that point, as illustrated in

ﬁgure(3). For more details the reader is referred to

(Betts, 2010)(Betts, 1998).

4 DYNAMIC COUPLING MAP

4.1 Dynamic Coupling in the

Unactuated Acceleration Space

Kinematic and dynamic coupling between the articu-

lated system and the base is usually regarded as un-

wanted perturbation that degrades positioning accu-

racy and operational dexterity. In this work we at-

tempt to exploit the dynamic coupling between the

unactuated subsystem and its associated active artic-

ulated subsystem. we will start ﬁrst by exploring the

Figure 3: Illustration of the state trajectory discretization

occurring in direct collocation optimization. System dy-

namics f (t

c

) are enforced to match the slope of the cubic

˙x

c

at the collocation point x

c

.

relationship between the acceleration space of the un-

actuated subsystem and the torque space of the active

articulating subsystem.

The dynamic equations of the system (2) can be de-

composed into two main equations; one describing

the dynamics of the passive free-ﬂoating base.

M

p

¨q

p

+ M

pa

¨q

a

+ N

p

= 0 (12)

and the second equation describing the dynamics

of the articulated system

M

a

¨q

a

+ M

T

pa

¨q

p

+ N

a

= τ (13)

For the sake of clarity, we dropped the dependence of

the force bias terms N

a,p

on q, ˙q.

Deﬁnition 4.1. The underactuated mechanical

system (2) is locally Strongly Inertially Coupled if

and only if

rank(M

bm

(q)) = n −m forall q ∈ B

where B is a neighborhood of the origin. The

Strong Inertial Coupling is global if the rank condi-

tion holds for all q ∈Q

the strong inertial coupling condition (Spong,

1998) requires that the number of active degrees of

freedom be at least as great as the number of passive

degrees of freedom.

Under the assumption of Strong Inertial Coupling we

may compute a pseudo-inverse M

†

pa

for M

pa

as

M

†

pa

= M

T

pa

(M

pa

M

T

pa

)

−1

(14)

Then active joints generalized accelerations

¨

ˆq in

(12) can be expressed as

¨q

a

= −M

†

pa

M

p

¨q

p

−M

†

pa

N

p

(15)

Dynamic Coupling Map: Acceleration Space Analysis for Underactuated Robots

551

By substituting (15) in (13), we can ﬁnd a relation

between the acceleration of the passive subsystem and

its acting surrounding, which include; Joint torques,

Nonlinear inertial forces, Gravity and wrench acting

on the end points of the articulated system, if exists.

¨q

p

M

T

pa

−M

a

M

†

pa

M

p

−M

a

M

†

pa

N

p

+ N

a

= τ (16)

we ﬁnally express equation (16) as

¨q

p

= Z

†

τ+

f

N

= ¨q

p

τ

+ ¨q

p

bias

(17)

where we introduce the dynamic coupling factor Z

†

as

Z

†

=

M

T

pa

−M

a

M

†

pa

M

p

†

(18)

Z is the Schur complement (Kolda and Bader, 2008)

of the inertia coupling M

bq

between the passive sub-

system and the active articulating system We denote,

as well, the bias term.

f

N = M

a

M

†

pa

N

p

−N

a

(19)

equation (17) provides an intuitive comprehension of

the inﬂuence of different forces on the motion of the

passive subsytem: ¨q

p

τ

is the acceleration of the pas-

sive subsystem due to the torques of the active artic-

ulated system, ¨q

p

bias

is the acceleration due to gravity

and non-linear inertial forces, and wrenches acting on

the articulated system such as contact forces.

Furthermore, we note that the inverse of the dy-

namic coupling factor Z

†

act as transmission factor

between the control input space τ and the acceleration

space of the passive subsystem. Z

†

is conﬁguration-

dependent matrix. Thereby, it is possible to search

for the conﬁgurationthat maximizes the dynamiccou-

pling factor which will result in a decrease in the ef-

fort expenditure. This formulation of the dynamic

equation helps in analyzing the effects of these three

sets of variables on the achievable set of free ﬂoating

base acceleration.

4.2 Dynamic Coupling Ellipsoid

The goal of the Floating base dynamic coupling ellip-

soid is to represent geometrically, in the acceleration

space, the ensemble of the accelerations achievable by

the unactuated subsystem. The torque limits at each

actuator in the articulated system are assumed to be

symmetrical, that is,

−τ

i,max

≤τ

i

≤ τ

i,max

(20)

The normalized joint torque vector

˜

τ can be expressed

as

˜

τ = L

−1

τ (21)

where the scaling matrix L is given by

L = diag(τ

1,max

,τ

2,max

,. . . ,τ

n,max

) (22)

The control input space can then be represented as m-

dimensional sphere deﬁned by

˜

τ

T

˜

τ ≤ 1 (23)

By solving Eq.(17) and substituting in Eq.(23) we get

the equation of an ellipsoid in the acceleration space

of the passive subsystem.

( ¨q

p

− ¨q

p

bias

)

T

(Z

†

L)

−T

(Z

†

L)

−1

( ¨q

p

− ¨q

p

bias

)

T

≤ 1

(24)

which simpliﬁes to

( ¨q

p

− ¨q

p

bias

)

T

˜

Z( ¨q

p

− ¨q

p

bias

)

T

≤ 1 (25)

where

˜

Z = Z

T

L

−2

Z (26)

By applying singular value decomposition,

˜

Z can

be decomposed into

˜

Z = U ΣV

T

(27)

where U ∈ R

m×m

and V ∈ R

n×n

are orthogonal ma-

trices. The principal axis of the n−m-dimensional

ellipsoid is determined by the eigenvectors u

i

, while

its length is speciﬁed by the corresponding eigenvalue

√

σ

i

. Furthermore, the term ¨q

bias

is responsible for

translating the ellipsoid center from its origin.

4.3 Dynamic Coupling in the Task

Space

In certain applications, it is more natural to reason

about the underactuated mechanical system in the

task space acceleration space, when it is more rel-

evant than the generalized accelerations space. We

extend the previous analysis to describe the dynamic

coupling between the active subsystem and the task

space, while taking into consideration the unactuated

nature of the mechanical system.

let X =

x

1

x

2

.. . x

d

T

denote the d-

dimensional task vector, which can represent the

Cartesian position of the end effector of the manip-

ulator or the center of mass of the underactuated sys-

tem or any conﬁguration dependent task can be for-

mulated as X = f(q). By taking the Taylor expansion

of this mapping we derive the Jacobian that maps the

task space velocities to the generalized velocities of

the underactuated system.

˙

X = J(q) ˙q (28)

The task space acceleration can be found by differen-

tiating Eq.(28) with respect to time yielding

¨

X = J(q) ¨q+

˙

J(q, ˙q) ˙q (29)

ICINCO 2016 - 13th International Conference on Informatics in Control, Automation and Robotics

552

we partition further Eq.(29) into passive and active

subsystems as follows

¨

X = J

p

¨q

p

+ J

a

¨q

a

+

˙

J(q, ˙q) ˙q (30)

From Eq.(13) we can express the generalized acceler-

ation of the active subsystem function in torque as

¨q

a

=

¯

Z(τ −N

a

) (31)

where

¯

Z = M

T

pa

−M

aa

M

−1

pp

M

pa

(32)

The generalized acceleration of the passive subsystem

can also be expressed as

¨q

p

= −M

−1

pp

M

pa

¨q

a

−M

−1

pp

N

p

(33)

By substituting Eq.(31) in Eq.(33) we get

¨q

p

= −M

−1

pp

M

pa

(

¯

Z(τ−N

a

)) −M

−1

pp

N

p

(34)

To ﬁnd the relationship between the Task space ac-

celeration to the control input space, we substitute

Eq.(34) and Eq.(31) in Eq.(29)

¨

X = Z

∗

τ+

¨

X

bias

(35)

where Z

∗

and

¨

X

bias

are deﬁned as

Z

∗

= J

a

¯

Z −J

p

M

−1

pp

M

pa

¯

Z (36)

¨

X

bias

= J

p

M

−1

pp

M

pa

N

p

−J

p

M

−1

pp

N

a

−J

a

¯

ZN

a

+

˙

J(q, ˙q) ˙q

(37)

Following the same procedure in 4.2 for deriving the

dynamic coupling ellipsoid in the generalized accel-

eration space , we can deﬁne the dynamic coupling

ellipsoid in the task space

(

¨

X −

¨

X

bias

)

T

(Z

∗

L)

−T

(Z

∗

L)

−1

(

¨

X −

¨

X

bias

) ≤ 1 (38)

5 EXPERIMENT RESULTS

5.1 Model Description

For the purpose of illustration, we focus our atten-

tion to the swing-up dynamic motion of the ’Gym-

nast’ robot shown in ﬁgure (2). The Gymnast is a

highly simpliﬁed approximation of an athlete swing-

ing his body on a high bar. The model is composed

of 3 links representing the arm, the trunk and the leg.

All 3 links are connected by 1-DoF rotary joints. The

arm is connected to the world through a passive joint,

while the second and the third joint are active and

torque controlled. Assuming symmetrical motion, we

restrict our attention to the sagittal plane.

The inertial parameters used are; m

1

= 1 Kg, m

2

=

m

3

= 2 Kg denoting the masses of the ﬁrst, second and

third link, respectively. The ﬁrst link has a length of

l

1

= 1m while the second and third links have lengths

of l

2

= l

3

= 2m. Density assumed to be homogeneous,

thus l

c

1

= 0.5m and l

c

2

= l

c

3

= 1m denotes the relative

positions of the center of mass. The torque limits for

actuators are given by L = diag(10 N.m,10 N.m)

The robot is modeled using MIT toolbox Drake

(Tedrake, 2014) in a Matlab environment. The

optimization problem is solved using a non-linear

optimization solver SNOPT based on Sequential

Quadratic Program (SQP) algorithm.

5.2 Results

The non-linear optimization solver ﬁnds a local-

optimal trajectory for the dynamic swing up motion

of the Gymnast robot. Figure (4) shows a compila-

tion of the motion from the down equilibrium position

q =

q

1

q

2

q

3

T

=

0 0 0

T

to the up-right un-

stable equilibrium position q =

π 0 0

T

. The ﬁrst

link is shown in blue, while the second and third links

are shown in red and yellow, respectively. Figure (6)

and ﬁgure (5) shows the evolution of the generalized

position and velocities during swing up motion, re-

spectively. We can note that although we have set an

initial guess of the time span of the motion to be t

0

= 6

seconds the optimization achieved the goal conﬁgura-

tion with an optimal time of t

∗

≃ 4. Figure (7) shows

the optimal torque control policy for the actuated sub-

system, τ

2

& τ

3

in this case.

The Dynamic coupling analysis is depicted in

Table(1), Table (2) & Table (3). Each column repre-

sents an instance of the motion with an interval of 0.3

seconds. The ﬁrst row shows snapshots of the conﬁg-

urations of the ’Gymnast’ robot during the swing-up

trajectory. The second row shows only the shape of

the corresponding dynamic coupling ellipsoids in the

task space. In this case, it offers an intuitive Carte-

sian acceleration of the terminal organ representing

the foot (colored in purple). The horizontal acceler-

ation is depicted on the abscissa, while the vertical

acceleration is depicted on the ordinate. The prin-

cipal axis and scale of the dynamic coupling ellip-

soid is determined by singular value decomposition

of (Z

∗

L)

−T

(Z

∗

L)

−1

in equation (38). The third row

shows the trajectory of the dynamic coupling ellipsoid

while taking into consideration the non-linear inertial

forces. Unlike row 2 which shows the dynamic cou-

pling ellipsoid centered at the origin, row 3 shows the

displaced ellipsoid due to the contribution of the non-

linear inertial forces

¨

X

bias

in equation (37). The vector

of displacement from the instantaneous origin is rep-

resented by an arrow.

Dynamic Coupling Map: Acceleration Space Analysis for Underactuated Robots

553

Table 1: Snapshots of the swing-up trajectory: The ﬁrst row shows the conﬁgurations of the ’Gymnast’ robot during the

swing-up trajectory. The second row shows the shape of corresponding dynamic coupling ellipsoids. The Third row shows

the trajectory of the dynamic coupling ellipsoid while taking into consideration the non-linear inertial forces. The ﬁrst link

with a passive joint is shown in blue. The second and third link with active joints are shown in red and yellow, rescpectively.

-3 -2 -1 0 1 2 3

x

-5

-4

-3

-2

-1

0

1

2

3

4

5

z

t = 0.00 sec

-3 -2 -1 0 1 2 3

x

-5

-4

-3

-2

-1

0

1

2

3

4

5

z

t = 0.30 sec

-3 -2 -1 0 1 2 3

x

-5

-4

-3

-2

-1

0

1

2

3

4

5

z

t = 0.60 sec

-3 -2 -1 0 1 2 3

x

-5

-4

-3

-2

-1

0

1

2

3

4

5

z

t = 0.90 sec

-1 -0.5 0 0.5 1

Horizontal acceleration (m/s

2

)

-1

-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

0.8

1

Vertical acceleration (m/s

2

)

Dynamic Coupling Ellipsoid

-50 0 50

Horizontal acceleration (m/s

2

)

-1

-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

0.8

1

Vertical acceleration (m/s

2

)

Dynamic Coupling Ellipsoid

-15 -10 -5 0 5 10 15

Horizontal acceleration (m/s

2

)

-5

-4

-3

-2

-1

0

1

2

3

4

5

Vertical acceleration (m/s

2

)

Dynamic Coupling Ellipsoid

-10 -5 0 5 10

Horizontal acceleration (m/s

2

)

-4

-3

-2

-1

0

1

2

3

4

Vertical acceleration (m/s

2

)

Dynamic Coupling Ellipsoid

-1 -0.5 0 0.5 1

Horizontal acceleration (m/s

2

)

-1

-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

0.8

1

Vertical acceleration (m/s

2

)

Dynamic Coupling Ellipsoid

-80 -60 -40 -20 0 20

Horizontal acceleration (m/s

2

)

0

5

10

15

20

25

Vertical acceleration (m/s

2

)

Dynamic Coupling Map

-80 -60 -40 -20 0 20

Horizontal acceleration (m/s

2

)

-15

-10

-5

0

5

10

15

20

25

Vertical acceleration (m/s

2

)

Dynamic Coupling Map

-100 -50 0 50 100

Horizontal acceleration (m/s

2

)

-15

-10

-5

0

5

10

15

20

25

Vertical acceleration (m/s

2

)

Dynamic Coupling Map

5.3 Discussion

Trajectory optimization proved to be an interesting

approach to generate high quality dynamic motion de-

spite its local optimality aspect. Moreover, its natu-

ral capacity to integrate all types of constraints (lin-

ear, nonlinear, complementarity problems), objective

functions and application-related heuristics allowed it

to gain traction in the robotics community. However,

after many iterations, the optimization solver outputs

a single local optimal trajectory without any insight

into the nature of the given trajectory. The swing up

motion example illustrates the emergence of nontriv-

ial solution to the torque control, velocity and position

trajectory from simple core principles (goal conﬁgu-

ration, torque limit, dynamic model).

We use the Dynamic Coupling map (DCM) to an-

alyze the dynamic motion given by the optimization.

To facilitate the navigation and explanation of the

DCM, let the location of each cell in table (1) be given

by the triplet (t,r, c), where t r and c corresponds to

the number of table, row and column, respectively.

The ’Gymnast’ starts from an equilibrium position de-

picted in cell(1,1,1), where the dynamic coupling el-

-3 -2 -1 0 1 2 3

x

-4

-3

-2

-1

0

1

2

3

4

z

t = 6.00 sec

Figure 4: Local-optimal swing-up motion.

lipsoid in the task space representing the maximum

acceleration capacity of the terminal organ (shown

in purple) is shown in cell(1,2,1). Since the starting

conﬁguration is singular the Jacobian-dependent el-

ICINCO 2016 - 13th International Conference on Informatics in Control, Automation and Robotics

554

Table 2: Continued.

-3 -2 -1 0 1 2 3

x

-5

-4

-3

-2

-1

0

1

2

3

4

5

z

t = 1.20 sec

-3 -2 -1 0 1 2 3

x

-5

-4

-3

-2

-1

0

1

2

3

4

5

z

t = 1.50 sec

-3 -2 -1 0 1 2 3

x

-5

-4

-3

-2

-1

0

1

2

3

4

5

z

t = 1.80 sec

-3 -2 -1 0 1 2 3

x

-5

-4

-3

-2

-1

0

1

2

3

4

5

z

t = 2.10 sec

-15 -10 -5 0 5 10 15

Horizontal acceleration (m/s

2

)

-1.5

-1

-0.5

0

0.5

1

1.5

Vertical acceleration (m/s

2

)

Dynamic Coupling Ellipsoid

-20 -10 0 10 20

Horizontal acceleration (m/s

2

)

-6

-4

-2

0

2

4

6

Vertical acceleration (m/s

2

)

Dynamic Coupling Ellipsoid

-10 -5 0 5 10

Horizontal acceleration (m/s

2

)

-1

-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

0.8

1

Vertical acceleration (m/s

2

)

Dynamic Coupling Ellipsoid

-10 -5 0 5 10

Horizontal acceleration (m/s

2

)

-8

-6

-4

-2

0

2

4

6

8

Vertical acceleration (m/s

2

)

Dynamic Coupling Ellipsoid

-100 -50 0 50 100

Horizontal acceleration (m/s

2

)

-15

-10

-5

0

5

10

15

20

25

Vertical acceleration (m/s

2

)

Dynamic Coupling Map

-100 -50 0 50 100

Horizontal acceleration (m/s

2

)

-40

-30

-20

-10

0

10

20

30

Vertical acceleration (m/s

2

)

Dynamic Coupling Map

-100 -50 0 50 100

Horizontal acceleration (m/s

2

)

-40

-30

-20

-10

0

10

20

30

Vertical acceleration (m/s

2

)

Dynamic Coupling Map

-100 -50 0 50 100

Horizontal acceleration (m/s

2

)

-40

-20

0

20

40

60

80

100

Vertical acceleration (m/s

2

)

Dynamic Coupling Map

Table 3: Continued.

-3 -2 -1 0 1 2 3

x

-5

-4

-3

-2

-1

0

1

2

3

4

5

z

t = 2.40 sec

-3 -2 -1 0 1 2 3

x

-5

-4

-3

-2

-1

0

1

2

3

4

5

z

t = 2.70 sec

-3 -2 -1 0 1 2 3

x

-5

-4

-3

-2

-1

0

1

2

3

4

5

z

t = 3.00 sec

-3 -2 -1 0 1 2 3

x

-5

-4

-3

-2

-1

0

1

2

3

4

5

z

t = 5.10 sec

-2 -1 0 1 2

Horizontal acceleration (m/s

2

)

-6

-4

-2

0

2

4

6

Vertical acceleration (m/s

2

)

Dynamic Coupling Ellipsoid

-10 -5 0 5 10

Horizontal acceleration (m/s

2

)

-6

-4

-2

0

2

4

6

Vertical acceleration (m/s

2

)

Dynamic Coupling Ellipsoid

-30 -20 -10 0 10 20 30

Horizontal acceleration (m/s

2

)

-8

-6

-4

-2

0

2

4

6

8

Vertical acceleration (m/s

2

)

Dynamic Coupling Ellipsoid

-50 0 50

Horizontal acceleration (m/s

2

)

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

Vertical acceleration (m/s

2

)

Dynamic Coupling Ellipsoid

-100 -50 0 50 100

Horizontal acceleration (m/s

2

)

-40

-20

0

20

40

60

80

100

Vertical acceleration (m/s

2

)

Dynamic Coupling Map

-100 -50 0 50 100

Horizontal acceleration (m/s

2

)

-40

-20

0

20

40

60

80

100

Vertical acceleration (m/s

2

)

Dynamic Coupling Map

-100 -50 0 50 100

Horizontal acceleration (m/s

2

)

-40

-20

0

20

40

60

80

100

Vertical acceleration (m/s

2

)

Dynamic Coupling Map

-100 -50 0 50 100

Horizontal acceleration (m/s

2

)

-40

-20

0

20

40

60

80

100

Vertical acceleration (m/s

2

)

Dynamic Coupling Map

Dynamic Coupling Map: Acceleration Space Analysis for Underactuated Robots

555

0 1 2 3 4 5 6

T ime(s)

-15

-10

-5

0

5

10

15

˙q (rad/s)

Generalized Velocity Profile

Passive Joint 1

Active Joint 2

Active Joint 3

Figure 5: Local-optimal Generalized Velocities Proﬁle for

the swing-up motion.

0 1 2 3 4 5 6

T ime(s)

-3

-2

-1

0

1

2

3

4

q (rad)

Generalized Configuration Profile

Passive Joint 1

Active Joint 2

Active Joint 3

Figure 6: Local-optimal Generalized Coordinates Proﬁle

for the swing-up motion.

0 1 2 3 4 5 6

T ime(s)

-10

-8

-6

-4

-2

0

2

4

6

8

10

T orque(N .m)

Joint Effort Profile

Joint 2

Joint 3

Figure 7: Local-optimal Torque Control Proﬁle for the

swing-up motion.

lipsoid fails to give any valuable information at this

conﬁguration, except that it is singular. The Gym-

nast starts applying torque at the shoulder and the

knee (Joint 2 & 3). However, it is clear from ﬁgure

(7) that the Gymnast hits already the torque limits at

10 (Nm). The Dynamic Coupling ellipsoid (DCE),

depicted in cell(1,2,2), shows the limited capacity of

the underactuated robot to accelerate in the vertical

direction (limited at a range of +/ − 1 m/s

2

) while

most of the actuation effort can be transmitted in the

horizontal direction range (+/ − 50 m/s

2

). We ob-

serve that DCE is almost degenerate due to the near

singular conﬁguration. In other words, it means that

the actuators solely fails to move the underactuated

robot directly to the upright conﬁguration. However,

in the following frames the robot exploit the passive

dynamics by swinging to acquire a vertical acceler-

ation capacity beyond what is provided by the ac-

tuators. By inspecting cell(1,3,2), we can see that

the DCE shape, determined by actuator torque limits,

doesn’t tell the whole story. That is because the DCE

is shifted from its origin by the vector

¨

X

bias

which is

inﬂuenced by the velocity-dependent nonlinear iner-

tial forces. Cell(1,3,2) shows that the DCE origin is

shifted to 20 m/s

2

due to the nonlinear inertial forces.

In the following frames, we ﬁnd that the optimiza-

tion starts exploiting this displacement to increase the

vertical acceleration capacity. Cell(2,3,3) shows that

optimization was able to increase the vertical acceler-

ation to about 90 m/s

2

thanks to the exploitation of

the velocity-dependent non-linear inertial forces. An-

other interesting observation, is that at this speciﬁc

instant at t = 2.2s in ﬁgure (7), we can see that the

robot reduce its independence on the torque effort and

start exploiting its own passive dynamics to pursue its

movement towards the goal conﬁguration. In 3, we

observe that the robot starts stabilizing its motion by

decelerating towards the goal conﬁguration with zero

velocity.

The current presentation of the Dynamic Cou-

pling Map highlight the importance of the velocity-

dependent nonlinear inertial forces to achieve highly

dynamic motions. The DCM maps the interplay of

the input control space and the dynamic coupling to

achieve a desired acceleration. Although the current

presentation provides ’a posteriori’ graphical analysis

of the dynamic capability of underactuated manipula-

tors, it raises an interesting question about the pos-

sibility of exploiting this map to design an optimal

trajectory ’a priori’.

6 CONCLUSIONS AND FUTURE

WORK

Motivated by the agile motion of humans and animals

in nature, we have employed Direct collocation

trajectory optimization to ﬁnd optimal control policy

for the swing up motion of a gymnast on high bar.

We have introduced the Dynamical Coupling Map

(DCM), a graphical technique, to gain insight into the

output trajectory of the optimization and analyze the

ICINCO 2016 - 13th International Conference on Informatics in Control, Automation and Robotics

556

acceleration capability of underactuated mechanical

systems. We highlighted the importance of the effects

of the nonlinear inertial forces on the acceleration

limits of underactuated system.

A perspective of this work, is to extend the cur-

rent analysis to ﬂoating-base systems, a special class

under actuated mechanical systems driven by the in-

teraction with the environment as well as the dynamic

coupling of its articulating system.

REFERENCES

Arai, H., Tanie, K., and Shiroma, N. (1998). Nonholonomic

control of a three-DOF planar underactuated manipu-

lator. IEEE Trans. Robot. Autom., 14(5):681–695.

˚

Astr¨om, K. J. and Furuta, K. (2000). Swinging up a pendu-

lum by energy control. Automatica, 36(2):287–295.

Betts, J. T. (1998). Survey of Numerical Methods for

Trajectory Optimization. J. Guid. Control. Dyn.,

21(2):193–207.

Betts, J. T. (2010). Practical Methods for Optimal Control

and Estimation Using Nonlinear Programming.

Gill, P. E., Murray, W., and Saunders, M. A. (2002).

SNOPT: An SQP Algorithm for Large-Scale Con-

strained Optimization. SIAM J. Optim., 12(4):979–

1006.

Hauser, J. and Murray, R. M. (1990). Nonlinear con-

trollers for non-integrable systems: the Acrobot ex-

ample. 1990 Am. Control Conf., (1):669–671.

Hauser, K. (2014). Fast interpolation and time-optimization

with contact. Int. J. Rob. Res., 33(9):1231–1250.

Ibuki, T., Sampei, M., Ishikawa, A., and Nakaura, S.

(2015). Jumping Motion Control for 4-Link Robot

Based on Virtual Constraint on Underactuated Joint.

Sc.Ctrl.Titech.Ac.Jp, pages 1–6.

Kolda, T. G. and Bader, B. W. (2008). Tensor Decomposi-

tions and Applications. SIAM Rev., 51(3):455–500.

Kurazume, R. and Hasegawa, T. (2002). A new index of se-

rial link manipulator performance combining dynamic

manipulability and manipulating force ellisoids. 1(8).

Lam, S. and Davison, E. (2006). The real stabilizability ra-

dius of the multi-link inverted pendulum. Am. Control

Conf. 2006, (2):1814–1819.

Libby, T., Moore, T. Y., Chang-Siu, E., Li, D., Cohen,

D. J., Jusuﬁ, A., and Full, R. J. (2012). Tail-assisted

pitch control in lizards, robots and dinosaurs. Nature,

481(7380):181–184.

Luca, A. D. and Oriolo, G. (2002). Trajectory Planning and

Control for Planar Robots with Passive Last Joint. Int.

J. Rob. Res., 21(5-6):575–590.

Lynch, K. and Mason, M. (1996). Dynamic underactu-

ated nonprehensile manipulation. Proc. IEEE/RSJ Int.

Conf. Intell. Robot. Syst. IROS ’96, 2:889–896.

Nagarajan, U., Kantor, G., and Hollis, R. (2013). Integrated

motion planning and control for graceful balancing

mobile robots. Int. J. Rob. Res., 32(9-10):1005–1029.

Olfati-Saber, R. (2001). Nonlinear Control of Under-

actuated Mechanical Systems with Application to

Robotics and Aerospace Vehicles. page 307.

Patel, A. and Braae, M. (2013). Rapid turning at high-

speed: Inspirations from the cheetah’s tail. IEEE Int.

Conf. Intell. Robot. Syst., pages 5506–5511.

Posa, M., Cantu, C., and Tedrake, R. (2013). A direct

method for trajectory optimization of rigid bodies

through contact. Int. J. Rob. Res., 33(1):69–81.

Shkolnik, A. and Tedrake, R. (2008). High-dimensional un-

deractuated motion planning via task space control.

2008 IEEE/RSJ Int. Conf. Intell. Robot. Syst. IROS,

pages 3762–3768.

Spong, M. W. (1995). Swing up control problem for the

acrobot. IEEE Control Syst. Mag., 15(1):49–55.

Spong, M. W. (1998). Underactuated mechanical systems.

Control Probl. Robot. Autom. Springer, pages 135–

150.

Tamegaya, K., Kanamiya, Y., Nagao, M., and Sato, D.

(2008). Inertia-coupling based balance control of a

humanoid robot on unstable ground. 2008 8th IEEE-

RAS Int. Conf. Humanoid Robot. Humanoids 2008,

pages 151–156.

Tedrake, R. (2014). Drake: A planning, control, and analy-

sis toolbox for nonlinear dynamical systems.

Todorov, E. (2004). Optimality principles in sensorimotor

control. Nat. Neurosci., 7(9):907–15.

Wachter, A. and Biegler, L. T. (2006). On the implemen-

tation of an interior-point ﬁlter line-search algorithm

for large-scale nonlinear programming, volume 106.

Wieber, P.-B. (2005). Some comments on the structure of

the dynamics of articulated motion. Ruperto Carola

Symp. Fast Motions Biomech. Robot.

Wieber Pierre-Brice, Tedrake Russ, K. S. (2015). Model-

ing and Control of Legged Robots. Springer, 2nd ed

edition.

Xin, X. (2013). On simultaneous control of the energy

and actuated variables of underactuated mechanical

systems-example of the acrobot with counterweight.

Adv. Robot., 27(12):959–969.

Xin, X. and Kaneda, M. (2007a). Analysis of the energy-

based swing-up control of the Acrobot. Int. J. Robust

Nonlinear Control, 17(16):1503–1524.

Xin, X. and Kaneda, M. (2007b). Design and analysis of

swing-up control for a 3-link gymnastic robot with

passive ﬁrst joint. Proc. IEEE Conf. Decis. Control,

23(6):1923–1928.

Dynamic Coupling Map: Acceleration Space Analysis for Underactuated Robots

557