Commit 7b5769d3 authored by Thomas Flayols's avatar Thomas Flayols
Browse files

update tp4 notebook (actuators), upload tp5 (solo12 control)

parent ebea72b1
This diff is collapsed.
This diff is collapsed.
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Controllers for Solo12\n",
"This notebook will be used as a base to design controller to be executed on the Solo12 real quadruped robot."
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {},
"outputs": [],
"source": [
"from math import pi\n",
"from ipywidgets import interact\n",
"from pinocchio.visualize import MeshcatVisualizer\n",
"%config Completer.use_jedi = False\n",
"import numpy as np\n",
"np.set_printoptions(precision=3,suppress=True)\n",
"from example_robot_data.robots_loader import Solo12Loader\n",
"import pinocchio as pin\n",
"Solo12Loader.free_flyer = False #Important, we are working with a fixed based model version (12dof)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Let's load solo12 and setup a viewer"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"You can open the visualizer by visiting the following URL:\n",
"http://127.0.0.1:7000/static/\n"
]
}
],
"source": [
"solo12 = Solo12Loader().robot\n",
"viz = MeshcatVisualizer(solo12.model, solo12.collision_model, solo12.visual_model)\n",
"viz.initViewer(loadModel=True)\n",
"viz.viewer.jupyter_cell()\n",
"q0 = solo12.q0\n",
"viz.display(q0)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Let us have some input control to vary the robot state (Front left foot - 3dof)"
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {},
"outputs": [
{
"data": {
"application/vnd.jupyter.widget-view+json": {
"model_id": "b140b0490450479a8842c69e9c404d0d",
"version_major": 2,
"version_minor": 0
},
"text/plain": [
"interactive(children=(FloatSlider(value=0.0, description='HAA', max=1.0, min=-1.0, step=0.01), FloatSlider(val…"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"@interact(HAA=(-1.0, 1.0, 0.01),HFE=(-1.0, 1.0, 0.01),KFE=(-1.0, 1.0, 0.01))\n",
"def change_q(HAA=0.,HFE=0.,KFE=0.):\n",
" q0[0:3] = HAA,HFE,KFE\n",
" print(q0[0:3])\n",
" viz.display(q0)"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Pinocchio cheat code"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Get the Jacobian of the Feet, expressed locally, aligned with the world:"
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"[[ 0. -0.32 -0.16 0. 0. 0. 0. 0. 0. 0.\n",
" 0. 0. ]\n",
" [ 0.32 0. 0. 0. 0. 0. 0. 0. 0. 0.\n",
" 0. 0. ]\n",
" [ 0.059 0. 0. 0. 0. 0. 0. 0. 0. 0.\n",
" 0. 0. ]]\n"
]
}
],
"source": [
"solo12.forwardKinematics(q0)\n",
"solo12.framesForwardKinematics(q0)\n",
"solo12.computeJointJacobians(q0)\n",
"J = solo12.getFrameJacobian(solo12.model.getFrameId('FL_FOOT'),pin.LOCAL_WORLD_ALIGNED)[:3]\n",
"print (J)"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
}
],
"metadata": {
"kernelspec": {
"display_name": "Python 3",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.6.9"
}
},
"nbformat": 4,
"nbformat_minor": 4
}
%% Cell type:markdown id: tags:
# Controllers for Solo12
This notebook will be used as a base to design controller to be executed on the Solo12 real quadruped robot.
%% Cell type:code id: tags:
``` python
from math import pi
from ipywidgets import interact
from pinocchio.visualize import MeshcatVisualizer
%config Completer.use_jedi = False
import numpy as np
np.set_printoptions(precision=3,suppress=True)
from example_robot_data.robots_loader import Solo12Loader
import pinocchio as pin
Solo12Loader.free_flyer = False #Important, we are working with a fixed based model version (12dof)
```
%% Cell type:markdown id: tags:
Let's load solo12 and setup a viewer
%% Cell type:code id: tags:
``` python
solo12 = Solo12Loader().robot
viz = MeshcatVisualizer(solo12.model, solo12.collision_model, solo12.visual_model)
viz.initViewer(loadModel=True)
viz.viewer.jupyter_cell()
q0 = solo12.q0
viz.display(q0)
```
%%%% Output: stream
You can open the visualizer by visiting the following URL:
http://127.0.0.1:7000/static/
%% Cell type:markdown id: tags:
Let us have some input control to vary the robot state (Front left foot - 3dof)
%% Cell type:code id: tags:
``` python
@interact(HAA=(-1.0, 1.0, 0.01),HFE=(-1.0, 1.0, 0.01),KFE=(-1.0, 1.0, 0.01))
def change_q(HAA=0.,HFE=0.,KFE=0.):
q0[0:3] = HAA,HFE,KFE
print(q0[0:3])
viz.display(q0)
```
%%%% Output: display_data
%% Cell type:code id: tags:
``` python
```
%% Cell type:code id: tags:
``` python
```
%% Cell type:code id: tags:
``` python
```
%% Cell type:code id: tags:
``` python
```
%% Cell type:code id: tags:
``` python
```
%% Cell type:code id: tags:
``` python
```
%% Cell type:code id: tags:
``` python
```
%% Cell type:markdown id: tags:
# Pinocchio cheat code
%% Cell type:markdown id: tags:
Get the Jacobian of the Feet, expressed locally, aligned with the world:
%% Cell type:code id: tags:
``` python
solo12.forwardKinematics(q0)
solo12.framesForwardKinematics(q0)
solo12.computeJointJacobians(q0)
J = solo12.getFrameJacobian(solo12.model.getFrameId('FL_FOOT'),pin.LOCAL_WORLD_ALIGNED)[:3]
print (J)
```
%%%% Output: stream
[[ 0. -0.32 -0.16 0. 0. 0. 0. 0. 0. 0.
0. 0. ]
[ 0.32 0. 0. 0. 0. 0. 0. 0. 0. 0.
0. 0. ]
[ 0.059 0. 0. 0. 0. 0. 0. 0. 0. 0.
0. 0. ]]
%% Cell type:code id: tags:
``` python
```
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment