In Proceedings of 8th Workshop on Humanoid Soccer Robots

13th IEEE-RAS International Conference on Humanoid Robots (Humanoids), Atlanta, GA, 2013.

Trunk Controlled Motion Framework

Stefan Glaser, Klaus Dorer

Hochschule Offenburg, Germany
Email: {Stefan.Glaser, Klaus.Dorer}@hs-offenburg.de

Abstract—In this paper we propose a motion framework for of the TcMF: walking and stopping on one leg. Results for
bipedal robots that decouples motion definitions from stabilizing those two applications in the RoboCup 3D soccer simulator
the robot. This simplifies motion definitions yet allows dynamic are presented in Section V. Finally in Section VI we propose
motion adaptations. Two applications, walking and stopping on some future work and conclude.
one leg, demonstrate the power of the framework. We show that
our framework is able to perform walking and stopping on one leg In the context of this paper, the coordinate frames follow
even under extreme conditions and improves walking benchmarks the right hand rule, where the x-axis points to the right, the
significantly in the RoboCup 3D soccer simulation domain. y-axis to the front and the z-axis upwards. Furthermore, we
use transformation matrices of the form Tab , which expresses
I. I NTRODUCTION the pose of b in the coordinates of frame a. In homogeneous
A robot motion can be defined in many ways. When coordinates Tab consists of a 3 × 3 rotation matrix Rab and a
using inverse kinematics, one automatically has to decide for 3 × 1 translation vector tab :
a reference frame for calculations. The obvious choice for the  a 
robot’s trunk/torso as reference frame suits well for the purpose Rb tab
Tab = (1)
of IK calculations. However, with respect to balancing motion 0 1
definitions such as walking, the torso as a twisting system lacks
the adaptation to the robot’s current tilt (x- and y-rotation) and The videos referenced in the text are available at [9].
thereby as well to its center of mass (CoM) (Figure 1).
II. R ELATED W ORK
There are many approaches to create motions of robots
and more specifically to create walking gaits for bipedal
robots [7]. Simple approaches define or learn motions in joint
space, i.e. define a choreography of joint angles that produce
the desired behavior. Especially for walking, zero moment
point (ZMP) based approaches gained popularity [3]. Many
successful teams in the RoboCup competition use ZMP-based
linear inverted pendulum models (LIPMs) for walking [1], [2],
[4].
ZMP-based methods have in common that they abstract the
Fig. 1. Locally defined motions do not adapt to the current tilt and CoM. body to a single point mass. This has the advantage that LIPM
can directly be applied reducing mathematical complexity. On
Extending the trajectory interpolations of the motion mod- the other side they do not consider the current orientation of the
els to also cope for the current situation (the robot’s tilt robot when mapping from the globally modelled trajectories to
and CoM) can be very complex and counter intuitive to the local trunk frame. This means that any deviation from the
the basic calculations. Moreover it has to be done for each assumed upright orientation of the torso leads to the problems
motion separately and makes testing and debugging of those shown in Figure 1.
calculations even more difficult.
In order to respond to such deviations, three recovery
The Trunk controlled Motion Framework described in this strategies are typically applied: ankle push recovery, hip push
paper addresses this issue by the introduction of a virtual recovery and step push recovery [8]. Ankle push recovery
reference frame. Motion trajectories defined in this frame applies forces to the ankle joints in order to keep the CoM
are then automatically adapted to the current situation during within the base of support. This is what our framework actively
execution. This way motion definitions keep as simple as in the applies. Hip push recovery uses angular accelerations in the hip
local case, but implicitly gain the power of dynamic situation to keep the CoM within the base of support. Step push recovery
adaption. The control strategy is only based on the current uses so called capture steps to react to major disturbances by
CoM and the orientation estimation of the trunk. Two common changing the step plan and therefore the base of control [8],
and well investigated measures of humanoid robots. [5]. The latter two are ’passively’ supported by the proposed
framework as explained later.
The remainder of this paper is organized as follows:
Section II puts our approach in relation to existing work. In One disadvantage of typical implementations of these
Section III we describe our approach of the Trunk controlled strategies is that the adjustments are embedded into the motion
Motion Framework. Section IV describes two applications model. That means they are tightly coupled to the specific
motion. The framework proposed here provides a generic of the CoM. However, they are not counteracting any tilt of
adjustment to the current tilt decoupled from specific motion the robot. If the orientation estimation of the robot would be
definitions. To demonstrate this, we show two applications of perfect, this tilt adjustment would basically try to preserve the
the framework, walking and stopping on one leg. Also, the current tilt of the trunk. In order to continuously push the
proposed framework explicitly models the orientation of the current z-axis of the trunk upright (towards the global z-axis),
torso eliminating the implicit assumption of an upright robot. we can manipulate RTM by applying an additional rotation,
Both is new to our knowledge. interpolating from the actual z-axis of the trunk to the 0
global
z-axis. The resulting adapted trunk frame is called T .
III. T RUNK C ONTROLLED M OTION F RAMEWORK
0 0
A. Approach RTM = RTT RTM (6)
0
The idea behind the Trunk controlled Motion Framework If we now use this manipulated tilt estimation RTM in for-
(TcMF) follows a simple thought: A balanced motion should 0 0

always support the center of mass. In order to achieve this, mula 3 to construct TTM and plug TTM into formula 2 to
0
motions should be defined with respect to a reference frame transform the motion trajectories into T , we get a motion that
with the center of mass as its origin. Furthermore supporting is not only adjusted to the current CoM and tilt of the robot, but
the center of mass usually means acting against the gravity, also continuously trying to push the trunk upright. The amount
regardless of the actual tilt of the robot. The orientation of the by how much we interpolate between the current and upright
reference frame should therefore always point upright, but still situation specifies the weighting between the two 0
adjustments.
0
facing the horizontal view direction of the robot. We call this Zero percent interpolation (T = T → RTT = RTT = I)
reference frame Motion Frame M. results solely in the first tilt adjustment. Whereas 100 percent
0 0

So far we simply switched the reference frame in which interpolation (T = M → RTT = RM T ) would simply
we define the motions from the local trunk frame T to the cancel out the first tilt adjustment, with the consequence that
virtual Motion Frame. In order to use an inverse kinematics the motion is still related to the current CoM, but without
solver, we now need to transform the motion trajectories back any further adjustment to the current tilt of the trunk. Any
into the trunk frame, using the following equation: interpolation in between describes a trade off between adapting
to the current tilt and pushing the trunk upright.
0
TTl = TTM TM
l (2) The interpolation rotation RTT can be constructed in differ-
0

where TM and TTl expresses the pose of limb l with respect ent ways. A simple method to construct RTT is using a quater-
l
to frame M respectively T . The transformation TTM from the nion slerp between T and M. However, a quaternion slerp
Motion Frame to the trunk follows directly from the definition: entails an implicit weighting between the Sagittal and Coronal
adjustment, which restricts the flexibility of the controller. We
  therefore use the Euler/Cardan angles convention to describe
