2008 April 1st, October 31st

Engineer student: Sylvain Garsault École Centrale Paris Grande Voie des Vignes 92 295 Châtenay-Malabry Year: 2008 Major: Systèmes Avancés Minor: Conception, développement, recherche Head of Major: Jean-Marie Détriché Coach of Minor: Philippe Gaucher Company: Joint Japanese-French Robotics Laboratory AIST-CNRS AIST-CNRS Joint Japanese-French Robotics Laboratory JRL AIST Tsukuba Central 2, Umezono 1-1-1, Tsukuba 305-8568 Japan Mentor: Professor Abderrahmane Kheddar Tel: +81-29-8619243 Fax: +81-29-8613443

Non-gaited dynamic motion with multi-contact transition

Mission: Given two sets of contact configuration for a humanoid with a non empty intersection, find a control law which brings it from an initial set to the second one. Subject (French): Génération de mouvement dynamique d’un humanoïde pour la transition stable entre deux états de contacts différents d’intersection non-vide.

Traineeship Report – Sylvain Garsault 2008

1

Executive Summary Non-gaited dynamic motion with multi-contact transition French version follows. Following recent works by A. Escande et al. [1], multi-legged robots now have at their disposal an efficient contact planner taking advantage of the numerous possibilities offered by the formation of arbitrary contacts between any part of the robot and any part of the environment. However there is a need for a new controller, generic enough to fully exploit the capabilities of multi-contacts planning. This is mainly due to the lack of a stability criterion for dynamic motion; indeed, Zero Moment Point (ZMP) - based criteria applied to the well-studied case of horizontal walking with coplanar contacts cannot be easily generalized to broader usage. In this work, we propose a new stability criterion for non-gaited dynamic transition between two sets of contacts. It is based on the idea that to each contact configuration it is possible to associate a space of admissible perturbation wrenches. Testing if a given robot dynamics – seen as a perturbation – belongs to the admissible space constitutes the core of this criterion. After preliminary works to understand the issue of robot stability, I realized a static trajectory generator based on an existing Posture Generator. This tool was then successfully used to carry on experiments on the HRP-2 humanoid robot [1]. I first studied the new stability criterion in the frame of a 2D “climber” robot model. This provided the basis to build the final criterion, applied to the case of a simplified 3D robot. Then, as an application I conceived a workflow for a trajectory generator based on the successive optimizations of the robot’s Center of Mass (CoM) trajectory. Finally, a first implementation on the humanoid platform HRP-2 shows promising results, including dynamic walk pattern, stairs climbing, and 3-support-limbs motions, paving the way to future preview controllers based on this new criterion.

Trajectoire dynamique non-cyclique avec transition multi-contacts À la suite des travaux récents d’A. Escande et al. [1], les robots polyarticulés ont à présent à leur disposition un planeur de contacts efficace capable de tirer partie des nombreuses possibilités offertes par la formation de contacts arbitraires avec n’importe quelle partie de leur corps et n’importe quelle partie de l’environnement. Cependant, il n’existe pas de contrôleur assez générique pour exploiter complètement les capacités du planning multi-contacts. Ceci est principalement dû à l’absence de critère de stabilité pour un mouvement dynamique. En effet, les critères fondés sur le Zero Moment Point (ZMP) appliqués au cas bien connu de la marche horizontale avec contacts coplainaires ne se generalisent pas facilement. Dans ce travail, nous proposons un nouveau critère de stabilité pour des transitions dynamiques non cycliques entre deux ensembles de contacts. L’idée directrice est qu’à chaque configuration de Traineeship Report – Sylvain Garsault 2008

2

contacts il est possible d’associer un espace des perturbations admissibles. Le critère repose sur la vérification de l’appartenance à cet espace admissible de la dynamique du robot, vue comme une perturbation. Après des travaux préliminaires qui m’ont aidé à la compréhension de la problématique de l’équilibre en robotique, j’ai réalisé un générateur de trajectoires statiques fondé sur un générateur de posture pré-existant. Cet outil a permi par la suite de conduire avec succès une expérience sur le robot humanoïde HRP-2. J’ai tout d’abord étudié le nouveau critère de stabilité dans le cadre d’un robot grimpeur 2D. Ce travail a servi de base à l’élaboration du critère 3D final, s’appliquant à un robot 3D simplifié. En guise d’application, j’ai posé les bases d’un générateur de trajectoires travaillant par optimisations successives des trajectoires décrites par le centre de masse du robot. Enfin, les premiers tests sur la plateforme humanoïde HRP-2 ont montré des résultats prometteurs, incluant la marche dynamique, l’ascenscion de marches d’escaliers et de pentes inclinées, et des mouvements à 3 membres-supports, ouvrant la voie à une commande prédictive s’appuyant sur le nouveau critère.

Traineeship Report – Sylvain Garsault 2008

3

CONTENTS

Contents Executive Summary

2

Introduction

7

1

2

Preliminary experiments 1.1 Understanding contact planning . . . . . . . . . . . . 1.2 Mission Objective . . . . . . . . . . . . . . . . . . . . 1.2.1 Statement of the problem . . . . . . . . . . . . 1.2.2 Previous attempts . . . . . . . . . . . . . . . . 1.2.3 Framework . . . . . . . . . . . . . . . . . . . 1.3 Trajectory Generation . . . . . . . . . . . . . . . . . . 1.3.1 Inputs . . . . . . . . . . . . . . . . . . . . . . 1.3.2 Structure and algorithm . . . . . . . . . . . . . 1.3.3 Outputs . . . . . . . . . . . . . . . . . . . . . 1.4 Playing the trajectory on the robot . . . . . . . . . . . 1.5 Results . . . . . . . . . . . . . . . . . . . . . . . . . . 1.5.1 First Experiment . . . . . . . . . . . . . . . . 1.5.2 Second Experiment . . . . . . . . . . . . . . . 1.6 Further improvements . . . . . . . . . . . . . . . . . . 1.6.1 Guarded command and trajectory regeneration 1.6.2 Limits . . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

8 8 9 9 9 10 10 10 11 11 13 13 14 14 16 16 16

Dynamic Stability Criterion 2.1 Planar Model . . . . . . . . . . . 2.1.1 Climber Model . . . . . . 2.1.2 Admissible Wrench Space 2.2 Linear Progamming Method . . . 2.2.1 Linearity and convexity . . 2.2.2 Implementation and results 2.3 Analytical Methods . . . . . . . . 2.3.1 Projection Method . . . . 2.3.2 Ray Casting Method . . . 2.4 3D Model . . . . . . . . . . . . . 2.4.1 Perturbation and dynamics 2.4.2 Chosen Criterion . . . . . 2.4.3 Algorithm . . . . . . . . . 2.4.4 Implementation and results

. . . . . . . . . . . . . .

. . . . . . . . . . . . . .

. . . . . . . . . . . . . .

. . . . . . . . . . . . . .

. . . . . . . . . . . . . .

. . . . . . . . . . . . . .

. . . . . . . . . . . . . .

. . . . . . . . . . . . . .

. . . . . . . . . . . . . .

. . . . . . . . . . . . . .

. . . . . . . . . . . . . .

. . . . . . . . . . . . . .

. . . . . . . . . . . . . .

. . . . . . . . . . . . . .

18 18 18 19 20 20 20 21 21 24 29 29 31 32 33

Traineeship Report – Sylvain Garsault 2008

. . . . . . . . . . . . . .

. . . . . . . . . . . . . .

. . . . . . . . . . . . . .

. . . . . . . . . . . . . .

. . . . . . . . . . . . . .

. . . . . . . . . . . . . .

. . . . . . . . . . . . . .

. . . . . . . . . . . . . .

. . . . . . . . . . . . . .

. . . . . . . . . . . . . .

. . . . . . . . . . . . . .

4

CONTENTS

3

Trajectory Generation 3.1 Statement of the problem . . . . . 3.2 Trajectory Generation . . . . . . . 3.2.1 Parametrization . . . . . . 3.2.2 Optimization . . . . . . . 3.2.3 Preview Control . . . . . 3.3 Implementation . . . . . . . . . . 3.3.1 Sequential formulation . . 3.3.2 Algorithm outlines . . . . 3.4 Results . . . . . . . . . . . . . . . 3.4.1 Walking on flat ground . . 3.4.2 Climbing stairs and ramps 3.4.3 Complex scenario . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

35 35 36 36 37 38 38 39 39 42 43 47 50

Conclusion

52

Acknowlegements

53

Bibliography

54

A Work environment A.1 Co-workers . . . . . . . . . . . . . . . . . . . . . . A.2 Work organization . . . . . . . . . . . . . . . . . . . A.3 Safety . . . . . . . . . . . . . . . . . . . . . . . . . A.3.1 Generic safety measures . . . . . . . . . . . A.3.2 Safety measures relative to work with robots B Climber Robot Model B.1 Kinematics . . . . . . . . . B.2 Actions . . . . . . . . . . . B.2.1 Reaction forces . . . B.2.2 Perturbation wrench B.2.3 Gravity action . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

56 56 57 57 57 58

. . . . .

58 58 59 59 60 60

C Available Double Description Method Libraries

60

D Implementation of B-Splines D.1 Imposing initial conditions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . D.2 Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

61 61 61

E Model file format E.1 Header . . . . . . . . . E.2 Splines and Constraints E.3 Initial state . . . . . . E.4 Full trajectory . . . . . E.4.1 List . . . . . . E.4.2 Sequence . . . E.4.3 Automatic . . . E.5 Objective function . . .

62 62 63 63 63 63 63 64 64

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

Traineeship Report – Sylvain Garsault 2008

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

5

CONTENTS

E.6 Files . . . . . . . . . . . . . . . . . . . . . . . . . . . E.7 Step sequence . . . . . . . . . . . . . . . . . . . . . . E.7.1 In the same file . . . . . . . . . . . . . . . . . E.7.2 In a separate file . . . . . . . . . . . . . . . . E.7.3 Import a motion sequence (Posture Generator) E.7.4 Import a scenario . . . . . . . . . . . . . . . .

Traineeship Report – Sylvain Garsault 2008

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

64 64 64 64 64 65

6

CONTENTS

Introduction Several studies on the dynamic stability of multi-legged robots in general, and humanoids in particular, apply mainly to the generation of stable walking patterns. And yet, those motions represent but a fraction of all the possibilities offered by a legged robot. Those possibilities are already harnessed by the latest contact support planners [1], taking advantage of the formation of arbitrary contacts between any part of the environment and any limb of the robot. So far, there does not exist any generic controler able to command a robot to follow one of those support contact sequences. The problem is to realize a stable transition between a contact configuration and another one with a non empty intersection. Some works [2] [3] show that this can be achieved for multi-legged climber robots provided the motion remains quasi-static. I studied these kind of transitions as an introduction to the problem of robot stability [1]. On the other hand, dynamic motions for walking pattern generation [4] can also be generated based on the use of the Zero Moment Point (ZMP), but this method cannot be applied to the general case of non coplanar contacts. The limiting factor is the lack of 3D dynamic stability criterion. In this work, I introduce a new dynamic stability criterion, based on the consideration that whole-body dynamics can be seen as a perturbation action being applied to the center of mass (CoM) of the robot. The feasibility of a motion becomes a consequence of the belonging of a robot dynamical state to the space of admissible perturbations. I first propose a way to compute analytically the space of admissible perturbation wrenches for a given contact configuration (in 2D and 3D with restrictions), and then show how to verify quickly that a perturbation vector is admissible. Finally, I apply this solution to the generation of dynamically stable CoM trajectories, obtained by optimization under the constraint of satisfying the criterion. The results are promising and include dynamic walks, stairs and ramp climbing, dynamic motions using three support limbs, etc. Some of them have been tested successfully in a dynamic simulator [5].

Traineeship Report – Sylvain Garsault 2008

7

CHAPTER 1. PRELIMINARY EXPERIMENTS

Chapter 1

Preliminary experiments To give me a solid first hands-on experience with robots dynamics, A. Kheddar asked me to assist A. Escande in the realisation of an experiment involving the HRP-2 robot. This experiment was to be carried on and recorded in due time to be uploaded along with a paper submission [1] for the 11th International Symposium on Experimental Robotics 2008 (ISER ’08). The main purpose was to demonstrate through a practical example the latest developments of A. Escande’s PhD Thesis on contact planning. This chapter introduces briefly to the notions needed to perform the required task, which is described in a second section.

1.1

Understanding contact planning

