"## Optimize the distance under non-collision constraint\n",

"Finally, let's use one of the classic python solvers (from scipy) to search for a robot configuration that optimizes the distance to the target, under the constraint that the distance to collision is positive.\n",

"For that, we need to define a cost function (taking the robot configuration and return a scalar) and a constraint function (taking again the robot configuration and returning a scalar or a vector of scalar that should be positive). We additionally use the \"callback\" functionnality of the solver, to render the robot configuration corresponding to the current value of the decision variable inside the solver algorithm.\n",

"We are going to use the \"SLSQP\" solver from scipy, which implements a \"sequential quadratic program\" algorithm and accepts constraints."

"We are going to use the \"SLSQP\" solver from scipy, which implements a \"sequential quadratic program\" algorithm and accepts constraints.\n",

"\n",

"**See [the notebook about SciPy optimizers](appendix_scipy_optimizers.ipynb) for details.**"

]

},

{

...

...

@@ -441,4 +443,4 @@

},

"nbformat": 4,

"nbformat_minor": 2

}

\ No newline at end of file

}

%% Cell type:markdown id: tags:

# Introduction to numerical robotics

This notebook is a very general introduction to Pinocchio. It presents the main method to manipulate the geometry model of a manipulator robot: set the configuration, compute the position of an effector, check the collision or the distance to the obstacle. The main idea is to give a brief introduction of the general topic: how to discover and learn a robot movement constrained by the environment, using iterative optimization methods.

%% Cell type:code id: tags:

``` python

importmagic_donotload

```

%% Cell type:markdown id: tags:

## Set up

For this class, we need the UR5 robot model (urdf, in the robotpkg_example_robot_data), the pinocchio python software, the python optimizers from scipy and for the display of the results: the gepetto viewer and the python matplotlib. We have that with this set of imports:

Let's first load the robot model and display it. You should first start Gepetto Viewer: in a shell (CTRL-ALT-T to open a terminal), run the command: gepetto-gui. A new GUI widow will open (with the logo of our team): this is the viewer. You are not suppose to close this window during the rest of the class, and if you do close it, remember to reopen it before trying to display anything from Python.

%% Cell type:markdown id: tags:

For this class, I wrapped the methods to load the robot model and create the obstacle field.

The scene should not be displayed in the viewer (check it). The robot and the red obstacles are encoded in the robot object (we will not need to see in depth what is inside this object). The object Target is the green dot that the robot should reach. You can change the target position by editing target.position, and display the new position with target.display().

You can display a new configuration of the robot with viz.display (take a numpy.array of dimension 6 in input):

%% Cell type:code id: tags:

``` python

viz.display(np.array([1.,-1.5]))

```

%% Cell type:markdown id: tags:

We also set up a target with is visualized as a green dot:

%% Cell type:code id: tags:

``` python

# %load tp0/generated/simple_path_planning_target

target=Target(viz,position=np.array([.5,.5]))

```

%% Cell type:markdown id: tags:

## Using the robot model

The robot is originally a 6 degrees-of-freedom (DOF) manipulator. Yet to make the example simple, we will only use the joints 1 and 2. The model has simply be loaded with "frozen" extra joints, which will then not appear in this notebook. Just load the model with reduced=False if you want to recover a model with full DOF.

%% Cell type:markdown id: tags:

The following function computes the position of the end effector (in 2d):

The free space is difficult to represent explicitely. We can sample the configuration space until a free configuration is found:

%% Cell type:code id: tags:

``` python

# %load tp0/generated/simple_path_planning_qrand

defqrand(check=False):

'''

Return a random configuration. If check is True, this

configuration is not is collision

'''

whileTrue:

q=np.random.rand(2)*6.4-3.2# sample between -3.2 and +3.2.

ifnotcheckornotcoll(q):returnq

```

%% Cell type:code id: tags:

``` python

viz.display(qrand(check=True))

```

%% Cell type:markdown id: tags:

Let's now find a valid configuration that is arbitrarily close to the target: sample until dist is small enough and coll is false (you may want to display the random trials inside the loop).

%% Cell type:code id: tags:

``` python

# %load tp0/generated/simple_path_planning_qrand

defqrand(check=False):

'''

Return a random configuration. If check is True, this

configuration is not is collision

'''

whileTrue:

q=np.random.rand(2)*6.4-3.2# sample between -3.2 and +3.2.

ifnotcheckornotcoll(q):returnq

```

%% Cell type:markdown id: tags:

## From a random configuration to the target

Let' s now start from a random configuration. How can we find a path that bring the robot toward the target without touching the obstacles. Any idea?

Let's try to have a better look of the configuration space. In this case, it is easy, as it is dimension 2: we can sample it exhaustively and plot it in 2d. For that, let's introduce another function to compute the distance to collision:

Now, let's sample the configuration space and plot the distance-to-target and the distance-to-obstacle field (I put 500 samples to spare your CPU, but you need at least 10x more for obtaining a good picture).

%% Cell type:code id: tags:

``` python

# %load tp0/generated/simple_path_planning_sample

defsampleSpace(nbSamples=500):

'''

Sample nbSamples configurations and store them in two lists depending

if the configuration is in free space (hfree) or in collision (hcol), along

with the distance to the target and the distance to the obstacles.

if'traj'inlocals():### Skip this cell if the previous block is not solved.

plotConfigurationSpace(hcol,hfree)

plt.plot(traj[:,0],traj[:,1],'r',lw=3)

```

%% Cell type:markdown id: tags:

## Optimize the distance under non-collision constraint

Finally, let's use one of the classic python solvers (from scipy) to search for a robot configuration that optimizes the distance to the target, under the constraint that the distance to collision is positive.