RTM tTCoM
TTM = (3) the difference from T to M, which implicitly requires the
0 1 separation of RMT into three elemental rotations. Since we only
where RTM describes the current tilt of the trunk and tTCoM consider the tilt of the trunk without the horizontal direction,
the position of the CoM with respect to the trunk frame. By RMT can be composed by only two elemental rotations, one
using a static CoM position together with the identity rotation around the x-axis (Sagittal axis) and one around the y-axis
to construct TTM , we end up with the same local motion as we (Coronal axis), which can be interpolated separately. The
had before. However, by using the current estimation for tilt interpolation itself is realized by a simple two-dimensional
and CoM to transform the motion trajectories, we get a motion proportional controller.
which is automatically adjusted to the current CoM and tilt of Until now, the above described framework tries to maintain
the robot. an upright direction of the trunk. While this may be sufficient
The current tilt is obtained from the orientation RW to support for example a walking motion, it lacks flexibility
T of
the trunk with respect to the world frame W by removing the with respect to general motions, which may intend to maintain
horizontal rotation part of the orientation: a specific tilt, different from upright. However, extending
the above framework to maintain an arbitrary tilt is straight
forward. The second adjustment, responsible for maintaining
RM M W
T = RW RT (4) an upright state, is constructed by an interpolation between the
where RM current state T and the upright situation M. More precisely
W by definition is the rotation around the z-axis about
the horizontal view angle of the robot. In our case, the current by an interpolated rotation between the current z-axis of the
tilt of the robot corresponds to the orientation of the trunk with trunk and the global z-axis. By switching the interpolation
respect to the Motion Frame. In order to plug formula 4 into target from the global z-axis to an arbitrary unit vector, we can
3, we need to invert formula 4: guide the second adjustment towards maintaining an arbitrary
tilt. We call this unit vector describing the intended tilt of
the trunk (intended) leaning vector. With the requirement
RTM = (RM
T )
−1
= (RM W −1
W RT ) (5) to specify an intended leaning vector to the TcMF, each
motion automatically describes its own adjustment target. An
So far motions are closed-loop in the sense that trajectories example motion definition together with the two adjustments
of legs are adjusted to any tilt of the trunk and movement is illustrated in Figure 2.
 intended
leaning by an inverse kinematics solver to determine the target angles
of the involved joints.
Z Z Z

Y Y Y
A key aspect of the TcMF is its generality. The three
components mentioned above can be implemented in a layered
structure. In our case, the IK solver is integrated in the robot
model abstraction, a layer below the actual motion definitions.
θ tilt adjustment
The TcMF can be integrated as an intermediate layer between
ζ leaning the motion model and the IK solver. By extending the interface
adjustment to the TcMF with a Boolean parameter, a motion model can
command the TcMF to replace the dynamic trunk controller
Fig. 2. An automatically adapted motion defined in the virtual motion frame. with a static adjustment to the intended leaning vector. To get
The general motion definition (left) is first adapted to the current tilt of the a completely static adjustment of the motion, we even use a
trunk (center) and then manipulated to push the trunk towards the intended static CoM position in the pelvis of the robot in this case. This
leaning vector (right).
way, the TcMF can be reduced to a shift of the motion about
this static CoM position, which allows for an easy integration
B. Framework Integration of existing motion definitions.

In the context of the TcMF, we consider motion definitions

based on inverse kinematics, that model trajectories in three- IV. A PPLICATIONS
dimensional space for all relevant limbs. A motion using the As explained before, the TcMF is a framework to decouple
TcMF typically consists of three components (see Figure 3): motion definitions from the problem of maintaining a specified
• Motion Model: trunk orientation. In this section we show two applications of
The motion model specifies the basic motion trajec- the TcMF, (omnidirectional) walking and stopping balanced
tories for all relevant limbs of the robot. It provides on one leg (for kicking).
the virtual targets (left/right foot/arm target poses in
the motion frame) together with an intended leaning A. Walking
vector for each cycle.
The walk motion consists of a simple trajectory of a linear
• Trunk Controller: movement for the support leg and a sinoidal movement for the
The trunk controller implements the above presented free leg. The trajectory of the right leg while moving forward
approach. It is responsible for the mapping from the with a step size of 9 cm is shown in Figure 4. The walk is
motion frame to the local trunk frame, dependent on specified as an 18 cycle (50 Hz) motion so a complete step
the current estimations for trunk orientation and CoM cycle takes 0.36 s. The trajectory has been optimized to the
position. maximum motor speed of the simulated Nao of 7.03 degrees
• Inverse Kinematics Solver: per 20 ms. Only in 3 of the 18 cycles the inverse kinematics
An arbitrary IK solver is required to map the adjusted is not limited by the maximum speed as shown in Figure 5.
target poses into joint space.

robot state

Motion Model
virtual targets &
trunk orientation intended leaning
& CoM
estimation
Trunk Controller
adjusted local
targets

IK-Solver
target angles
Fig. 4. Right foot trajectory when walking forward.

Independent from this trajectory, a torso leaning can be

Fig. 3. Component diagram of a motion using the TcMF. specified. Figure 6 shows two examples of static leaning
definitions. Images three and four show a 30 degree forward
In the beginning of each execution cycle, the motion model leaning while walking forward at 0.8 m/s. Images five and six
is triggered to update its target state (e.g. to progress in the show a 20 degree sideward leaning while walking forward at
modelled motion trajectories). After that, the virtual targets 0.5 m/s. The CoM is shown as an olive circle in the wire
and the intended leaning vector are forwarded to the trunk frames. Dynamic leaning definitions are possible. A more
controller, which maps the virtual targets to the local trunk natural example would be to lean forward dependent on the
frame. The resulting adjusted set of target poses is then used forward acceleration of the robot.
Fig. 6. Walk as an application of the TcMF. Image one and two: Nao upright. Image three and four: walking with 30 degrees forward leaning. Image five and
six: walking with 20 degrees sideward leaning.

V. R ESULTS
Experiments in this section have been conducted with the
RoboCup 3D soccer simulation server version 0.6.6 [6] on a
simulated Nao robot.

A. Walking
To demonstrate the effectiveness of TcMF a first experi-
ment let the robot walk forward for four seconds with a speed
of 0.5 m/s. The first two seconds the desired torso leaning was
Fig. 5. Motor speeds of the right leg when walking forward. set to 30 degrees forward leaning. The last two seconds the
desired torso leaning was upright again to see how effective
the trunk controller can adjust to the new leaning.
B. One Leg Stopping Figure 8 shows the x and y components of the torso’s
z-vector over time. Starting from upright position, it takes
A considerable amount of time can be saved for kicking, the robot less than one second to get into 30 degree forward
if the robot does not have to stop on both legs before kicking. leaning. When the desired trunk leaning is changed to upright
It also makes it possible to place the support foot at the side at 2s after the start, the robot is upright again within almost half
of the ball. This section describes how TcMF is used to stop a second. During the whole time the robot remains walking.
on one leg and keeping balance for kicking.

One leg stopping is divided into three motions: a final step

motion, a get on leg motion and a balance on leg motion. The
final step motion is defined by the ball relative position of the
support foot, the desired global kick direction and which of
the two legs should be support foot. The motion itself places
the second last step before getting on one leg. Unlike walking
this motion performs 2/3 of the step distance by the support
leg to shift the CoM to the final balancing leg. Also it makes
sure to turn appropriately. Since the Nao robot is not able to
do inward turns, any remaining turn angle to the right, if the
final support foot is the right foot, have to be done with the
second last step. The get on leg motion performs the final step
and specifies an appropriate trunk leaning. It also makes sure
that the free foot is started to be moved towards a kicking Fig. 8. Adjustment of torso orientation over time.
position. Finally, it also performs any remaining turn to the
desired kick direction. Information like which is the support Figure 9 shows the range of torso rotations in which the
foot is gained from the previous motion by a mechanism called robot is still able to walk. The setup is like above, 2 seconds
motion morphing. The balance on leg motion finally stabilizes walking with a leaning followed by two seconds walking
the robot standing already on one leg, adjusts the height of the upright. The leaning was varied from 30 degrees backward
hip to proper kick height and defines the final trunk leaning leaning to 30 degrees forward leaning and for each of these
required for the kick. The kick itself is currently performed values also from 30 degrees left to 30 degrees right leaning.
in joint space outside the TcMF. The end of each motion is Each point in Figure 9 is the average of 40 runs counting the
shown in Figure 7 for a straight kick. average probability not to fall down.
Fig. 7. Stop on one leg as an application of the TcMF. First image: end of final step motion. Second image: end of get on leg motion. Third image: end of
balance on leg motion. Fourth to Sixth image: same from front.