Basically, contact planning consists for a humanoid robot in finding a roadmap from its current position to a desired target position. This task is performed by a software called planner, that can be run in advance on a computer or directly on the robot. All planners take as inputs a model of the environment and a target position, and supply a sequence of postures leading to the desired target. While most planners try to find a way by avoiding obstacles of the environment, the driving idea of A. Escande’s planner is to use any part of the robot and any element of the environment as possible contacts paving the way to the target, thus making use of potential obstacles rather than skipping them. A contact is defined as a pair Fr , Fe where Fr is a frame tied to a robot limb and Fe a frame tied to the environment. The output of the planner is a sequence of such contacts. In reality, the planner grows a set of contacts by evaluating the performance of the postures associated to candidate contacts. A posture is defined by any particular configuration of the robot, i.e. the values taken by its degrees of freedom. Finding a posture satisfying some specific conditions is not a trivial problem, and has been addressed by A. Escande through a software component called the Posture Generator. This component, plugged to the planner, is able to generate whenever possible a posture verifying any given set of constraints. Those constraints are usually: securing contacts, avoiding self-collisions (between limbs of the robot), avoiding collisions with the environment (i.e non-intentional contacts), satisfying joints’ limits and keeping static equilibrium. In addition, the Posture Generator provides an evaluation of the current posture according to a given criterion. This score is returned to the planner, which takes it into account in the process of choosing the next best set of contacts. The ultimate output of the planner is a sequence of postures paving the robot’s move towards its target. There are in fact more postures than sets of contacts. The reason is that a “geometrical contact” can take 2 states: used in the robot’s equilibrium or not, which is equivalent to say that the contact is part of the support polygon or not. For instance, making a single step with the right Traineeship Report – Sylvain Garsault 2008

8

CHAPTER 1. PRELIMINARY EXPERIMENTS

foot consists in 4 successive postures. While the left foot remains on the ground and is used to ensure the robot’s equilibrium throughout the step, the right foot is first used, then removed from the support polygon while still in contact with the ground, put in contact with the ground at another place, and finally added back to the support polygon. To keep its balance, the robot has to shift the ground projection of its centre of mass (GCoM) to ensure it is always lying inside the support polygon, as concisely exposed in [6].

1.2 1.2.1

Mission Objective Statement of the problem

In order to show the possibilities of the planner through a practical example, a challenging demonstration was carried out in which the robot was to stand-up from a table starting from a sitting posture on a chair and make a few steps away. The planning of this scenario was ready, and the key postures were available. But to play it on the actual HRP-2 platform, postures are not enough. The robot requires to receive a vector indicating the state of each of its 30 degrees of freedom (DoF) every 5 milliseconds. The problem to solve can be stated simply: given a sequence of key postures from the planner, find a way to generate the robot’s command between those postures.

1.2.2

Previous attempts

Early simpler experiments had already been performed by commanding the robot with a basic interpolation command law between key postures [7]. But this solution presented several flaws: • Since joint angles were simply interpolated, there was nothing to ensure that the robot was not auto-colliding during its transition from a posture to another. This is in particular true when legs are “crossing”. It was not possible to perform a simple walk with this method. • Similarly, even if key postures are avoiding collisions with the environment, interpolation does not guaranty it (fig. 1.1). • For the same reasons, it does not guarantee closed loop chain constraints satisfaction, and does not guaranty to keep contacts, even in a perfect-match model of the environment. • Most importantly, interpolation does not allow control on the ground projection of the Centre of Mass of the robot (GCoM). This means that it can leave temporarily the support polygon during a transition, thus making the robot fall. • In addition – but this is not a limitation related to interpolation – only static equilibrium is guaranteed by key postures. This means that dynamics can result in additional forces, potentially threatening the humanoid’s stability. It is then necessary to command movements slow enough to be close enough to the quasi-static hypothesis. The two first points are purely geometrical aspects and could be manualy checked frame by frame in an appopriate visualization environment. But the last two points require to consider forces and dynamics, and could only be tested with the OpenHRP simulator intended to validate the experiment before playing it on the actual robot. If a problem occured at that step, eveything needed to be checked again after modification. This framework could not be applied efficiently to elaborated experiments such as those allowed by the contact planner. Traineeship Report – Sylvain Garsault 2008

9

CHAPTER 1. PRELIMINARY EXPERIMENTS

Figure 1.1: Interpolation in configuration space does not guaranty avoidance of collision with the environment. Left: A key posture issued by the planner. Middle: the following key posture in the sequence given by the planner. Right: an intermediate posture resulting from interpolation in configration space: the right arm is in collision with the semi-transparent pole, even though the key postures were collision-free.

1.2.3

Framework

After preliminary brainstorming sessions in presence of A. Kheddar and A. Escande, the issues to be addressed and a rough framework of realization were decided. To address at the same time the problems of auto-collisions, collisions with the environment and static stability, it was decided to discretize the transition between two key postures by creating sub-postures, each of which veryfing the above constraints. This could conveniently be carried on with the existing Posture Generator. On the contrary, the problem of dynamics was deemed a really challenging topic of research which did not present trivial solutions in the litterature. Taking into account an important deadline, it was decided to leave this issue to be addressed in the remaining of my internship. Since there is no consideration of timing in the chosen framework, it is not possible to discretize a transition between two steps every 5 milliseconds as required by the robot’s sequence player. Instead, an arbitrary number of intermediate frames are computed by the Posture Generator and –associated with a manually defined timing for each step– interpolated with a time-dependant function computed at step-level in order to generate the configurations at the required framerate.

1.3 1.3.1

Trajectory Generation Inputs

The first part of the solution, involving the discretization of a step delimited by two key postures, has been called Trajectory Generation. It comes as C++ classes I designed as a wrapper around the existing Posture Generator 1.0 by A. Escande. As an input, it receives a single motion sequence file containing a sequence of steps. A step is described by the following information: • The initial key posture, as computed by the planner Traineeship Report – Sylvain Garsault 2008

10

CHAPTER 1. PRELIMINARY EXPERIMENTS

• The fixed contacts configuration, as determined by the planner (and used in the previous posture): contact spot on the robot’s body, position and orientation, belonging to the support polygon. • A discretized trajectory for non-fixed contacts (either contact released at end of last step or created at the beginning of next step). These trajectories are discretized B-Splines partially generated by a 3D Studio Max script, and then tuned manually.

1.3.2

Structure and algorithm

The above information is stored in a hierarchy of objects: motion sequence, steps, fixed contacts, moving contacts. These objects are manipulated by classes that perform the computations, mainly a trajectory builder. The process is detailed in algorithm 1. After importing the motion sequence from the planner, the key postures are first regenerated (in case the objective functions used by the planner and this Posture Generator slightly differ) to ensure continuity with the following generations. Then, for each step, the intermediate postures are generated. The number of frames are either imposed by the number of discretization points on the moving contacts trajectory (if any) or arbitrarily fixed. For each frame, an objective posture is computed by an interpolation method close to the one previously used as trajectory generator. Then the moving contacts (if any) are added to the list of fixed contacts. Finally, a new posture is generated. While the constraints ensure that contacts with the environment are respected, that moving extremities follow an admissible trajectory and static equilibrium is verified, the objective function tends to smooth the transition from initial to final key postures. This is particularly noticeable for steps without moving contacts, where the only difference comes from the state of the geometrical fixed contacts. When one contact enters or leaves the support polygon, the robot has to shift its GCoM, and this is done by asking the generator to favor an intermediate posture.

1.3.3

Outputs

The output of the Trajectory Builder has two main goals: allow an easy visualization of the generated trajectory (for debug purpose mainly), and serve as input for the robot’s command modules. • The motion file contains all the generated postures, written as a sequence of configurations. Degrees of freedom are separated into internal ones, free-flyer’s position and free-flyer’s orientation. It can be read by a scene visualization software developed by the JRL to allow for user-friendly inspection of the generated trajectory in its environment, in particular for collision checks, It also provides the base information to the robot’s command modules, both in simulator and on the actual platform. • The script file contains information about contacts state, needed to operate the robot’s stabilizer from the script system onboard HRP-2 (see below). • The ZMP file contains control data to command the robot’s stabilizer written as a sequence of ZMP desired positions (see below). The modular aspect of the software makes it simple to add new export methods, and I have developed several additional modules to assist researchers in the process of realizing actually an experiment: visualization of key-postures, human-readable step-by-step information about contacts (useful help for the person in charge of the experiment’s safety), etc. Traineeship Report – Sylvain Garsault 2008

11

CHAPTER 1. PRELIMINARY EXPERIMENTS

Algorithm 1 Trajectory Generation 1: Read motion sequence 2: Load anti-collisions data 3: for each step do 4: Create set of fixed contacts for this step 5: Regenerate initial and final postures: qinit and qfinal 6: m = number of moving contacts 7: if m! = 0 then 8: n = number of discretization points for end-effectors 9: else 10: n = arbitrary integer 11: end if 12: for i ∈ [0 · · · n − 1] do 13: Get moving contacts position (if any) 14: Add moving contacts to the set of contacts for this frame (if any) 15: Compute the interpolation factor xi ∈ [0, 1], x0 < · · · < xi−1 < xi < xi+1 < · · · < xn 16: qobj = xi qinit + (1 − xi )qfinal 17: Compute posture(contacts, anti-collision, qobj ) 18: end for 19: end for 20: for each posture do 21: Export robot’s configuration 22: Export GCoM position, Chest orientation and Stabilizer’s state 23: end for

Traineeship Report – Sylvain Garsault 2008

12

CHAPTER 1. PRELIMINARY EXPERIMENTS

1.4

Playing the trajectory on the robot

The process of carrying on an experiment in which the robot is following a pre-computed trajectory is called “playing” a trajectory. The HRP-2 robot is equipped with a Linux-based real-time system, which updates directly a proportional-derivative controller for each actuated joint. It also features a stabilizer, to counter the unbalancing effects of perturbations and compliance in the joints (ankle in particular). This stabilizer has knowledge of the robot’s state through the configuration vector (internal DoFs) and accelerometers located in its chest (free-flyer’s DoFs). The user has to provide the desired Zero-Moment Point (ZMP) position [8] [9] expressed in the robot’s waist frame and the desired waist orientation. Unfortunately, the provided stabilizer is only meant to be used for walking activities, and its behaviour is potentially harmful when more than two contacts are used, or when contacts are made with other limbs than the feet. For that reason, the stabilizer must be shut down automatically in those cases, but kept on the rest of the time. The usual way to play a trajectory is to provide a sequence player, a control script and motion data files. • The sequence player is a C++ plugin providing the system with a new command for the actuated DoFs every 5 milliseconds. Since data read from the motion and ZMP files are not expressed at that frequency (they do not have a notion of time), it is necessary to perform an adaptaptation prior to feeding the controllers. Each step is manually given a duration in seconds, with in mind that the equilibrium is only valid for a quasi-static motion. These timings are compiled in a speed file, also part of the motion data sent to the robot. To obtain the vector of configuration at a given time (in practise, a multiple of 5ms), the data is interpolated through time according to that manually written step-timing file. The result is sent respectively to the joints controller and the stabilizer. I also implemented logging capabilities, for post-experiment analysis. • The control script is written in Python and manages the robot’s behaviour at higher level. It interacts with the researcher carrying the experiment through a Graphical User Interface called the Auditor, offering some control during the execution of the trajectory (start / stop / pause). It is also in charge of switching on and off the stabilizer, according to the script file provided. • The motion data files comprise the 3 outputs of the Trajectory Generator (motion, script and ZMP files, see 1.3.3) and the manually adjusted step-timing file. Experiments with the robots obey to strict procedures, in order to guaranty workers’ and robots’ safety. The first compulsory measure is to validate the scenario with the dedicated simulator called OpenHRP-2, prior to any real test. The simulator’s environment is an exact replica of the robot’s onboard system, so that the scripts and binary plugins tested on the simulator are exactly the ones copied on the robot once the test passed.

1.5

Results

The initial objective of my work was to illustrate the contact planner capabilities for an article [1] submitted to the International Symposium of Experimental Robotics (ISER 2008). The Trajectory Generator was applied to this example and proved to work reliably, providing trajectories that were successfully played and on time for the paper submission. The same set of tools and procedures has been later successfully applied to another experiment realized in order to illustrate the capability of the planner to handle task constraints. Traineeship Report – Sylvain Garsault 2008

13

CHAPTER 1. PRELIMINARY EXPERIMENTS

1.5.1

First Experiment

Context To illustrate the capabilities of the contact planner, a challenging scenario was elaborated in which the robot, initially seated at a table, was meant to stand up from its chair and make a few steps away from the table. The very constrained nature of the initial stance forces the robot to seek the help of contacts made with its hands: first on the table, then twice on the top of the chair. This illustrates clearly the unique aspect of this planner, being able to make use of obstacles in the environment as helpers in the completion of its motion towards the final objective. Workflow The output of the planner gives 81 different postures, meaning that the whole motion sequence comprises 80 steps. The trajectory for moving end-effectors is then designed by hand with 3D Studio Max by Autodesk. A special plugin written by A. Escande auto-generates the splines, greatly simplifying the task of the end-user who only has to check if the proposed trajectory seems admissible (in particular does not lead to collisions) and modifiy them if needed. This plugin outputs a motion file in which the splines have been discretized into a given number of points (typical precision spans from 15 to 30 frames). This motion file constitutes the input data for the Trajectory Generator. Data is loaded and frames are generated following the algorithm in 1. If the optimization process fails for some frames, the problem is identified using a visualization interface for the generated motion. Often, the issue is solved by tuning a difficult spline or decreasing the safety margin for anti-collision detection with some specific limbs and the environment. Once a complete motion has been successfully generated, the input is validated, and usually remains unchanged until the end. Simulation and Experiment The 3 motion files and a first estimation of the speed file are then loaded on the OpenHRP-2 simulator. The whole motion is simulated, ending with the successful termination of the script or with the robot falling. In this latter case, a particular step has been played too quickly, violating the quasi-static approximation. Slowing down the step by extending its duration in the speed file would solve the issue. Once the simulations have been successfully passed with gradually more conservative parameters, the experiment is said to be validated, and trials on the real platform can begin. Tight security measures must be taken while manipulating with the robots (see A.3.2). In particular, a permanent member of the JRL would validate the simulation and monitor any experiment carried on afterwards. In the case of this experiment, the full 80-steps motion sequence was validated and successfully played the same evening (fig. 1.2).

