-
Notifications
You must be signed in to change notification settings - Fork 38
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
* Implement MANN::generateDummyMANNOutput and MANN::generateDummyMANNInput in ML component * Restructure MANNAutoregressive to properly handle the reset: - define the MANNFootState class - move all the quantities required to reset the state in MANNAutoregressive::AutoregressiveState - simplify the generation of the initial AutoregressiveState required to start the trajectory generation * Restructure MANNTrajectoryGenerator - eliminate the need to pass foot positions in MANNTrajectoryGenerator::setInitialState - enhance MANNTrajectoryGenerator with reset capabilities similar to MANNAutoregressive - correct the position and time scaling of MANNTrajectoryGenerator to accurately scale the CoM, base, and feet positions * Add MANNTrajectoryGenerator and MANNAutoregressive jupyter notebooks
- Loading branch information
1 parent
136302a
commit 83c8257
Showing
16 changed files
with
1,591 additions
and
491 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,232 @@ | ||
| { | ||
| "cells": [ | ||
| { | ||
| "cell_type": "markdown", | ||
| "metadata": {}, | ||
| "source": [ | ||
| "# MANNAutoregressive\n", | ||
| "This notebook contains the code showing how to use MANNAutoregressive to generate a walking trajectory for the ergoCub humanoid robot. \n", | ||
| "\n", | ||
| "If you are interested in some details of the algorithm, please refer to the paper: [ADHERENT: Learning Human-like Trajectory Generators for Whole-body Control of Humanoid Robots](https://github.com/ami-iit/paper_viceconte_2021_ral_adherent).\n", | ||
| "\n", | ||
| "## Import all the required packages\n", | ||
| "In this section we import all the required packages. In particular, we need `iDynTree` to correctly visualize the robot, `numpy` to perform some basic operations, `manifpy` to perform some basic operations on manifolds, `resolve_robotics_uri_py` to correctly locate the `ergoCub` model. Finally `bipedal_locomotion_framework` implements the MANN network." | ||
| ] | ||
| }, | ||
| { | ||
| "cell_type": "code", | ||
| "execution_count": null, | ||
| "id": "1e357295-6730-47d6-9119-bd09ed598e45", | ||
| "metadata": {}, | ||
| "outputs": [], | ||
| "source": [ | ||
| "from idyntree.visualize import MeshcatVisualizer\n", | ||
| "import idyntree.bindings as idyn\n", | ||
| "from pathlib import Path\n", | ||
| "import numpy as np\n", | ||
| "import manifpy as manif\n", | ||
| "import bipedal_locomotion_framework as blf\n", | ||
| "import resolve_robotics_uri_py" | ||
| ] | ||
| }, | ||
| { | ||
| "cell_type": "markdown", | ||
| "metadata": {}, | ||
| "source": [ | ||
| "## Prepare the ingredients\n", | ||
| "To correctly run the notebook, we need to prepare the ingredients. In particular, we need to:\n", | ||
| "- Get the network model: In this case we use the model trained on the ergoCub robot. The model is available [here](https://huggingface.co/ami-iit/mann/resolve/main/ergocubSN000_26j_49e.onnx).\n", | ||
| "- Load the configuration file: The configuration file contains all the parameters needed to correctly run the network and generate a proper set of inputs for the network. In detail we have two configuration files:\n", | ||
| " - `config_mann.toml`: This file contains the parameters needed to correctly load and run the network.\n", | ||
| " - `config_joypad.toml`: This file contains the parameters needed to correctly generate the input for the network. In particular, it contains the parameters needed to correctly map a joypad input to the network input.\n", | ||
| "- Load the robot model: In this case we use the `ergoCub` model. We load the model only to correctly visualize the robot.\n", | ||
| "\n", | ||
| "### Get the network model\n", | ||
| "What we need to do is to download the network model and save it in the `config` folder. The model is available [here](https://huggingface.co/ami-iit/mann/resolve/main/ergocubSN000_26j_49e.onnx). \n", | ||
| "Moreover we load the parameters needed to correctly run the network." | ||
| ] | ||
| }, | ||
| { | ||
| "cell_type": "code", | ||
| "execution_count": null, | ||
| "id": "08029010-6814-4101-af0f-ac2940560eac", | ||
| "metadata": {}, | ||
| "outputs": [], | ||
| "source": [ | ||
| "# We use urllib to download the onnx model from huggingface\n", | ||
| "import urllib.request\n", | ||
| "import os\n", | ||
| "url = \"https://huggingface.co/ami-iit/mann/resolve/main/ergocubSN000_26j_49e.onnx\"\n", | ||
| "urllib.request.urlretrieve(url, \"./config/ergocubSN000_26j_49e.onnx\")\n", | ||
| "\n", | ||
| "# Get the configuration files\n", | ||
| "config_path = Path(\"__file__\").parent / \"config\" / \"config_mann.toml\"\n", | ||
| "params_network = blf.parameters_handler.TomlParametersHandler()\n", | ||
| "params_network.set_from_file(str(config_path))\n", | ||
| "\n", | ||
| "joypad_config_path = Path(\"__file__\").parent / \"config\" / \"config_joypad.toml\"\n", | ||
| "params_joypad = blf.parameters_handler.TomlParametersHandler()\n", | ||
| "params_joypad.set_from_file(str(joypad_config_path))" | ||
| ] | ||
| }, | ||
| { | ||
| "cell_type": "markdown", | ||
| "metadata": {}, | ||
| "source": [ | ||
| "### Load ergoCub model\n", | ||
| "We load the `ergoCub` model to correctly visualize the robot.\n", | ||
| "The model is loaded using `iDynTree` and `resolve_robotics_uri_py` to correctly locate the model. Please notice that the model is loaded specifying the same joint order used to train the network." | ||
| ] | ||
| }, | ||
| { | ||
| "cell_type": "code", | ||
| "execution_count": null, | ||
| "id": "e3817e56-9e8d-464e-9f0b-6577a2c0f2fe", | ||
| "metadata": {}, | ||
| "outputs": [], | ||
| "source": [ | ||
| "# Get the path of the robot model\n", | ||
| "robot_model_path = str(resolve_robotics_uri_py.resolve_robotics_uri(\"package://ergoCub/robots/ergoCubSN001/model.urdf\"))\n", | ||
| "ml = idyn.ModelLoader()\n", | ||
| "ml.loadReducedModelFromFile(robot_model_path, params_network.get_parameter_vector_string(\"joints_list\"))\n", | ||
| "viz = MeshcatVisualizer()\n", | ||
| "viz.load_model(ml.model())" | ||
| ] | ||
| }, | ||
| { | ||
| "cell_type": "markdown", | ||
| "metadata": {}, | ||
| "source": [ | ||
| "## Network initialization\n", | ||
| "In this section we initialize the network. In particular we first instantiate the network and then we load the parameters. The parameters are loaded from the `config_mann.toml` file." | ||
| ] | ||
| }, | ||
| { | ||
| "cell_type": "code", | ||
| "execution_count": null, | ||
| "metadata": {}, | ||
| "outputs": [], | ||
| "source": [ | ||
| "# Create the trajectory generator\n", | ||
| "mann_trajectory_generator = blf.ml.MANNAutoregressive()\n", | ||
| "assert mann_trajectory_generator.set_robot_model(ml.model())\n", | ||
| "assert mann_trajectory_generator.initialize(params_network)\n", | ||
| "\n", | ||
| "# Create the input builder\n", | ||
| "input_builder = blf.ml.MANNAutoregressiveInputBuilder()\n", | ||
| "assert input_builder.initialize(params_joypad)" | ||
| ] | ||
| }, | ||
| { | ||
| "cell_type": "markdown", | ||
| "metadata": {}, | ||
| "source": [ | ||
| "We now need to reset the network to the initial state. In particular, we need to reset the joint state and the base pose in a given configuration already seen by the network during traning." | ||
| ] | ||
| }, | ||
| { | ||
| "cell_type": "code", | ||
| "execution_count": null, | ||
| "metadata": {}, | ||
| "outputs": [], | ||
| "source": [ | ||
| "# Initial joint positions configuration. The serialization is specified in the config file\n", | ||
| "joint_positions = params_network.get_parameter_vector_float(\"initial_joints_configuration\")\n", | ||
| "\n", | ||
| "# Initial base pose. This pose makes the robot stand on the ground with the feet flat\n", | ||
| "base_pose = manif.SE3.Identity()\n", | ||
| "initial_base_height = params_network.get_parameter_float(\"initial_base_height\")\n", | ||
| "quat = params_network.get_parameter_vector_float(\"initial_base_quaternion\")\n", | ||
| "quat = quat / np.linalg.norm(quat) # Normalize the quaternion\n", | ||
| "base_pose = manif.SE3([0, 0, initial_base_height], quat)" | ||
| ] | ||
| }, | ||
| { | ||
| "cell_type": "markdown", | ||
| "metadata": {}, | ||
| "source": [ | ||
| "We finally ask the input builder to generate an input that mimics a joypad input. In particular, we ask the input builder to generate an input that mimics a joypad input with the axes moved to the following values:\n", | ||
| "- `left_stick_x`: 1.0\n", | ||
| "- `left_stick_y`: 0.0\n", | ||
| "- `right_stick_x`: 1.0\n", | ||
| "- `right_stick_y`: 0.0" | ||
| ] | ||
| }, | ||
| { | ||
| "cell_type": "code", | ||
| "execution_count": null, | ||
| "metadata": {}, | ||
| "outputs": [], | ||
| "source": [ | ||
| "input_builder_input = blf.ml.MANNDirectionalInput()\n", | ||
| "input_builder_input.motion_direction = np.array([1, 0])\n", | ||
| "input_builder_input.facing_direction = np.array([1, 0])" | ||
| ] | ||
| }, | ||
| { | ||
| "cell_type": "markdown", | ||
| "metadata": {}, | ||
| "source": [ | ||
| "## Network execution\n", | ||
| "We now visualize the robot in the viewer and we run the network for 150 steps. In particular, we run the network for 150 steps and we visualize the resulting robot motion at each step." | ||
| ] | ||
| }, | ||
| { | ||
| "cell_type": "code", | ||
| "execution_count": null, | ||
| "metadata": {}, | ||
| "outputs": [], | ||
| "source": [ | ||
| "viz.jupyter_cell()" | ||
| ] | ||
| }, | ||
| { | ||
| "cell_type": "code", | ||
| "execution_count": null, | ||
| "metadata": {}, | ||
| "outputs": [], | ||
| "source": [ | ||
| "# Reset the trajectory generator\n", | ||
| "mann_trajectory_generator.reset(joint_positions, base_pose)\n", | ||
| "for _ in range(150):\n", | ||
| "\n", | ||
| " # Set the input to the builder and\n", | ||
| " input_builder.set_input(input_builder_input)\n", | ||
| " assert input_builder.advance()\n", | ||
| " assert input_builder.is_output_valid()\n", | ||
| "\n", | ||
| " # Set the input to the trajectory generator\n", | ||
| " mann_trajectory_generator.set_input(input_builder.get_output())\n", | ||
| " assert mann_trajectory_generator.advance()\n", | ||
| " assert mann_trajectory_generator.is_output_valid()\n", | ||
| "\n", | ||
| " # Get the output of the trajectory generator and update the visualization\n", | ||
| " mann_output = mann_trajectory_generator.get_output()\n", | ||
| " viz.set_multibody_system_state(mann_output.base_pose.translation(),\n", | ||
| " mann_output.base_pose.rotation(),\n", | ||
| " mann_output.joint_positions)" | ||
| ] | ||
| } | ||
| ], | ||
| "metadata": { | ||
| "kernelspec": { | ||
| "display_name": "Python 3 (ipykernel)", | ||
| "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.10.12" | ||
| } | ||
| }, | ||
| "nbformat": 4, | ||
| "nbformat_minor": 5 | ||
| } |
Oops, something went wrong.