The robot is able to walk with 30 degrees forward and

25 degrees backward leaning, if there is no sideward leaning.
It is also able to walk with 20 degrees side leaning in
both sides if there is no forward-backward leaning. Even a
15 degrees forward and sideward leaning can be sustained
while walking and corrected upright when desired after two
seconds. The asymmetry of forward versus backward leaning
is due to limitations of the robot’s hip and knee pitch joint’s
working areas. The walked distance in the green area is almost
unaffected by the leaning of the torso as can be seen in
Figure 10 [Videos 1-4].

Fig. 10. Distance walked with leaning.

other robots or accelerations, trunk control does not have a

significant impact on the forward benchmark [Videos 5-8].

TABLE I. C OMPARISON OF BENCHMARK RESULTS ( SIGNIFICANTLY

BETTER RESULTS IN BOLD FONT ).

Benchmark Classic (uncontrolled) Trunk controlled

Forward (m/s) 0.85 0.84
Backward (m/s) 0.75 0.78
Sideward (m/s) 0.48 0.56
Diagonal (m/s) 0.62 0.73
Turning (deg/s) 158.1 179.4

B. One Leg Stopping

To evaluate stopping on one leg, the robot walked straight
for 1.4 seconds and then stopped on the left leg. We varied
Fig. 9. Possibility to not fall down while walking forward with the specified
leaning. the speed at which the robot was walking from 0 to 0.85 m/s
and the desired horizontal turn angle from 0 to 100 degrees.
In a next and less artificial experiment, the robot was Each result is an average of 40 trials.
benchmarked on more typical motions that occur during a Figure 11 shows the reliability of the one leg stopping
soccer game. Each of the results is an average of 100 runs motions. Even with 0.8 m/s the robot is able to stop on one leg
of 6 seconds each. Table I shows the results. The trunk con- reliably for a range of turn angles. For a considerable speed of
trolled motions show significantly better results in backward, 0.6 m/s an effective turn of 50 degrees is reliably achievable.
sideward, diagonal and turning tasks. For the latter two the
difference is directly visible when looking at the motion. Figure 12 shows the effective turning angle for the various
Especially during fast turning the robot drifts from its upright settings. The values are somewhat lower than the desired
orientation from time to time. Trunk control establishes upright angles especially for huge turns. The main reason is that the
orientation quickly, while without trunk control the robot fix amount of time we defined for a step does not suffice for
even falls in six of the 100 turn motions. Since the forward huge turns. The yaw-pitch joint in the hip is at its limit [Videos
walk motion is relatively stable by itself in the absence of 9,10].
 able to support generally stable motions during execution by