1.5.2

Second Experiment

Context The Trajectory Generator proved usefull in generating postures with task constraints. This experiment consists for the HRP-2 robot to sit on a chair while holding in its right hand a glass of water that must be kept perfectly horizontal. Traineeship Report – Sylvain Garsault 2008

14

CHAPTER 1. PRELIMINARY EXPERIMENTS

Figure 1.2: Some representative postures taken during the experiment. The robot, initially seating on the chair, needs to lean on the table then on the chair before standing up and walking away.

Traineeship Report – Sylvain Garsault 2008

15

CHAPTER 1. PRELIMINARY EXPERIMENTS

Workflow The workflow is almost the same used for the first experiment, except that in order to shorten the length of the manipulation, the output of the planner was filtered to keep only interesting parts, 30 steps in total. To simplify this task, a .NET Graphical User Interface was written by A. Escande. Simulation and Experiment Demonstrating the efficiency of the proposed method, a successful trajectory was generated, simulated and played on the same day the scenario was established.

1.6 1.6.1

Further improvements Guarded command and trajectory regeneration

Following the first brainstormings with A. Kheddar and A. Escande, the idea of guarded command and on-the-fly trajectory re-generation had been mentionned. The principle is to make the experiment robust to slight discrepancies on actual obstacle positions. For that purpose, end-effectors’ trajectories were to be voluntarily prolongated through the obstacles supposed to be used as contacts, and stopping the actual movement only when firm contact was detected by force sensors. This was to prevent the end-effector to stop before contact (obstacle positionned too far) or push too hard on it and jeopardize the robot’s balance (obstacle positionned too close). In order to go on with the rest of the pre-computed motion, the following trajectories were to be re-computed online (meaning during the experiment) by taking into account the new actual position of the contacts and the new initial posture. To perform that task, I developped a class named Trajectory Regenerator, meant to be called at the beginning of a new step with the new posture as input. The principle was simple: fixed contacts were updated by direct kinematics with their new configuration in order to be help in place during the following step, and moving contacts trajectories were smoothly interpolated between current erroneous position and final absolute position as stated in the motion sequence, following: newPos(t) = (1 − c(t))ePos + refPos(t)

(1.1)

where newPos and refPos denote respectively the actual and the reference contact positions, and ePos the initial position error ePos = newPos(0) − refPos(0). c : [0, T ] → [0, 1] is the interpolation function, ensuring that contact breakage and establishment are made properly. The heavy computations required by the online trajectory regeneration could not be performed on the robot itself, meaning that an external computer had to be used for this task in a remotebrain scheme. The robot would have to host a CORBA server communicating with the remote client through a WiFi connection. In addition, a completely new plugin able to manage onboard force sensors had to be designed. While all this technical knowledge was available among the JRL members, those tasks clearly reached beyond the scope of my internship and have not been carried on during my stay in Tsukuba. The good performance obtained with blind replay (by opposition to this responsive solution) could not justify this additionnal programmation effort.

1.6.2

Limits

While the Trajectory Generator was successfully used twice for demonstrations that could not have been performed otherwise, it has strong limitations, mainly due to the hypothesis on which it is based Traineeship Report – Sylvain Garsault 2008

16

CHAPTER 1. PRELIMINARY EXPERIMENTS

• The strongest of these hypothesis is the quasi-static approximation. The robot’s weight approaches 60kg, quickly generating intertial forces when put into motion. In addition, each of the roughly 10kg-legs create unbalancing rotation moments that cannot be taken into account in the current CoM positionning. This leads to slow movements, barely comparable with a natural human motion. • The hypothesis of rigid body is clearly not verified with some joints, in particular the ankle. The spring effect of these joints, even partly compensated by the stabilizer, creates strong unbalancing effects and modify the instant of contact establishment and release (with possibly dangerous contact bouncing). To counter these noxious effects, it was necessary to tune the equilibrium criterion used by the Posture Generator, for example by reducing the size of support polygons, or by shifting the stability zone of the feet in the inner direction. Those dynamics-related limitations justify the need for a new stability criterion taking into account the dynamical effects. Other problems to be solved are depicted in [10].

Traineeship Report – Sylvain Garsault 2008

17

CHAPTER 2. DYNAMIC STABILITY CRITERION

Chapter 2

Dynamic Stability Criterion The concept of stability criterion is exposed in [6], and is basically a function able to determine, given a certain number of parameters, if a legged robot can fulfill its main objective. Cr : x ∈ X 7→ {true, false}

(2.1)

where x is a vector belonging to the space of robot states X. Cr is a stability criterion if Cr(x) = true ⇒ x is feasible

(2.2)

The definition of a feasible motion x depends on the model of study. This chapter presents the successive stability criteria I studied and implemented on mechanical models of growing complexity. All rely on the same idea that whenever there exists a point in the parameters space ensuring the robot stability, it is possible to determine the set of admissible perturbation actions that keep the robot stable. These perturbation actions belong to a space, named below the admissible wrench space W. For a given set of parameters required to build W, the stability criterion simply determines if a perturbation – the dynamics can be seen as such – is lying inside of W.

2.1

Planar Model

The first studied model is a planar point-mass robot in evolution in a 2D space. In all this section, the motion is supposed to be quasi-static, thus neglecting any dynamics effect.

2.1.1

Climber Model

Most previous works on planar robots in static equilibrium concentrated on finding if there exist forces applied to the robot that respect a number of constraints (uniterality, non-sliding condition, joint torque limits [2]) or finding the admissible positions for the robot’s CoM [3]. While reusing the same formalism for a climber-type robot, I focused instead on determining, for a given robot configuration, the space of admissible perturbation forces that can be applied without breaking any constraint. The model is a planar climber (fig. 2.1) whose mass is entirely concentrated in its center C. It can form a contact with the environment using the tip Ci of any of its 3 legs, rigidly attached to the free-flyer in Ai . Each leg (fig. 2.2) consists of 2 limbs and 2 actuated joints (Ai , Bi ). The kinematics used for this model are detailed in Appendix B. A perturbation wrench fpert is applied to a point U belonging to the free-flyer. Traineeship Report – Sylvain Garsault 2008

18

CHAPTER 2. DYNAMIC STABILITY CRITERION

Figure 2.1: Mechanical model of a planar climber robot.

Figure 2.2: Leg modeling.

2.1.2

Admissible Wrench Space

Torques formulation Let T denote the joints torque vector. Since the free-flyer DoFs are not actuated, the first 3 torques are nil. T = [0, 0, 0, Tb1 , Tc1 , Tb2 , Tc2 , Tb3 , Tc3 ]T (2.3) Each action x applied on the robot generates the following torque vector: τa = JTa .a

(2.4)

with Ja = J(q)a the Jacobian matrix written at the point where the action is applied. The static equilibrium equation projected on joints torques leads to: X

T=

τa

(2.5)

a∈{actions}

where the actions are defined in B. Conditions of existence In this model, two types of constraints are taken into account: conditions on contacts and joints torque limits. Contacts are modeled as unilateral point-on-surface contacts with a dry friction coefficient µ. This is translated in 2D by a set of 3 inequalities:     fxi ≤ µi fyi

−f

≤ µi fyi

xi    f ≥0 yi

Traineeship Report – Sylvain Garsault 2008

i ∈ {1, 2, 3}

(2.6)

19

CHAPTER 2. DYNAMIC STABILITY CRITERION

