Planning and Fast Re-Planning of safe motions

work of LENGAGNE Sébastien during his Ph-D : 2006-2009

1 Motivations

1.1 Introduction

This work was done during the Ph-D of Sébastien LENGAGNE under the supervision of Philippe FRAISSE and Nacim RAMDANI, in the DEMAR Team Project of the INRIA. The team is located at the LIRMM. The DEMAR Team Project aims at restoring movements of paralized limbs.

The Ph-D goals were to develop methods to generate safe motions for the standing up and the walking of paraplegian people. Nevertheless, the application to paraplegian was not possible yet, and we apply our method to the HOAP-3 robot. We showed how to generate safe motions for its deambulation.

1.2 Application to humanoid robots

Humanoid robots are very complex systems, with heavy models. The best way to compute accurate motions is to do off-line motion planning. This method allows to have a benchmark motion which ensures the safety and the balance of the robot. During on-line running, the benchmark motion is coupled with a control system to deal with unpredicted events. Indeed, since the motion is computed off-line, it is adapted to one situation.

During the Ph-D, the dynamic model of the HOAP-3 robot was used to plan the motions of the limbs (12 degrees of freedom).

2 Planning Algorithms

2.1 Mathematical formulation of the problem

Motion planning can be translated into finding the best joint trajectories q(t) that solve this problem :

find		q(t)
that minimizes		F(q(t))
such as ∀ i, ∀ t ∈ [0,T]		g_i(q(t)) ≤ 0
and ∀ j		h_j(q(t_j)) = 0

Where :

F(q(t)) is the objective (or criteria) function to be minimized. It can be the motion duration or the energy consumption (for a robot). This function depends on the systems properties and on the desired application.
g_i(q(t)) is the set of the inequality constraints, it can be decomposed into two categories :
- the first one translates the physical limits of the system. These limits must be satisfied over the whole motion duration
- the second translates a desired behaviour (such as equality constraints)
h_j(q(t_j)) is the set of the equality constraints. It is used to set some functions to values at any motion moment.

This problem is called an infinite programming problem, because the joint trajectories and the inequality constraint can be decomposed into an infinite set of values.

2.2 Motion Parametrization leads to SIP problem

There is no tool to solve directly the infinite programming problem. Therefore, we need to parametrize the joint trajectories to get a finite set of parameters to topimize. In this case, we use a Bsplines parametrization, and we get :

q_i(t) =

N_s

∑

j=1

p_i,j × B_j(t)

By this way, we have to optimize the parameter vector : X = [p_1,1,p_1,2,...p_i,j, T] with T the motion duration. The motion planning problem becomes :

find		X
that minimizes		F(X)
such as ∀ i, ∀ t ∈ [0,T]		g_i(X) ≤ 0
and ∀ j		h_j(X) = 0

This problem is defined as a Semi-Infinite Programming problem (SIP problem), since there is a finite number of parameters that must satisfy a set of continuous constraints which can be decomposed into an infinite set of values. Currently, there is no algorithm that can solve the SIP problem, thus we need to discretize the inequality constraints functions.

2.3 Time-Point Discretization : ’State-of-the-art’ Motion Planning Method

The usual method to do the discretization is to consider a time-grid T, and to compute the constraint value for the time value of the grid. This formulation leads to the following expression of the inequality constraints :

∀ i, ∀ t_k ∈ T g_i(X,t_k) ≤ 0

with T = {t₀ = 0, t₁,...,t_N−1,t_N = T}.

Figure 1: Time-Point discretization : this picture comes from the PhD Thesis

The Figure 1, shows a result of a motion planning process with a time-point discretization. The represented function is the ZMP of the HOAP-3 robot in the sagital plane that translates the balance of the robot. To ensure the safety of the robot, this function must stay with [-0.04;0.068].

The crosses represented the value of the function that are taken into account to evaluate the safety. These values stay within the limits but it is clear that the continuous function comes out from the limits. We illustrate this constraint violation with the following video.

So, we see that the time-point discretization does not allow to generate safe motions for the humanoid robot.

2.4 Time-Interval Discretization : Safe Motion Planning Method

To plan safe motions we proposed the guaranteed discretization method which consists in evaluating the maximum and the minimum of the function over time-interval (see Figure 2)

Figure 2: Time-Point discretization : this picture comes from the PhD Thesis

So we transform the expression of the inequality constraints and obtain :

∀ i, ∀ [t]_k ∀ τ ∈ [t]_k max g_i,k(X,τ) ≤ 0

IT = { [t]₁, [t]₂,...,[t]_N}

with [t]_n = [ t_n−1, t_n]

We use the Interval Analysis to get a conservative estimation of the maximum and minimium values. Therefore, the optimization problem gets range of the continuous functions and can produce a solution without any constraint violation. We implemented it on the HOAP-3 robot to get the following experiments :

This method ensures the security of the robot, but the CPU-time is too high ( more than 20 hours see the Ph-D report). We had to find a faster method to ensure the constraint validity.

2.5 Hybrid Motion Planning Method

We proposed the Hybrid Motion Planning Method which aims at producing safe motions in the same range of CPU-time as classical motion planning (which are not safe).

This method is iterative and follows this algorithm

Optimization with classical discretization (results are shown on Figure 3)
Computation of the constraint violation thanks to Guaranteed Discretization (results on Figure 4)
if there is one constraint violation we penalize the limits at the current time and return to step 1. (result on Figure 5).

Figure 3: Optimization with classical discretization : this picture comes from the PhD Thesis

Figure 4: Computation of the constraint violation thanks to Guaranteed Discretization : this picture comes from the PhD Thesis

Figure 5: Penalization of the limits : this picture comes from the PhD Thesis

This method allows to create a database of motions that was used to realize this experiment with the HOAP-3 Robot (more details in Sebastien LENGAGNE’s website).

3 Fast Re-Planning Methods

3.1 The Problem Under Study

We optimized a kicking motion thanks to hybrid motion planning method. We characterize this motion by the position (x,h) of the foot at the mid-duration time which corresponds to the impact, such as shown in Figure 6

Figure 6: Representation of the parameter (x,h) of a kicking motion.

We generated a kicking motion while assuming the location of the ball at x = 1cm and h = 3cm. We present the impact we obtain on Figure 7

before collision collision after collision

Figure 7: Optimal kicking motion with a ball position x=1cm

before collision collision after collision

Figure 8: Optimal kicking motion with a ball position x=3cm

This video shows the optimal kicking motion for the two ball positions (x = 1cm on the left and x = 3cm on the right). We can see that the left kicking makes the ball to get faster.

To compute a new moton adapted to the current ball position, we introduce a method which modifies the previous optimal kicking motion, in a very small CPU time while ensuring constraint satisfaction.

Our idea is to replace the set of inequality constraint ∀ t ∈ [0,T], g(X,t) < 0, which is time consumming, by a set of bounds on the parameter X ∈ [X], when [X] is the feasible set of parameters. This way, re-planning consists in an optimization process with only bounds on the parameters, the new equality constraints h_k ′ and possibly the cost function J′.

minimizes	J′(X,t)
subject to	X ∈ [X]
and	∀ k h_k ′(X) = 0

Where h_k′ (X) is the new set of equality constraint which will translate the new position x=3cm of the ball.

3.2 Computation of the Feasible Sub-set

We propose the CFSS Algorithm (Computation of Feasible Sub-set) that will compute a feasible sub-set [X] that contains the optimal vector X and satisfy all the inequality constraint functions.

3.2.1 First step : Computation of the feasible sub-set

Principle

The algorithm starts by computing the interval vector [W_i ], by using the distance between the optimal vector X and the first constraint violation along each direction such as presented in Figure 9. Then it contracts this interval vector by searching [z] a box that violates at least one constraint and reject it, by decreasing δ_k :