maintaining a certain trunk stability. External disturbances and
deviations from the expected tilt are automatically incorporated
in the motion during execution and thus less harmful.
The TcMF is based on the estimations for the position of
the CoM and the orientation of the trunk. While the CoM
can be calculated quite exactly and is less important in the
adjustment, the estimation for the orientation of the trunk is
more complex and error prone and have crucial influence on
the adjustment. Any drift in orientation is directly reflected by
the TcMF and if the drift in orientation estimation becomes
bigger than the interpolation rate of the trunk controller,
the TcMF will rapidly cause the robot to collapse. Another
problem is a flickering orientation estimation, which has direct
impact on the smoothness and amplitudes of the resulting
Fig. 11. Probability of not falling down when stopping at different speeds
and with different turn angles.
motion trajectories. Particularly in the case of a kick trajectory,
where smoothness, speed and accuracy are very important, this
framework might not be the best choice. However, the TcMF is
only dependent on the x- and y-rotation part of the orientation
estimation and well known probabilistic methods, like the
Kalman Filter, provide more stable orientation estimations by
incorporating multiple sensor information.
The current framework uses a proportional control strategy
for adapting to the intended leaning vector. While first results
show an acceptable general control behavior, different control
strategies might perform better across different motions. By
taking the velocities and accelerations of the CoM into ac-
count, more sophisticated control strategies can be applied to
react more systematically to external disturbances. Apart from
classical controllers, machine learning techniques can be used
to learn beneficial control strategies.
Fig. 12. Effective turn amount when stopping.
R EFERENCES
[1] M. Friedmann, J. Kiener, S. Petters, H. Sakamoto, D. Thomas, and O. von
VI. C ONCLUSION Stryk. Versatile, high-quality motions and behavior control of humanoid
soccer robots. In Workshop on Humanoid Soccer Robots of the IEEE-
With the introduction of a virtual reference frame for RAS Int. Conf. on Humanoid Robots (Humanoids), pages 916, 2006
motion definitions in combination with the mapping to the [2] C. Graf and T. Röfer, A Closed-loop 3D-LIPM Gait for the RoboCup
Standard Platform League Humanoid Nashville, TN, USA: Proceedings
trunk frame of the robot, we constructed a framework which of the Fifth Workshop on Humanoid Soccer Robots in conjunction with
is able to dynamically adjust motions to the current tilt and the 2010 IEEE-RAS International Conference on Humanoid Robots,
CoM of the robot. The intended leaning vector allows motion 2010
definitions to describe the intended tilt of the trunk during [3] S. Kajita, F. Kanehiro, K. Kaneko, and K. Yok, The 3D Linear Inverted
the motion in an intuitive way. By using a control strategy to Pendulum Mode : A simple modeling for a biped walking pattern
interpolate between the current and intended tilt of the trunk, generation Pendulum Mode International Conference on Intelligent
Robots and Systems, no. 4, pp. 239-246, 2001
the framework is able to support motions in reaching and
[4] D. Lee, et.al. RoboCup 2011 humanoid league winners RoboCup 2011:
holding an intended tilt during their execution. The presented Robot Soccer World Cup XV, pp. 37–50, Springer, 2012
framework can be easily integrated as an intermediate layer
[5] M. Missura and S. Behnke, Lateral Capture Steps for Bipedal Walking
between the actual motion model and the IK solver. An 11th Bled, Slovenia: IEEE-RAS International Conference on Humanoid
additional parameter, allowing to switch between the dynamic Robots (Humanoids), October 2011
controller and a static transformation, provides more flexibility [6] O. Obst and M. Rollmann, SPARK A Generic Simulator for Physical
and helps integrating existing motion definitions. Multiagent Simulations Computer Systems Science and Engineering,
20(5), September 2005
In general, the approach described in this paper is not able [7] N. Onn, M. Hussein, T. H. Hing, L. W. Ying, M. Z. Zain, M. S. Che Kob,
to dynamically adjust an arbitrary motion in such a way that Human Gait Modelling Considerations of Biped Locomotion for Lower
the robot keeps stable and balanced. The suggested framework Limb Exoskeleton Designs Malaysia: 13th International Conference on
Robotics, Control and Manifacturing Technology, April 2013
merely provides a generalized version of a motion adjustment
to the current tilt and CoM of the robot, which is abstracted [8] S. Yi, B. Zhang, D. Hong, and D. Lee, Active stabilization of a humanoid
robot for impact motions with unknown reaction forces Intelligent
by the intended leaning. This is just one low level approach Robots and Systems (IROS), 2012 IEEE/RSJ International Conference
of maintaining the balance. If the actually executed motion on intelligent robots and systems, pp. 4034–4039, October 2012
is generally unstable, the TcMF will not prevent the robot [9] http://www.et-it.hs-offenburg.de/prof/kdorer/robocup/go/videos/HSR13/
from falling down. However, as results show, the TcMF is