Furthermore, joint actuators are limited in term of maximum saturation torque they can withstand: (

Ti ≥ −Sati Ti ≤ Sati

i ∈≥ 3

(2.7)

Definition For a given configuration q, the problem to solve is: what are the admissible perturbation wrenches fpert verifying (2.5), (2.6) and (2.7)? We define the admissible wrench space W: Criterion 1. w ∈ Rn is admissible ⇔ w ∈ W The following sections describe three different methods to obtain W.

2.2 2.2.1

Linear Progamming Method Linearity and convexity

For a fixed configuration vector q, the problem is linear in the forces and torques. This leads to two remarks: 1. W is a convex polytope, possibly unbounded (case of singularities in joints configuration). 2. Knowing 2 points w1 , w2 ∈ W, then the whole segment [w1 w2 ] is also in W. A first approach, designed to give a rough idea of the admissible wrench space’s form, is based on the second remark. W is 3-dimensionnal; to simplify the visualization, an arbitrary planar section of W is chosen, for example by imposing mpert = 0. Then, it is easy to verify that the point w1 = [0, 0, 0] belongs to W. Now, if one is able to find the furthest point w2 ∈ W in a given direction, then [w1 w2 ] ∈ W. The area contained inside the plot diagram obtained by repeating this operation in a sufficient number of directions is a rough representation of the admissible space (fig. 2.4) With the notations introduced in Appendix B, applying the largest perturbation force in a given direction is equivalent to choosing a properly oriented KU frame and maximizing fpert . This is a linear programming formulation: max(fpert ) under (2.5), (2.6) and (2.7)

2.2.2

(2.8)

Implementation and results

An implementation of this algorithm has been written in Maple language, and executed on a simple case: only two coplanar horizontal contacts are activated (fig. 2.3), the third leg being unused. The figure 2.4 shows the obtained representation of W. The form of the obtained intersection reveals the convex linear aspect of W. This solution is simple and quick to compute (linear in the number of sampling points), but it only gives a sampled version of the admissible space. This means that it does not provide a quick method to determine if a given perturbation wrench w is admissible. To do so, it is necessary to find the direction of w, compute the maximum wmax in that direction and compare it with w. Criterion 2. w ∈ W ⇔

wmax .w kwmax k2

≤1

To have a faster criterion, it is necessary to find methods providing an analytical expression of the polytope W. Traineeship Report – Sylvain Garsault 2008

20

CHAPTER 2. DYNAMIC STABILITY CRITERION

Figure 2.3: Climber Robot model ported to the Maple environment. This shows the particular configuration used to generate fig. 2.4. Green: free-flyer. Red: limbs. Blue: possible contacts (only two activated here). The application point of fpert is U (0.02, 0.01).

2.3

Analytical Methods

The two following methods answer to the same question by giving an analytical (by opposition to numeric) expression to the criterion w ∈ W.

2.3.1

Projection Method

Matrix formulation Starting from the previous remark that W was a convex polytope, the goal of this method is to obtain a set of inequations defining affine half-spaces in the 2D plane (fpert x , fpert y ); the intersection of those half-spaces define the intersection of W with the hyperplane mpert = 0. Relation (2.5) gives 9 equations: the first 3 are Newton and Euler laws, the next 6 equations link joint torques with forces applied on end-effectors. Thus it is possible to choose the vector of variables for this problem as: x = [fx1 , fy1 , fx2 , fy2 , fx3 , fy3 ]

(2.9)

The first 3 equations of (2.5) are expressed in matrix form Cx + C0 fpert = d

(2.10)

where fpert = (fpert x , fpert y ) and d is the gravitation term. Similarly, the contact inequalities (2.6) are translated in matrix form: 



 

−µi " # 0   fxi   −1 −µi  0   fy ≤   i 0 −1 0 1

Traineeship Report – Sylvain Garsault 2008

(2.11)

21

CHAPTER 2. DYNAMIC STABILITY CRITERION

Figure 2.4: Intersection of W with the plane mpert = 0 sampled in 300 different directions. Joint saturations are treated the same way: 

Ti





Sati



    −Ti      = Ttorques x ≤ Sati   .   .  . .

.

(2.12)

.

where Ttorques is obtained by the last 6 lines of (2.5). These contact and torques - related inequalities boil down to the following matrix formulation: Ax + A0 fpert ≤ b

(2.13)

In fact, A0 is a nul-matrix, due to the fact that: • perturbation force fpert is applied directly on the root body of the robot: in its Jacobian, the columns related to the actuated degrees of freedom (DoF) are nul, while at the same time the torque limitations only appear on those actuated DoF. • contact-related inequalities only involve contact forces. In the end, the inequalities are summed up in: Ax ≤ b Traineeship Report – Sylvain Garsault 2008

(2.14) 22

CHAPTER 2. DYNAMIC STABILITY CRITERION

Case of only two contacts To simplify the study, only contacts (2) and (3) are now considered. This reduces the state vector to x = [fx2 , fy2 , fx3 , fy3 ]T . Relation (2.14) results in 14 inequalities: 4 torque limits ×2 (minimum and maximum), 3 non-sliding conditions per contact ×2 contacts. From (2.10), it is possible to extract 3 linear relations: • First 2 lines of (2.10) express fpert x and fpert y as functions of x.  fpert = d[1] − (Cx)[1] x f

pert y

= d[2] − (Cx)[2]

(2.15)

where [i] indicates the ith component of the vector. • Injecting those expressions in the third line (expressing that the total moment applied to the robot must be zero) will result in a linear relation between components of x. (Cx)[3] + (C0 fpert )[3] = d[3]

(2.16)

Algorithm The force space in which x is evolving is 4-dimensionnal. The idea behind algorithm (2) is to find all the vertices defining the 4-dimension polytope W. In the contact forces space, such a candidate vertex is defined as the intersection of 4 hyperplanes, corresponding to 4 independent linear constraints (or equalities) on x. Three are chosen among the 14 defined by each row of (2.14). The fourth is the intersection (2.16). Then, each candidate vertex is tested against all constraints; only those abiding by all constraints in (2.14) are kept as admissible polytope’s vertices. Finally, these vertices are projected on the fpert 2D-space using relation (2.15). Algorithm 2 Projection Method 1:  ← 10−6 2: for (i, j, k) ∈ [1..14], j < i, k < j do 3: equalities ← {(Ax − b)[i, j, k] = 0, (Cx + C0 fpert − d)[3] = 0} 4: sol ← solution of equalities 5: if sol has a solution x and Ax − b ≤  then 6: Vertices ← Vertices, x 7: end if 8: end for 9: for each vertex x ∈ Vertices do 10: projection ← projection, fpert = (d − Cx)[1, 2] 11: end for 12: return Convex Hull of projection

Criterion 3. w ∈ W ⇔ w ∈ convex hull of the admissible contact forces projected in the perturbation space. The Projection Method offers an analytical expression of W as a polytope defined by its vertices, whatever its dimension. Note that determining if a point belongs to a convex hull requires to solve a LP, as in (2.2). Traineeship Report – Sylvain Garsault 2008

23

CHAPTER 2. DYNAMIC STABILITY CRITERION

Results As can bee seen on fig. 2.5, the Projection Method leads to the same results as the optimization method. But execution of algorithm (2) under Maple 11 takes around 30s for this simple case, and the complexity is a combinatorial function of the number of constraints and degrees of freedom. Indeed, there is no simple way to derive the final set of projected vertices from the full set of vertices obtained before line 5 of algorithm (2). It is first necessaty to eliminate all the vertices that violate one or more contraints, and then project them on the (fpert x , fpert y ) plane to finally only keep the convex hull. To scale up to a 3D real robot, a more efficient method needs to be found.

Figure 2.5: Comparison between Optimization Method (red) and Projection Method (blue). Note the additional blue point in the middle of the space, resulting from the projection of a polytope’s vertex on this particular plane. Only points belonging to the convex hull will be kept in the end.

2.3.2

Ray Casting Method

This method is taking advantage of the previous two solutions: while keeping the intuitive LP maximization of the perturbation in a given direction (2.2), it also profits from the polytope properties and the matrix formulation developped in the projection method (2.3.1). Traineeship Report – Sylvain Garsault 2008

24

CHAPTER 2. DYNAMIC STABILITY CRITERION

Matrix formulation We slightly re-write the matrix formulation associated with the Linear Program (2.8) to account for the directionnal search. The variable vector becomes x = [fpert , fx2 , fy2 , fx3 , fy3 ]T , where fpert is the intensity of the perturbation force. The objective function is defined as fobj = c.x where c = [1, 0, 0, 0, 0], to maximize the intensity of the perturbation force. fpert is parametrized by choosing a direction α in which a “ray” will be cast: fpert = fpert .α where α = [cos(θ), sin(θ)].(2.10) is re-written as Aeq x = beq (2.17) where beq = d and h

i

T Aeq = JU α C

(2.18)

Aineq x ≤ bineq

(2.19)

h

(2.20)

(2.14) is re-written as where bineq = b and i

Aeq = 0 A

For a given α, the Linear Program is then stated as follows: Maximize c.x under

 Aeq x = beq A

ineq x

(2.21)

≤ bineq

Using the matrix formulation, the Linear Program (2.21) is first solved for a given initial α. This gives a feasible x (i) for which fpert is maximum. This x solution is injected in (2.19) to determine which constraints are saturated for this solution. This translates to inequalities being actually equalities in some (at least two) lines of (2.19). Numerically, a constraint is declared saturated whenever |aT .x − bi | <  with  small (typically 10−6 ). If there are more than two saturated constraints, it means that a corner of the admissible space has been reached. A first naive solution consists in casting a new ray in a slightly different direction. The problem is then re-written for a general direction (no dependancy in α) with x0 = [fpertx , fperty , fx2 , fy2 , fx3 , fy3 ]T

(2.22)

giving the similar formulation: A0 eq x0 = b0eq

(2.23)

A0 ineq x0 ≤ b0ineq

(2.24)

where

h

A0ineq = 0 0 A

i

(2.25)

In (2.24), only the two lines (i, j) associated with the previously determined saturated constraints are kept. These two active constraints give two equalities, and together with the three equalities derived from (2.17), they form a system of 5 independant equations and 6 variables. A∗ x0 = b∗ with



A0 eq



(2.26) 

b0eq



∗ 0   0  A∗ =   A ineq [i]  and b =  bineq [i]  b0ineq [j] A0 ineq [j]



Traineeship Report – Sylvain Garsault 2008







25

CHAPTER 2. DYNAMIC STABILITY CRITERION

A0 ineq [i] being the ith line of A0 ineq Thus, it is possible to express 5 variables of the x0 vector as affine functions of fperti , i ∈ {x, y}. This results in an affine relation between fpertx and fperty (ii) defining a line in the perturbation force plane and the vector of computed reaction forces [fx2 , fy2 , fx3 , fy3 ] (iii). (i), (ii) and (iii) constitute the result of a ray casting. Algorithm The idea of this method is to use a very limited number of LP resolutions to build the admissible perturbation force space. The algorithm is summed up in fig. 2.6. A first ray is cast in a random direction (a), then in the opposite direction (b). If the lines are not parallel, this ensures to find two lines intersecting at a point. A third ray is cast in the direction opposite to this point, giving a third line (c). In case two lines are parallel, the ray is cast in the direction of the lines, modulo π. At the end of this step, the admissible space is closed. Then, for each intersection point, the constraints are checked by injecting the computed x vector in (2.19). If all the constraints are respected, then the point is a vertex of W, and the remaining points are examined. If not, then there exists a line materializing the violated constraint, found by solving the LP problem in the direction of the violating intersection point (d). The new line and its two new intersection points are added to the list. The algorithm stops when there is no violating point left.

Figure 2.6: Ray Casting Method. Chose a random direction, cast a ray and find the line associated to the saturated constraint (a). Cast a ray in the opposite (+π) direction (b). Cast a ray in the direction opposite to the intersection point (c). Evaluate in turn each intersection point, cast a ray if needed (d).

Criterion The lineBuffer returned by the Ray Casting algorithm is to be interpreted as an intersection of affine half-spaces of the form aT .x − b ≤ 0. Stacking those inequalities leads to a compact matrix Traineeship Report – Sylvain Garsault 2008

26

CHAPTER 2. DYNAMIC STABILITY CRITERION

Algorithm 3 Ray Casting Method 1: α ← rand() ∗ 2π 2: ptIdx ← 0 3: lineBuffer ← lineBuffer, Cast(α) 4: lineBuffer ← lineBuffer, Cast(α + π) 5: if pt ← Intersection(lineBuffer[1], lineBuffer[2]) then 6: ptIdx ← ptIdx + 1 7: ptBuffer[ptIdx] ← pt 8: lineBuffer ← lineBuffer, Cast(Angle(pt)) 9: Add Intersection Points 10: else 11: lineBuffer ← lineBuffer, Cast(Angle(lineBuffer[1])) 12: lineBuffer ← lineBuffer, Cast(Angle(lineBuffer[1] + π)) 13: Add Intersection Points 14: end if 15: while ptIdx 6= 0 do 16: pt ← ptBuffer[ptIdx] 17: if ViolateConstraints(pt) then 18: lineBuffer ← lineBuffer, Cast(Angle(pt)) 19: Add Intersection Points 20: else 21: ptIdx ← ptIdx − 1 22: end if 23: end while 24: return lineBuffer

Algorithm 4 Cast(α) 1: Write LP problem for α 2: x ← Solve(LP problem) 3: for all line i in Aeq do 4: if |Aeq [i]x − be q[i]| <  then 5: activeConstraints ← activeConstraints, i 6: end if 7: end for 8: Write the 5 × 6 system 9: Solve it in fpertx 10: x∗ (fpertx ) ← [fperty , fx2 , fy2 , fx3 , fy3 ]T (fpertx ) 11: return x, x∗

Traineeship Report – Sylvain Garsault 2008

27

CHAPTER 2. DYNAMIC STABILITY CRITERION

formulation: ARC x − bRC ≤ 0

(2.27)

This analytical representation of W as a polytope defined by its limiting hyperplanes makes the criterion very easy to compute. It becomes a simple sign check on each component of the result vector: Criterion 4. w ∈ W ⇔ ARC w ≤ bRC

Implementation and results The result is shown on fig. 2.7, for a perturbation force applied on U (u1 = 0.02, u2 = 0). Without optimization, the whole admissible space takes around 2 seconds to be computed under Maple 11.

Figure 2.7: Admissible perturbation force space, generated with u1 = 0.02, u2 = 0. Each active constraint has a color code: sliding (red), unilaterality (green), torque (blue). A RGB synthesis is then generated to reflect multiple active constraints. The ray-casting algorithm has then been ported to C++. A wrapper was written to use the LPsolve 5.5 C-library [11] as numeric linear program solver. A Graphical User Interface (GUI) written in .NET was then developped to visualize the admissible space in real-time. The perturbation force application point and all the numerical parameters of the model can be changed in a user-friendly manner, and the results displayed immediately in the graph window. Note that even though 3 legs are displayed, only 2 are used for stability in the implemented algorithm. This result was presented during the JRL meeting held on July, 10th. Traineeship Report – Sylvain Garsault 2008

28

CHAPTER 2. DYNAMIC STABILITY CRITERION

Figure 2.8: The Ray Casting algorithm ported to C++ with a .NET interface

2.4

3D Model

The previous solutions gave a good overview of the underlying at one’s disposal to build a representation of W. But they suffer from limitations mainly due to the simplifications implied by working in a 2D-space. For instance, the concept of opposite direction used in the Ray Casting method (2.3.2) do not generalize easily to higher dimensions, not mentionning the fast-increasing complexity of algorithms used for instance in the projection method (2.3.1).

2.4.1

Perturbation and dynamics

The driving idea behind this new stability criterion for a robot evolving in the 3 dimensions of space is to consider dynamics as a perturbation w. This perturbation action is 6-dimensionnal (one force vector and one moment) and is applied to the CoM of the robot, adding its contribution to the gravity and contact actions. The dynamics of a rigid multi-body 3D robot can be written ( [9] and [6]): " # " # " # " # " T # M1 (q) N1 (q, q) ˙ G1 (q) τ J1 (q) q¨ + = + + T f (2.28) M2 (q) N2 (q, q) ˙ G2 (q) 0 J2 (q) where M is the inertia matrix, N the Coriolis effects, G the gravity action, τ the vector of applied torques on the robot’s joints, and JT the transpose Jacobian associated to the generalized contact forces vector f = [fc i ]T . The subscript 1 refers to the actuated degrees of freedom (DoF); the subscript 2 to the unactuated part of the dynamics. This latter part is 6-dimensionnal, and summarizes the Newton-Euler laws for a rigid body in 3D space. Introducing the perturbation wrench w, (2.28) can be re-written: "

M1 (q) 0

#

"

q¨ +

#

N1 (q, q) ˙ 0

"

=

G1 (q) G2 (q)

Traineeship Report – Sylvain Garsault 2008

#

" #

τ

+

0

+

" T # J1 (q)

JT2 (q)

"

f+

0 JTpert

#

w

(2.29)

29

CHAPTER 2. DYNAMIC STABILITY CRITERION

where Jpert = Id6 and w = −M2 (q)¨ q − N2 (q, q) ˙

(2.30)

Admissible reaction forces and joint torques A movement is feasible if it generates admissible reaction wrenches and joint torques. Contacts are modelized as point-on-surface contacts: no reaction torque can be applied, only a force. Such a reaction force is admissible if it respects non-sliding and uniterality conditions: kft k ≤ µfn

(2.31)

where µ is the dry friction coefficient and f is split into its respectively normal fn = fn n and tangential ft components to the contact surface. This defines the admissible space for a reaction force as an unbounded 3D cone C centered about the normal n at the contact point (fig. 2.9), and as narrow as µ is low. Unfortunately, (2.31) is a quadratic relation. To keep the advantages of a linear formulation, a classical approximation [12] [13] consists in using a linearized cone C 0 instead (fig. 2.10).

Figure 2.9: The friction cone according to Coulomb’s law. A force belonging to C 0 verifies a linear relation, i.e. there exists a matrix Acone and a vector bcone such that: f ∈ C 0 ⇔ Acone f ≤ bcone (2.32) The number of lines in Acone and bcone reflects the number of facets of C 0 : increasing the precision of the approximation (number of facets) increases the computational cost (number of lines) of the criterion (2.32). In addition, this approximation is conservative, meaning that C 0 ⊂ C. For multiple contacts, an admissible set of contact forces is simply defined as a set of forces belonging to their respective friction cone. Thus the generalized force vector f in (2.29) is admissible if it belongs to F, the product space composed of the friction cones at each contact. A joint torque τi is admissible if its intensity stays in a range ±τlimiti permitted by its actuator. With n the number of actuated DoF on the robot, let T be a polytope in dimension n whose hyperfaces are defined by τi = ±τlimiti , ∀i ∈ [1 · · · n]. Finally, (2.28) describes a feasible motion if f ∈ F and τ ∈ T . For a given robot’s state X = [q, q, ˙ q¨], it is possible to express limits on τ as inequalities on f using the upper part of (2.29): M1 (q)¨ q + N1 (q, q) ˙ − JT1 (q)f ≤ τlimit Traineeship Report – Sylvain Garsault 2008

(2.33) 30

CHAPTER 2. DYNAMIC STABILITY CRITERION

of the form A(q)1 f + b(q, q, ˙ q¨)1 ≤ 0

(2.34)

thus defining a new polytope F 0 (X) and a new criterion: f is admissible ⇔ f ∈ F ∩ F 0 (X). Now looking at the lower part of (2.29) corresponding to the unactuated part of the dynamics: − JTpert w − G2 = JT2 f

(2.35)

A2 w + b2 = JT2 f

(2.36)

of the form: Let W = JT2 (F ∩ F 0 (X)). Finally, the stability criterion can simply be defined as a test on: Criterion 5. w admissible ⇔ A2 w + b2 ∈ W

2.4.2

Chosen Criterion

First to be noticed while looking at the torque-related inequalities (2.34): if it is necessary to apply torque limits on τ then the criterion will have to be dependant on a given robot’s configuration (F 0 = F 0 (X)), and cannot be expressed directly as a linear function of contact forces. On the contrary, the non-sliding conditions in F only depend on contact points positions, whatever the configuration of the robot. In addition, relying on W = JT2 (F ∩ F 0 (X)) means intersecting 2 polytopes in a n-dimension space. In practise, this becomes computationnaly excessive when reaching n close to 10. By comparison, the HRP-2 robot by Kawada Ind. has 30 actuated degrees of freedom. While being one of the major novelty of the previous 2D criterions, this has lead us to abandon at first constraints on admissible torques. This is equivalent to considering that the robot is completely rigid and can deliver any actuation torque. This hypothesis greatly simplifies the criterion (5). Having now a robot reduced to its CoM, it is simpler to express its dynamics in a fixed galilean reference frame R. In that frame, the contact points are fixed and the robot’s CoM position is given by the vector p. G2 is expressed simply as: "

G2 =

mg

#

(2.37)

p × mg

where g is the local gravity field, and m is the total mass of the robot. In the wrench expression (2.30), the Coriolis term becomes zero because the reference frame is galilean, and the inertia term is that of a solid body: " # m¨ p w=− (2.38) p × m¨ p + L˙ ∗ where L∗ is the angular momentum of the robot relative to its CoM. At first, we consider that the robot does not have any rotation inertia, leading to L∗ = 0 and "

w=−

#

m¨ p

(2.39)

p × m¨ p

Finally, with Aw + b = −w − G2 , the criterion is stated as: "

Criterion 6. The motion is feasible ⇔ m

p ¨−g

p × (¨ p − g)

Traineeship Report – Sylvain Garsault 2008

#

∈ JT2 (F)

31

CHAPTER 2. DYNAMIC STABILITY CRITERION

2.4.3

Algorithm

Looking at criterion (6), the problem to solve is testing if a given 6D-vector belongs to JT2 (F). We follow here a workflow based on the work by Barthelemy et al. [13]. The first step is to compute the polytope F, obtained by linearizing each friction cone as exposed in (2.32). p discretization vectors fci of same normal component are chosen regularly spread on the surface of C. Let Fc denote the linear approximation of C (fig. 2.10), i.e. the positive linear combination of the previously defined discretization vectors: Fc =

p X

ai fci = pos(fci )

(2.40)

i=1,ai ≥0

It represents the approximated admissible space for the force applied on the robot at a given contact point. This operation is repeated for each of the n contact points defined in the model, leading to: F=

n Y

Fcj

(2.41)

j=1

Figure 2.10: The linearized friction cone (yellow) is included in the full friction cone (transparent blue). f is the positive linear combination of the discretization vectors fci . Then, the jacobians J2c j for each contact point are computed, with a reference point chosen as the origin of a fixed galilean frame. The transpose jacobian defines a linear application from the contact-force space to the unactuated DoF’s space. Thus, the image of a linear combination of fc i vectors through J2c Tj is the linear combination of the image vectors wji = J2c Tj (fc i ): Wj = J2 Tj (Fcj ) = posi (wji )

(2.42)

W = JT2 (F) = posi,j (wji )

(2.43)

This representation of the polytope W is called a Vertex representation, or V-representation. While being very simple to compute, it is not easy to check that a given vector w lies within W. It requires the resolution of a Linear Program to try and find the coefficients ak and vectors P wk ∈ W satisfying w = k ak wk . There exists a second representation for a polytope called H-representation in which the polytope is defined as an intersection of half-spaces limited by affine hyperplanes: W = {w ∈ R6 , Aw ≤ b} (2.44) Traineeship Report – Sylvain Garsault 2008

32

CHAPTER 2. DYNAMIC STABILITY CRITERION

where the matrix A and vector b form the H-representation. This representation conversion is carried out by a dedicated Double Description Method (DDM) library. Based on indications by Barthelemy et al. [13], I chose to rely on the C Double Description Library (cddlib) by Fukuda ( [14], [15]), because its internal arithmetic – while being based on double-precision floating-point numbers only – has a sufficient precision for our purpose, and because it can handle highly degenerated sets of data. In addition, it is a completely standalone C code that can be easily wrapped in a C++ framework. However, while being fully satisfactory in its current use, cddlib might not be sufficient for a future usage. See Appendix C for a short review of available DDM libraries. "

Criterion 7 (Final criterion). The motion is feasible ⇔ Aw ≤ b, where w = m

2.4.4

p ¨−g

#

p × (¨ p − g)

Implementation and results

The algorithm ported to C++ takes as input a model describing each contact point with its geometrical (position, normal vector, number of discretization points) and mechanical (dry friction coefficient µ) properties and generates the corresponding admissible polytope (fig. 2.11). Note that the whole computation is independent of any robot configuration and only depends on the contact configuration, making it suitable for offline computation.

Figure 2.11: Hyperplanes limiting the admissible polytope. This is an intersection of the full 6D-polytope with the subspace of nul torques, in the case of a single contact point. Indeed, description conversion with cddlib remains a bit long for a a real-time use aboard the robot. The library performance has been evaluated for a pattern of 4 contact points located at the summits of a rectangle (representing a foot-contact). The timings, averaged on 1000 computations run on an Intel Xeon 3.4 GHz workstation, are in the order of 10ms (first line of table 2.1). Traineeship Report – Sylvain Garsault 2008

33

CHAPTER 2. DYNAMIC STABILITY CRITERION

Interestingly, adding several parallel feet to the set of contacts does not lead to a linear increase in the execution time, demonstrating that cddlib can quickly solve degenerated problems (many constraints are identical). Number of feet 1 2 3 4

Conversion time (ms) 8.809 8.272 11.05 13.53

Table 2.1: cddlib timing results On the other hand, computing the criterion (7) is extremely fast, which allows its repeated use in heavy computation loops such as optimization programs for instance.

Traineeship Report – Sylvain Garsault 2008

34

CHAPTER 3. TRAJECTORY GENERATION

Chapter 3

Trajectory Generation 3.1

Statement of the problem

Having at one’s disposal a quick dynamic stability criterion is crucial to test if a CoM trajectory q(t) is feasible. A motion is said feasible if ∀t the contact forces are admissible. In the construction of the criterion (7), some hypothesis have been made: the robot is reduced to a point of mass m moving in 3D, and it is supposed that any torque can be applied to the joints. In those conditions, the robot can be described as a single rigid body in movement in a fixed galilean frame (fig. 3.1) with its entire mass m concentrated at the center of mass (CoM): q(t) is reduced to the trajectory p(t) of a CoM in 3D.

Figure 3.1: The simplified mechanical model: a single rigid body in movement in a fixed reference frame. This simplification has an impact on the only remaining constraint, i.e. the preservation of contacts. The criterion (7) only ensures that in case of physical contact, it is possible to find admissible contact forces. It is then necessary to add the constraint that a point of the robot is actually in contact with the environment, in other words ∀t there exists a configuration q(t) for the whole robot verifying contacts and CoM position p(t). This question involves inverse kinematics and is non-trivial. The problem to solve is the following: being imposed a timed sequence of contact configurations, determine a CoM trajectory enabling the dynamically stable transition between those contact configurations. In the following, we will examine the fundamental case of 2 successive sets of contacts whose intersection is not empty1 . The workflow is very similar to the method employed in 2D by Kajita et al. [4] which lead to the elaboration of a Walking Pattern Generator, a tool used on a daily basis by the JRL. The main difference lies in the use of a 6D polytope instead 1

An empty intersection, i.e. jumping, is not excluded and can be a subject for further studies using this same framework.

Traineeship Report – Sylvain Garsault 2008

35

CHAPTER 3. TRAJECTORY GENERATION

of a 2D convex hull to determine if a given perturbation – or dynamics – is admissible. This work can therefore be seen as a generalization to 3D and multi-contact of the existing pattern generator. The movement comprises 3 different contact states: initial contact set, final contact set, and a transition defined as the intersection of the initial and final sets (fig. 3.2). Let us denote cj , j ∈

Figure 3.2: A basic movement: (1) Initial contact set and admissible polytope. (2) Transition set and polytope. (3) Final contact set and polytope. [1, p] the contact point positions, Wi , i ∈ {1, 2, 3} the three admissible perturbation wrench P polytopes, and Ti , 3i=1 Ti = h their respective duration with h the total time horizon.

3.2 3.2.1

Trajectory Generation Parametrization

The CoM trajectory is a function p(t) = [px , py , pz ]T (t) expressed in a fixed galilean frame ˙ p ¨ ]T (t). It is assumed that the initial attached to the environment. The robot state is x(t) = [p, p, state x0 = x(0) is known. The choice of the parametrization as , s ∈ [0 · · · n] such that p(t) = p(t, as ) depends on the constraints applied. • We want to ensure continuous acceleration: p ∈ C 2 • The initial state x0 is imposed • Since the stability constraints will be expressed in the as , it is preferable to have a compact parametrization with a low n A good candidate family of functions for these criteria are the B-Splines. Each component pe (t) of p(t) is assigned to be such a B-Spline of degree k = 3 with n + 1, n ≥ 2 parameters (or base functions) and m + 1 knots t0 < t1 < ... < ti < ... < tm−1 . The relation between those 3 integers is given by: m=k+n+1 (3.1) One component of p(t) is then expressed as: pe (t, as∈[0..n] ) =

n X

as Ns,k (t)

t ∈ [0, 1]

(3.2)

s=0

Traineeship Report – Sylvain Garsault 2008

36

CHAPTER 3. TRAJECTORY GENERATION

where Ns,k is the sth base function of degree k. In addition, the B-Splines are chosen to be clamped – i.e. they go through their initial and final control point – because it simplifies greatly the formulas used to impose x0 . This is obtained with a special configuration of the knot vector such that: (

ts∈[0..k] = 0 ts∈[m−k..m] = 1

(3.3)

The other ts∈[k+1..m−k−1] are called internal knots, and are left free for the moment. See Appendix D for the details of implementation.

3.2.2

Optimization

The idea is to solve the problem as an optimization program on the splines parameters as , minimizing an objective function fobj under the constraints of dynamic stability and feasible inverse kinematics. Variables With a given initial state x0 , the variables to optimize are the n − 2 parameters as left free for each spline (see Appendix D), i.e. 3n − 6 parameters in total for the whole robot. Constraints The main constraint is for the dynamics to lay inside the admissible polytop associated at time t, according to the stability criterion (6): "

m

p

1 ¨ ( ht ) − g p h2 ¨ ( ht ) − × ( h12 p

#

∈ W(t), ∀t ∈ [0, h]

g)

(3.4)

where the time has been scaled to account for B-Splines being defined on [0, 1]. The second type of constraints is to ensure that it is actually possible for the robot to achieve physical contacts with the envrionment. Since there is absolutely no consideration of inverse kinematics in the model (the robot is modeled as a simple point), the CoM is constrained to stay within a “reasonable” distance from the contact points: dj (t) = lj min ≤ kp(t) − cj k ≤ lj max , j ∈ [1..p]

(3.5)

Objective function The ojective function is of the form: fobj (p(t)) =

X

αi fi (p(t))

(3.6)

i

where fi is one term to minimize and αi its weight in the sum. ... • It has been found that natural human motions tended to minimize jerk p 2 (t) • To induce some damping, it is also necessary to minimize speed, p˙ 2 (t) Traineeship Report – Sylvain Garsault 2008

37

CHAPTER 3. TRAJECTORY GENERATION

• It is important, in particular to solve inverse kinematics, that the CoM of the robot describe very small variations on the z axis, thus minimizing (pz (t) − zoff (t))2 , with zoff (t) a vertical offset referenced by the lowest of the contact points. • Towards the end of a motion when damping effect becomes unsignificant, it is more natural for the robot to adopt a position horizontally centered on its contacts, by minimizing (ph (t) − c(t))2 where ph is the horizontal component of the trajectory and c the horizontal isobarycenter of contact points. The above functions depend on time t; they are integrated along the whole trajectory to constitute a global time-independent cost function. Jerk and speed cost functions are split along the 3 axis x, y, z to allow for more precise minimization by tuning their respective coefficient αi . The horizontal position cost function has also been split along x and y. Problem formulation Optimizing a CoM trajectory means optimizing the free splines parameters as to minimize the objective function fobj under the sequential constraints (3.4) and (3.5).

3.2.3

Preview Control

The objective in the long run is to implement this trajectory generator as an onboard preview ˙ p ¨ ]T (t0 ). Given an x0 in controller. At a given date t0 , the robot has a known state x0 = [p, p, input, the previous optimization generates a new optimal CoM trajectory starting at t0 on a time horizon h. This trajectory is used as a command for the control system. At t0 + ∆t, a new state x∆t is issued and taken as new initial state for another optimization, thus closing the loop, and allowing for perturbation elimination. For now, the optimization is slightly too computational intensive to be run on-line (see implementation 3.3). But an interesting offline application of the preview control is the generation of whole scenario trajectories. Since it would be too expensive to optimize a full trajectory lasting several tens of seconds and involving a dozen different contact configuration, it is split into smaller subtrajectories on a sliding time horizon h. At a chosen rate ∆t, a new sub-trajectory is created. Its initial state is imposed by the previous trajectory’s state at t + ∆t, and it is then optimized on the horizon h. In the end, the smaller splines are stitched together to form a full CoM trajectory describing the contact scenario. The whole trajectory itself might be sub-optimal, but it is remarkable that the system is able to generate a feasible global motion by “seeing” each time only h seconds ahead. This is the main application developped during the remaining of my intership.

3.3

Implementation

I developped the dynamic trajectory generator as a C++ software internally called Contact Based Pattern Generator (CBPG), by reference to the currently used Walking Pattern Generator. It relies on other tools for dedicated tasks: • The double description method cddlib [15] is used to compute the admissible wrench space for each contact configuration prior to any optimization. • The framework of the Posture Generator 2 written by A. Escande is used as a high-level interface with the optimization engine FSQP [16]. Traineeship Report – Sylvain Garsault 2008

38

CHAPTER 3. TRAJECTORY GENERATION

• A modified version of the previously described static trajectory generator (section 1.3) is used to solve the inverse kinematics.

3.3.1

Sequential formulation

The main difference between implementation and the theory developped above is in the way time is treated. Indeed, verifying that a constraint is verified ∀t is not a trivial matter, even for a C 2 function. A workaround supported by some optimization engines such as FSQP is the use of sequential constraints. The horizon h is discretized in H time steps lasting δt, and time becomes an integer index. Constraints are imposed only at each discretization instant ti , i ∈ [0 · · · H] and are flagged as sequentially linked for the optimizator. Ojective functions become simple discrete sums instead of integrals. Other control algorithms verify constraints every 100ms [17]; it is the value currently used, but it can be modified according to observed behaviour in simulation or reality. This approximation allows many optimizations, in particular the precomputation of splines base functions values.

3.3.2

Algorithm outlines

Input files A trajectory generation is a problem described in a so-called model file (see Appendix E for its complete format), containing mechanical information (mass of the robot, gravity intensity, minimum and maximum distances to the contacts), splines parametrization (the desired number of parameters for each spline, length of time horizon), optimization parameters (objective function weights) and a timed sequence of contact configurations. This sequence can be expressed in various ways thanks to import methods supporting several formats. The model understands the concept of group of contacts, i.e. a number of punctual contacts grouped together under a unique denomination (ex: left foot). This allows to quickly create scenarii in a user-friendly manner by writing in a scenario file the successive contact configurations with their timings and contact group positions. The system is also compatible with the format used by the static trajectory generator (section 1.3), which allows to quickly play already existing scenarii. Initialization The program first parses the model file, and loads the scenario or sequence of contact configurations. Then it “solves” the splines problem. It is absolutely crucial (in terms of convergence and speed of execution) to be able to re-use the splines parameters found in a previous optimization as starting guess for the current one. To achieve this, it is necessary that splines share the same knots ts (see section 3.2.1), whatever their starting point in time. As can be seen on fig. 3.3, there are p possible families of splines for a given problem, with p the time between two successive free knots. Families are thus identified by their starting time modulo p. Each of them is precomputed at initialization, to allow instant access to their derivatives and gradients in the core of the program (see Appendix D). Then the whole motion family is created, using the same p-spaced knots as the atomic families. Optimization loop One optimization loop is characterized by a starting time and an initial state. The starting time tstart modulo p selects a family of splines. The family is initialized with the robot state x(tstart ) Traineeship Report – Sylvain Garsault 2008

39

CHAPTER 3. TRAJECTORY GENERATION

Figure 3.3: There are only p different families of splines. On this figure, p = 3. Splines Sp0 and Sp3 share exactly the same knots configuration: they are identical. Except for the first and last ”clamped“ knots, all the free internal knots are common to the different families of splines, allowing to share the corresponding parameters at spline initialization (fig. 3.4). computed on the family optimized during the previous loop. • For the first loop, the family is initialized with the robot state x(tstart ) loaded from the model file. If none is provided, the corresponding splines parameters are left free to be optimized. The remaining free parameters are initialized with the value of the last fixed parameter. • For the following loops, the CoM state x(tstart ) is computed on the family optimized during the previous loop. For each knot common to the current family and the previous one (overlapping), the corresponding parameters are used as initialization guess for the current family (fig. 3.4). The remaining free parameters are initialized with the value taken by the last guessed parameter. If there is no overlapping (not recommended), the family is initialized as in the first loop. For each knot of the previous family not overlapping with the current one, the corresponding parameters are stored in the whole motion family (stitching). The next operation consists in creating the sequential constraints. For each ti , i < H: • l stability constraints are created, one for each of the l lines in the matrix associated to the H-Representation (cf. 2.4.3) of the contact configuration valid at t = tstart + ti . If t is a transition time between two sets of contacts, the most restrictive of the two is chosen, i.e. the one with fewer contacts. • 2 × ncg distance constraints are created, respectively for maximum and minimum allowed distance seperating the CoM and any of the ncg contact groups centers (the barycenter of the underlying point contacts) valid at t. The objective function is tstart -dependent too, through the position terms. For each ti , i < H, the objective horizontal and vertical positions are computed from the contacts at t and the position term is added to the objective function fobj . The optimization problem is finally sent to be processed by the FSQP engine [16]. Whatever the success, the optimized family parameters are stored, ending the loop. Traineeship Report – Sylvain Garsault 2008

40

CHAPTER 3. TRAJECTORY GENERATION

Figure 3.4: The new spline (blue), starting at t = 0.1, is initialized to fit the previous computed spline (red). Dynamics of the red spline at t=0.1 impose the parameters for the first 3 internal knots. For all the remaining common knots, parameters are copied from the red spline. This allows the new spline to fit exactly the previous one until the m − k-th internal knot, even in the case of non-uniform knot repartition. Exporting When all the required optimization loops have been performed, the result is stored as the whole motion family parameters. A new family is created, with the same knots and same parameters, but with arbitrary discretization. A typical value is 5ms, since it is the rate of update of the robot actuators state on HRP-2 platform (see 1.4). This allows to output a file containing timed CoM positions for the whole motion. Solving key postures Key postures for each contact configuration transition are first generated using a version of the static trajectory player (see 1.3) modified to impose the CoM position previously computed, instead of using a static stability criterion. In case of success, the result is a sequence of key postures qi verifying contacts and CoM location. Otherwise, it indicates that the desired position of the CoM is not achievable. Currently, this problem is solved by putting more sever constraints on minimum and maximum distances to contacts. Generating end-effectors trajectories When a new contact is created, the supporting limb must come in contact with the environment during the step preceding this contact configuration. Similarly, when a contact is suppressed, the corresponding limb must break the contact during the step following the current configuration. This is only taken into account at this stage because it requires to know the initial (creation) or final (suppression) position of the limb involved. This information is contained in the previously generated key postures. Two methods are currently in use: • a manual method used for static trajectory generation (see 1.3), based on manually tuned B-Splines. • an automatic generation based on polynomials. Traineeship Report – Sylvain Garsault 2008

41

CHAPTER 3. TRAJECTORY GENERATION

This second method generates a minimum-jerk trajectory by finding the coefficients for a polynomial of degree 5 e(t) = e5 t5 + e4 t4 + e3 t3 + e2 t2 + e1 t + e0 (3.7) To create or break the contact cleanly, we want to impose a nul speed and an acceleration directed towards the normal to the contacts at initial and finale instants. Let pi and p¨i (resp. pf and p¨f ) the desired initial (resp. final) position and acceleration. Solving the relation at t = 0 for a single axis leads to: e0 = pi

(3.8)

e1 = 0 p¨i e2 = 2

(3.9) (3.10)

The relation at final instant t is obtained by inverting the matrix:  3 t  2 3t M= 

6t

The result is:  

e3

t5

4t3

5t4  

12t2

 

(3.11)

20t3

pf − pi − t p¨2i



   e4  = M−1    

e5

t4

−t¨ pi p¨f − p¨i

   

(3.12)

The output is a file containing the trajectory of the end-effectors sampled every δt (usually every 5ms). Solving inverse kinematics The whole body motion can now be generated by solving inverse kinematics. This is performed by the same modified static trajectory generator, following the algorithm 5. Algorithm 5 Inverse Kinematics 1: for all i ∈ [0 · · · N ], N δt = T do 2: get fixed contacts configuration 3: if needed, read end-effector configuration 4: read CoM position 5: solve inverse kinematics q by imposing fixed / moving contacts and CoM position 6: write q in the output file 7: end for The output file, if sampled at a rate of 5ms, can be used directly to command a simulation or the HRP-2 robot itself.

3.4

Results

To test the validity and performance of the CBPG, it has been run on several scenarii of growing complexity. Traineeship Report – Sylvain Garsault 2008

42

CHAPTER 3. TRAJECTORY GENERATION

3.4.1

Walking on flat ground

The case of biped walk on a flat horizontal ground has been intensively studied, and constitutes a good starting point for comparison with the new method. The scenario here is to make the robot start from a double support configuration, make 4 steps, and stop on a double support configuration (fig. 3.5) Contact configuration

Figure 3.5: Top left to bottom right: HRP-2’s footprints representing the sequence of contacts configuration to make 4 steps.

CoM trajectory The whole motion lasts 10.8 seconds and is made of 13 sub-trajectories optimized on an horizon of 3 seconds every 600 milliseconds. The computation time for this example with the chosen parameters for the objective function is 90s on an Intel Xeon 3.4 GHz workstation. The figure 3.6 shows the subtrajectories in the x-y plane. The figures 3.7 and 3.8 represent the full trajectory, in xy and xz planes. In the case of walking on an horizontal floor, it is possible to compute the Zero Moment Point (ZMP), usually prefered as dynamic stability criterion. On figure 3.7, the ZMP always lies in the support polygon, showing that the motion obtained with the new criterion is admissible. Finally, the whole motion in 3D is represented on figure 3.9. Simulation results The generated motion has been tested in the constraint-based dynamics simulator used by the JRL [5]. The results show a stable motion and a good follow of the desired CoM by the simulated CoM (the two curves are almost indistinguishable on fig. 3.10. Traineeship Report – Sylvain Garsault 2008

43

CHAPTER 3. TRAJECTORY GENERATION

Figure 3.6: Top left to bottom right: the sequence of intermediate sub-trajectories. Each of them is 3s-long, and two sub-trajectories are separated by 600ms.

Traineeship Report – Sylvain Garsault 2008

44

CHAPTER 3. TRAJECTORY GENERATION

Figure 3.7: The CoM (blue) and and ZMP (red) trajectories in the xy plane, sampled every 100ms. The ZMP never falls out of the support polygon, ensuring dynamic stability.

Figure 3.8: The CoM (blue) and and ZMP (red) trajectories in the xz plane, sampled every 100ms. The variations of CoM altitude are strongly minimized by acting on the corresponfing coefficient in the objective function. Traineeship Report – Sylvain Garsault 2008

45

CHAPTER 3. TRAJECTORY GENERATION

Figure 3.9: The CoM (blue) and and ZMP (red) trajectories in 3D, sampled every 100ms.

Figure 3.10: A screenshot of the simulator. On the left, the robot has performed the 4 steps without falling. On the right, the CoM trajectory computed by the simulator, in the xy plane.

Traineeship Report – Sylvain Garsault 2008

46

CHAPTER 3. TRAJECTORY GENERATION

3.4.2

Climbing stairs and ramps

Contact configuration In this scenario, the robot makes a few horizontal steps before climbing a flight of stairs, pausing on top, then continuing on a ramp. The steps are 10cm high, and the ramp is tilted by 5% (fig. 3.11).

Figure 3.11: HRP-2’s footprints representing the sequence of contacts configuration to make 3 steps, then climb 4 stairs, and finally make 4 steps on a ramp.

Trajectory The whole trajectory (fig. 3.16) is made of 42 sub-trajectories (fig. 3.12), created every 600ms. The preview horizon is 3 seconds, and the whole motion lasts 27.6 seconds. This motion took 60s to compute.

Traineeship Report – Sylvain Garsault 2008

47

CHAPTER 3. TRAJECTORY GENERATION

Figure 3.12: All the sub-trajectories used to compute the complete motion. Each one is 3s-long and two successive sub-trajectories are separated by 600ms.

Figure 3.13: A close-up view on which it is possible to see the successive sub-trajectories. Only the extremities vary from one spline to the next, giving a “hairy” impression to the collection of splines. Traineeship Report – Sylvain Garsault 2008

48

CHAPTER 3. TRAJECTORY GENERATION

Figure 3.14: The full CoM trajectory in the xy plane, sampled every 100ms.

Figure 3.15: The full CoM trajectory in the xz plane, sampled every 100ms.

Figure 3.16: The full CoM trajectory in 3D, sampled every 100ms. Traineeship Report – Sylvain Garsault 2008

49

CHAPTER 3. TRAJECTORY GENERATION

3.4.3

Complex scenario

In a joint scenario built with Jean-Rémy Chardonnet, we decided to exploit a third contact to help the robot go over an obstacle. The robot uses its right arm to form a contact with a vertical pole. Some configurations illustrating this scenario (obtained with a manually built command law) can be seen on figure 3.17. Various CoM trajectories obtained for different values of the parameters in the objective function are displayed on figure 3.18. The motion lasts 6s, and is composed of six 3s-long sub-trajectories, separated by 600ms. The computation takes less than 60s to be carried.

Figure 3.17: Top left to bottom right: some configurations illustrating the scenario in which the robot leans on a pole with its right arm to go over an obstacle.

Traineeship Report – Sylvain Garsault 2008

50

CHAPTER 3. TRAJECTORY GENERATION

Figure 3.18: Three possible CoM trajectories, obtained for different parameters in the objective function. The trajectories are sampled every 100ms.

Traineeship Report – Sylvain Garsault 2008

51

CHAPTER 3. TRAJECTORY GENERATION

Conclusion In this work, after showing the limits of quasi-static motions, a new dynamic stability criterion was introduced. It has lead to the design of dynamic trajectory generator, able to command a humanoid robot during the transition from a contact configuration to another. Though still in development, it has already shown promising results, by managing to generate motions in conditions where other pattern generators failed to work (non-coplanar and non-horizontal contacts). The development of this application is crucial for the JRL, since it paves the way to the onboard generation of dynamic trajectories based on preview control. In addition to an article to be submitted next year, the work will carry on, in particular with the realization of experiments on the HRP-2 platform scheduled for early next year. There is still room for improvement is the way the inverse kinematics constraints are handled, or in the modeling of rotation inertia effects (ellipsoid model). This internship gave me the opportunity to concentrate on the same subject for 7 months, with a feeling of gradually understanding better the complex issues at stake, and being able to some extent to influence the course of my research. My subject involved many different subjects, from rigid body mechanics to algebra and numerical optimization, to object programming and lowlevel considerations, which renewed my interest for it on a daily basis. Of course, I improved a lot technically, in particular to meet the laboratory’s quality standards in terms of C++ development, but also in my relationships thanks to the rich interactions with researchers from many different backgrounds in an international context and in a foreign institute. To conclude, it was a formidable window on the world of scientific research and a wonderful first working experience.

Traineeship Report – Sylvain Garsault 2008

52

CHAPTER 3. TRAJECTORY GENERATION

Acknowlegements I would like to thank my mentor Abderrahmane Kheddar and my Japanese host Kazuhito Yokoi, who allowed this research to be carried on in the frame of the Joint Japanese French Robotics Laboratory (JRL), using the facilities offered by the National Institute for Advanced Industrial Science and Technology (AIST-Tsukuba). I would especially like to thank Adrien Escande who followed my research from the beginning, as well as Olivier Stasse, Pierre-Brice Wieber and JeanRemy Chardonnet, who all worked closely with me at some point. Finally, I would like to thank my co-trainees Martin Battaglia, Mehdi Benallègue, Karim Bouyarmane, and Loic Magnan, who shared this formidable experience with me, and all the Japanese people I met during my stay in Tsukuba, for their kindness and warm welcome.

Traineeship Report – Sylvain Garsault 2008

53

BIBLIOGRAPHY

Bibliography [1] Adrien Escande, Abderrahmane Kheddar, Sylvain Miossec, and Sylvain Garsault. Planning support contact-points for acyclic motion and experiments on HRP-2. In International Symposium of Experimental Robotics, Athens, Greece, 14-17 July 2008. [2] Teresa G. Miller, Timothy W. Bretl, and Stephen M. Rock. Control of a climbing robot using real-time convex optimization. In IFAC Symposium on Mechatronic Systems, September 2006. [3] Timothy Bretl and Sanjay Lall. A fast and adaptive test of static equilibrium for legged robots. ICRA, 2006. [4] Shuuji Kajita, Fumio Kanehiro, Kenji Kaneko, Kiyoshi Fujiwara, Kensuke Harada, Kazuhito Yokoi, and Hirohisa Hirukawa. Biped walking pattern generation by using preview control of zero-moment point. In ICRA, pages 1620–1626, 2003. [5] J-R. Chardonnet, S. Miossec, A. Kheddar, H. Arisumi, H. Hirukawa, F. Pierrot, and K. Yokoi. Dynamic simulator for humanoids using constraint-based method with static friction. In IEEE International Conference on Robotics and Biomimetics, pages 1366–1371, Kunming, China, December 17-20 2006. [6] P.-B. Wieber. On the stability of walking systems. In Proceedings of the International Workshop on Humanoid and Human Friendly Robotics, 2002. [7] Adrien Escande, Abderrahmane Kheddar, and Sylvain Miossec. Planning support contactpoints for humanoid robots and experiments on HRP-2. pages 2974–2979, Beijing, China, October 2006. [8] Sergei Poskriakov. Humanoid Balance Control : A comprehensive review. PhD thesis, Université de Genève, 2006. [9] P.-B. Wieber. Modélisation et commande d’un robot marcheur anthropomorphe. Phd thesis, Mines de Paris, 2000. [10] Abderrahmane Kheddar and Adrien Escande. Challenges in contact-support planning for acyclic motion of humanoids and androids. In 39th International Symposium on Robotics, pages 740–745, Seoul, Korea, October 15-17 2008. [11] Michel Berkelaar, Kjell Eikland, and Peter Notebaert. lpsolve version 5.5, open source (mixed-integer) linear programming system. http://sourceforge.net/projects/lpsolve, 2008. GNU LGPL (Lesser General Public Licence). Traineeship Report – Sylvain Garsault 2008

54

BIBLIOGRAPHY

[12] C. Collette, A. Micaelli, C. Andriot, and P. Lemerle. Robust balance optimization control of humanoid robots with multiple non coplanar grasps and frictional contacts. Robotics and Automation, 2008. ICRA 2008. IEEE International Conference on, pages 3187–3193, May 2008. [13] S. Barthélemy and P. Bidaud. Stability measure of postural dynamic equilibrium based on residual radius. In RoManSy’08 : 17th CISM-IFToMM Symposium on Robot Design, Dynamics and Control, 2008. [14] Komei Fukuda and Alain Prodon. Double description method revisited. In Selected papers from the 8th Franco-Japanese and 4th Franco-Chinese Conference on Combinatorics and Computer Science, pages 91–111, London, UK, 1996. Springer-Verlag. [15] Komei Fukuda. cddlib version 0.94f, C-implementation of the double description method for computing all vertices and extreme rays of the polyhedron. http://www.ifor.math.ethz.ch/∼fukuda/cdd_home/cdd.html, 2008. GNU General Public Licence. [16] C. T. Lawrence, J. L. Zhou, and Tits A. L. User’s guide for cfsqp version 2.5: A c code for solving (large scale) constrained nonlinear (minimax) optimization problems, generating iterates satisfying all inequality constraints. Technical report, Institute for Systems Research, University of Maryland, 1997. [17] H. Diedam, D. Dimitrov, P.-B. Wieber, M. Katja, and M. Diehl. Online walking gait generation with adaptive foot positioning through linear model predictive control. In IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pages 2685–2690, 2008. [18] Free Software Foundation. GNU Multiple Precision Arithmetic Library (GMP) version 4.2.4. http://gmplib.org/, 2008. GNU LGPL Lesser General Public Licence. [19] David Avis. lrs: reverse search vertex enumeration program. http://cgm.cs.mcgill.ca/ avis/C/lrs.html, 2006. GNU GPL General Public Licence. [20] Polylib. http://icps.u-strasbg.fr/polylib/, 2007. GNU GPL General Public Licence. [21] Newpolka. http://pop-art.inrialpes.fr/people/bjeannet/newpolka/. GNU GPL General Public Licence. [22] R. Bagnara, P. M. Hill, and E. Zaffanella. The Parma Polyhedra Library: Toward a complete set of numerical abstractions for the analysis and verification of hardware and software systems. Science of Computer Programming, 72(1–2):3–21, 2008.

Traineeship Report – Sylvain Garsault 2008

55

APPENDIX A. WORK ENVIRONMENT

Appendix A

Work environment My internship took place in AIST-Tsukuba (National Institute for Advanced Industrial Science and Technology), an extensive research center located north of Tokyo, in Ibaraki prefecture, Japan. I was affected to the JRL (Joint Japanese-French Robotics Laboratory), a common laboratory created in 2003 and run by CNRS (Centre National de la Recherche Scientifique, France) and AIST. JRL-Japan is co-directed by Kazuhito YOKOI and Abderrahmane KHEDDAR. There also exists JRL-France, located in LAAS (Laboratoire d’Architecture et d’Analyse des Systèmes, Toulouse, France). The “open-space” organization and the proximity with the experimental area created a friendly and motivating atmosphere, encouraging exchanges between permanent researchers, Post-Doc fellows and Ph.D students, whatever their nationalities.

A.1

Co-workers

My mentor, the JRL-Japan co-director Abderrahmane Kheddar (CNRS researcher), followed my entire internship. He inspired many key ideas, and was the main architect of my research topic. I was to report to him about my research status at least once a week (see A.2 below), and often more frequently when my work would reach a turning point. I also worked on a daily basis with the Ph.D student Adrien Escande, whose work I was intensively using and sometimes extending. He was my first interlocutor in case of problem and my main source of knowledge. I also worked closely towards the end of my internship with the Ph.D student Jean-Rémy Chardonnet, on common experiments built around his dynamic simulator and my trajectory generator. I would often call to permanent researchers or experimented Ph.D students for help and advice on technical points. This includes in particular: • Olivier Stasse (CNRS researcher) for issues related to pattern generation and general stability questions, and experiments on HRP platform. • Florent Lamiraux (CNRS researcher) for software versionning tools. • Pierre-Brice Wieber (INRIA researcher) for questions related to mathematics (optimization, B-Splines, SciLab) and their software implementation. • Sylvain Miossec (CNRS researcher) whose work on whole-body motion optimization shared a lot of issues with my research topic. • Nicolas Mansard (CNRS researcher) for questions related to Stack of Tasks and pseudoinverse. Traineeship Report – Sylvain Garsault 2008

56

APPENDIX A. WORK ENVIRONMENT

• Ee Sian Neo (AIST researcher) for experiments-related technical issues The open space organization fostered constructive interaction between researchers, and I would often ask (and sometimes be asked) small technical questions or theoretical points. Meeting rooms with whiteboards where at our disposal for longer and more organized discussions. International discussions (7 different nationalities represented) always took place in English and were never an issue, due the researchers’ excellent level in this language. However, one might expect more frequent exchanges between AIST and CNRS staff – especially at Ph.D and Master students level – especially while sharing the same working space and tools.

A.2

Work organization

Work hours in a research center are generally flexible, and JRL is no exception. My schedule would be punctuated with meetings, experiments and collective work session (in particular with A. Escande and J.R. Chardonnet). As can be expected, people are very enthusiast about their research and attendance is never a problem, hence the flexible schedule. Indeed, a vocal reminder is played twice every night to urge workers to finish their task and return home, which might be overlooked when concentrating hard on one’s research. Once a week at least, I would have a research status meeting. A. Kheddar and A. Escande would always attend this meeting, and we would often ask for specialists to come and discuss some particular topic. After stating the progress I made during the elapsed week of work, I would discuss the remaining difficulties, and suggest a course of action to solve them. Once a month, the whole laboratory has a JRL meeting during which each member is allowed one slide and one minute (the use of videos and demonstrations is promoted) to present a research status and a plan for the following month. The meeting generally begins with important announcements, such as coming and leaving members, accepted papers, conferences attendance, etc.

A.3

Safety

Every workers in AIST must be aware of safety measures, and the administration requires that every new members follow half a day of training to safety procedures.

A.3.1

Generic safety measures

The training educates the staff about both industrial and natural hazards. Japan is a country where natural phenomena can be surprising for the foreigners, and thus a particular attention must be paid to the prevention of those risks by informing on the right behaviour to have. Each worker must own a safety helmet displaying name and blood type, to wear immediately in case of earthquake. Tall drawers must be closed when not in use, in order to prevent heavy objects from falling. The safest places are below tables and desks. It is asked to keep at home in case of strong wind, heavy rain or actual typhoon. Using a bicyle and an umbrella at the same time is not allowed. Data protection is also emphasized, and laboratory directors periodically remind their coworkers to register every portable device they might use, such as USB key or external hard drive. The internal network is closely monitored for suspect trafic. As a consequence, most peer to peer programs are banned and any new network interface (ex: personal laptop) must be registered prior to access the ressource. Traineeship Report – Sylvain Garsault 2008

57

APPENDIX B. CLIMBER ROBOT MODEL

Finally, there is a monthly security meeting of the whole JRL held by the co-directors. Issues ranging from alcohol driving to recent leakage in some buildings are announced.

A.3.2

Safety measures relative to work with robots

While most safety procedures exposed in the training concern the manipulation of dangerous equipment and matters, some are specific the experiments with robots. A permanent member of the JRL must be present during any manipulation, and it is asked to limit the number of people in the experiment area. The robots must always be assured by a lifter (a harness preventing them to tip over in case they fall), and every experiments must be recorded on video tape. One person, preferably well acquainted with the current experiment, must hold at all time an emergency push button, cutting all power feeding the motors when pushed. In addition, it is vital to push this button if an earthquake takes place during and experiment.

Appendix B

Climber Robot Model B.1

Kinematics

The climbing robot is defined in fig. 2.1and fig. 2.2, and evolves in a 2D space parametrized by the fixed galilean frame FW . The free-flyer has 3 DoFs: two for position, one for rotation. The state vector for the free-flyer can then be parametrized as follows: xff = [x0 , y0 , θ0 ]T

(B.1)

And its transformation matrix in the world frame:  " W R

A0 =

#

0

C

0

1

cθ0

 sθ0  =  0 

−sθ0

 

cθ0

0

0 x0

0

0 y0    1 0

0

0

1

(B.2)



where RW 0 is the rotation matrix of the free-flyer (indexed by 0) in the world frame FW and C is the position vector of point C in FW . c and s denote respectively the cosine and sine functions. To symmetrize the problem, it is convenient to define the 3 constant transformation matrices: 

cai

 sai  A Ai =  0 

Traineeship Report – Sylvain Garsault 2008

−sai cai 0 0



0



0 Ai     1 1

(B.3)



58

APPENDIX B. CLIMBER ROBOT MODEL

with

l A1 = [0, √ , 0]T , a1 = π/2 3 l l A2 = [ , √ , 0]T , a2 = −π/6 2 2 3 l l A3 = [− , − √ , 0]T , a3 = 7π/6 2 2 3 For each limb, the transformation matrix is of similar form: 

cx −sx 0 lx cx

 sx  Ax =  0 

cx

0

(B.4) (B.5) (B.6)

 

0 lx sx  

0

1

0 

0

0

1

 

(B.7)

where x is one of the actuated joint angles bi or ci . Finally, let q denote the configuration vector composed of all variables necessary to describe a unique configuration for the climber: q = [x0 , y0 , θ0 , b1 , c1 , b2 , c2 , b3 , c3 ]

(B.8)

Kinematics are computed by multiplying the previous matrices. For instance, the frame attached to the end-effector C1 is expressed in the world frame FW as: TC1 = A0 .AA1 .AB1 .AC1

B.2

(B.9)

Actions

The climber is subject to three types of actions: the reaction forces applied to the tip of each leg, the perturbation wrench applied to a given point U and the weight applied to the CoM C.

B.2.1

Reaction forces

For each contact point Ki , a frame is attached to the contact surface and its normal. Its expression in the world frame is:   cki −ski 0 xKi  ski  Ki =  0 

cki 0

0

0



0 yKi   1

0 

0

1

 

(B.10)

where Ki = [xKi yKi 0]T and ki the angle formed by the normal vector and the world’s vertical axis. There exists a contact if Ki = Ci . Contacts are modelized as point-on-surface unilateral contacts, thus no torque can be applied to the tip of each leg. The total wrench expressed in the previous frame has the following expression: 

Fxi



  Fyi    fi =   i ∈ {1, 2, 3}  0   

(B.11)

0

Traineeship Report – Sylvain Garsault 2008

59

APPENDIX C. AVAILABLE DOUBLE DESCRIPTION METHOD LIBRARIES

B.2.2

Perturbation wrench

Similarly, the total perturbation force and moment being applied at point U tied to the robot’s body is expressed in a KU frame:   0    fpert    fpert =    0   

(B.12)

mpert

B.2.3

Gravity action

Finally the weight expressed in the CoM frame: 

0



  −mg    p=   0   

(B.13)

0

Appendix C

Available Double Description Method Libraries If a greater precision is needed, cddlib can be recompiled against the GNU Multiple Precision Arithmetic Library (GMP) [18] to provide arbitrary-precision floating-point arithmetics. If performance becomes an issue, other DDM libraries using the latest available algorithms might be more appropriate. There exist a small number of available DDM libraries, most of them placed under an opensource license: • lrslib [19], compatible with cddlib format • PolyLib [20] • NewPolka [21] • Parma Polyhedra Library (PPL) [22] can be considered as a second-generation library, since it is based on previous works Traineeship Report – Sylvain Garsault 2008

60

APPENDIX D. IMPLEMENTATION OF B-SPLINES

Appendix D

Implementation of B-Splines D.1

Imposing initial conditions

An interesting property of the B-Splines is the local support of the basis functions, meaning that a modification of as will only affect the curve for t ∈ [ts ..ts+k+1 [. This makes possible imposing the initial position, speed and acceleration conditions just by setting the first 3 parameters a0 , a1, a2. The dth derivative of a B-Spline of degree k is another B-Spline of degree k − d based on the same knots and whose parameters obey to: adi =

k−d+1 (ad−1 − ad−1 s ) ts+k+1 − tk+d s+1

(D.1)

Together with the clamped character of the chosen splines, this gives particularly simple expressions for a0 , a1, a2. Imposing position p0 : a0 = p 0

(D.2)

tk+1 0 p + a0 k 0

(D.3)

tk+1 tk+2 00 tk+2 tk+2 p + (1 + )a1 − a0 k(k − 1) 0 tk+1 tk+1

(D.4)

Imposing speed p00 : a1 = Imposing acceleration p000 : a2 =

D.2

Implementation

B-Splines have been implemented as discretized functions: it only possible to ask for a spline property (derivative or gradient) at a discretization instant. A spline of degree k and n + 1 parameters is written as: p(t, as∈[0..n] ) =

n X

as Ns,k (t)

t ∈ [0, 1]

(D.5)

s=0

where Ns,k is the sth base function of degree k, recursively defined by the Cox-de Boor formula: (

Ns,0 (t) = Ns,k (t) =

1 if t ∈ [ts , ts+1 [ 0 otherwise

t−ts ts+k −ts Ns,k−1 (t)

Traineeship Report – Sylvain Garsault 2008

(D.6) +

ts+k+1 −t ts+k+1 −ts+1 Ns+1,k−1 (t)

61

APPENDIX E. MODEL FILE FORMAT

At the creation of a new spline with m specified knots ti and l + 1 discretization instants uj , an array of discretization values is computed. For each uj , the value of the k + 1 non nul base functions, from degree 0 to k, are computed using the Cox-de Boor formula (D.6). All the values are kept in an 3D array A indexed by the order, the discretization instant and the reduced index of base function. The reduced index of base function is also precomputed; it is an array I[uj ] whose value is the index of the first non-nul base function at uj . Thus, A[k] i.e. the submatrix indexed by the order k contains: 

NI[0],k (u0 )

    A[k] =  NI[j],k (uj )   



... NI[0]+i,k (u0 ) ... NI[0]+k,k (u0 ) ... ... NI[j]+i,k (uj ) ... ...

NI[l],k (ul , k) ...

NI[l]+i,k (ul )

...

    NI[j]+k,k (uj )    

(D.7)

NI[l]+k,k (ul )

From that point, a recursive algorithm based on (D.1) computes the non-nul parameters–basefunction products ads Ns,d and stores them in A[d], overwriting the previous values. Thus, any derivative of the spline can be instantly computed. The values of the spline itself are obtained with: p(uj , a) =

k+1 X

aI[j]+v A[k][j][v]

(D.8)

aI[j]+v A[k − d][j][v]

(D.9)

v=0

And for the dth derivative: pd (uj , a) =

k+1 X v=0

Even quicker is the computation of the gradient of any derivative (required by the optimization engine FSQP): grada pd (uj ) = [0, · · · , 0, A[k − d][j], 0, · · · , 0]T (D.10) | {z } | I[j]−1

{z

k+1

}

Appendix E

Model file format This appendix describes the format used to describe a model of trajectory generation in a text file. The order of the following parameters cannot be changed.

E.1

Header

MODEL 6.0 The version of the model file format. Latest is 6.0, but older files are still compatible. Traineeship Report – Sylvain Garsault 2008

62

APPENDIX E. MODEL FILE FORMAT

E.2

Splines and Constraints

DEGREE The degree k of the splines. NB_PARAMETERS_PER_SPLINE The total number of parameters used for a spline. Note that most of the time, the 3 first parameters are fixed by dynamics. HORIZON The length of a spline, expressed as a multiple of time steps (∆T = 100ms). DISTANCE_MIN_TO_CONTACTS The minimum distance to respect between the CoM and the center of any contact group. DISTANCE_MAX_TO_CONTACTS The maximum distance to respect between the CoM and the center of any contact group. MASS The mass of the robot, in kg. GRAVITY The acceleration of gravity, in m.s−2 MIN_FORCE Not implemented, always read 0. MIN_DIST_HYPERP The minimum distance between two affine normal vectors to declare their associated hyperplane different. INERTIA_MARGIN The stability margin to keep concerning momenta, in N.m−1

E.3

Initial state

The initial state is specified by the keyword DYNAMICS followed by 9 pairs of strings and numbers: POS_X, POS_Y, POS_Z (initial position, m), SPE_X, SPE_Y, SPE_Z (speed, m.s−1 ), ACC_X, ACC_Y, ACC_Z (acceleration, m.s−2 ).

E.4

Full trajectory

This parameter begins by the keyword DYNAMICS, followed by one of the 3 possible options:

E.4.1

List

This is to specify by hand all the start times for the successive trajectories. LIST followed by the number of trajectories and the list of start times

E.4.2

Sequence

Automatically builds a list of starting times. FROM start time for the first trajectory TO the start time for the last trajectory BY the time elapsed between two consecutive trajectories. Traineeship Report – Sylvain Garsault 2008

63

APPENDIX E. MODEL FILE FORMAT

E.4.3

Automatic

Computes the full trajectory described in the step sequence. The trajectories are separated by p time steps. ALL END_PAUSE followed by the length of the optional pause to add at the end of the sequence, in time steps.

E.5

Objective function

OBJECTIVE_FUNCTION followed by the number of parameters. Each parameter is then indicated by a pair {string, weight}, where the string indicates which is the component to minimize, and weight the coefficient of this term in the objective function.

E.6

Files

COM_FILE_NAME the root name for the ouput files. STATISTICS TRUE/FALSE to turn on the statistics logging (execution time of several parts in the program). Followed by a file name when set to TRUE.

E.7

Step sequence

There are four ways to specify a step sequence.

E.7.1

In the same file

BEGIN_STEP_SEQ followed by the step sequence. This is a way to quickly write a short step sequence in the same model file, but is not recommended for larger files.

E.7.2

In a separate file

INCLUDE_STEP_SEQ followed by the path to the step sequence file.

E.7.3

Import a motion sequence (Posture Generator)

Keyword IMPORT_PG_MOTION_SEQUENCE. This option loads a file compatible with the static trajectory generator built around the posture generator. MOTION_SEQ_FILE path to the motion sequence file CONTACTS_FILE path to the file describing the contact groups SPEED_FILE path to the file indicating the timing for each step IMPORTED_FILE the name of the file that will contain the imported step sequence MU Coulomb’s dry friction coefficient to apply to all contacts. NB_CONE_DISCRETIZATIONS The number of discretization points to create on each contact. Traineeship Report – Sylvain Garsault 2008

64

APPENDIX E. MODEL FILE FORMAT

E.7.4

Import a scenario

Keyword IMPORT_SCENARIO. Writing a scenario is the easiest way to create a step sequence, while keeping the notion of contact groups to allow the generation of inverse kinematics later on. SCENARIO_FILE path to scenario file CONTACTS_FILE path to the file describing the contact groups

Traineeship Report – Sylvain Garsault 2008

65