with	[X] = X + δ_k [W]
find	[z] ⊂ [X]
such as ∃ j, ∃ t ∈ [t]	Sup [g]_j ([z],t) > 0

Once the software finds a solution, [z], it stops and δ is chosen such as:

[z] ⋂ X + δ_k+1 [W] = ∅

When it cannot find a solution of [z], then it finishes with the as large as possible sub-set [X_i]

Figure 9: Example of a feasible set and of its inner approximation : the feasible sub-set [X]

3.2.2 Second step : Expansion of the feasible sub-set

Principle

On Figure 10, we can see that the sub-set [X] is not as large as possible, it could be extend in X₁⁺ or X₂⁻ directions.

Figure 10: Example of the expansion process. The final sub-set [X] rely on the order of the expansion.

The second step is to expand the feasible sub-set in all the direction. Nevertheless, the user will have to specify the order of the expansions. (the order will impact the result).

3.3 Application to the Kicking-Motion

We choose to re-plan the optimal motion to make the robot kick a ball at 3cm high and located at the position x = 3cm. Figure shows the faisable impact position (x,h) for the feasible sub-set [X]

Figure 11: Feasible Impact position (x,h) for moion parameter in the feasible sub-set [X] .

We can see, that the position (x = 3cm, h = 3cm) is in the feasible set.Thus, we proceed to the optimization of the problem :

find

X ∈ [X]

such as

) = 3 cm

(1)

The optimization software spent 1.5 second of CPU time to find a solution X. The re-planned motion is presented on Figure 11, the impact is at the desired position (x = 3cm, h=3cm).

before collision collision after collision

Figure 12: Re-planned kicking motion

This video shows a kicking motion adapted to a ball position of 1cm (on the left) and apply this motion with a ball at 3cm (on the right) for the HOAP-3 Robot and a replanned motion adapted to a ball placed at 3 cm. We can see that the kicking on the left is better than the one on the middle since the position of the ball is the one expected. The kicking on the right is also better since it was replanned to fit with the ball position x = 3cm.

4 Publications and Download

4.1 Publications

[Humanoids2007] : S.Lengagne, N.Ramdani, P.Fraisse (2007) Guaranteed computation of constraints for safe path planning IEEE International Conference on Humanoid Robots, Humanoids 2007, Pittsburg, PA, USA. pp. 312-317 download
[Humanoids2008] : S.Lengagne, N.Ramdani, P.Fraisse (2008) A new method for generating safe motions for humanoid robots IEEE International Conference on Humanoid Robots, Humanoids 2008, Korea South. pp.105-110 download
[ICRA2009] :S.Lengagne, N.Ramdani, P.Fraisse (2009) Safe motion planning computation for databasing balanced movement of Humanoid Robots IEEE International Conference on Robotics and Automation, ICRA 2009, Kobe, Japan. pp. 1669-1674.download
[IROS2009] :S.Lengagne, N.Ramdani, P.Fraisse (2009) Planning and Fast Re-Planning of Safe Motions for Humanoid Robots : Application to a Kicking Motion 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems, St Louis, MO, USA. Finalist Robocup Best Paper Award download

Ph-D Thesis of Sébastien Lengagne (in french) download

4.2 The Available Softwares

In the download files we will consider a simple model to explain our methods. This model is also implemented in the safe_motion_planning file.

We consider a 2-D robot with three links such as showed here :

Figure 13: Example of a model.

This example is implemented in the files to download

We want to compute a motion for which, the effector starts at the position (x = -0.75,y = 2.5) and finishes at (x = 0.75,y = 2.5) and computes the feasible sub-set around this motion.

4.3 Download

All the files are available here.

We can download three tar-files which contains the C++ files :

GDL version 1.0 The Guaranteed Discretization Library
safe_motion_planning version 1.0 To compute motions with classical, safe or hybrid method.
CFSS version 1.0 The Computation of Feasible Sub-Set algorithm

This document was translated from L^AT_EX by H^EV^EA.