For that, we need to define a cost function (taking the robot configuration and return a scalar) and a constraint function (taking again the robot configuration and returning a scalar or a vector of scalar that should be positive). We additionally use the "callback" functionnality of the solver, to render the robot configuration corresponding to the current value of the decision variable inside the solver algorithm.

We are going to use the "SLSQP" solver from scipy, which implements a "sequential quadratic program" algorithm and accepts constraints.

**See [the notebook about SciPy optimizers](appendix_scipy_optimizers.ipynb) for details.**

Look at the output of the solver. It always returns a variable value, but sometimes the algorithm fails being traped in an unfeasible region. Most of the time, the solver converges to a local minimum where the final distance to the target is nonzero

"From (Laitenberger, M., Raison, M., P\u00e9ri\u00e9, D., & Begon, M. (2015). Refinement of the upper limb joint kinematics and dynamics using a subject-specific closed-loop forearm model. Multibody System Dynamics, 33(4), 413-438)."

"From (Laitenberger, M., Raison, M., Périé, D., & Begon, M. (2015). Refinement of the upper limb joint kinematics and dynamics using a subject-specific closed-loop forearm model. Multibody System Dynamics, 33(4), 413-438)."

"There are several optimizers in SciPy, in the module scipy.optimize. You can simply install them with +pip install scipy. \n",

"\n",

"You may find the user manual of this module in https://docs.scipy.org/doc/scipy/tutorial/optimize.html#tutorial-sqlsp.\n",

"\n",

"In this serie of notebooks, we mostly use BFGS, a quasi-Newton constraint-free algorithm, and SLSQP, a sequential QP solver accepting both equality and inequality constraints.\n",

"In this serie of notebooks about robotics, we mostly use BFGS, a quasi-Newton constraint-free algorithm, and SLSQP, a sequential QP solver accepting both equality and inequality constraints.\n",

"\n",

"We will then need the two +fmin functions from the scipy.optimize module, as well as +numpy to represent algebraic vectors."

"# Optimize cost without any constraints in CLSQ\n",

...

...

@@ -175,7 +130,7 @@

"source": [

"Now, SLSQP can also handle explicit constraints. Equality and inequality constraints must be given separately as function from the parameter $x$ to a vector stacking all the numerical quantities, that must be null for equalities, and positive for inequalities.\n",

"\n",

"We introduce here, as an example, two set of polynomial "

"We introduce here, as an example, two set of polynomial constraints."

"print('\\n *** Xopt in c-lsq = %s \\n\\n\\n\\n' % str(xopt_clsq))\n"

]

},

{

"cell_type": "markdown",

"metadata": {},

"source": [

"That's all for now, folks."

]

}

],

"metadata": {

...

...

%% Cell type:markdown id: tags:

# Optimizers in SciPy

This notebook is a very brief introduction to SciPy optimizers, documenting the example appendix/scipy_optim.py.

%% Cell type:markdown id: tags:

There are several optimizers in SciPy, in the module scipy.optimize. You can simply install them with +pip install scipy.

You may find the user manual of this module in https://docs.scipy.org/doc/scipy/tutorial/optimize.html#tutorial-sqlsp.

In this serie of notebooks, we mostly use BFGS, a quasi-Newton constraint-free algorithm, and SLSQP, a sequential QP solver accepting both equality and inequality constraints.

In this serie of notebooks about robotics, we mostly use BFGS, a quasi-Newton constraint-free algorithm, and SLSQP, a sequential QP solver accepting both equality and inequality constraints.

We will then need the two +fmin functions from the scipy.optimize module, as well as +numpy to represent algebraic vectors.

%% Cell type:code id: tags:

``` python

# %load appendix/generated/scipy_optim_import

importnumpyasnp

fromscipy.optimizeimportfmin_bfgs,fmin_slsqp

```

%% Cell type:markdown id: tags:

They are generally following a similar API, taking as main argument the cost function to optimize +f, the initial guess +x0, and optiminally a callback function +callback and some constraints.

The cost objective should be defined as a function mapping the parameter space $x$ to a real value $f(x)$. Here is a simple polynomial example for $x \in R^2$:

%% Cell type:code id: tags:

``` python

# %load appendix/generated/scipy_optim_cost

defcost(x):

'''Cost f(x,y) = x^2 + 2y^2 - 2xy - 2x '''

x0=x[0]

x1=x[1]

return-1*(2*x0*x1+2*x0-x0**2-2*x1**2)

```

%% Cell type:markdown id: tags:

The callback takes the same signature but returns nothing: it only works by side effect, for example printing something, or displaying some informations in a viewer or on a plot, or possibly storing data in a logger. Here is for example a callback written as the functor of an object, that can be used to adjust its behavior or store some data.

print('\n *** Xopt in BFGS = %s \n\n\n\n'%str(xopt_bfgs))

```

%% Output

===CBK=== 1 1.010000 -0.000000

===CBK=== 2 2.014799 1.009848

===CBK=== 3 2.000000 1.000000

Optimization terminated successfully.

Current function value: -2.000000

Iterations: 3

Function evaluations: 12

Gradient evaluations: 4

*** Xopt in BFGS = [1.99999977 0.99999994]

%% Cell type:markdown id: tags:

In that case, the gradients of the cost are computed by BFGS using finite differencing (i.e. not very accurately, but the algorithmic cost is typically very bad). If you can provide some derivatives by yourself, it would greatly improve the result. Yet, as a first draft, it is generally not too bad.

Now, SLSQP can also handle explicit constraints. Equality and inequality constraints must be given separately as function from the parameter $x$ to a vector stacking all the numerical quantities, that must be null for equalities, and positive for inequalities.

We introduce here, as an example, two set of polynomial

We introduce here, as an example, two set of polynomial constraints.