From b5b9b70c2b04900fdb632f25cfc727804a83548b Mon Sep 17 00:00:00 2001 From: Alexis DUBURCQ Date: Sun, 5 Sep 2021 15:25:35 +0200 Subject: [PATCH] [python/viewer] Fix meshcat backend. (#408) * [core] Shuffle constraint solver iterations iif necessary, no matter if constraint set changes. * [core] Move contact point at the interface with the ground for impulse model. (#404) * [core/python] Fix bindings signature of some controller's method. * [python/plot] Fix missing grid for single plot tabs. * [python/viewer] Fix viewer compatibility with meshcat>=0.3.1. * [python/viewer] Increase meshcat recorder timeout. * [python/viewer] Fix meshcat viewer 'has_gui' method. * [python/viewer] Fix meshcat notebook viewer. * [python/viewer] Properly close opening viewer if raises exception at init. * [python/viewer] Add height map rendering capability to Panda3d backend. * [python/viewer] Add support of jupyterlab and vscode notebooks locally using Meshcat backend. * [gym/common] Check that simulation data is available before plotting. * [gym/common] Fix 'play_interactive' not disabling 'is_training' flag. * [gym/common] Add flag to 'play_interactive' to ignore 'is_done' state. * [gym/rllib] Support obs normalization for PPO spatial regularization. * [gym/rllib] Raise clear exception if observation space is not dict in PPO. * [gym/rllib] Replace L2-norm temporal smoothness regularization by L1-norm. * [gym/rllib] Fix L2 reg not backpropagating gradient. * [gym/rllib] Scale spatial loss by observation distance. * [misc] Fix CI dependency install. * [misc] Relax ray version requirement. Co-authored-by: Alexis Duburcq --- .github/workflows/manylinux.yml | 2 +- .github/workflows/ubuntu.yml | 3 +- .github/workflows/win.yml | 2 +- CMakeLists.txt | 2 +- README.md | 2 +- core/src/engine/EngineMultiRobot.cc | 29 +- core/src/solver/LCPSolvers.cc | 36 +- examples/tutorial.ipynb | 1463 ++++------------- .../gym_jiminy/common/envs/env_generic.py | 24 +- .../gym_jiminy/common/envs/env_locomotion.py | 2 +- python/gym_jiminy/common/setup.py | 2 +- .../gym_jiminy/rllib/gym_jiminy/rllib/ppo.py | 99 +- .../rllib/gym_jiminy/rllib/utilities.py | 25 +- python/gym_jiminy/rllib/setup.py | 2 +- .../unit_py/data/atlas_standing_meshcat_1.png | Bin 0 -> 27598 bytes .../data/cassie_standing_meshcat_3.png | Bin 0 -> 9751 bytes .../data/cassie_standing_meshcat_4.png | Bin 0 -> 9975 bytes .../data/cassie_standing_meshcat_5.png | Bin 0 -> 9757 bytes .../data/cassie_standing_panda3d_6.png | Bin 0 -> 7811 bytes .../data/cassie_standing_panda3d_7.png | Bin 0 -> 6679 bytes .../unit_py/test_pipeline_control.py | 12 +- python/jiminy_py/setup.py | 5 +- python/jiminy_py/src/jiminy_py/dynamics.py | 4 +- python/jiminy_py/src/jiminy_py/plot.py | 1 + .../src/jiminy_py/viewer/meshcat/index.html | 32 +- .../src/jiminy_py/viewer/meshcat/recorder.py | 6 +- .../src/jiminy_py/viewer/meshcat/server.py | 36 +- .../src/jiminy_py/viewer/meshcat/wrapper.py | 43 +- .../viewer/panda3d/panda3d_visualizer.py | 96 +- .../jiminy_py/src/jiminy_py/viewer/viewer.py | 31 +- python/jiminy_pywrap/src/Controllers.cc | 6 +- 31 files changed, 704 insertions(+), 1261 deletions(-) create mode 100644 python/gym_jiminy/unit_py/data/atlas_standing_meshcat_1.png create mode 100644 python/gym_jiminy/unit_py/data/cassie_standing_meshcat_3.png create mode 100644 python/gym_jiminy/unit_py/data/cassie_standing_meshcat_4.png create mode 100644 python/gym_jiminy/unit_py/data/cassie_standing_meshcat_5.png create mode 100644 python/gym_jiminy/unit_py/data/cassie_standing_panda3d_6.png create mode 100644 python/gym_jiminy/unit_py/data/cassie_standing_panda3d_7.png diff --git a/.github/workflows/manylinux.yml b/.github/workflows/manylinux.yml index 62756c1a8..4ceab5afd 100644 --- a/.github/workflows/manylinux.yml +++ b/.github/workflows/manylinux.yml @@ -66,7 +66,7 @@ jobs: - name: Build project dependencies run: | ./build_tools/build_install_deps_linux.sh - "${PYTHON_EXECUTABLE}" -m pip install "gym>=0.18.3" "stable_baselines3>=0.10" "importlib-metadata>=3.3.0" + "${PYTHON_EXECUTABLE}" -m pip install --prefer-binary "gym>=0.18.3" "stable_baselines3>=0.10" "importlib-metadata>=3.3.0" ##################################################################################### diff --git a/.github/workflows/ubuntu.yml b/.github/workflows/ubuntu.yml index 1fdcad88d..49eec3a1c 100644 --- a/.github/workflows/ubuntu.yml +++ b/.github/workflows/ubuntu.yml @@ -52,7 +52,7 @@ jobs: "${PYTHON_EXECUTABLE}" -m pip install --upgrade numpy "${PYTHON_EXECUTABLE}" -m pip install tensorflow "${PYTHON_EXECUTABLE}" -m pip install "torch==1.8.0+cpu" -f https://download.pytorch.org/whl/torch_stable.html - "${PYTHON_EXECUTABLE}" -m pip install "gym>=0.18.3" "stable_baselines3>=0.10" "importlib-metadata>=3.3.0" + "${PYTHON_EXECUTABLE}" -m pip install --prefer-binary "gym>=0.18.3" "stable_baselines3>=0.10" "importlib-metadata>=3.3.0" "${PYTHON_EXECUTABLE}" -m pip install "ray[default,rllib]<=1.4.0" # Type checking is not working with 1.4.1 ##################################################################################### @@ -121,6 +121,7 @@ jobs: --disable=fixme,abstract-method,protected-access,useless-super-delegation \ --disable=too-many-instance-attributes,too-many-arguments,too-few-public-methods,too-many-lines \ --disable=too-many-locals,too-many-branches,too-many-statements \ + --disable=unspecified-encoding,logging-fstring-interpolation \ --generated-members=numpy.*,torch.* "gym_jiminy/" mypy --allow-redefinition --check-untyped-defs --disallow-incomplete-defs --disallow-untyped-defs \ diff --git a/.github/workflows/win.yml b/.github/workflows/win.yml index 4e8f73e83..3ed4e40bb 100644 --- a/.github/workflows/win.yml +++ b/.github/workflows/win.yml @@ -50,7 +50,7 @@ jobs: - name: Build project dependencies run: | python -m pip install "torch==1.8.0+cpu" -f https://download.pytorch.org/whl/torch_stable.html - python -m pip install "gym>=0.18.3" "stable_baselines3>=0.10" "importlib-metadata>=3.3.0" + python -m pip install --prefer-binary "gym>=0.18.3" "stable_baselines3>=0.10" "importlib-metadata>=3.3.0" & "./build_tools/build_install_deps_windows.ps1" ##################################################################################### diff --git a/CMakeLists.txt b/CMakeLists.txt index 97cc4dd51..bb9781a17 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.10) # Set the build version -set(BUILD_VERSION 1.6.29) +set(BUILD_VERSION 1.6.30) # Set compatibility if(CMAKE_VERSION VERSION_GREATER "3.11.0") diff --git a/README.md b/README.md index dc92267d3..b3e22277c 100644 --- a/README.md +++ b/README.md @@ -24,7 +24,7 @@ Beside a strong focus on performance to answer machine learning's need for runni - C++ core with full python bindings, providing frontend API parity between both languages. - Designed with machine learning in mind, with seemless wrapping of robots as [OpenAI Gym](https://github.com/openai/gym) environments using one-liners. Jiminy provides both the physical engine and the robot model (including sensors) required for learning. - Easy to install: `pip` is all that is needed to [get you started](#getting-started) ! -- Dedicated integration in jupyter notebook working out-of-the-box - including 3D rendering using [Meshcat](https://github.com/rdeits/MeshCat.jl). This facilitates working on remote headless environnement such as machine learning clusters. +- Dedicated integration in Google Colab, Jupyter Lab, and VSCode working out-of-the-box - including interactive 3D viewer based on [Meshcat](https://github.com/rdeits/MeshCat.jl). This facilitates working on remote headless environnement such as machine learning clusters. - Cross-platform offscreen rendering capability, without requiring X-server, based on [Panda3d](https://github.com/panda3d/panda3d). - Rich simulation log output, easily customizable for recording, introspection and debugging. The simulation log is made available in RAM directly for fast access, and can be exported in raw binary, CSV or [HDF5](https://portal.hdfgroup.org/display/HDF5/Introduction+to+HDF5) format. - Available for both Linux and Windows platforms. diff --git a/core/src/engine/EngineMultiRobot.cc b/core/src/engine/EngineMultiRobot.cc index 96dc33cea..c428b7914 100644 --- a/core/src/engine/EngineMultiRobot.cc +++ b/core/src/engine/EngineMultiRobot.cc @@ -1171,9 +1171,9 @@ namespace jiminy { stepper_ = std::unique_ptr( new RungeKuttaDOPRIStepper(systemOde, - robots, - engineOptions_->stepper.tolAbs, - engineOptions_->stepper.tolRel)); + robots, + engineOptions_->stepper.tolAbs, + engineOptions_->stepper.tolRel)); } else if (engineOptions_->stepper.odeSolver == "runge_kutta_4") { @@ -2920,6 +2920,9 @@ namespace jiminy { constraint->reset(q, v); constraint->enable(); + auto & frameConstraint = static_cast(*constraint.get()); + vector3_t & positionRef = frameConstraint.getReferenceTransform().translation(); + positionRef.noalias() -= depth * nGround; } } } @@ -2996,11 +2999,15 @@ namespace jiminy } else { - // Enable fixed frame constraint and reset it if it was disable + // Enable fixed frame constraint and reset it if it was disable, + // then move the reference position at the surface of the ground. if (!constraint->getIsEnabled()) { constraint->reset(q, v); constraint->enable(); + auto & frameConstraint = static_cast(*constraint.get()); + vector3_t & positionRef = frameConstraint.getReferenceTransform().translation(); + positionRef.noalias() -= depth * nGround; } } } @@ -3047,19 +3054,19 @@ namespace jiminy // Compute normal force float64_t const fextNormal = - std::min(contactOptions_.stiffness * depth + contactOptions_.damping * vDepth, 0.0); - fextInWorld = fextNormal * nGround; + fextInWorld.noalias() = fextNormal * nGround; // Compute friction forces vector3_t const vTangential = vContactInWorld - vDepth * nGround; float64_t const vRatio = std::min(vTangential.norm() / contactOptions_.transitionVelocity, 1.0); float64_t const fextTangential = contactOptions_.friction * vRatio * fextNormal; - fextInWorld -= fextTangential * vTangential; + fextInWorld.noalias() -= fextTangential * vTangential; // Add blending factor if (contactOptions_.transitionEps > EPS) { - float64_t const blendingFactor = -depth / contactOptions_.transitionEps; - float64_t const blendingLaw = std::tanh(2 * blendingFactor); + float64_t const blendingFactor = - depth / contactOptions_.transitionEps; + float64_t const blendingLaw = std::tanh(2.0 * blendingFactor); fextInWorld *= blendingLaw; } } @@ -3378,7 +3385,7 @@ namespace jiminy vectorN_t const & stiffness = mdlDynOptions.flexibilityConfig[i].stiffness; vectorN_t const & damping = mdlDynOptions.flexibilityConfig[i].damping; - quaternion_t const quat(q.segment<4>(positionIdx).data()); // Only way to initialize with [x,y,z,w] order + quaternion_t const quat(q.segment<4>(positionIdx)); // Only way to initialize with [x,y,z,w] order vectorN_t const axis = pinocchio::quaternion::log3(quat); uInternal.segment<3>(velocityIdx).array() += - stiffness.array() * axis.array() @@ -3756,7 +3763,7 @@ namespace jiminy i, pinocchio::LOCAL, jointJacobian); - uAugmented += jointJacobian.transpose() * fext[i].toVector(); + uAugmented.noalias() += jointJacobian.transpose() * fext[i].toVector(); } // Compute non-linear effects @@ -3823,7 +3830,7 @@ namespace jiminy // Convert the force from local world aligned to local frame frameIndex_t const & frameIdx = frameConstraint.getFrameIdx(); pinocchio::SE3 const & transformContactInWorld = data.oMf[frameIdx]; - forceIt->linear() = transformContactInWorld.rotation().transpose() * fextWorld; + forceIt->linear().noalias() = transformContactInWorld.rotation().transpose() * fextWorld; // Convert the force from local world aligned to local parent joint jointIndex_t const & jointIdx = model.frames[frameIdx].parent; diff --git a/core/src/solver/LCPSolvers.cc b/core/src/solver/LCPSolvers.cc index 8e264e2e3..db05aa836 100644 --- a/core/src/solver/LCPSolvers.cc +++ b/core/src/solver/LCPSolvers.cc @@ -89,14 +89,38 @@ namespace jiminy https://github.com/dartsim/dart/blob/master/dart/constraint/PgsBoxedLcpSolver.cpp */ assert(b.size() > 0 && "The number of inequality constraints must be larger than 0."); - /* Reset shuffling counter. + /* Adapt shuffling indices if the number of indices has changed. Note that it may converge faster to enforce constraints in reverse order, since usually constraints bounds dependending on others have lower indices by design. For instance, for friction, x and y */ - indices_.resize(b.size()); - std::generate(indices_.begin(), indices_.end(), - [n = static_cast(indices_.size() - 1)]() mutable { return n--; }); - lastShuffle_ = 0U; // Do NOT shuffle indices right after initialization + size_t const nIndicesOrig = indices_.size(); + size_t const nIndices = b.size(); + if (nIndicesOrig < nIndices) + { + indices_.resize(nIndices); + std::generate(indices_.begin() + nIndicesOrig, indices_.end(), + [n = static_cast(nIndices - 1)]() mutable { return n--; }); + } + else if (nIndicesOrig > nIndices) + { + size_t shiftIdx = nIndices; + for (size_t i = 0; i < nIndices; ++i) + { + if (static_cast(indices_[i]) >= nIndices) + { + for (size_t j = shiftIdx; j < nIndicesOrig; ++j) + { + ++shiftIdx; + if (static_cast(indices_[j]) < nIndices) + { + indices_[i] = indices_[j]; + break; + } + } + } + } + indices_.resize(nIndices); + } // Normalizing for (Eigen::Index i = 0; i < b.size(); ++i) @@ -111,6 +135,8 @@ namespace jiminy bool_t isSuccess = ProjectedGaussSeidelIter(A, b, lo, hi, fIdx, false, true, x); if (isSuccess) { + // Do NOT shuffle indices unless necessary to avoid discontinuities + lastShuffle_ = 0U; return true; } } diff --git a/examples/tutorial.ipynb b/examples/tutorial.ipynb index 4ee437e82..ae4062cb8 100644 --- a/examples/tutorial.ipynb +++ b/examples/tutorial.ipynb @@ -2,7 +2,6 @@ "cells": [ { "cell_type": "markdown", - "metadata": {}, "source": [ "# An introduction to Jiminy\n", "\n", @@ -27,37 +26,12 @@ "The robot is constructed from a URDF - this builds a jiminy.Model object - but extra information needs to be provided as well for a simulation: which sensors to use and what are their caracteristic ? Which joints have a motor attached and what are its properties ? What are the contact points with the ground (if any) ? All this are informations gathered to build a full robot.\n", "\n", "So let's get our first example running: we set the inverted pendulum away from its upward position and watch it fall." - ] + ], + "metadata": {} }, { "cell_type": "code", - "execution_count": 2, - "metadata": { - "tags": [] - }, - "outputs": [ - { - "data": { - "application/vnd.jupyter.widget-view+json": { - "model_id": "2cca83eadffa4a86904a50015cb21fff", - "version_major": 2, - "version_minor": 0 - }, - "text/plain": [ - "HBox(children=(FloatProgress(value=0.0, max=10.0), HTML(value='')))" - ] - }, - "metadata": {}, - "output_type": "display_data" - }, - { - "name": "stdout", - "output_type": "stream", - "text": [ - "\n" - ] - } - ], + "execution_count": 1, "source": [ "import os\n", "from pkg_resources import resource_filename\n", @@ -98,998 +72,101 @@ "\n", "# Launch the simulation\n", "simulator.simulate(simulation_duration, q0, v0)" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "The simulation generates a log of its comuptation: this log can be retrieved by using ```simulator.get_log``` - and written to a file for latter processing by the engine with ```simulator.engine.write_log```." - ] - }, - { - "cell_type": "code", - "execution_count": 4, - "metadata": {}, + ], "outputs": [ { + "output_type": "display_data", "data": { - "application/javascript": [ - "/* Put everything inside the global mpl namespace */\n", - "/* global mpl */\n", - "window.mpl = {};\n", - "\n", - "mpl.get_websocket_type = function () {\n", - " if (typeof WebSocket !== 'undefined') {\n", - " return WebSocket;\n", - " } else if (typeof MozWebSocket !== 'undefined') {\n", - " return MozWebSocket;\n", - " } else {\n", - " alert(\n", - " 'Your browser does not have WebSocket support. ' +\n", - " 'Please try Chrome, Safari or Firefox ≥ 6. ' +\n", - " 'Firefox 4 and 5 are also supported but you ' +\n", - " 'have to enable WebSockets in about:config.'\n", - " );\n", - " }\n", - "};\n", - "\n", - "mpl.figure = function (figure_id, websocket, ondownload, parent_element) {\n", - " this.id = figure_id;\n", - "\n", - " this.ws = websocket;\n", - "\n", - " this.supports_binary = this.ws.binaryType !== undefined;\n", - "\n", - " if (!this.supports_binary) {\n", - " var warnings = document.getElementById('mpl-warnings');\n", - " if (warnings) {\n", - " warnings.style.display = 'block';\n", - " warnings.textContent =\n", - " 'This browser does not support binary websocket messages. ' +\n", - " 'Performance may be slow.';\n", - " }\n", - " }\n", - "\n", - " this.imageObj = new Image();\n", - "\n", - " this.context = undefined;\n", - " this.message = undefined;\n", - " this.canvas = undefined;\n", - " this.rubberband_canvas = undefined;\n", - " this.rubberband_context = undefined;\n", - " this.format_dropdown = undefined;\n", - "\n", - " this.image_mode = 'full';\n", - "\n", - " this.root = document.createElement('div');\n", - " this.root.setAttribute('style', 'display: inline-block');\n", - " this._root_extra_style(this.root);\n", - "\n", - " parent_element.appendChild(this.root);\n", - "\n", - " this._init_header(this);\n", - " this._init_canvas(this);\n", - " this._init_toolbar(this);\n", - "\n", - " var fig = this;\n", - "\n", - " this.waiting = false;\n", - "\n", - " this.ws.onopen = function () {\n", - " fig.send_message('supports_binary', { value: fig.supports_binary });\n", - " fig.send_message('send_image_mode', {});\n", - " if (mpl.ratio !== 1) {\n", - " fig.send_message('set_dpi_ratio', { dpi_ratio: mpl.ratio });\n", - " }\n", - " fig.send_message('refresh', {});\n", - " };\n", - "\n", - " this.imageObj.onload = function () {\n", - " if (fig.image_mode === 'full') {\n", - " // Full images could contain transparency (where diff images\n", - " // almost always do), so we need to clear the canvas so that\n", - " // there is no ghosting.\n", - " fig.context.clearRect(0, 0, fig.canvas.width, fig.canvas.height);\n", - " }\n", - " fig.context.drawImage(fig.imageObj, 0, 0);\n", - " };\n", - "\n", - " this.imageObj.onunload = function () {\n", - " fig.ws.close();\n", - " };\n", - "\n", - " this.ws.onmessage = this._make_on_message_function(this);\n", - "\n", - " this.ondownload = ondownload;\n", - "};\n", - "\n", - "mpl.figure.prototype._init_header = function () {\n", - " var titlebar = document.createElement('div');\n", - " titlebar.classList =\n", - " 'ui-dialog-titlebar ui-widget-header ui-corner-all ui-helper-clearfix';\n", - " var titletext = document.createElement('div');\n", - " titletext.classList = 'ui-dialog-title';\n", - " titletext.setAttribute(\n", - " 'style',\n", - " 'width: 100%; text-align: center; padding: 3px;'\n", - " );\n", - " titlebar.appendChild(titletext);\n", - " this.root.appendChild(titlebar);\n", - " this.header = titletext;\n", - "};\n", - "\n", - "mpl.figure.prototype._canvas_extra_style = function (_canvas_div) {};\n", - "\n", - "mpl.figure.prototype._root_extra_style = function (_canvas_div) {};\n", - "\n", - "mpl.figure.prototype._init_canvas = function () {\n", - " var fig = this;\n", - "\n", - " var canvas_div = (this.canvas_div = document.createElement('div'));\n", - " canvas_div.setAttribute(\n", - " 'style',\n", - " 'border: 1px solid #ddd;' +\n", - " 'box-sizing: content-box;' +\n", - " 'clear: both;' +\n", - " 'min-height: 1px;' +\n", - " 'min-width: 1px;' +\n", - " 'outline: 0;' +\n", - " 'overflow: hidden;' +\n", - " 'position: relative;' +\n", - " 'resize: both;'\n", - " );\n", - "\n", - " function on_keyboard_event_closure(name) {\n", - " return function (event) {\n", - " return fig.key_event(event, name);\n", - " };\n", - " }\n", - "\n", - " canvas_div.addEventListener(\n", - " 'keydown',\n", - " on_keyboard_event_closure('key_press')\n", - " );\n", - " canvas_div.addEventListener(\n", - " 'keyup',\n", - " on_keyboard_event_closure('key_release')\n", - " );\n", - "\n", - " this._canvas_extra_style(canvas_div);\n", - " this.root.appendChild(canvas_div);\n", - "\n", - " var canvas = (this.canvas = document.createElement('canvas'));\n", - " canvas.classList.add('mpl-canvas');\n", - " canvas.setAttribute('style', 'box-sizing: content-box;');\n", - "\n", - " this.context = canvas.getContext('2d');\n", - "\n", - " var backingStore =\n", - " this.context.backingStorePixelRatio ||\n", - " this.context.webkitBackingStorePixelRatio ||\n", - " this.context.mozBackingStorePixelRatio ||\n", - " this.context.msBackingStorePixelRatio ||\n", - " this.context.oBackingStorePixelRatio ||\n", - " this.context.backingStorePixelRatio ||\n", - " 1;\n", - "\n", - " mpl.ratio = (window.devicePixelRatio || 1) / backingStore;\n", - "\n", - " var rubberband_canvas = (this.rubberband_canvas = document.createElement(\n", - " 'canvas'\n", - " ));\n", - " rubberband_canvas.setAttribute(\n", - " 'style',\n", - " 'box-sizing: content-box; position: absolute; left: 0; top: 0; z-index: 1;'\n", - " );\n", - "\n", - " var resizeObserver = new ResizeObserver(function (entries) {\n", - " var nentries = entries.length;\n", - " for (var i = 0; i < nentries; i++) {\n", - " var entry = entries[i];\n", - " var width, height;\n", - " if (entry.contentBoxSize) {\n", - " if (entry.contentBoxSize instanceof Array) {\n", - " // Chrome 84 implements new version of spec.\n", - " width = entry.contentBoxSize[0].inlineSize;\n", - " height = entry.contentBoxSize[0].blockSize;\n", - " } else {\n", - " // Firefox implements old version of spec.\n", - " width = entry.contentBoxSize.inlineSize;\n", - " height = entry.contentBoxSize.blockSize;\n", - " }\n", - " } else {\n", - " // Chrome <84 implements even older version of spec.\n", - " width = entry.contentRect.width;\n", - " height = entry.contentRect.height;\n", - " }\n", - "\n", - " // Keep the size of the canvas and rubber band canvas in sync with\n", - " // the canvas container.\n", - " if (entry.devicePixelContentBoxSize) {\n", - " // Chrome 84 implements new version of spec.\n", - " canvas.setAttribute(\n", - " 'width',\n", - " entry.devicePixelContentBoxSize[0].inlineSize\n", - " );\n", - " canvas.setAttribute(\n", - " 'height',\n", - " entry.devicePixelContentBoxSize[0].blockSize\n", - " );\n", - " } else {\n", - " canvas.setAttribute('width', width * mpl.ratio);\n", - " canvas.setAttribute('height', height * mpl.ratio);\n", - " }\n", - " canvas.setAttribute(\n", - " 'style',\n", - " 'width: ' + width + 'px; height: ' + height + 'px;'\n", - " );\n", - "\n", - " rubberband_canvas.setAttribute('width', width);\n", - " rubberband_canvas.setAttribute('height', height);\n", - "\n", - " // And update the size in Python. We ignore the initial 0/0 size\n", - " // that occurs as the element is placed into the DOM, which should\n", - " // otherwise not happen due to the minimum size styling.\n", - " if (width != 0 && height != 0) {\n", - " fig.request_resize(width, height);\n", - " }\n", - " }\n", - " });\n", - " resizeObserver.observe(canvas_div);\n", - "\n", - " function on_mouse_event_closure(name) {\n", - " return function (event) {\n", - " return fig.mouse_event(event, name);\n", - " };\n", - " }\n", - "\n", - " rubberband_canvas.addEventListener(\n", - " 'mousedown',\n", - " on_mouse_event_closure('button_press')\n", - " );\n", - " rubberband_canvas.addEventListener(\n", - " 'mouseup',\n", - " on_mouse_event_closure('button_release')\n", - " );\n", - " // Throttle sequential mouse events to 1 every 20ms.\n", - " rubberband_canvas.addEventListener(\n", - " 'mousemove',\n", - " on_mouse_event_closure('motion_notify')\n", - " );\n", - "\n", - " rubberband_canvas.addEventListener(\n", - " 'mouseenter',\n", - " on_mouse_event_closure('figure_enter')\n", - " );\n", - " rubberband_canvas.addEventListener(\n", - " 'mouseleave',\n", - " on_mouse_event_closure('figure_leave')\n", - " );\n", - "\n", - " canvas_div.addEventListener('wheel', function (event) {\n", - " if (event.deltaY < 0) {\n", - " event.step = 1;\n", - " } else {\n", - " event.step = -1;\n", - " }\n", - " on_mouse_event_closure('scroll')(event);\n", - " });\n", - "\n", - " canvas_div.appendChild(canvas);\n", - " canvas_div.appendChild(rubberband_canvas);\n", - "\n", - " this.rubberband_context = rubberband_canvas.getContext('2d');\n", - " this.rubberband_context.strokeStyle = '#000000';\n", - "\n", - " this._resize_canvas = function (width, height, forward) {\n", - " if (forward) {\n", - " canvas_div.style.width = width + 'px';\n", - " canvas_div.style.height = height + 'px';\n", - " }\n", - " };\n", - "\n", - " // Disable right mouse context menu.\n", - " this.rubberband_canvas.addEventListener('contextmenu', function (_e) {\n", - " event.preventDefault();\n", - " return false;\n", - " });\n", - "\n", - " function set_focus() {\n", - " canvas.focus();\n", - " canvas_div.focus();\n", - " }\n", - "\n", - " window.setTimeout(set_focus, 100);\n", - "};\n", - "\n", - "mpl.figure.prototype._init_toolbar = function () {\n", - " var fig = this;\n", - "\n", - " var toolbar = document.createElement('div');\n", - " toolbar.classList = 'mpl-toolbar';\n", - " this.root.appendChild(toolbar);\n", - "\n", - " function on_click_closure(name) {\n", - " return function (_event) {\n", - " return fig.toolbar_button_onclick(name);\n", - " };\n", - " }\n", - "\n", - " function on_mouseover_closure(tooltip) {\n", - " return function (event) {\n", - " if (!event.currentTarget.disabled) {\n", - " return fig.toolbar_button_onmouseover(tooltip);\n", - " }\n", - " };\n", - " }\n", - "\n", - " fig.buttons = {};\n", - " var buttonGroup = document.createElement('div');\n", - " buttonGroup.classList = 'mpl-button-group';\n", - " for (var toolbar_ind in mpl.toolbar_items) {\n", - " var name = mpl.toolbar_items[toolbar_ind][0];\n", - " var tooltip = mpl.toolbar_items[toolbar_ind][1];\n", - " var image = mpl.toolbar_items[toolbar_ind][2];\n", - " var method_name = mpl.toolbar_items[toolbar_ind][3];\n", - "\n", - " if (!name) {\n", - " /* Instead of a spacer, we start a new button group. */\n", - " if (buttonGroup.hasChildNodes()) {\n", - " toolbar.appendChild(buttonGroup);\n", - " }\n", - " buttonGroup = document.createElement('div');\n", - " buttonGroup.classList = 'mpl-button-group';\n", - " continue;\n", - " }\n", - "\n", - " var button = (fig.buttons[name] = document.createElement('button'));\n", - " button.classList = 'mpl-widget';\n", - " button.setAttribute('role', 'button');\n", - " button.setAttribute('aria-disabled', 'false');\n", - " button.addEventListener('click', on_click_closure(method_name));\n", - " button.addEventListener('mouseover', on_mouseover_closure(tooltip));\n", - "\n", - " var icon_img = document.createElement('img');\n", - " icon_img.src = '_images/' + image + '.png';\n", - " icon_img.srcset = '_images/' + image + '_large.png 2x';\n", - " icon_img.alt = tooltip;\n", - " button.appendChild(icon_img);\n", - "\n", - " buttonGroup.appendChild(button);\n", - " }\n", - "\n", - " if (buttonGroup.hasChildNodes()) {\n", - " toolbar.appendChild(buttonGroup);\n", - " }\n", - "\n", - " var fmt_picker = document.createElement('select');\n", - " fmt_picker.classList = 'mpl-widget';\n", - " toolbar.appendChild(fmt_picker);\n", - " this.format_dropdown = fmt_picker;\n", - "\n", - " for (var ind in mpl.extensions) {\n", - " var fmt = mpl.extensions[ind];\n", - " var option = document.createElement('option');\n", - " option.selected = fmt === mpl.default_extension;\n", - " option.innerHTML = fmt;\n", - " fmt_picker.appendChild(option);\n", - " }\n", - "\n", - " var status_bar = document.createElement('span');\n", - " status_bar.classList = 'mpl-message';\n", - " toolbar.appendChild(status_bar);\n", - " this.message = status_bar;\n", - "};\n", - "\n", - "mpl.figure.prototype.request_resize = function (x_pixels, y_pixels) {\n", - " // Request matplotlib to resize the figure. Matplotlib will then trigger a resize in the client,\n", - " // which will in turn request a refresh of the image.\n", - " this.send_message('resize', { width: x_pixels, height: y_pixels });\n", - "};\n", - "\n", - "mpl.figure.prototype.send_message = function (type, properties) {\n", - " properties['type'] = type;\n", - " properties['figure_id'] = this.id;\n", - " this.ws.send(JSON.stringify(properties));\n", - "};\n", - "\n", - "mpl.figure.prototype.send_draw_message = function () {\n", - " if (!this.waiting) {\n", - " this.waiting = true;\n", - " this.ws.send(JSON.stringify({ type: 'draw', figure_id: this.id }));\n", - " }\n", - "};\n", - "\n", - "mpl.figure.prototype.handle_save = function (fig, _msg) {\n", - " var format_dropdown = fig.format_dropdown;\n", - " var format = format_dropdown.options[format_dropdown.selectedIndex].value;\n", - " fig.ondownload(fig, format);\n", - "};\n", - "\n", - "mpl.figure.prototype.handle_resize = function (fig, msg) {\n", - " var size = msg['size'];\n", - " if (size[0] !== fig.canvas.width || size[1] !== fig.canvas.height) {\n", - " fig._resize_canvas(size[0], size[1], msg['forward']);\n", - " fig.send_message('refresh', {});\n", - " }\n", - "};\n", - "\n", - "mpl.figure.prototype.handle_rubberband = function (fig, msg) {\n", - " var x0 = msg['x0'] / mpl.ratio;\n", - " var y0 = (fig.canvas.height - msg['y0']) / mpl.ratio;\n", - " var x1 = msg['x1'] / mpl.ratio;\n", - " var y1 = (fig.canvas.height - msg['y1']) / mpl.ratio;\n", - " x0 = Math.floor(x0) + 0.5;\n", - " y0 = Math.floor(y0) + 0.5;\n", - " x1 = Math.floor(x1) + 0.5;\n", - " y1 = Math.floor(y1) + 0.5;\n", - " var min_x = Math.min(x0, x1);\n", - " var min_y = Math.min(y0, y1);\n", - " var width = Math.abs(x1 - x0);\n", - " var height = Math.abs(y1 - y0);\n", - "\n", - " fig.rubberband_context.clearRect(\n", - " 0,\n", - " 0,\n", - " fig.canvas.width / mpl.ratio,\n", - " fig.canvas.height / mpl.ratio\n", - " );\n", - "\n", - " fig.rubberband_context.strokeRect(min_x, min_y, width, height);\n", - "};\n", - "\n", - "mpl.figure.prototype.handle_figure_label = function (fig, msg) {\n", - " // Updates the figure title.\n", - " fig.header.textContent = msg['label'];\n", - "};\n", - "\n", - "mpl.figure.prototype.handle_cursor = function (fig, msg) {\n", - " var cursor = msg['cursor'];\n", - " switch (cursor) {\n", - " case 0:\n", - " cursor = 'pointer';\n", - " break;\n", - " case 1:\n", - " cursor = 'default';\n", - " break;\n", - " case 2:\n", - " cursor = 'crosshair';\n", - " break;\n", - " case 3:\n", - " cursor = 'move';\n", - " break;\n", - " }\n", - " fig.rubberband_canvas.style.cursor = cursor;\n", - "};\n", - "\n", - "mpl.figure.prototype.handle_message = function (fig, msg) {\n", - " fig.message.textContent = msg['message'];\n", - "};\n", - "\n", - "mpl.figure.prototype.handle_draw = function (fig, _msg) {\n", - " // Request the server to send over a new figure.\n", - " fig.send_draw_message();\n", - "};\n", - "\n", - "mpl.figure.prototype.handle_image_mode = function (fig, msg) {\n", - " fig.image_mode = msg['mode'];\n", - "};\n", - "\n", - "mpl.figure.prototype.handle_history_buttons = function (fig, msg) {\n", - " for (var key in msg) {\n", - " if (!(key in fig.buttons)) {\n", - " continue;\n", - " }\n", - " fig.buttons[key].disabled = !msg[key];\n", - " fig.buttons[key].setAttribute('aria-disabled', !msg[key]);\n", - " }\n", - "};\n", - "\n", - "mpl.figure.prototype.handle_navigate_mode = function (fig, msg) {\n", - " if (msg['mode'] === 'PAN') {\n", - " fig.buttons['Pan'].classList.add('active');\n", - " fig.buttons['Zoom'].classList.remove('active');\n", - " } else if (msg['mode'] === 'ZOOM') {\n", - " fig.buttons['Pan'].classList.remove('active');\n", - " fig.buttons['Zoom'].classList.add('active');\n", - " } else {\n", - " fig.buttons['Pan'].classList.remove('active');\n", - " fig.buttons['Zoom'].classList.remove('active');\n", - " }\n", - "};\n", - "\n", - "mpl.figure.prototype.updated_canvas_event = function () {\n", - " // Called whenever the canvas gets updated.\n", - " this.send_message('ack', {});\n", - "};\n", - "\n", - "// A function to construct a web socket function for onmessage handling.\n", - "// Called in the figure constructor.\n", - "mpl.figure.prototype._make_on_message_function = function (fig) {\n", - " return function socket_on_message(evt) {\n", - " if (evt.data instanceof Blob) {\n", - " /* FIXME: We get \"Resource interpreted as Image but\n", - " * transferred with MIME type text/plain:\" errors on\n", - " * Chrome. But how to set the MIME type? It doesn't seem\n", - " * to be part of the websocket stream */\n", - " evt.data.type = 'image/png';\n", - "\n", - " /* Free the memory for the previous frames */\n", - " if (fig.imageObj.src) {\n", - " (window.URL || window.webkitURL).revokeObjectURL(\n", - " fig.imageObj.src\n", - " );\n", - " }\n", - "\n", - " fig.imageObj.src = (window.URL || window.webkitURL).createObjectURL(\n", - " evt.data\n", - " );\n", - " fig.updated_canvas_event();\n", - " fig.waiting = false;\n", - " return;\n", - " } else if (\n", - " typeof evt.data === 'string' &&\n", - " evt.data.slice(0, 21) === 'data:image/png;base64'\n", - " ) {\n", - " fig.imageObj.src = evt.data;\n", - " fig.updated_canvas_event();\n", - " fig.waiting = false;\n", - " return;\n", - " }\n", - "\n", - " var msg = JSON.parse(evt.data);\n", - " var msg_type = msg['type'];\n", - "\n", - " // Call the \"handle_{type}\" callback, which takes\n", - " // the figure and JSON message as its only arguments.\n", - " try {\n", - " var callback = fig['handle_' + msg_type];\n", - " } catch (e) {\n", - " console.log(\n", - " \"No handler for the '\" + msg_type + \"' message type: \",\n", - " msg\n", - " );\n", - " return;\n", - " }\n", - "\n", - " if (callback) {\n", - " try {\n", - " // console.log(\"Handling '\" + msg_type + \"' message: \", msg);\n", - " callback(fig, msg);\n", - " } catch (e) {\n", - " console.log(\n", - " \"Exception inside the 'handler_\" + msg_type + \"' callback:\",\n", - " e,\n", - " e.stack,\n", - " msg\n", - " );\n", - " }\n", - " }\n", - " };\n", - "};\n", - "\n", - "// from http://stackoverflow.com/questions/1114465/getting-mouse-location-in-canvas\n", - "mpl.findpos = function (e) {\n", - " //this section is from http://www.quirksmode.org/js/events_properties.html\n", - " var targ;\n", - " if (!e) {\n", - " e = window.event;\n", - " }\n", - " if (e.target) {\n", - " targ = e.target;\n", - " } else if (e.srcElement) {\n", - " targ = e.srcElement;\n", - " }\n", - " if (targ.nodeType === 3) {\n", - " // defeat Safari bug\n", - " targ = targ.parentNode;\n", - " }\n", - "\n", - " // pageX,Y are the mouse positions relative to the document\n", - " var boundingRect = targ.getBoundingClientRect();\n", - " var x = e.pageX - (boundingRect.left + document.body.scrollLeft);\n", - " var y = e.pageY - (boundingRect.top + document.body.scrollTop);\n", - "\n", - " return { x: x, y: y };\n", - "};\n", - "\n", - "/*\n", - " * return a copy of an object with only non-object keys\n", - " * we need this to avoid circular references\n", - " * http://stackoverflow.com/a/24161582/3208463\n", - " */\n", - "function simpleKeys(original) {\n", - " return Object.keys(original).reduce(function (obj, key) {\n", - " if (typeof original[key] !== 'object') {\n", - " obj[key] = original[key];\n", - " }\n", - " return obj;\n", - " }, {});\n", - "}\n", - "\n", - "mpl.figure.prototype.mouse_event = function (event, name) {\n", - " var canvas_pos = mpl.findpos(event);\n", - "\n", - " if (name === 'button_press') {\n", - " this.canvas.focus();\n", - " this.canvas_div.focus();\n", - " }\n", - "\n", - " var x = canvas_pos.x * mpl.ratio;\n", - " var y = canvas_pos.y * mpl.ratio;\n", - "\n", - " this.send_message(name, {\n", - " x: x,\n", - " y: y,\n", - " button: event.button,\n", - " step: event.step,\n", - " guiEvent: simpleKeys(event),\n", - " });\n", - "\n", - " /* This prevents the web browser from automatically changing to\n", - " * the text insertion cursor when the button is pressed. We want\n", - " * to control all of the cursor setting manually through the\n", - " * 'cursor' event from matplotlib */\n", - " event.preventDefault();\n", - " return false;\n", - "};\n", - "\n", - "mpl.figure.prototype._key_event_extra = function (_event, _name) {\n", - " // Handle any extra behaviour associated with a key event\n", - "};\n", - "\n", - "mpl.figure.prototype.key_event = function (event, name) {\n", - " // Prevent repeat events\n", - " if (name === 'key_press') {\n", - " if (event.which === this._key) {\n", - " return;\n", - " } else {\n", - " this._key = event.which;\n", - " }\n", - " }\n", - " if (name === 'key_release') {\n", - " this._key = null;\n", - " }\n", - "\n", - " var value = '';\n", - " if (event.ctrlKey && event.which !== 17) {\n", - " value += 'ctrl+';\n", - " }\n", - " if (event.altKey && event.which !== 18) {\n", - " value += 'alt+';\n", - " }\n", - " if (event.shiftKey && event.which !== 16) {\n", - " value += 'shift+';\n", - " }\n", - "\n", - " value += 'k';\n", - " value += event.which.toString();\n", - "\n", - " this._key_event_extra(event, name);\n", - "\n", - " this.send_message(name, { key: value, guiEvent: simpleKeys(event) });\n", - " return false;\n", - "};\n", - "\n", - "mpl.figure.prototype.toolbar_button_onclick = function (name) {\n", - " if (name === 'download') {\n", - " this.handle_save(this, null);\n", - " } else {\n", - " this.send_message('toolbar_button', { name: name });\n", - " }\n", - "};\n", - "\n", - "mpl.figure.prototype.toolbar_button_onmouseover = function (tooltip) {\n", - " this.message.textContent = tooltip;\n", - "};\n", - "mpl.toolbar_items = [[\"Home\", \"Reset original view\", \"fa fa-home icon-home\", \"home\"], [\"Back\", \"Back to previous view\", \"fa fa-arrow-left icon-arrow-left\", \"back\"], [\"Forward\", \"Forward to next view\", \"fa fa-arrow-right icon-arrow-right\", \"forward\"], [\"\", \"\", \"\", \"\"], [\"Pan\", \"Left button pans, Right button zooms\\nx/y fixes axis, CTRL fixes aspect\", \"fa fa-arrows icon-move\", \"pan\"], [\"Zoom\", \"Zoom to rectangle\\nx/y fixes axis, CTRL fixes aspect\", \"fa fa-square-o icon-check-empty\", \"zoom\"], [\"\", \"\", \"\", \"\"], [\"Download\", \"Download plot\", \"fa fa-floppy-o icon-save\", \"download\"]];\n", - "\n", - "mpl.extensions = [\"eps\", \"jpeg\", \"pdf\", \"png\", \"ps\", \"raw\", \"svg\", \"tif\"];\n", - "\n", - "mpl.default_extension = \"png\";/* global mpl */\n", - "\n", - "var comm_websocket_adapter = function (comm) {\n", - " // Create a \"websocket\"-like object which calls the given IPython comm\n", - " // object with the appropriate methods. Currently this is a non binary\n", - " // socket, so there is still some room for performance tuning.\n", - " var ws = {};\n", - "\n", - " ws.close = function () {\n", - " comm.close();\n", - " };\n", - " ws.send = function (m) {\n", - " //console.log('sending', m);\n", - " comm.send(m);\n", - " };\n", - " // Register the callback with on_msg.\n", - " comm.on_msg(function (msg) {\n", - " //console.log('receiving', msg['content']['data'], msg);\n", - " // Pass the mpl event to the overridden (by mpl) onmessage function.\n", - " ws.onmessage(msg['content']['data']);\n", - " });\n", - " return ws;\n", - "};\n", - "\n", - "mpl.mpl_figure_comm = function (comm, msg) {\n", - " // This is the function which gets called when the mpl process\n", - " // starts-up an IPython Comm through the \"matplotlib\" channel.\n", - "\n", - " var id = msg.content.data.id;\n", - " // Get hold of the div created by the display call when the Comm\n", - " // socket was opened in Python.\n", - " var element = document.getElementById(id);\n", - " var ws_proxy = comm_websocket_adapter(comm);\n", - "\n", - " function ondownload(figure, _format) {\n", - " window.open(figure.canvas.toDataURL());\n", - " }\n", - "\n", - " var fig = new mpl.figure(id, ws_proxy, ondownload, element);\n", - "\n", - " // Call onopen now - mpl needs it, as it is assuming we've passed it a real\n", - " // web socket which is closed, not our websocket->open comm proxy.\n", - " ws_proxy.onopen();\n", - "\n", - " fig.parent_element = element;\n", - " fig.cell_info = mpl.find_output_cell(\"
\");\n", - " if (!fig.cell_info) {\n", - " console.error('Failed to find cell for figure', id, fig);\n", - " return;\n", - " }\n", - "};\n", - "\n", - "mpl.figure.prototype.handle_close = function (fig, msg) {\n", - " var width = fig.canvas.width / mpl.ratio;\n", - " fig.root.removeEventListener('remove', this._remove_fig_handler);\n", - "\n", - " // Update the output cell to use the data from the current canvas.\n", - " fig.push_to_output();\n", - " var dataURL = fig.canvas.toDataURL();\n", - " // Re-enable the keyboard manager in IPython - without this line, in FF,\n", - " // the notebook keyboard shortcuts fail.\n", - " IPython.keyboard_manager.enable();\n", - " fig.parent_element.innerHTML =\n", - " '';\n", - " fig.close_ws(fig, msg);\n", - "};\n", - "\n", - "mpl.figure.prototype.close_ws = function (fig, msg) {\n", - " fig.send_message('closing', msg);\n", - " // fig.ws.close()\n", - "};\n", - "\n", - "mpl.figure.prototype.push_to_output = function (_remove_interactive) {\n", - " // Turn the data on the canvas into data in the output cell.\n", - " var width = this.canvas.width / mpl.ratio;\n", - " var dataURL = this.canvas.toDataURL();\n", - " this.cell_info[1]['text/html'] =\n", - " '';\n", - "};\n", - "\n", - "mpl.figure.prototype.updated_canvas_event = function () {\n", - " // Tell IPython that the notebook contents must change.\n", - " IPython.notebook.set_dirty(true);\n", - " this.send_message('ack', {});\n", - " var fig = this;\n", - " // Wait a second, then push the new image to the DOM so\n", - " // that it is saved nicely (might be nice to debounce this).\n", - " setTimeout(function () {\n", - " fig.push_to_output();\n", - " }, 1000);\n", - "};\n", - "\n", - "mpl.figure.prototype._init_toolbar = function () {\n", - " var fig = this;\n", - "\n", - " var toolbar = document.createElement('div');\n", - " toolbar.classList = 'btn-toolbar';\n", - " this.root.appendChild(toolbar);\n", - "\n", - " function on_click_closure(name) {\n", - " return function (_event) {\n", - " return fig.toolbar_button_onclick(name);\n", - " };\n", - " }\n", - "\n", - " function on_mouseover_closure(tooltip) {\n", - " return function (event) {\n", - " if (!event.currentTarget.disabled) {\n", - " return fig.toolbar_button_onmouseover(tooltip);\n", - " }\n", - " };\n", - " }\n", - "\n", - " fig.buttons = {};\n", - " var buttonGroup = document.createElement('div');\n", - " buttonGroup.classList = 'btn-group';\n", - " var button;\n", - " for (var toolbar_ind in mpl.toolbar_items) {\n", - " var name = mpl.toolbar_items[toolbar_ind][0];\n", - " var tooltip = mpl.toolbar_items[toolbar_ind][1];\n", - " var image = mpl.toolbar_items[toolbar_ind][2];\n", - " var method_name = mpl.toolbar_items[toolbar_ind][3];\n", - "\n", - " if (!name) {\n", - " /* Instead of a spacer, we start a new button group. */\n", - " if (buttonGroup.hasChildNodes()) {\n", - " toolbar.appendChild(buttonGroup);\n", - " }\n", - " buttonGroup = document.createElement('div');\n", - " buttonGroup.classList = 'btn-group';\n", - " continue;\n", - " }\n", - "\n", - " button = fig.buttons[name] = document.createElement('button');\n", - " button.classList = 'btn btn-default';\n", - " button.href = '#';\n", - " button.title = name;\n", - " button.innerHTML = '';\n", - " button.addEventListener('click', on_click_closure(method_name));\n", - " button.addEventListener('mouseover', on_mouseover_closure(tooltip));\n", - " buttonGroup.appendChild(button);\n", - " }\n", - "\n", - " if (buttonGroup.hasChildNodes()) {\n", - " toolbar.appendChild(buttonGroup);\n", - " }\n", - "\n", - " // Add the status bar.\n", - " var status_bar = document.createElement('span');\n", - " status_bar.classList = 'mpl-message pull-right';\n", - " toolbar.appendChild(status_bar);\n", - " this.message = status_bar;\n", - "\n", - " // Add the close button to the window.\n", - " var buttongrp = document.createElement('div');\n", - " buttongrp.classList = 'btn-group inline pull-right';\n", - " button = document.createElement('button');\n", - " button.classList = 'btn btn-mini btn-primary';\n", - " button.href = '#';\n", - " button.title = 'Stop Interaction';\n", - " button.innerHTML = '';\n", - " button.addEventListener('click', function (_evt) {\n", - " fig.handle_close(fig, {});\n", - " });\n", - " button.addEventListener(\n", - " 'mouseover',\n", - " on_mouseover_closure('Stop Interaction')\n", - " );\n", - " buttongrp.appendChild(button);\n", - " var titlebar = this.root.querySelector('.ui-dialog-titlebar');\n", - " titlebar.insertBefore(buttongrp, titlebar.firstChild);\n", - "};\n", - "\n", - "mpl.figure.prototype._remove_fig_handler = function () {\n", - " this.close_ws(this, {});\n", - "};\n", - "\n", - "mpl.figure.prototype._root_extra_style = function (el) {\n", - " el.style.boxSizing = 'content-box'; // override notebook setting of border-box.\n", - " el.addEventListener('remove', this._remove_fig_handler);\n", - "};\n", - "\n", - "mpl.figure.prototype._canvas_extra_style = function (el) {\n", - " // this is important to make the div 'focusable\n", - " el.setAttribute('tabindex', 0);\n", - " // reach out to IPython and tell the keyboard manager to turn it's self\n", - " // off when our div gets focus\n", - "\n", - " // location in version 3\n", - " if (IPython.notebook.keyboard_manager) {\n", - " IPython.notebook.keyboard_manager.register_events(el);\n", - " } else {\n", - " // location in version 2\n", - " IPython.keyboard_manager.register_events(el);\n", - " }\n", - "};\n", - "\n", - "mpl.figure.prototype._key_event_extra = function (event, _name) {\n", - " var manager = IPython.notebook.keyboard_manager;\n", - " if (!manager) {\n", - " manager = IPython.keyboard_manager;\n", - " }\n", - "\n", - " // Check for shift+enter\n", - " if (event.shiftKey && event.which === 13) {\n", - " this.canvas_div.blur();\n", - " // select the cell after this one\n", - " var index = IPython.notebook.find_cell_index(this.cell_info[0]);\n", - " IPython.notebook.select(index + 1);\n", - " }\n", - "};\n", - "\n", - "mpl.figure.prototype.handle_save = function (fig, _msg) {\n", - " fig.ondownload(fig, null);\n", - "};\n", - "\n", - "mpl.find_output_cell = function (html_output) {\n", - " // Return the cell and output element which can be found *uniquely* in the notebook.\n", - " // Note - this is a bit hacky, but it is done because the \"notebook_saving.Notebook\"\n", - " // IPython event is triggered only after the cells have been serialised, which for\n", - " // our purposes (turning an active figure into a static one), is too late.\n", - " var cells = IPython.notebook.get_cells();\n", - " var ncells = cells.length;\n", - " for (var i = 0; i < ncells; i++) {\n", - " var cell = cells[i];\n", - " if (cell.cell_type === 'code') {\n", - " for (var j = 0; j < cell.output_area.outputs.length; j++) {\n", - " var data = cell.output_area.outputs[j];\n", - " if (data.data) {\n", - " // IPython >= 3 moved mimebundle to data attribute of output\n", - " data = data.data;\n", - " }\n", - " if (data['text/html'] === html_output) {\n", - " return [cell, data, j];\n", - " }\n", - " }\n", - " }\n", - " }\n", - "};\n", - "\n", - "// Register the function which deals with the matplotlib target/channel.\n", - "// The kernel may be null if the page has been refreshed.\n", - "if (IPython.notebook.kernel !== null) {\n", - " IPython.notebook.kernel.comm_manager.register_target(\n", - " 'matplotlib',\n", - " mpl.mpl_figure_comm\n", - " );\n", - "}\n" - ], "text/plain": [ - "" - ] + "HBox(children=(FloatProgress(value=0.0, max=10.0), HTML(value='')))" + ], + "application/vnd.jupyter.widget-view+json": { + "version_major": 2, + "version_minor": 0, + "model_id": "50963f48cbd64d77b0b7ceee27f27adc" + } }, - "metadata": {}, - "output_type": "display_data" + "metadata": {} }, { - "data": { - "text/html": [ - "" - ], - "text/plain": [ - "" - ] - }, - "metadata": {}, - "output_type": "display_data" + "output_type": "stream", + "name": "stdout", + "text": [ + "\n" + ] } ], + "metadata": { + "tags": [] + } + }, + { + "cell_type": "markdown", + "source": [ + "The simulation generates a log of its comuptation: this log can be retrieved by using ```simulator.get_log``` - and written to a file for latter processing by the engine with ```simulator.engine.write_log```." + ], + "metadata": {} + }, + { + "cell_type": "code", + "execution_count": 2, "source": [ "# Get dictionary of logged scalar variables\n", "log_data = simulator.log_data\n", "\n", "# Let's plot the joint position to see the pendulum fall.\n", - "%matplotlib notebook\n", + "%matplotlib inline\n", "import matplotlib.pyplot as plt\n", "\n", "plt.plot(log_data['Global.Time'], log_data['HighLevelController.currentPositionPendulum'])\n", "plt.title('Pendulum angle (rad)')\n", "plt.grid()\n", "plt.show()" - ] + ], + "outputs": [ + { + "output_type": "display_data", + "data": { + "text/plain": [ + "
" + ], + "image/png": "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" + }, + "metadata": { + "needs_background": "light" + } + } + ], + "metadata": {} }, { "cell_type": "markdown", - "metadata": {}, "source": [ "The results of a simulation can also be visualized in a 3D viewer: either `gepetto-gui` or `meshcat`. We use the latter here as it can be integrated in jupyter." - ] + ], + "metadata": {} }, { "cell_type": "code", - "execution_count": 5, - "metadata": { - "scrolled": false - }, + "execution_count": 3, + "source": [ + "camera_xyzrpy = ([5.0, 0.0, 2.0e-5], [np.pi/2, 0.0, np.pi/2])\n", + "simulator.replay(camera_xyzrpy=camera_xyzrpy)" + ], "outputs": [ { + "output_type": "display_data", "data": { + "text/plain": [ + "" + ], "text/html": [ "\n", - "
\n", - " \n", + "\" style=\"\n", + " width: 100%; height: 100%; border: none;\">\n", + " \n", "
\n", " " - ], - "text/plain": [ - "" ] }, - "metadata": {}, - "output_type": "display_data" + "metadata": {} } ], - "source": [ - "camera_xyzrpy = ([5.0, 0.0, 2.0e-5], [np.pi/2, 0.0, np.pi/2])\n", - "simulator.replay(camera_xyzrpy=camera_xyzrpy)" - ] + "metadata": { + "scrolled": false + } }, { "cell_type": "markdown", - "metadata": {}, "source": [ "Let's now add a controller: a simple PD to hold the pendulum straight." - ] + ], + "metadata": {} }, { "cell_type": "code", - "execution_count": 6, - "metadata": {}, - "outputs": [ - { - "data": { - "application/vnd.jupyter.widget-view+json": { - "model_id": "ee9647b3373f4ae7ba49b7cf09e53f45", - "version_major": 2, - "version_minor": 0 - }, - "text/plain": [ - "HBox(children=(FloatProgress(value=0.0, max=10.0), HTML(value='')))" - ] - }, - "metadata": {}, - "output_type": "display_data" - } - ], + "execution_count": 4, "source": [ "Kp = 5000\n", "Kd = 0.05\n", @@ -2706,38 +1951,34 @@ "\n", "# Update the simulator to use the new controller\n", "simulator.set_controller(controller)" - ] + ], + "outputs": [ + { + "output_type": "execute_result", + "data": { + "text/plain": [ + "jiminy_py.core.hresult_t.SUCCESS" + ] + }, + "metadata": {}, + "execution_count": 4 + } + ], + "metadata": {} }, { "cell_type": "markdown", - "metadata": {}, "source": [ "## Adding external forces.\n", "\n", "External forces can be applied to the system through a force profile : a function outputing a force on\n", "a specific body as as a function of time dans state." - ] + ], + "metadata": {} }, { "cell_type": "code", - "execution_count": 7, - "metadata": {}, - "outputs": [ - { - "data": { - "application/vnd.jupyter.widget-view+json": { - "model_id": "250013949ba94016bf2620029d3e9a0f", - "version_major": 2, - "version_minor": 0 - }, - "text/plain": [ - "HBox(children=(FloatProgress(value=0.0, max=10.0), HTML(value='')))" - ] - }, - "metadata": {}, - "output_type": "display_data" - } - ], + "execution_count": 5, "source": [ "# Apply a force of 500N in the Y direction between t = 2.5 and t = 3s\n", "def force_profile(t, q, v, f):\n", @@ -2752,14 +1993,37 @@ "\n", "# Replay the simulation with new controller and external forces\n", "simulator.replay(camera_xyzrpy=camera_xyzrpy)" - ] + ], + "outputs": [ + { + "output_type": "display_data", + "data": { + "text/plain": [ + "HBox(children=(FloatProgress(value=0.0, max=10.0), HTML(value='')))" + ], + "application/vnd.jupyter.widget-view+json": { + "version_major": 2, + "version_minor": 0, + "model_id": "0b45b87c06234cb69683051d7f105407" + } + }, + "metadata": {} + }, + { + "output_type": "stream", + "name": "stdout", + "text": [ + "\n" + ] + } + ], + "metadata": {} } ], "metadata": { "kernelspec": { - "display_name": "Python 3.6.9 64-bit", - "language": "python", - "name": "python36964bitab75b9a2d67240bd8443b741e8c3d97d" + "name": "python3", + "display_name": "Python 3.6.9 64-bit" }, "language_info": { "codemirror_mode": { @@ -2772,6 +2036,9 @@ "nbconvert_exporter": "python", "pygments_lexer": "ipython3", "version": "3.6.9" + }, + "interpreter": { + "hash": "31f2aee4e71d21fbe5cf8b01ff0e069b9275f58929596ceb00d14d90e3e16cd6" } }, "nbformat": 4, diff --git a/python/gym_jiminy/common/gym_jiminy/common/envs/env_generic.py b/python/gym_jiminy/common/gym_jiminy/common/envs/env_generic.py index 093e7a1ec..e85f073ad 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/envs/env_generic.py +++ b/python/gym_jiminy/common/gym_jiminy/common/envs/env_generic.py @@ -784,11 +784,17 @@ def plot(self, **kwargs: Any) -> None: # Call base implementation self.simulator.plot(**kwargs) + # Extract log data + log_data = self.simulator.log_data + if not log_data: + raise RuntimeError( + "Nothing to plot. Please run a simulation before calling " + "`plot` method.") + # Extract action. # If telemetry action fieldnames is a dictionary, it cannot be nested. # In such a case, keys corresponds to subplots, and values are # individual scalar data over time to be displayed to the same subplot. - log_data = self.simulator.log_data t = log_data["Global.Time"] tab_data = {} if self.logfile_action_headers is None: @@ -863,6 +869,7 @@ def replay(self, enable_travelling: bool = True, **kwargs: Any) -> None: def play_interactive(env: Union["BaseJiminyEnv", gym.Wrapper], enable_travelling: Optional[bool] = None, start_paused: bool = True, + enable_is_done: bool = True, verbose: bool = True, **kwargs: Any) -> None: """Activate interact mode enabling to control the robot using keyboard. @@ -900,8 +907,10 @@ def play_interactive(env: Union["BaseJiminyEnv", gym.Wrapper], assert isinstance(self, BaseJiminyEnv), ( "Unwrapped environment must derived from `BaseJiminyEnv`.") - # Enable play interactive mode flag + # Enable play interactive flag and make sure training flag is disabled + is_training = self.is_training self._is_interactive = True + self.is_training = False # Make sure viewer gui is open, so that the viewer will shared external # forces with the robot automatically. @@ -927,11 +936,13 @@ def play_interactive(env: Union["BaseJiminyEnv", gym.Wrapper], # Define interactive loop def _interact(key: Optional[str] = None) -> bool: - nonlocal obs, reward + nonlocal obs, reward, enable_is_done action = self._key_to_action( key, obs, reward, **{"verbose": verbose, **kwargs}) obs, reward, done, _ = env.step(action) env.render() + if not enable_is_done and env.robot.has_freeflyer: + return env.system_state.q[2] < 0.0 return done # Run interactive loop @@ -948,8 +959,9 @@ def _interact(key: Optional[str] = None) -> bool: if self.simulator.is_simulation_running: self.simulator.stop() - # Disable play interactive mode flag + # Disable play interactive mode flag and restore training flag self._is_interactive = False + self.is_training = is_training def train(self) -> None: """Sets the environment in training mode. @@ -1098,6 +1110,10 @@ def refresh_observation(self) -> None: # type: ignore[override] .. note:: This method is called and the end of every low-level `Engine.step`. + .. note:: + Note that `np.nan` values will be automatically clipped to 0.0 by + `get_observation` method before return it, so it is valid. + .. warning:: In practice, it updates the internal buffer directly for the sake of efficiency. diff --git a/python/gym_jiminy/common/gym_jiminy/common/envs/env_locomotion.py b/python/gym_jiminy/common/gym_jiminy/common/envs/env_locomotion.py index a6ced6910..a61822642 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/envs/env_locomotion.py +++ b/python/gym_jiminy/common/gym_jiminy/common/envs/env_locomotion.py @@ -391,7 +391,7 @@ def compute_reward(self, # type: ignore[override] if 'energy' in reward_mixture_keys: v_mot = self.robot.sensors_data[encoder.type][1] command = self.system_state.command - power_consumption = sum(np.maximum(command * v_mot, 0.0)) + power_consumption = np.sum(np.maximum(command * v_mot, 0.0)) power_consumption_rel = \ power_consumption / self._power_consumption_max reward_dict['energy'] = - power_consumption_rel diff --git a/python/gym_jiminy/common/setup.py b/python/gym_jiminy/common/setup.py index 3355eb5a3..32410d91e 100644 --- a/python/gym_jiminy/common/setup.py +++ b/python/gym_jiminy/common/setup.py @@ -55,7 +55,7 @@ # Disable automatic forward compatibility with newer versions because # numba relies on llvmlite, for which wheels take some time before # being available on Pypi, making the whole installation process fail. - # Version >=0.53 is required to support Python 3.9. + # >=0.53 is required to support Python 3.9. "numba==0.53.1", # Standard interface library for reinforcement learning. # - 0.17.3 introduces iterable space dict diff --git a/python/gym_jiminy/rllib/gym_jiminy/rllib/ppo.py b/python/gym_jiminy/rllib/gym_jiminy/rllib/ppo.py index 923f3e361..daa14dd61 100644 --- a/python/gym_jiminy/rllib/gym_jiminy/rllib/ppo.py +++ b/python/gym_jiminy/rllib/gym_jiminy/rllib/ppo.py @@ -11,8 +11,9 @@ from ray.rllib.policy.policy import Policy from ray.rllib.policy.sample_batch import SampleBatch from ray.rllib.policy.view_requirement import ViewRequirement -from ray.rllib.utils.typing import TensorType, TrainerConfigDict from ray.rllib.utils.torch_ops import l2_loss +from ray.rllib.utils.typing import TensorType, TrainerConfigDict + from ray.rllib.agents.ppo import DEFAULT_CONFIG, PPOTrainer from ray.rllib.agents.ppo.ppo_torch_policy import ( ppo_surrogate_loss, kl_and_loss_stats, setup_mixins, PPOTorchPolicy) @@ -21,6 +22,7 @@ DEFAULT_CONFIG = PPOTrainer.merge_trainer_configs( DEFAULT_CONFIG, { + "noise_scale": 1.0, "symmetric_policy_reg": 0.0, "caps_temporal_reg": 0.0, "caps_spatial_reg": 0.0, @@ -57,18 +59,30 @@ def ppo_init(policy: Policy, policy._mean_global_caps_loss = 0.0 policy._l2_reg_loss = 0.0 - # Convert to torch.Tensor observation bounds - policy._observation_space_low = \ - torch.from_numpy(policy.observation_space.low).to(dtype=torch.float32) - policy._observation_space_high = \ - torch.from_numpy(policy.observation_space.high).to(dtype=torch.float32) + # Check if the policy has observation filter. If so, disable element-wise + # observation sensitivity. + obs_filter = policy.config["observation_filter"] + if obs_filter == "NoFilter": + policy._is_obs_normalized = False + elif obs_filter == "MeanStdFilter": + policy._is_obs_normalized = True + else: + raise NotImplementedError( + "Only 'NoFilter' and 'MeanStdFilter' are supported.") + + # Extract original observation space + try: + observation_space = policy.observation_space.original_space + except AttributeError as e: + raise NotImplementedError( + "Only 'Dict' original observation space is supported.") from e - # Convert to torch.Tensor observation sensitivity data - observation_space = policy.observation_space.original_space - for field, scale in observation_space.sensitivity.items(): - if not isinstance(scale, torch.Tensor): - scale = torch.from_numpy(scale).to(dtype=torch.float32) - observation_space.sensitivity[field] = scale + # Convert to torch.Tensor observation sensitivity data if necessary + if not policy._is_obs_normalized: + for field, scale in observation_space.sensitivity.items(): + if not isinstance(scale, torch.Tensor): + scale = torch.from_numpy(scale).to(dtype=torch.float32) + observation_space.sensitivity[field] = scale # Transpose and convert to torch.Tensor the observation mirroring data for field, mirror_mat in observation_space.mirror_mat.items(): @@ -113,29 +127,28 @@ def ppo_loss(policy: Policy, # Append the training batches to the set train_batches["prev"] = train_batch_copy - if policy.config["caps_spatial_reg"] > 0.0 or \ - policy.config["caps_global_reg"] > 0.0: + if policy._spatial_reg > 0.0 or policy.config["caps_global_reg"] > 0.0: # Shallow copy the original training batch train_batch_copy = train_batch.copy(shallow=True) # Generate noisy observation based on specified sensivity - offset = 0 - observation_noisy = observation_true.clone() - batch_dim = observation_true.shape[:-1] - observation_space = policy.observation_space.original_space - for field, scale in observation_space.sensitivity.items(): - scale = scale.to(device) - observation_space.sensitivity[field] = scale - unit_noise = torch.randn((*batch_dim, len(scale)), device=device) - slice_idx = slice(offset, offset + len(scale)) - observation_noisy[..., slice_idx].addcmul_(scale, unit_noise) - offset += len(scale) - torch.min(torch.max( - observation_noisy, - policy._observation_space_low.to(device), - out=observation_noisy), - policy._observation_space_high.to(device), - out=observation_noisy) + if policy._is_obs_normalized: + observation_noisy = torch.normal( + observation_true, policy.config["noise_scale"]) + else: + offset = 0 + observation_noisy = observation_true.clone() + batch_dim = observation_true.shape[:-1] + observation_space = policy.observation_space.original_space + for field, scale in observation_space.sensitivity.items(): + scale = scale.to(device) + observation_space.sensitivity[field] = scale + unit_noise = torch.randn( + (*batch_dim, len(scale)), device=device) + slice_idx = slice(offset, offset + len(scale)) + observation_noisy[..., slice_idx].addcmul_( + policy.config["noise_scale"] * scale, unit_noise) + offset += len(scale) # Replace current observation by the noisy one train_batch_copy["obs"] = observation_noisy @@ -225,8 +238,7 @@ def value_function(self, *args: Any, **kwargs: Any) -> torch.Tensor: action_mean_prev = action_dist_prev.deterministic_sample() # Compute the mean action corresponding to the noisy observation - if policy.config["caps_spatial_reg"] > 0.0 or \ - policy.config["caps_global_reg"] > 0.0: + if policy._spatial_reg > 0.0 or policy.config["caps_global_reg"] > 0.0: action_logits_noisy = logits["noisy"] if issubclass(dist_class, TorchDiagGaussian): action_mean_noisy, _ = torch.chunk(action_logits_noisy, 2, dim=1) @@ -249,21 +261,23 @@ def value_function(self, *args: Any, **kwargs: Any) -> torch.Tensor: if policy.config["caps_temporal_reg"] > 0.0: # Minimize the difference between the successive action mean policy._mean_temporal_caps_loss = torch.mean( - (action_mean_prev - action_mean_true) ** 2) + (action_mean_prev - action_mean_true).abs()) # Add temporal smoothness loss to total loss total_loss += policy.config["caps_temporal_reg"] * \ policy._mean_temporal_caps_loss - if policy.config["caps_spatial_reg"] > 0.0: + if policy._spatial_reg > 0.0: # Minimize the difference between the original action mean and the # one corresponding to the noisy observation. policy._mean_spatial_caps_loss = torch.mean( - (action_mean_noisy - action_mean_true) ** 2) + torch.sum(( + action_mean_noisy - action_mean_true) ** 2, dim=-1) / + torch.sum(( + observation_noisy - observation_true) ** 2, dim=-1)) # Add spatial smoothness loss to total loss - total_loss += policy.config["caps_spatial_reg"] * \ - policy._mean_spatial_caps_loss + total_loss += policy._spatial_reg * policy._mean_spatial_caps_loss if policy.config["caps_global_reg"] > 0.0: # Minimize the magnitude of action mean @@ -285,8 +299,8 @@ def value_function(self, *args: Any, **kwargs: Any) -> torch.Tensor: if policy.config["l2_reg"] > 0.0: # Add actor l2-regularization loss l2_reg_loss = 0.0 - for name, params in model.state_dict().items(): - if "bias" not in name: + for name, params in model.named_parameters(): + if not name.endswith("bias"): l2_reg_loss += l2_loss(params) policy._l2_reg_loss = l2_reg_loss @@ -308,7 +322,7 @@ def ppo_stats(policy: Policy, stats_dict["symmetry"] = policy._mean_symmetric_policy_loss if policy.config["caps_temporal_reg"] > 0.0: stats_dict["temporal_smoothness"] = policy._mean_temporal_caps_loss - if policy.config["caps_spatial_reg"] > 0.0: + if policy._spatial_reg > 0.0: stats_dict["spatial_smoothness"] = policy._mean_spatial_caps_loss if policy.config["caps_global_reg"] > 0.0: stats_dict["global_smoothness"] = policy._mean_global_caps_loss @@ -322,7 +336,7 @@ def ppo_stats(policy: Policy, before_loss_init=ppo_init, loss_fn=ppo_loss, stats_fn=ppo_stats, - get_default_config=lambda: DEFAULT_CONFIG, + get_default_config=lambda: DEFAULT_CONFIG ) @@ -340,6 +354,7 @@ def get_policy_class( get_policy_class=get_policy_class ) + __all__ = [ "DEFAULT_CONFIG", "PPOTorchPolicy", diff --git a/python/gym_jiminy/rllib/gym_jiminy/rllib/utilities.py b/python/gym_jiminy/rllib/gym_jiminy/rllib/utilities.py index 455a6b1e1..fabf8bdd2 100644 --- a/python/gym_jiminy/rllib/gym_jiminy/rllib/utilities.py +++ b/python/gym_jiminy/rllib/gym_jiminy/rllib/utilities.py @@ -309,7 +309,7 @@ def build_policy_wrapper(policy: Policy, action_space = policy.action_space # Build preprocessor to flatten environment observation - observation_space_orig = policy.observation_space + observation_space_orig = observation_space if hasattr(observation_space_orig, "original_space"): observation_space_orig = observation_space.original_space preprocessor_class = get_preprocessor(observation_space_orig) @@ -552,16 +552,19 @@ def train(train_agent: Trainer, # Ascii histogram if requested if verbose: - plt.clp() - plt.subplots(1, 2) - for i, (title, data) in enumerate(zip( - ("Episode duration", "Total reward"), - (duration, total_rewards))): - plt.subplot(1, i) - plt.hist(data, HISTOGRAM_BINS) - plt.plotsize(50, 20) - plt.title(title) - plt.show() + try: + plt.clp() + plt.subplots(1, 2) + for i, (title, data) in enumerate(zip( + ("Episode duration", "Total reward"), + (duration, total_rewards))): + plt.subplot(1, i) + plt.hist(data, HISTOGRAM_BINS) + plt.plotsize(50, 20) + plt.title(title) + plt.show() + except IndexError as e: + logger.warning(f"Rendering statistics failed: {e}") # Backup the policy if checkpoint_period > 0 and iter_num % checkpoint_period == 0: diff --git a/python/gym_jiminy/rllib/setup.py b/python/gym_jiminy/rllib/setup.py index 191f560c8..3e773ab37 100644 --- a/python/gym_jiminy/rllib/setup.py +++ b/python/gym_jiminy/rllib/setup.py @@ -29,7 +29,7 @@ packages=find_namespace_packages(), install_requires=[ f"gym_jiminy~={version}", - "ray[default,rllib]<=1.5.1", + "ray[default,rllib]>=1.4.0", "plotext" ], zip_safe=False diff --git a/python/gym_jiminy/unit_py/data/atlas_standing_meshcat_1.png b/python/gym_jiminy/unit_py/data/atlas_standing_meshcat_1.png new file mode 100644 index 0000000000000000000000000000000000000000..666a0ce358eba5d383fdc3a4af21d99ea48e52c9 GIT binary patch literal 27598 zcmYhjc{tSTA2&WNCoK|Eb~=u-mNn~0%93RW*%?XpeK)r0g9?`9Y@vjGFsF?H!*wZLzuMXm3grT+Bm$n~eQ3 z>!`R|M6d*9{)yMyTthT5UHQR5#Dj#{_hKaMgIX#DD!^~{)PEZaM;ps zkHNVg4Vb_E@1Omj1OE8$!-pjAMyWwMPgqb+{XVs+y5>>``Q2R z`imXX%52^U! zpWl5gIA52@b$CCR3dQn4lAYu<&i>+^d;e*LyZgn_!ia6#ucnTGhEp_BXIDBEJ>SG6uO9*IVhrGIX=0c}ppG5I~`B8h_ z&-ZwWF~_KTLvGxIcj{`nmAT_jxOzsOOQlIpwQ`^SOke9ju1qu_>(TomZC)~*L-Xe$ zzqmM>Pwn0q#Czi8RSSlm%E~bRh27dL!Ie~yHzmEG0v#JYyn<1?GngA0y}cpr zGT?ngI6|+asPE_DQB@0w_Kpq|r8@aAT+*)Gj3Y+nts=Z5iji(~XRpsFs&?bGdi?|` zNDQ*ho_G4{GoOA!l`b|@yJTaoFTGfF?m=LX=d;o}a~<_jo5DMhxn<<(>FE#iQN`fx zMaa3zpZ}FN^FDNUT#7^@;p8uNeKgR49femGi}o8)AIhoPwuMp0>s)+pTDJ=mmv5ik z9ea|%+NtO`u78PzrP{W+wz(OxJolLU9-@$>l+3qKohGcCrn!c5Z}23Hwej|!YMELE z59X+c*GGOU>gmm>fb6rs2$-u^|2n-6`}BDG#)S)IZppolIC;#q_sp67hsAi5eTbcf zV5y?FxA*Gua$Fn<(Y}Vw=t}9GEiEnWO%Xf_^xfa2`=3o(eok3!6;dj@X>*do2AaoVI#!aNsJR zQ2ZcS-`*-KNH4H7r;Lo49(L5y6bm8^6;P>E4^DKJ(|Dtr%gYM%d5BUv?A+C70qNrn z(*6^AN!le8JW4%$$G`$5IFBfd@>~8LeVs_7?d+5_Ef&;C@-m;{<>mEh-z$sz8p|1v zSz+mHeghWn9ytWQ76R{hGG#ZgRV=#CIQ;Tu0~cJjr?(eW`Xdx7!17}LD*xkdzt)zP z@Y$Xurb_~nl9C6MXphW%bx5las7NIDZtNY#pn5A~hro#H5KU-nQzI0%u{Bp$ND%G2 z!HoqzyFJBMG46RMa(|nOxA)oEzn@-K0vWpG!FE!~VyjPkdS#446)`FlEv=Qr)EdCy zSIXh2qQKo@?)&(lAkJN~#wOH4Pc33sI(e3aEwZOtcj#M>+A~A)pD|y3wl(`+39*0p z`}gmsvN;Afa4UGH56z2PBrM9dc{9OrC#-EFRI>M`{ilwOqiDf{$B~hYaH8G8vN3x+ z{kH982w&F?T-)BU%FrJwB-0--)WsYKxH0jNOqiEY? zT+?(|#rrdFH1f_sNdFvP04_DP`Qt;5?xAZ}Q8}b$2d+X=X5yI}y1xjGwii?|7Dj9YS2~m_vfMPuwa-FOYg-3HD>hZ;{vN`$kk8-{y6uW|bAfmpfn_H&n zJsC=y3eghA$aYCDQDdv;H=!szWmG4x7~bG30BIKjg?C7oAP2<;*wJR0u@N`J$zKy&F&;>OO^6AGvf3UX+&?z(*=cx#Q2* zOJk5-AhSwQnKGn>g$N=Z#os$DCV>wE9}($?xIOhKg`?grJh2*2$#TjJKiP>N1RbVx z@1Y)Z*GEZxVGF$p*?}!6rZzJ(6YLzuUeVDWlVHSm@1{(XLXukK{*NF#LSxWnCbr>~ z+D$0lC+HsK*RRLYT!AY5Y~w$F?yh%TvJYQrn3|drO)yXBUsUG`qTGSYb%&LC|r`qCXs8ygCZ?&0B4<^~EXy4uMqOtAEkF-M~pumA5fRmj7e zHkJh!I-9@$o$A14m4!&lw8=10RaKqxZW?w4o2(Lr0*`e{RUT$KHTJpDi}C1jny^Y{ zrju2g@SwBY+2_~JR@=3JjT77DfWn*VHCKDYzhCoFAv@)kv3K)d>fCz`Hn_6P*YACA z$)le2)>Y2A&a9_}exs}7ez@OOv-n94JQMx*tWufV5?2KrMOk2330Boj^RLg$ywd9j zO0%wHSEwIS;*@H!1cVeTb`L_*jDOd)=#K7qcWWTGI;GZ26&s+|Ti%6&VsY{fk{CZe ze&_fH#vIiYm1H@_O++Cj!|c!QnT*%$slvLigiiZ<11RKc$ShL}EgE`!Ma!5&E6d4& zYs|gDA1jF|vv@Y9@ybLTX4)m~Ec79#p;#mxENG!LmG}Odnw6eHLb_?9TIaz%Ie_+} zEMxE&rq(}(uQVenK^kV-_Uc#t1)@B`4_uh8QgH-TDrge zY9oWwQ4DlfU3PY`2QC&&p2+6}B`2}?9dV6vG3=I+Vy}r_6$*tN9E^{1vceXsc_`$$ z~d4{ljOSHmrOrC$~l6lj-#+u_hc@3q7+zvyzo7HSzk&2{FAHRkAEaeCkJDo!3I zhmkb~U0q28^y&QcsJCgt=bpa~A}1MLJ={!>>4@NI9b2OXP3Yo39&KiK{7|JZo!NrY zphlWE(&KN9j$B`N;JT_cgDCVO#5%mxE-5-XAc3V3{rT5Q6VY5Rsn^l2{JM}{Mj;%m zwLEk%gs5bi4J<4O8r&OHCLIX044jt-98_wb3i+65Mg;pumC4yUvXe%-0)>;RCzk z9za^HqGAd6M2fB^+V%SROJLQc&OX;k5m;GS$yBw@D)p%Qtz?LM#93AGSF-mqjh>j8 z7_v6K5~xzpC5u>)I7hE%PVYe6c78^Kwsn;&|G5^KRmKvE_Q8Cuj(*xj^ z2fxcomZ|37ski84myK3|r7V2-#zd&wZ!BxgbA#IGQ?xH%8%gdv1%}gWw1OCqH~z=V z_zu9VO6sW{k8M&k=j3K{Kc!u1nfk3!Y1<~gRe!NLVy6Gl3z4sAF3j^a+RoUd;^qOl znEI6d_}bvLvpb_P50!N~N}YFj{H3J{HVX$^nzzcpCi0TB&6-iR6@tWlFFVnRki7_y zV6iC68dOg+GW+k+NPsU>gAGPg!G$Dw?Vn9b|wMrDxKlVk61Y2sHutg|o23L0u7-${{4A53hYIP&HI_XWVsQdCl%{y13=g6|)q2L6!#w*0!9Wl^t2F zNGTxwi9%%Ig&UviY}eDh>B8K_XUO9?c?Y2YGjxAjq@TUujuM zg?MvfjY>iPP*r z^*jPDCs&$5LX`XnG|S*>kISeazKRu#mFLeSH6Oa;fmLQ)vG~!J!6)lNOL{O|BlXUFA(}Tl z%js{ojK8xjg-k?CzwT(AcZzSf2j#8qJR2d)b1Cm)mnd@=s|?i@x1HrAXD72CD|7aO zh%P&E{mVyDM-7J0{fzI+{CBT-3Rs_cn98Ymx%3eSD2{D$)&m35DdcsxfqDz2!0i&p zCcf}WmQ3f2jr1rR#jP?5S1iF(fhw++-Z!^s2I;iWICJ6dZE&jVx@pX3PO;T^*+_Lk z3o6sH=`mRjxIn~@KuM~2>ez7HlIyclSdn(~@K&pH9m3c5-?H~3S`Lu)OhC?Fi}$$7 zFHys)DNEOL=KmQ;H~AoXK0v(6iL1xp-2U*v*zigZCOr+O{Lw$0`vL!;k5!hLx0SW^ z#Q3MB! zvNGp(yrNZ8UoSuYGayk^hGEej2vHJ9(moMkcC@&2&;JP7PT#GL4xNVjidT3d~!-6$Z^@es(L z(SQ7^Fx@OxzE?<6i4&WBRnl$N8*8OgQY_o4qL81P+}7`iL06MeqW2}d!r-zPSvEhu z^2x1%#fuO)+li~s&OfzMC~%R51)#sX-{8M}*^xh~`l zsbGB;8{g}?pa}YjCJ3dDN0md?osQLj-Gq!Vv47IK%pw52%08t#hOCunWq`$UO)>7=k$Ff0z(0g(*M;bCw^TSpVWgyTwm%Q0KkeGW~XB<1P~l|OpoFXf}%gyAEV!CP4%$U1+<&%<_&B&tH=kJ;w^Xuz+@DJJ6mX<%KreJAr zU)Gd4qH2}DRP>saYg(XWwUq4qN*%@8K47=6vTZ~n!v^zphuYXWE$%6cJVQNw#Z?oU zqX+(a3#X(th(Cku;wq3VY4+-$=>Q>Y-S51 z+ZL3G9eEgkuNXZjNiHDYD*ZVG?Am*PfuQDm?(jEvSE(cEJuV%42(H+$>Z)$g8;rwBWRW|I)PWhzUXk$?h9Vt#8LI#*my6xsfG zV*Sd$pHHrvjc!F>IyyX3Gumz*Rl5hNdP!^>WLq%q4W$*#HxDn1RMG9d9u$1Gba9 zde+rXFC78F!rRLW5G*E**BRoo<)fo!gvRJ=qNMXdFH0+?FtIFh0Wsy+b3?C`HC55V zhB@scRUadW*jh;#Si}7ZbSSo{yy~NGd;9U;`KM}E|6=D_#bwI^mm`J59YOV5)%G1x zGhbqqk~2-ZW*#bj-Hb%a(+YYHbu?Hr5S5Y`1Kf`kZk|}YV$@xkGkW zw1(Xqwv~Y}Q|8t|7>1}u`DMm z&Re#hLN}hI2_FMAo^HQ<;Xc!5uJ8+T2+4x4{g?RjKBr}Gnc!@k@2CZ1$y5E4nI=FX z0n4!oqCGK`*j>$AEn&S94tT>7*7NGtB zJ{{W1%UMh8XR@UW5pJa*INrz(Al0l=R#(qOw6({+-96wfUcS-+n$f^!N`_2AGTgR^$e{i{VGcSejoE?6&;UT&$T-n$%8EUug&86Ku zm)y!{Oup;B(CIIe@2{96K9JBo^-Tq>)iMs>(oV@R+1lE|SKHyrt0?t>XJ3)_s7lkx zp~bA7YtcX(k(vqivR7j`wZZMJt@l?$I_RjwQKkzQF8H)=Xaik>MpwB|n!%H#O`%Y# zDG6~Vw;9mEpGyPpA1E3~B?vL!XX~pqK(*U8dWzn!ATR2{g6hRhO?4bAxpAcyu@1{F z)J;GGp;*G?WFVhQWTah)Ki=LNh1UYgrU<8X6``S${GmCQ+E>k?#g<4>S~Yh~{HESA z^@+TuHm?L;=Z;vl~=2kU4_Dg`<6kGLKO~Xs9$FOOO}+^XI?g z7PVJ2ScP>rA1f*h%F7%;v6+@a>NlO-yanc8MUMWAlh>6E9jB#xSh&xAJUD;oftDZy+Z`)nLIJ_(+ov)Lu z^9m1h_qpMBo3X!mQVD0aB5T%tJ0SG!B)R-#(_f^as z!xiGKTep^&Cx?cTy$C>5TmZ`|6k1qRgg4Cr+SEL6fxH?QOP6&OC=vRxoJ&`dlx`2IPfeWK!si0lZQGbFuA4V8tjJ zdDOmk{G}>$ea$rCac<2c>E;YpyAz?@0H0oN3n+!7@KW&BMFc!eSj!kJq?rGHMn^}1 zA`QgV<<-@t!{U85Xj^>|1&yp zT7?*vWSoEAxz#%C`ZilTdG=>*rBd>&lD9k$y4ls3Lj^jbTPY7?s+oXgxo8LQS@b&i zzxoV5^JiO>eUQ7#siO8}IJjifA3@)ii=8izb(llQ1jw$TL@EZ$RxhUt#~bF6-x-$U zek+X)qCu}M5Zf-j_~L%#sMXkq1&cH5OfP`DCZh6cF8T7OY8wlTY9%!YQg1_mw-(9|M7McMRNIz4kDG{P0inNysfymxcDa3&|Ay1z10 zrorE^WccT$svN?UkARVPWK-j(PoJ8bt!^n52w1&xnV3(>C41M|X z+<}X>Miflx4S%mLooB}Q0Xgh^2pE!PN0fYj<;3RZX3;@kk!98>Iq)hcCw^>f`!Z6c z;IdX~;cdeBI8ac3PD}tb1~h+B9?oPIB`)Tt;#}gVD#`I*$lu+2wjb)q*~l??$~s(; z$PPTANI3`F)HN92*4heHR4ixu(Xdl83>3Gu<>id)R*haqpeuOD^9afEBqbF8WGP_s zXH+oIki&n+A6 z@3GpULleL~VQqzRi%UUW()*0|LQ%eyUM=Iws;c-0e7$r!Js=8ddYnGBt36o=u4T_4>j&K-2-e*e}jEZ`2~;M%xyt>xynT;%>wvI*@; zzd>CrS**j+gXEHCDW^tEv1<^*Ze2SIXm+!C3JFnBQ9%uh3=gNzbiDvrN;W5AdswHy z5a^AP7@nk_g~PWlx6HQa0EKk}yn+iY1cfcgErZ1)p*Su>?)=!chuxp;Q=uNzl&A#+ z+?WHMATIyH{=w`+sV6@rl;xrpuC4gM2uBrl!^4N&HW1ZDL@u-kQ z7B=T*EPjXiNS>;6o6mn8g_ifB6lL|R!+6Fj z4ysNZI2`+Z@ptNx>rzMa9j!qWbE{}GaT2xv&IPBO<~4fQ=Y`*&Afjha@+ZIqWv8qj zY4&@L4?n5&cT|47_xop>WX}1zWctJS2PMO=s%xca%fT&xG`kEN;DF9sG7Rv!G;@K_ z0VB!;LS$FilYofm4&G(Dl|1{+fz|X@pKX>AOr7gpfR>cd+b^%$yB6{De>(Y%)|f`& zxFvIzmZb~&bMHj-7(6svRjjKxjF8RQZC8(sJ8UAX^!Qa0lCmyN21(JC6q*qymHq&h zZu#)&BHFHnB`gpJ6w+;R#YQh9k(vyXu#G!7d6>rzOF9YB2FKHlI33+^GGl?Y?qweL z_uQK2x25&_A5dJiVVYuQC_J_Pb*t@@H3U4%2~BNXH2}(fA*q@YeA2rRVt7AQ^%-t) zQBOx4XKZN90oLF9&NM??^1X^{XOl06^{=j=(fWpq<Ws>2Q)^L`O+jFni=xEy4{{94j#@+*q6E1GReo~1Ww4T^)sQ8cUB>Ig8nXVmGCzawm5i-wcaB!-=xBDI^;>eKa`M@@cD6ms7P%KxG90 z)O-GQz$3TZt6Hg~p#lCIXHVLNm8wedRY?>vb}m2vAa|;=B%%0Wf3ET^UAfX9kEI{r zwgEVd{(QHIH|SV!$tioUKM^D#!QA!v@$2X+NzA9?6#zsdLsJf|LSd(8r2$YxVG6KB z&>5uW3S@~Hb zKB-JVv6HMrlKc;s@u>{PMh5{;7TOs-dHwU(KYY!dG#8n;)M`fT!0&fg@8+MH`28 z!6TTN3c5?oAuE4hzBn1Zw6oE6Wht1=@QXYwJ1YzHdMkL@g(09mxZT7D6=@||Hlq&e zfQnSR$IINAlVz+erujUdmBmlZ0&!Yx|BQm3n5IjTsJB z{w)V6M@ap3X~l)1;ownuDr^MW4!2%0(uK^h!d4c3FS zNFojJ^(5XjVLv;X4^wb%0O7Q+0*FxM57rMzVt_B4CJfHUWL8K*6yjawCsrEWa*a8h ztb$Ec^l-x|a1j@@%YKbaR#@{VJe1B#pK3MLzRl)@_?gs5K)OM ze$WdJ^M}TFSNXtu5(z?fg{7ZdPKMr|l!W9v4F4>Uf=+$AmLSBxvB)08&qa2=xW5t; z%dnp|KpuV-xUS>=CPTC|khRQFa>`>V<9qqQqfDEA7E6_CaU<=H!a`)evSvpvH&HRo zhI&esRx1f~0cu#=z&jv?3-`Iok5Eh~N$Tw!m6sC3!#OMhUCtCMS~gae>zbUA zk-KzhNq|X()y=x!Z?nuX`S-Y}X0~Uhoi3*5ZTX`oWS@LpTQZW`L`gyv8il-&a=GO6 z;dZniS1v52V@$^Tb{)tJy>NJkwP%rY*)Q&&kYx-PRJ)`7eX6%++ z_%Ty;(dtVMS!?!zprp_4>+CXWm>P2zxr^$G-uNeZ-IKubyi*?5bq#&eS0T^lk7uT* zXVRrF1jT3a^$4mg=ET?ZtArFJsq9RHcnc_3z-%9^4~*U=e_Ie$v$42pomr(9RM=Nr zNfxj6fB!X2z`XqRBV&W2%nBjJ=>m@AI8Ea@w6}8Qp@n@eyudod>!W0a=mYvGCEnvU zdTs=s|5a!NyjFdh|4?Q8rRp|#%mWg5bp*Qzd z&q#Qdn;bvGKu60-Wl;Jh6sv}shr!CP#j4@DM&7_-glSm;E7Vh3wA}qU9Kd!kj)`eBnc_D1zu4yDx+W4CYf^f5(Ibb;0}i(1F9XWY>)l| zmXAw70IqRGcKH>HMi~wo>mWb%I3VKS6=7$zu;xdhXC)5>{kOT5WKdY*i<0f-H&K%) z+qYJv>cD4C_yv}c9djkTesswTCg-quRiaxHVV~^_h=L#fCh|6yy0vV0;h#dq z87h^;{m79eE#t+6Tl|m3%2R{9p}OXV%EB*;B|*QL*xwIB;c+!3ELDL~-uq#7k{7z9 zf#q;fbE2b{`{b^9P0pXEeP!=6S^Du^deK!2*JB|KZ;An*JvqSF_i?VXl-(njn__Z? zRW;#`7T%x4-EDXq4g4z}n%e&<6c7~tIguuUfd2HeQ#||J5=z{vjM#KtERx^+*1@4{ zr;Gj6D4Kkn>zf1lDJ^4z8|ku4W!YAM0sHaw`fcxk-krgd$M2m78PWte)D1W}PG++k zmgU*~I!C{if?38Jy$FpUyP&TRxalQ^lyPYW*I{x1ELl~^KY@3JX?m$aq$rY$s|%!l z>>9qW(UAK)gW)03GsEyQ``8?CZXo#x`TP35PORD1Q7KS}bW5K7L7_%RN7J>kwQNu1 z#~(BA5$bG~lX7xhOADYRd4tX%nWc~iim6|IQc@C?8l)VtdkbbdFCWZHj4Zhm(WFQl zHM(mwoD(Y(_ozz(#L!|#FeeLtp8q|{WYI6pjNJk-CsV>A!vx6d**Aoab&!6w#Nx%^ z7T%sx|BYs&X=^&KYD%$-fAfPb1Vs*3~ef?e#qk|Bqd(`*nvh`(1C7l=+Y8+Uz_yyJJY zc}V_(8%zbl+GDHW$S5B!fy$J(&~yD5!cd1YkYouQ#2Y9mO~ z*u%NA$=CU(FFN*ZMx-JfRrKJT}r9`l`KtP%dWZ+AGA}Hm;$cue_cVmOwKKpJgfc z4&C%`iaDAJ*`(71*Q(YoI_A7}eo}w$XemkS$UJg$t*~LW{tU5s(pYb>$`)~CakMot zG4WJ(1Bx=Q0J#gW2@K2Ynws#Q3Pc2nze{@iK2n1f)Lf44)z#G!YTIb{u~^8YxfkR(QqjNe1&egz&Up1#$1v5ikmxXbf6-3P(v*S2vFS@6}vpL;>Zb=DHTYTTecB%mI( zjP_QdnY#xI!$Qso32m%weCt5ge3$ETk9`uZ~h10fcVjUKH_qQz; zv&w-bWK?u#6c^lJ?KShe;m3OKVJUHv_b~K2duo(rrLrNm!;N$1Ycu;*_dgQ~T-I(% zN#(u2Il|6{4{B;^ioSNXL?}UMDjuGV=G1!Q*h|*GoC>s4Z=4fH=eVc)?GyP=U*|?Q zBX0>E)l>blocw=Y)yLoKdvx$7{9iy2YBZZyczOofoSqEc-0$NrUqioHqgPXsO}Cml zY}ZaF67BmyO`zCeZM>T=66muXi)x20KvLUQy5M0^%B)hrFsUjgMjjRtLMwb~Zhu== zF0{qZY_2IvnsC%6ngHtd;gFmiTp`CGaHE}q)=kUMpO1?(arfVlvTk@W)-Kkq%-Dn+ z$~hrN%_AZ0`~$CXD>u4kKo9GC3OY-bdE!(IeGitiih2FREI#wwKwBwsjhAytHMOn? z5kHG31t5n#&>an4;jH>uPAbp|KAZHaD&(fc(P&AypyaJvcR<`4bf*`mDmLw>oI%_l z8oGX0IVqR2bemP{YvW3{2eIw@BO4o=sYDBUq(!OotK1?VL-<-S@2V%#cb<%E~wVJ%HhB@{SM|mhn*E2+KND`4s9KPY^sBUgPU8Q{GcSpx)91`GZh_ORaNC& zw@0x*0#=~i6Z`7+%A>*BN<4)dju{eRLWtD4&o56~<3WF|t^C80wq)H!i|qBgVKtM7 z%4{WJYZuzsQ|7&m!%) zOZ%Hi3TaImrF~7e)VyWiKItm|Ra(1-t#@PS8hdIrZ%m9_q|aW2TWZ$j4#6Y@I5Rsm8W4U#PWWSeeV~gBAq-7T&r>|YA@_G%kULnKK;-&1*>tKw zLz9Jt1^9^s!d7nLN-(_+d#42$$9WQCG*pC(qvI@?sb~(PewbbM@no*P#bhKPteE{g z5f((Ml6bC;dqCR+>SHa>_p zUMiL<`$G&w39lZ9^s-3JJ|9NG-NYwo_a&a+iLKzNW;cf6q zcMYjxQ8%XITrwOQUbBEGTs^!dl;t$_$**gxHRaC1An>CAfsVemh#v0xd{;A&iZ=N` zT%)&335AEZjBRd?R#<>1F@NxDDrBv?y84X9HKF9$K$QX-lF`&4Eorb-W7lL5;5o!t zYHK+~|MD~9J@%e4M?r4Tbrve2Ahgn6Db_tqNC;+hCFaAG;o(M0h%bAd&6#Jowtl-O zXsaJJIv0hwzqNka;?0R@m6-auqP}?4?KQjL@8#%tf6b9gyNO^Iu~ObbNl_pokAN*# z6uASM#f1IgynWQpxC>B^O!Xp3szlwiry(Itj=4!_wH)D&<|enGTlj+&|EWUllA4+t zW1IFMWUpXeft2BwFh6HhXi(U>FBZ*~kXN7GFF}S_7j}RV7z&NbEXXu*;M$=xa3KWV z8-=8se}Mc7n)f*i$WfE$@;vxwL8cZ2&cMExkdSDD8-*CLg7||ZW{C#M`s&&mEvzl@ zX!sArez8bPTpYkcGrpfpuosV(%U%Q1Iy^kQ+;%7sH7u;FprG)3R($@%<6M~7iDz<= zX;baFJ^1J2y{A%ObI`Lbd4HKM%dOt@X$n~&Ndb3{l4Tv{nB@c@@6k*ub?+1%OWHYF zLLDyQjMT5B&d<*SND9KAEG)ntuQJCBbe_*gTCPdnL`pR^-lQZ3$iV`foSZyRBY6{Dd^M) z$UNV+P7JxKgBer%D~PZ6T-SGL=BcfzT40-0?d|O)4vtc1WN;Cs1I#9JN^T0i+U~pe zT{Z@$AkU6dMpUN@X)1s3Ie&B_1UPV&xmk0>kPsjz%mB%JpUF=kOVTsz*H*UtML%us z*RN~G{yt8AR6^G#3#8vGMr8*B`o^Zt;1_lIdfv3gTNI_0|DxlzPVZOfWDX6N0 zjwqU*BDzH8YK%D~%11$w6@vP$KP6#EY|ioiVH4wI<;O?qd^hDyY>x~_B9DOWqUbXX zoS%~6#m#w)2J2HR5YT?n^@f55mOv1*9o|cx=Q!wqvgb^V>R3ZeyUOpUtM8?!s2^G? z#uORcX!N3m6f%5QmQaP}fo@9}1`CKPfi4_+J^I1Q^>82q5!WuAIW-kG11bv6V~4z0 zKJ5C17;}<2i6u8dlNHqfxpLZ`8a3}G8sZ?!ceZ$zcPOwD4+Rd=2=<*-`F=& zZIiI527^onKCOY|k>l8uXeltv3cv7{kBKQ}mG&{^wXJ|rvP1eBs=7M245Ufdw?JUM zs%aV|Ld6n_@s#*DFMJ$AIMx;PGG8m zg=PRjJqO_ki_Bb&4q{P7ND8BVd5E<q`e2pY$3z*@{?A6}GJ>WF0MH8&6%u#qG$bg8I zE`+r)(ItdTIAXkMA6r{NwrF9&nXebH-P%g9XX?!>yk5GyOjEw-D&cb(d8YQjD@5+D zzlr9*}m&a|LCVd-$bFhC99k~whK99n*Sph5zE zPvWM&YV=CcE&;+T7$cGZX-!4uk}2<&>I%B*qS}itheDDcAHW)*zvB$k`<}}4tSIBv z4ZYO|P;LFq;3;psO)2BeIoX+WoQphH7-D^1hqJk;m=I`h!;X_*3XquH03&Jk=s@!C z?nGk_F!knK=eECsHeCcY9%N0jKt#FD1-qR;1?+!dUBhtl%=bAbLh`)91OxtfX26Sp z7K)0Bf?GThE6=g{T;=VzQ2NK;NWGVq6{DKMY-?}%A&+ybUV*{G{@Tan3?tgp8j*t3 z;XCA>C(b2J6O8Z?_w8xHW1?P2vIDUln)#=Qz0Hn2r9HCK)}iJ_M>aq*W|b6RTj50d zG%AV;fVmz13ft_z?DIC{!Jb2g#2WLNl>gNGzFNt%Ckc&pl5{B|%OS30yDBC$4PVHg zf7Q9_k~8Ucx!{YJ%d5fOTCpVtt2qD;0o^29IxCnf)+&n}G2*KPRXYCpKEJ}Wu-S;1 z&yY9b&=T70IoSR6>$T{4HZhUYiN(^et$Uk(#vGhHQbK=XxA=NxV5ZpbYpK_C^=Bq3BtDvdo-Z)?it@pBzDR6po{KC7hFaU3?szm&3=^SSToZ~7td zJb;de+uo-FabOKJV>+0#G$=vruT-mx9SR=Fcslh|c~@QjgnnUe%s~xf*te;2{mf`O zwab2-=hAJc^pl0%Z+t#B{U8_cH_*ANnmQQo-M%^f(STI(idswE?=lXcp(X0?L|O*1Y>SC z6+3MOa%qEE*lkOjBW)f!shrWJuP7?VS?^Qk>hEY)K1eV@E)%j_u#D`4P+ zg@rN+<2?oJmcN}#uOAp?Vv7E}efZv*V>XeXpEZv@M|YJs(aYQz)Y*WY^A=bY7Lp}? zG^@@q0hP|%yE=%fcJbkvD&hh@C~%S$I_ukZuLJ`?a31wtUzYydXTdSW=J-}Qox(8-`gC=mF8SXwgp=D`+c z$F3F#bXr=5?ktc07X&6oGWJDfzVtZ~X+e}1xGTWyZHBX23l|-_I=ULdSGU@oBm8da zy$52vbjQLwPGTBQ`@`pd{E|E#TO%|gN}XMhQEy&Cy)y-=^vEu*gWV;4jb0y`wo$Kr zCCwIvS1m|(vH}w+L(3vlh+4^jrQhGL&D3uO?Z^>;2uTk>W5Jm0*^|9~0ueiT zf{aC-mKK=ZA3nLx7Sux&=yA zC^_!n(xru-_hOR@C-P$eo`-=*pP2WxTHmYvP8 zX9fB6T5=HmzCf{E(qNZtVbv93a=K|iouyXTgQ4vlgKduK!^3vBTr~Bala*o?1G_Vl zsog#zy4@q)M192xE7fl(7FF3Y>Mz0jk7z@b{s0ngD?9FE@~zOy^69cBuGkHNPWt!r zWotLH+{m%Tjb5Bu!_?xVQIEc-vVB{&*fVDu?us?kzB-=q%g>6_e6eksfcrc3IDZ3h z8JH8@l<7)V+e0CLF~7Jx#V?PMMJ*^N2XrS2sD)Ke0)00O;byG&gL8O9#Av@ndvhn( z##{MjkI5Ic%bgY1Ks+q-M}e(WC=`rO04tec&2(MweLZ59>Rq$7m^cNH6pCh5H!gXM z_dvD)tQiV5O?cSqdfS)Rvr4lYpF|U+nQf}Z0GI_N{9oa|f$XdT9>B^$=g2KXOSQ&9 zk%tjb0ZKU!mv`?cA7eY_AP!OkX8TjXNx}cK z0NzIRYaZq^VP4K`@x|Y2D}gT0&%W0#C538{6jIqA#;`0YoCmBBki32F`*yENMYyTW zWOx|}gaKFeM`+kPWW-InuZx0Q_HhIf#5VWQnsZ54wpJS#W8R(k#i1~o3mKZ@jhEK7EoooQ|zH&Ufx?Df=ieBg)t7CeH)t(&Y3s7!CFN z|2q5fcqrHR|LL@z5-EjjB^^YWtV5WiGsqq)WJ|J_CHvl~4n=V+DcdL-k|KmGLr9}+ zhoOmKkUd+Bo#A^u=v%&@<@b91&VT3Cc%J*Y@9VzrYk9xl7k2|4V$>6+MNe}tEA9E5 z?+^aH!aN7x-OsxD*kibZEHHHU zqO?EY>&z*-j%=~~x00w01v(}4el}4%BlDBsTs|y2gp{V=oq{H4}ke z>fk7FB2~;f$-0ti^D8$}feQ^#>LKCN6nDNz%ZA{aJ@ds}S#-y?FVQD^FuXD}ArEWyV_$6_|Qu~s4eCfhbdgSqN?2}5rM~-rG{&Q1@ zRap$|pFcHC4`mml`R+9oqg*nI0i+)!0&BA!5=Fw?mtr@+m4rz$`|!>-SjEQL5^Ho( zM(ksaT4GK46+1gM6*85n5*_y2?PfMv>0UXJcme=iH>Ig6GCmLeq3E*wp{D1fLyB+E z9mhT97m5YH%!|jr-+UtlnGr43!{MXYIL0OED{}agQq@D|x}~3_41-+SZYSXRa~@A~>u7Pi6*LrXow_UBw897nb%vLYW}cHS z_M@8Clv-g^2NJ)YXDxcCvi8ze)bA;Xi)Zg)mP_{*6KCo%$Ch|CIePX7<*_#ma{ig9 zrKjP9^+}2qwl)wG!P-l55s4$8$Mw`xT@A=b8sxPxjN_Bn<W>^BLzqO*4c|;QgGFDnz2GZOV{<%vyVr(V%R*t~tpK4|woiL}| zYT+3ador96lJji%&AxyMclWfp&eD7X$E3oY9Yq?#(|*2(uKOkLLa9q<*l)s+JFkhh z+s?FKF~D8?Q#Ye!>X!Schw&`i{T?rW&u4;-6>4vyJOzW!^OnkJT8+JC`sZ%=K77e! zXSq;Rk2R|AP4pwp{q>=GeUFM6y#tFWTp~)W)5^USr&Gv70H_Z+vu6vGSb(+vq31rH z7ATsivnY}^mjc{KAO&_^jjCPfSWd1^zEjee6Hp>w=sTMU3B=uhPVj;SFU+S+#$#rwm&-EE9^H5Po>sm)v|}m`n|E>4 zMX^9CXA8)tGy}RmnH$2cnw`P-EO?w!CPm!Nx~G)bL#t*PfAow zu|Kd3(;t3HU+VqPHJ{sU9=MXvdu+$eM%$?x9Tt^|yAQXaNXbYro6F@L*gW=hsKhMppe6@U9L&CM2}92^ zghKS&Bu6w80IzPGSquzBBn+jhr#Ea*)KTP3blZXAUL}dvpRRGx9s`P$fLfDe9&-7X zc@?`Z=tk8Zix1^JWs_!hfvxIWG_N5)zc_c838kfvP6~4PdZ#?y^2F;Bk@9*-oFIvg z_|)|1g|tq)g#*JZzsy~~dU#@YSB;@Gqd_40UeSq#I2twtE0-42x=xLx8>t&G%#NiNZP$90lV7O11l ze*q|7sU*c6vVH)6lEp*NOO`)cv^PSK_@Y+&L-#$JT9u(suZyS;-wBY@(gYnm(}m3XP^Lvjsot@g z9H9{D;8eY|sy`ZLTOM2ht&~G*dt-OLN}Y|RMGnlPrM1hH7z?@GEGxQz8gYAeEaY

+tYc^TPUtetHboFkdyZj+@L5Mj~ zTx0H8po+j##zMB}b|9KSne8)&hSsz*_7Pi_n;oN%;A7BaLA>DmdDg5?5_M@CXn`!@ z^GPvhRB>h#(oGsmRX+aWu{d3~tHkKk=&!+5bQi|?Aph~k-m+y{lk?nV&UZtPwZcte z-VS&NIF-$Zq%C}DiZmcfq8_d40`>AK^5qw0WG-I*qQkCSr^)u!Yf$6(QbD|@LW4-W zRPDQKeW6Ov%ubTGq0D&@9B@O2S&ChpPdxypCAFdeNb}Bx+f*GmAHzjD+v*I;q(+sO z|GMCS=6L7w!Wb9{P$rg5<(<(jtN&jur}UQLAv2#hf9C?#4cs!4922PR(B&%~Mg{W> z_~;L3nU~pR0I3?*^VH6sBPfgt?%n7M$ll+)gf*ILJFyF8-Du19c7A>_++(HnIw+5Y zR==59xYbxfqq+5hpEIk-8BU`4E!NN;zIdEs!&T5XDdcdUaGd{m!<)9dqs!x`kpqzhZ#zZyn19ox82v7t17@i{vfvG#>mzyO0>5&qy zBh^#+2{SQdMY)k-c0gu^B=12G2RrXgrc4S~h}O^wTcQZ^qiiu~Yv#qMKYgn5-yX5@;N#j|-4q>bU+%tU&B_i3a@> zU8g6urme~Xp~1cD9%eUd|8%0xSUvs2mAyevEI1ee zE9Vc$Y`LM1$d$;>2RCdh|2E{1MVtW%nc@8Nfj&ofBHWd26VqW4$X{%w+G;skmNJ(yP52ikzr)OnKOpXOVW~hv*U1u~s-nT>MsVgh+@-20yF0BfkrQ2|-^NsOryQyN=bf2of|ISYs{AolKr z;#OGCTxk|Ne{PS$Yn^d$nzE$ zzf25}^#2xmP?V55evwhG!~uKR63sDqyy|erM0t)VAv$wRT=Ui`-Wzm83q%&w{t7m? zk?0a;Jx0Xa5a zHvj+xfXgK_ZUUMC2AL+D%s>GD|fJzQ?!JRUO(9 zI9uFF1a=cdn?6%#*a14}NCj@{jQlu2T(qxgH~&gh-}Kl%+V2+(Hs#%&9xqEvOQ@X` zUWd}6m4gGw@f}cxjgpp;>etCaKcXR?^pEX1+}?@pw_eIOU>62cw9{Nc@DGZ)0cC;R z6d{t4ph7meG*1@_`8vJOQ4BX?Jfu7f4LO*DKN|4ApZEjy zdkgY#K9NDm_!!8t1APGY&-AX&S-Lz$0yh_H30B{Le+ZZcQ`2uDUqpg`#Eo<|;9Vsx%Bfb0(}O+;?Ny7J)!#m^h% zTy1S}0ySU_F2tyz*jtm#v8zTgW0>a;nQL=2-^CK8h|Q@MGbxBP*g;VqjiCn~z$+ql z07#Z2$QvJ;Q{Q)N(+w5SZKPZg2XJ!{9+TIKcAIbDLfUcazPsJV62#&6FhX&CefX0d-vK+*pARP+2yU z7_dFMsHQyDGcgC{W1khqEGUiw0oB99!_5}hTJV4Fcu}>z9sW~GC-%QB;6e_E2AWxx zSrot}SQO_D?YIdMKY(Kigrk7=%g+b0g4$lf0h@67ylMud>|?Cp47+yqDgsn^`Eqn@ z%naZ{k_W*GozNWZMeZ3xR1zN(%6v28gYXeTwA1vnDAdt>=DWBn0RDm6kkTGR=@N2p z%jZcLi)#!ckZKMdC9G+gdg#KrjJUoJ$Ux@}hHb|%J z_=POVL*is+#wDIrWI>%r;^-v-yCTqCM2-~5XG8ps1K#YOEfyTWNCoY$3*`-Rh1-UK zqplX&PV_av-8?ktCdc12RF1MQA2Ti^gtvc>@OF)E%e9=HsV8HMv*z}M&hBzN8%aii6{$*p< z{GtywcO@YANdQu>!8FhV%92utu86ZnK(xXe9-0i2KIhf@yaU=Jf;nIGEDLA?A3+~Z zkL3VeHb4>L;N+ppcT%YmS`s1rw+Po!2}eMx8G|KWwYc^&H#-|9(sQ7mhb)O*`c_$0 zRb^b@Xk|1*^Bo7Z7Oqr&+R5!n?bhXXrqan;gl3k2Z<|eVWqEni#Bu)ymxO>a_^*>I zm230taPf)tu*4?e(X0p8G)NiE!lecI+yfC4UXa5rpzgkTY0rTbvMQ$*=mKiJ=##1Og#~eSO5xfNF|h!m zen9JCwvEvNfPh12KBo9QB&zZ;a@I!L#F~-NHR8n2+A&rx+>3A=2QBl%aP#?ripwpm z+s6jXw(A_1KsqYUI<+}l<|u*r;YV4EU>}fzNO$xjJPg$)ORDWvU`@xpp=`qFAh2f0 zH@Kmm=4!#;@o2Ly<2jLI^6~Kob4?`x(O|*=&#!?#`)1FDAR;01e+4kRh+M^Tm^X2OLK773zq-lweC{&a-X)KG0qskL zsj)F#&H(H4Cad6SxfCn70UUyY?a}r2Wc0`Q7%RY^NV%Qx&(5|5QpSi%-T({ACTFM# z)s?WjNd*p=cf#Y?Bt;$uIS;UYDWBzMQlBz&-9L7eH1@X;@Pm=qXGJ-6xIYoAcgy!l zb7A8qj16Mt1$XTSyvMw0Q!16{isrV1OL)!~i4kP3r#_>JP5qxzhl^%C?LZT}q?NRC zatF#hzRpD=MB3DI!hC56E)89_aTfg)biS%!X9Tbt3j-mTk|K%sv5!Af*buB>cM~r;UdwD#7)Cd8;!ya?#)kC&kw z?8r}xzG!lBVal${@tUw~=C3g&(dBYOneNX2cv20Pr+g_f#nv%a00*z!?bPo3H(UDN zp5a0^&@Mzsv>*J)g@xxhQX6+)ga<{A(mb5px{5M=y~{_NK|Yy^E$#c9&+roTFIt>5 z>F(CQty-Bu$$Wik>>c|4uDr3f5S~%&o4`vTP2x>UYWMIZ5`ukaE|VZZ+HF1MCWcK4 z-TLjgD)>}c9mWPF868)y5ghw1NiLdXEC#cu5)I+sO6Ma7xzUBcPNfcEw4MH*60BMo z7@t+`doD}bUs&crV35a~O(mpVfh|upV*|wN)GKeiaZKK;SN}MsAsi`s|9(%l4^E0) z0aF4BD}0;Y7VLx**SxrnIAIZqDnr`}}|4fa$H?GVIQJlBqIBv=R-{ZV=sTRJ(7gM)Zq1+e%)m=wk9eWXJk2c!oPVjQH_1kz z8L{5A;_9RUG>~ZF|GDSIf9n|m;u_FT;waSepq0jvw@81AM|G<$G7$XjYKx31JNUO} zY+wM;S!4a38-9;K{(kz!f9z0k54GC1BmZn4(!)Y{?HMUr&|c$v4~u&sa)pfJKaM_k zzc4Zk_5a-9qw&Mf?LdBRozV*VjPR=U)*tJGk(Rjq(|mzdAWcNp+Kc4xT^-^14RhIA z`*U-WtBAAmV>p{uJC;E7jXY%S)f=7&I}_&VdoPpIJCW(ogInXr&*M)Z6YzccjL%pg zOR(O1Wc|4jr+%6*aOHl?*ININ_0g|4M~V9R*BfScgW0Sv zZewc{i9dG0Iv!tJ$K3t?2ayli(3@q=pg7j%WkV+y*wFCtm)AbtT0{@AzXuy;`oCH2 z>MJ+?e7*h3nu)z$HQDuv&OQg`_P-XkZiAP@khUw|Cx647LNNB@yTPkLto~=4&i#w+ zCp)T5)j$k*eYbo!akMHjHR~px`@tMp#`T6akWl$y1Z$(Ny#Rs0zby2c0j&+7ar^4l zS)Z<`x5y*b`_BBk9a*>K@K0m6=daq%%%0NYyWBoxxk+OG2+(K8+MIGAH44WnAU&kgY*Bi@(rK+Z?>fI znjx+qX#eMHUtK_qW#iYrP(~JWz2{DDx(m{f=s!>Xy0x$EDC=FTyXd>ALjRbb)@9uk zUvVId_5GFqX7tb$;>Y{{OS_}BZ@p^016KGyEZ>F`V9m6$44KEzsJB%K&dHf<1$ZQy7@m4zZZZ2 literal 0 HcmV?d00001 diff --git a/python/gym_jiminy/unit_py/data/cassie_standing_meshcat_3.png b/python/gym_jiminy/unit_py/data/cassie_standing_meshcat_3.png new file mode 100644 index 0000000000000000000000000000000000000000..3d21a3c6ee370ef9770039061dc83afb946ce805 GIT binary patch literal 9751 zcmaKSeLU0q`~P%Eby{7;ODM7;K%L^g8fl$Mzk{FxbJNOXtp9346jCbSSzy5xj21%$6{Z{NACKk=cPe4(Y;XJ$E1BaKV$0Gxq(9NQ8^M4Zy4vZvn6=) zpGkJ?dwd15%2&y7#qO%FfJqf5q~&jd#po;{5inSTdB%Y%780=@{4eEf*hbhkll$9X z`|SRDRtkfiZE=x+ZBTu*eJAYn^H+z?!eZ=<@4#S<8|=W(g7}?gz65OF;~)0JP9M84 z3WIG3?9~MSJqT91tb>D}kpHZMZRq+U3)}YYsRj5u0#Lm@(YOvKeJB>}by*HjE`7Bx z^nXy^!`%X2Ny-4qxdCAPF0IYrZ}G7npUDL~Ul$We7ZY7wq$u9$trpnc>c9p-xdD*3 zsjLQD-+2UWC4K-s^|-tq?3c5{RRVU$QoJ_*ob)#;O<5AK+bZJos=l^`!O9^1y8io= zt4w7Kd|L!D1)YEOjfPkz@iv>msZ+K9u8tXqAFLi#Or*%`4WRmWiZmdwNz6c>k$CH5 zy>IfmyRrjz8V-H+-PRD-hk$%mMF*X4b?X?h{KP*+UJj;UufL$ZAkw?KVBZA(uvOw4 zm(W%aedrM00aDNCL284vep&nhq_%e!rvPtSwd;Xy z*90UX<~q&*Jgi6%>k6_8l9=CgXwm=83iM`e`KhU0-#&+EihoLTy2V7+j`#S|^f&oJ z>+M{B1Xc)NGd;-EAW0N$bC!U;{s3v=;ux@#@65tOUTnM2DOb%*Y?0sTZbz}$_zy#5 z*K8jW)w^Fphns1gXw)t{P? zLK3`<_!6J&-nYWkr;_9c(Q2Yp3})eV0ao-JhA_U>BM1A_(pU-+#$qvqlW){lI11_l zXF)WDq$Bf;s6>)?T=p?sM_NK-D40(^!F;s2h>l^yv-S?ZoYEFHs(K2xz$Ow!0>tFC z{BpQdY~xf!39XPxq%ngL9!gkCA9{$+o}S>SurRyrZ!|H#KG^ugz-@O-Md%2V(p~MB zH7+G3W!hEgN7pjtUBeb-I9^f-Mm=W|3CwCgz95=jZDcVuH8rq8reJy~TTdUk><#M} zsHF-N$gv8c?4kDgIy^q-@SJ{j$(HIs6YI3D3_*9#GF<_n4FX9rF_*rsDe6ERAL z$U@e57WyUzLm_D}f`S9Ienn!NKFxgn>eVxENMX!Ob5P?HzC)TYl|uT+kQiEWtu-3E7>@+AGj^nwW3CJZfWd!BnE{-9s6lvS-_6wkr?PD^Q6SO z$s@6jGeHb~P+t)qFDoZU>*<*usMXAF&v?OH9<&wp`2>!8Vz{1hO|Dq?Y~6^Z_Ld)O z3R7_*f)tD`40ve)yLDQrQjex?$Qx+KHRa~!>QfnEBZ)L8v|o?ci7t0BRjJrB+D|`y z86@&j+-oZ;G?(2#FKM|x*Q#2wz!zlmYqOctfuFu|s0K@4LX#$KL#eoa_U1ny z{jlG^-$`espW6{atWhdG9RJBbY_v7CElr(4Mr*f~qasHXDiVKpygg}`I@4SKDm=O6 z8G}t-SC#J#HnuWOQ+F1AmZ#|8%m8cs)i|>PU}uBqETutvb3IHY|DCt+LqjtTCZpr1 zr>8fV5E~ntb67I=jK%geHOJQZl|Ikxy;~%$t)u2M8Su6MH@95+#a>gV$L7w1+LFrgaqr`RGPJJj_({LhTUTZnM#bVWswt;P1iU3$Te&ZH1lYpQBR5-)-b zZ;K!gEiXqPvD&Di0R}^9i{wtk@D&i8o)GA{=1H;?Ypi<-jmhIz{E8xKS<4@ZTJFk@ z=5*V#2Wl&7Yr~m6#cs*+M0w?(i-`o%z2Y`A(}FH}=g3>WzJ+@2r3(4oWnhL6<8AJ2e3CzeR__jX+HNE-8T`r2k>+^KJ7n~Yyz>fopSl+M@%V>(Pw1ecuklyjOvfUzj--&Ic^~(# z8yu`nC?sm9xQ7P?UBQlWxm=(_nDL#47d}?BeccC>$Mo7gr3Nn-bg2ap1IIh^OwxL2 zG$wI+xzKC~W#cdv{iHp_TqXGDDk9DC7_ECN>5ff}MVaqpYx%nmQcsRVWi_bmE_tAw zd?{<3>{Nos_r1dtPLVTZ`wv#;cX@03xw^TH6xR&xmW?yICEDnNZJs=vhcqnmqi1TP z2t`Hjl0p$ZlS3Qr5R)A+3vnRGA#o!t-)?rhCN)GW=tZ?jVEVj708A zYGyK-adB}K``OCo9qEp2L%Mq%m&ePFUY_7FFqAgOyjp2^-bd>#R>t93jQS|OM0iw# z%8@MrQfA#86H(9dPB@$fWFrrF2zc6fXF)L45a2(?Ag+_y7EqRI_ssrsk< zCqrn4H6(8Bcp(Bw4M7dn_2rciH3R4L**w0gs%mbh7i+F6XsLgQgn3zCUr#0*q+gbd zZM-o)Tz0I`k4`8flcI%pORm3ZoSsHFAdyH12NIfhQ@){n?mF-ajBqlvpNi{bNX*P0 zXju2z&y+&?i(TKP&r!dzcT0Ya;n$4G0@fTjs+BeP=j-cRb93-W{AZ3}UR8BGlWuB% zsf$JV+W!goI53Bd{9TFX=fjJTEhPz0h{;(rHD}ct(P!@6!tO60ru#iH9)jXY0RnjU z*RRk0=q=YZ(l1Ati`o$L_m`6=s>nfv&@eY`zq&}@$g-7b+-y~@uqR#UCxQ4KWK^?~ zvBISwf6`ApSw<1NsuK8RabxG?JT-LWBM3KYj{Uv8|FoH)tkbNFRjN=JR-qso6Bt-Q zVCEdg<0pS`Y@TMEC=MNclky>muTB8(se*P&pPph8sbZa#@yP5;cw77DeS-;4)#_$aWz-`_xk|#O5D}Gl z@oRmP5lQC-30zk70m8e^S)wj=C$$pW5fB*FG=OdF+9HWYqmft;Qdw+v34xHG(!t?! z6LQGZ(xWlW))!qQK4rZOdSMWO0_%V+L_2w)MW%@l+A93Bv7@v{;G(Z{; zCwV)}$LHwi>Q3gLeTJ6niQ=5E_T9DN6-be9J39G#%)Xy`*SIvhxp|0D%{q_b_1Xt5 z$%}5XvxVkt4BG+9Ck{s~?t7JUI6mj);u58r(?toGip|D+eojEocMVw?cTH3(Bvb(f zgIk&q=*#^hF*39OnXuR`Vfa)^MmnS(bL`ZUch2b>?Ep2q=H;SUpf*0+{6Z*GX{;=3 z@h*mU-}z#wXhrlQbNOpKgEf{i7G6J_?hH(`MIOUh5F0J-1=$e?Squf+*k?#xge&)tzKYZQ7W$ele46W#P!$eGLI-9AG z1LqUD!<-b$YT-hU4CT>>s7LH3I-_6F@Gk>WRYgT$z{n>rt&Gcu2E#t|?blbygSYjJ zRUcn>=lvJi4Wvr@)a1rUBb2A7a^6QKrz0DINRK>Ro9d3NDPVJ^=9O+yBS+GM{2yEH zc8nBWD7I%41STqZEEdQJ^tne>@;bd1hS2Qb^_6c&W>=Ir5#eZk#r0wEh|(A@C8sMH z`RVQzmd45np^hHRTw0DYWwFp)_*`9wJL=~^<3z@ESGkUc552J_kw{IS{DF<3Q0jSC ztgVgLtM1=sBDZtfR7>|HOAoQ9{e7a_S0{O+TN*W5i01vqG>cg(YJ}NwSbyoLV|u{OD|`7D zhu6b?F|cd!ew>u)(fZN@gDPLBm_%{ zX5bGtxc%TF(MYWP4Q=j7GqTV!wX*IeU+<2)U5vxw215)c=^#~1`$f;g*H^WD@1Q37 zRkGX9FvV;~V0WyE=in+XYYlb`^<#%;h2|D~xcVT~z3=VY^FB=(Pl?I?#LB*@Ygl)6 z%F0}Kp{?h^S88)6DqG_~y7jAAZOMAuijEZCRCP9fArmx7oIY|ZwsH6ipI%*@sq1qz zBRjNvLh$;Y_o#o5Cfm1%Uwzf>e&~KPnc@r%t)JL9iP(#y@Jl012yY| zQlx*kI-Y*n+&z4v>!o?#s* zPO;o!i`($n;=qQ_WW=tHF%wg8htO5p%B!lXk`KPJW%McdosZ@#Xg|zW9^B~Q2oDyn zEGcN;d>2tVL?HYmEqw=+{P%dv(@RxEGZh>HBkaKG<#s+dIaEv_WJS&kg^kA+=!|GS zD_K4vCu|%SN~8@oU9mo~J2Bssz8N^T_+FYAzR&U$_PiXb{}An4e}xAE0Wbn^%ZtXA z$Czvp7gNnC5h?F?j^c~>?ZEKdE{r}npPTPpZMbxIj_dp^B_t%o(wI(NDgG0+LFa|E zp6ia`CfWIUHrAYNI9u!EUT0O}x4;dVtja}2y4AB@9FEtgUJ!d&Wlar@L!)Y=!G=tE zVf!r0)-vK`Udl&=q3?DUVX=2Ht&T6ObM0D+VbWruipJP>S$l^VhpS z)ZaNz?S3Vz9j*J-1g3bJDN%YwIrKk0!3GL^;iAXLe#2!G2{Y^c7i8gGHy)uW40itW z&)ohshlC+OD&{`Qi1Ba=8D|$vJCxB!NKah)SrEwNpHJ z*5y*G9)l#{i0aT$H_{koQFoKvba5bm4JsLzdor{{7M0h{lb@p`7QNQNT`uAAy6>oR zk|0!N=I`<|P+Bwsg;X=)V{nwe|L|2$7w>Y+IAtleB3g3T?aLrTR6MA_I2AJ6&sagN zr+n@fjUe4WHW!C$4 zyihK^abN@XyJk&O*6&K|P3-O!1vmV#1G~q?d%A9cqNRtz)d__(OyI~Hiq(zix#MgK ze;MVeE&BR})5I1`JB9ml<}v)^Y`M~XfbZ}_$t|BgeQIGMu`CL&e}qBlUHKAKa32V# zCs$cXB!tO;DAiW6^iifshKj4p8egjQb&g))C#gr{3Cv`g)OZQOFq}0jrUxSK+>~Ir zo3AJx78F!jhBk0OPl^xlmNu>6GFUHcwMX8pTMSj42V{?QEK9^BC3l-&9umxhd?&?{ zUagX+PpyC#{*jj7H8E3$a(;s5g02EcOT05a`P*ULvF_H^Aj&;ZM{rpZJ5vSA7>+r0 zc~?x*Uri3E0b#Bv9NbORd3`EhAoSO%TQCuM58K|r2((dy#1o21ZRJ;y$n?vHvRAq( z@?n@5DUFuBcKF$zf{+MuzIQpCd9CQ=@{irnHf-Fc^Gd~ zHOwp<6n7N`>tz-<<+kkAQBdkQYNU3K$80QrFJY1=C9`;Z-`$NX*E?f3!s(H6SVfg# zN&79V-YjBoj>_+g5XDd7f+hH;)kUVqY zZHmj9NhxFcGEG7B2k9P-|B)UF2ND2%)ixXKXqhU=z8aMriq3@v#Cw;}92Qz& zx4i2!-JCOtG}sE{H}HM}KbgwIKbr*|(Cj!gWvw726bx1){V%5failX}Cj| z7Py3sEWU&fX|;nOu2xPhK{u^$wy*Nm#VPLH3mjgbT8Ll%D^nro8u-VUNm9D}YTIVA{8U^YgklG&30%dJg6QU%f zUi8>}>&$PUtTM4q9lfs6Qhq~UF;ug#wfw5rL3JUCjd34Pd2@wWy%}g8+_gJQ9D6H&co>;OAJ-ACJkxlYt51Z%$fkJ8R{A1m&$D z8pWEB!UU9UxndBZ(NP?-ZM#MHg#pBf)h(Qw{0B@Ea#p|(`BmsGNTYvS@_|KrPqmn0X;08M)Udlw3{orcs zM`17q2J|t)NMdMdkaaicYG~{z#`H+ErR3!tCLU8AOsMJqXc%^M@?mTEdpdEG9g9p@ z0 zi}%X_y_k0Vo$xFrMMYWpueY3EN)`T;)QrS}hko>vrfxtyFG|N?CaLbA8v?p<;-X-Z zcrzm(xv$;_(QC9g%yWF{?~1`-PQtEfq(A7E{(TfAI`Hn9l|a^FKnlz9r3u`@< zR$4s%wy^c*w50JS+#WTLSV^(my+2Ren`_m15bY1u}9p zxw`r!?y}OTiiBdUIDU2B9XDJU>2kLJ>XwiyZDm|Y6&B71miA=x3uO?yPe0fwYqwFy zG=XrI)l|3ipJ};m%>V(|qTl+AVhFxLzhPde$GGaIS9|BD+_+Xj4XQTD=s0S9)fNbp)xT#s4 z_N5?0qcGd~Q3Q#nJyqLdBT%dzYHB^BM@e{JI75v?__bMJvwfm_V93p5!^Hbs@a zf8iLZ;F<|q?L_a4Mwil=0@$m=;#hCp&Ec` z(Lo9`?ON$C5!iQWp~##bCDE)B3cgqtx9O3rtZcM^CN8Kz790s$%zN}(-PY;r7H2n< z50cTLfq{WKdz@+mM)T5~>=&Lj9Ljr>Q&f&8T&)cNP3N0-zZUM0vC{=@__mYjmqEX< zdE^$o8g#(@GVbook+UyVTb7I ztthj_&z+t9HrQggl6F^lEvvtG$Z@jOZ^HHil_|B(U!!LD&uVjnAQxofh2W z0?o`Smfm!Ff#y2em%2r}3Rsp0xNRlD$5?mJyY9QXs41r@#ccJMB$h-N6z=ht+>G1Q zFbDcNwj&4fDgzeA{rG)Gh95#qrxT7M$LZSap{Q%Y!ByR#WuI(}J^#DrjDU?1`IA7a zj?+YiNA}Ktjy>O{8cHw(m58_w2z{tlQ2BMAbj)E;+H21A2=fw!nNqCUxW&tvL!ys0 zPJ&TPN?uAqO1nUv!eFuo?8!m=$w5zNqCk@!`I^47jt&xd4ko8l(a;GXlN}Awhj zW$`zqr70?fi*puUNKi*I2KuQrPA!(L(R*xamEt^M*o%@fizXwQ1=&(QR(Ny1UQCh% zgoy$-fJu9t-2%|RHPG=|XYD^Q7(9QQ695n)z}VxF7_f~!{|yF)5a(48@!!xbgrq{y zEy#<%fv{r0_MMs-9|xi08&V1Z*lW-@1WFej`)4m~9~Od=*O1j@F`oV18NS0Tp+(Rc z#4UdSm9C+xyTkxFgvT2(OFdyRzd>L%gyKU#@z2CCFa%qdo)vBe+d|-K{4cM-ZXj>^ zhQ^wTA=iI{#}ID23eg6O(f`A1yMagnMg*YmAs`P^=b}~}E`#-sb&T`;|82%Uc*31@e_QvEhX0evZN(`1J4lSHvV*h>oftAO$kbPd z1RyJ4H36^ffd1N|yDDJo)d7oD{}GR699y#+BVPr8%714_tF{Xv`1pVOA~bOTky+Iv zwC(>b?f=ym$HaPFomzlS3Hjoxyu@QB|K*GSZRx914v@wn4U9eC14MmDJP)C{#_Iol zhX0Q%LaGB-50J+N@j%TgV=W*Qtjh1-MAya@)+S4K6pK%K_}|_oR=QoMv3T|c8jJbP z5d*~oDUfY_H~aG4))41we)!$c2jl?X&%QuQp{Ed+tE|#}KqR^M3p54-`Qc+pAh0!6 z6qSnIVKvx6Aq<*j`EGavioG_!1n&MelmwZmCM1_L;+Y_b`_*U&88@WH)!~=_%|i2` zFQK!3H~RtkBsBZtY78uLbr@%rvUsBAyOE@|5tUMr(j&hN3!`7C9$?@LcFD->9O10X G?f(Z{O#koz literal 0 HcmV?d00001 diff --git a/python/gym_jiminy/unit_py/data/cassie_standing_meshcat_4.png b/python/gym_jiminy/unit_py/data/cassie_standing_meshcat_4.png new file mode 100644 index 0000000000000000000000000000000000000000..eb7ed383f6c24062e116e045000f58e2ac56073a GIT binary patch literal 9975 zcmbulX&~EK8#bPzn6~IjON-9*(W zz3CF9rRi0~wqWZ5^<95Laj$F&dI#`1gmaWXtuH>mmQiPr*F5ox+4UHO`?k(RL79BN z#5VU=R8j&ijvhUzy|fg6#0~=aTho!6F9?S~-aq^kDNq9cpD%2ZfXF(#t4l$C{ZBLa zSp0GiME!x(Mu@trCiv;&m!gNh{PN&LAq3LA&lL=FKIk9{P3_^IrHb&y}qR6!uWq)ULGK71*D$gg@ASnz;IfU0Wp*r$DzzZLDq zS~Q%!`9Jq%D$v zEgnb-qD_C6@72D~y8i!hfvmIFWIlSwrL&U2+G30PD*s#bilcH6$V0J$)<~5q3oQJ_ z%&q}eV1keC@f+Ck$q$B`)%us=z9@8;&W29{D~KHJi!)W*WXP_AeEZ@3uM9u#v(Eg- z@g?q@wx=-{zV6DM*#;t{X$hr)OBswCn;55bRA|xeFEktThx1}WCtr3i;}}ach}rj= zM2SZFxm2nQeNkxay+R457N=E1S@&?kIeu>8)5f?)vQB3!W2J+wuzC@`#EguC-EgcX zE||xTH4r761W*kek+-@`^V?fn1I6H#XlRhg{a6_DeX zQWR6E++{}mQDirXU{!_SM_+Y9@cT&NDy9LmWf&+`fm3PV7mDIJzwJSkX0z_e3pf;}jR%!nNp zSNW;t46Jmn@i&TBx==6K#@>rAO4?qY8ICYvO^~m-{Urn|p+n83o|aS)v7{f5@TKn_ z68?Nmrpr;cPB-Hzq6-WCO5c{EE+5&M8!>XM zkok(itEi|zp)!c|QP)U1nDEA{0^G{*elwoZ5@E20-Th`LuSeRrX}tHKOz*8~L%e;% z(d-!3(9mBKg2a~L9D2-HvUcoMH&{jG92r6A_p7}e*)=>o9APp@rK)U_-rtT+b0F!+ z;2M{h@r(NALXx@ZZh6eq^z<|z$I^mB53_5PRA-Ponzrtp{&fd=^x2AwS~Snw%UWM4 zsWrw7o;_aXEo8bh&Zd(OUUu8N!{Uj1P?6(0C!@G)SB_=PMG-=d^-OkGpqNzp$~=5^ z9v)82VJt)}GW{zX7D{k~5&Pn8Z|uk(xM|61=csMBgzL7kqLNxK805Nq2R5B@16!PoeFwHUF`f4?`qWM-jZjPYOnY^a8WTZ!38{jT0_L zm^8*L0FBXZz2Ri*V>McqjaRR(K95}XgP0$LZjngoQO|MF9E)ru5{U$Ysd93Abwivm zDxYCzGV^zdxp0!=70u=$S7Tt5gk~Q&9KP_JjA!QS_EbM+{#jKVz{C#@4#v&*`FqFA zw9dMcbXtb1jDw%Jrup^RVQg09Am+cc?`x7!ErkYBu~gC}qdY~6s|9|{K7XXJ)qA||Dk*kf&tkJF{`UIZRAe*HEFD`DJfoplm-Ah+Ym4NFS}#cA1EO@zw& zux4_#AtBnH!jCcIDpTu6ODU{$KQ||`X`4>@;e`s-sp=;t-TPC|;2h7pT;oR(Dq9Gw z7_M>^-mqxJ?;8_`4bY)2?bSg#$_T!Vi_4*e$A|xBRu@(F8W|Wo1x}z(V}`m!GxE3X z!tX1GcCgv(v1tM4T?eoJwE`{ZP3wi>b(e?kzZsP`Icedwd-v{8Q&(8k+jO(CFD2QJh$8ako8~YDp|Q{`j4)s@#{<;dDtxvM9g2lx@6vc2~#usHy!9nq6CF1 zCQbSrdH=ts!WcX>Ajq!gf=zZ-PK;5+s){B z9E50_m<)B#9LeFQ6J|EfY7N~7H-nU(Z`Ejfgs$9|Dt46kn7wQQugkcj|M8M;H?I;$ zh*{pIq_+C^Xm~tACwmZ&2-M4wOKc$@cj+h}+b6&Vcg&=-w1u8llhe}_ZO^BFI53$Q zFEpj%c-V{+EGQNeQMxMx+2$=dU05{NZJ<6-g^w+a7@_v@?67Dv-2r+q*3rs|j7Gig z>FEhIVmoiDuPg=Whh&ZHz04_ot4f$h_FwbN@FUWZ-Hc%B;lT23*}eoX^b6hs8$OSY z8Kq9v>Cp#E2Y2%K)9@Ds2E$Of!=6v4hZRhD!|y_@!bvf!^B3mYM`uS!+H_@;@cq$2 zE%D|}3Z~yEZiXL;_xF#!c65&knTkeTHl_ws^~Or|Ipmf=kcf_DK^4LAR_RTQbB!PM zc~a{VbVvgS7dEe&BD>Mv#6hm0{+o@7iHRlkw0gzLN-GfWS)9fh72@ph>SDaXPLosz z<&~!;rd+F^}m8k_B+_lN6-$lV`liJ z9$HA};XT?saIS@h)?hHHetkiDIo%Auu(UTtX%6rhza`)J;fl;v`q=YV7WSb_I@;&~ zr*m!@>3&&zIueaN^v!A?3~-XskJ9o7U-nbkJw487inS)2!?D#!MWL`bTqeRJpP|G+ zr(m&nt8F(x)!j?m_Q+DshGS0XF?DgpB#=JBWHPD86{Hh5FCt95po;o;_2~gwL#!sCsA*DI!JI0wIy*YdQg!V)_)-q?oV_n>>-lWaH;&hd3gN>>6kqfh9HPt`Z zT9xK#XWwh{MmSMggW~yYLYw&lA@m?o5R6KCI%`6ld&h82feUiR-rnK-DA2ip`_>Q% z^2N&-Gdt0hVz<2TyKNEMtww&o={n+dJy_?lyaE-=mcHAApvf&vN#DxHk_XPPi@31i z=OrQW3*KX-$!osNBwuBa?5J4%#$)*T0cPw}5epNd`u8{BI4~THt#dnXHF*-t(38Cd z>Y8Tk5?eh-+w#bxk*LQW7>|1{6Ya&ER^P(b-;xI2TgS6zl;Vp6TioY;5kxZKaL4T8 zMsxmagXr#nTuv}DFkjf|dupE1Z8e`@YC-<=Oi}WL^Wmc5#BkQHyW1#$fXoenX*f$oKDu zp4GnQGn~0T#;uQnb+q^O~%+Y z-79su6yeiFJc%n%P*CWYq_*^h#?QTB!uii#4#&3~irQ^Q2F=sHW(t& zeCMW1n7*zqTmAgY{Nk4&d zxEH0NPzR$X6Sx+c$kkV4409f5eK$UkjEF60V&1t_l%8J|+VJqaRlqsuz&AHREvN44 zNaxo2qa)UclQL27&)8x}Z_qdS0owwFYkw)dE>W{n# z6EwP<1g8`@VYRI*`+~v>PFyxa3ZA>YB|O%elvhx2Q3zY#LoRcvS1QBQ4j#gQ#Xxl_ zQoZ36KO-qLGC#b9L~56sC-drW7T6=>y6fCvEHZc_(|4eL|AzXpj`L;&K`lh}+%AiR<1!@!AzhNPVU?9)p0x2d(S&T z6JdyrVYEAj9cdGP|Kc;X^*k&nI5;@3(I=llYxx~^ksO?ZS>9lel>f&bgajxBykOdv zd!g24V^#yqD9`XK{c{CogkUPLTPW&x4|!#F@f8FzdSF`^chA;!s-O;+4RcLLz`|jp zu=SWtPww5bl|MCs%59P=3&;l7U{V`=u%V7Xpy}AaC}l;1R}6BYcOToyOn@u=J8+eo zigbVO;+7#NodC}Mt1FX?71dr-G)c#XNJpbkf!DkhUEMNlK3lH1aw9WT6XObP*C=l+MMVc`^{#j6@H|v;@2Xm|*+a3;3vd; z(XQ5c9GD98G=up#IXO`fZwWvDqc)JApBqTt;c7zy_jdBwix5_RLTKklRfD_w&tWyd z6ZKcDHNiU>W54C)*KgzOGB%wq5k&3x7_a9Z$vSIm{?cIQrupoRpY-TeN2Nc3t&qXu)C|l zE#z!f#t$WT3k+z}VDWpi9i{i`lwwCJCW|UkBVA#X=;&xzVzhf!L%Ltzd8b#@#!gwW z?+kH_U`^zRACzNDi}ZtWSI^MObG;m}@o#w{b!#oUa2E z8{0r1Dy4lNEGTTn6VzLecb25hD<$=HVMvoi)*XBPk`P>Mhv~;bghZ5vN2qzR+`?hRU!=L1LU$o5#B1v~gaf zRVfM;qA>p0&ePeYBV|HS%F&8$WYkqQ8yK*ge-k43iw`ld12sb>tFWcz@kVDSg3+%J zNlxw%6<_U>J|ujb0N*7jEG#TIa7l)Hik7696sf#B6VkXX=IlVrY_=X_c`#5^= zdjC`WguS<#kK9)1P@)xeW3>8~p_}O|ZZbg*RI1t4N4sH%PaEG96krEzdDJq?yRZKv zF!D(SmM|aNRN-3qh;Eu5&5im_>!G4eZ?0U^MYkZUvBB6 ztd33IWlsmLdK=aoF7|etj`G*_fF77$35vOG8_LLrbS}xFNv4KvDnNj;6ZLJSLY3-z z#VW&*HnXs(eNHe60;W(IGdGN;6K8{PiF4oB)R{IYUx~e;oBmRHZWH6P*z$>#(M1T?@|X2N z?)KYN-lzE?)>lP>J}X`3B>a>>_V0m-8=asO$?3_Ua)pNP7x!z_DHU2bt3j?PiW|M+ zA$p4hqcoTr6(x8@lAmpUw6i!()qBcpmMb;=PrD^QLZ_*}t6|HYiwWH+=Jy>I`-IwN zUU^QwhvunR;h^KKln{BTrX|p{$(g_5F{a~>yas=+3sLbuD|y9iF!f+w$*1|D*ri|Ok)IimSmFT7^)?}9YdaNw>NI()W{7=5{id7aCT<}Jh+><*)F1xu0n zISK279;iZnF#Tx`C=}|zU1&!?Hm~#2NRm?bQ>8=K%b>Mty|tkCUrchjgmJ?iGsx)# zM+fHmM)eG-jRrEo)E)%P33xrL(Vrj{$X^Ux?+Zz}F=#g=bAmFO@6g`;p#Cq)hWBwk ze;QZW2Ol*%TKq7WS{^_Ad-Us>2X}OzS0k5!Q+DTbAe-B_?yU;wnr_-Yw&hVvdq37umF|7i7usWL=n6tH zLlhAty-pc=Qvb+_!eDCLbe0V)oWkWtMdrIWk+E!bYYw7G<4n~Jxj=tR`2GrQ#iYvN z*tf=YkzJ+w6*)ddE3K2E{ppUFz9||@-)80~`mUjs164Ob`^luE8egfDWMNeb?En@G z>#JGZKD>+44dg+g>Q{V@yx?I@8$(WM1zX4louh^&B{(@1r7a5M+%h8bkyo{&x*yz8 zFgj}OhBLLr{baCPo{H_nyduc=0u%(;f|}t6x~yXzne`Rd`mvqW{O3B4Fx$#e?xCf< zpfWV#^u4(OoI@krr||AOT0G@YTXL9oLG}Qz;pYta84z;C0eCCUL!xmL9z?`xKeu+C z2?1AtVlDozF;}~-+{E0ia?IcM==xtl$5#&($vt)FaHG-nP0m&4U6@{7=HS;<$K=&>mscSK87FC5RM&XmRfaqCYlMgzV;m2Dr*@GU~ zVR4@kNv+&sryF?NRTu|1L;APjS(MJG8Kv(x+i-wm1lJP?Feh>~B)jtx;rRYK{oqWW z+Rj}z91DBVMI)I_yFVFfG_Ah3%55vurx1ne@9$@7=sp6LFJ&3=59bQI_*=`ACz8*=~b6s|3)u3bCFnIN)Fy zRw$_lfRrG2IHRS^Jj8Tn#GhDVYFpLwI4vsEta{Pc5FcYsmsh`G2u{R^KPq`kSv7x^ z?J@9vt^-jK3{q{WmG%mIQUZSTPY(D|(~rA{*4V>e2jGw!YMa4{E0cIsuIYJRBRe

S@3)NYzoZoTQMw@Xne55T^U-`qbDzaO+HNRr9rteK59DCcVY;uT<=* zs_=AD<<;4j+q&X5itgFdk{8%gWtz`ndOeLe>K-3o0Mg`O7hS!n*aaW|WI?&SGQsXL z)Mo^I&j`+||0#O6cdfwN*R;t4qI&N?Cx>DN==*LTZg%&z%e$9;Vkib6GeMq9`R5<9 z25$Fb*;~u8rqTaQybP=qDBk#9w1Z~hltskJetLThl{o_Szo0{ccQ zWj4NV6ld}t&+%w(v~%_V*D}NJjImRBM4aIbh$tve{O*(%kzDqx|R;u0Su?`6DXDmY%e7royG z<8aT$i^xUGPG7N7(3VsQdxs@eKA~NY9!MZ#S$Vl{t_h!HaqC{MQ_&F1slWlRruyXV zJl3yFf&_U+&YMfGJ6Wr#YXeG?(}ngR`4&5L&Lm;%Y&DBeC@~BYXmJtp&;;ZKcPbVa zLSwqq(4KY2JXxj!!FJH?AFlAg-7T09ER9$P#!7sz4w7L&YQjr@GhUX8e#l#d@V0i$ z!+`$U>>b${yUBB++C*FUM!Pn6X%V#Emy6H}s>>Y`Ccip_$OpNS-m0y>Zwn6#0|y8S z^`cr=Pifif7sU8W8!DJ-G}R9DI(N%^ue_nqCT_@qqBy8ORL>ZscV0t4c1uf3G3>h> zJdz;G!W$YHUC!3%07Z&kUZ?7cD93|PE)I^L@UN~Ykxn=Pqh!WSp`FKW_i6|RZ>x-v z%&9C;S$omqgWkVIUKfP{xUB>fws~Ei#U5}#Qv$VP%3T?zg1%9+xOZ(G)d%kubYY*{ zEx(tHce)|t`_F+Z@`r;nBL_k2f#ZBjaQtVTqPe+ra7%)AL# zI55Q*?D}k~@h4O?;q+%5TvG(Cett6s`6DLY7#g@~_0+ zq`dy)a?TneC|%5r_#l(`N%8PSy1s)l|FV6bJD?|gHnvU%Xt^ zF%i?@!&J`gXvjzJ6npy@mvF|4Fn(|-fE@|Pj&Gn1iwJ0Mm3EoN4HMQ7|;5zm&shI8S(0j%`@jT5mWuG!WX{Z<{@MS^^!$9&*<)SUdZWck@Ij)-xR|Ce$9 zJN-q(arwY}5z|UQLHCi|0TTEp7KDhfvW8Z2Rz#KfprAGE7r$y3u-+Quf{N`EPlW$e z^1o~A|2pf}oSHSPAF+nk#Kiw}jDX_-gn!iM7ruCl_|yN6HBraW`Mv&LuSUM}`JU}_&iS0rd7lSATbgg% zvTq9v2HST2+*unKOyUO^Ox%96IC!#q*KQRU?9jydvu7?vWHE+ae!EUf-Lu5HJnV$z zhev(N8r%BrfL_Xde2S{=!?AZ}uMgiy%*!#?kxd+0XPF#;#&(3z3U1k#b+c=N1` zbaHRXGVjbDWdBedE=tjOKPH&Kb{Dqe-hT&+)6K3F04a%H3q6{id<7UR_W8t{W3V`T zi@Pvb)51^pHo~@BY?6ZgV2^oG4uhFYYJV>dduwxW18lQ$qd07Hfh73u>B((iamr>f z*d4v2lHl)V4EXO6uyVsrS25Uz%17J5L*;N-oa?4U@Q@r2s9uzjg`HO4s|p@Q0m^qK zH`PjfOL?l?6MS{b9#Gz&DgoR6;h8n~8xm{lzKwvrh{%VZL`0!Qiy}@M1!92Hjst+Q z7$9F>|2h${PT4AAC3>*_;kHl2FL5&vJq22O$m9!^{YzVcXb-?k_9qD9uwU~b{S^O} z5>kphA^q2)b)_Krp>L9#fKAXkHbCm<*F{>Q^ee=!oYn@+ShiF0{lZhfu--vfH zNb-w)q4hzTzzcu*>Uxl?eSj=cySxSV0~(Sv@>C)4lW*JtFY?`WDIqt5cJ%AssRlsK z50Qne0Qvs9hbq@fC^?(ssQX81V?%iDDcz%=RctMI@Re9BwYYe7A%_P-EDQ#>T{1v% zqeU_H7{5|Y)T+N_M5>@0IH0vuPBCe4w!D1K3g=y{R$KxpmWGmZ6;nE39zL?x+AkB4|OeOHlH0DVg1{ ztlD7gnp1Xsrh$P$wq;@2Oi_`3cE=6(z?z1Jc5ejgMto^q8TiI}S0{fE<>4UFk+oGe za0$>5x;bsTk>PPF{eu8W@zSD9jL<^{KaVc9crT~A50p}QIdTiBV#dEIs*e6PysScW zLQl4}wKX-J;dR^~_-IG?xTLucu((N&9~T(0DtX(&{bR6f;Tq!+Y6j1HWU1AWn@qGi!ZI|I-1|JioT)l$q z{gdfrE}RWm^L1In*;?Q^J{EK{{_HEEKa2~~sr_Bf4c6xg^9#wP+1c4q>U*|s>L*6u z3gMP|hxD3VV&wSEEjIG121;75pV}{NXJ^M6#`kL?FtiqZTv5^B$cSJ*T)0AmgMa9X z0`D>e0f#&>tROQ|b|PniwPm#2b9B*pgct9@eSfdmE`VyRN~$>gkQK|t_hVgh5Vi`L z#E%3F!!M_y!aJivH9S0AGX6{<**mY;ZTI^wuf^3FY&d?{WhK>x9-?U%N-NIT)YIz%(pQA`3#&eUNpDkmf)_(jg! zlAJneI6@t}O*}Sy)a^pt#;mg8;bH9`Ls=sYxrbNzu^3vXA^Tz>xi)Chf=I>AtY%Wz zJRHz7GcydcH(?8JZmdq<9w^aPOEvx^japflY;SMxj9sdX#K@U;N%u{2c?j|>g8`)5 zwkdwVIRRF92Jom0rgfyc>!n{T82?A%kAnu;9dd-Zd$b&sK%Tn%e(_1uk}cVHu)ift zs(?o?t1Dw0VrS|UPDe2YtAX-XYMF|ql{Gcmd$ux%YJ+)8jH_7Bc?rwZliM2UgD%nT z75wb5A`@(XreYMi;ML$tZhx6`^mw8Ua~O{!5D3-Pp{n7j#=2f1KKgSFew0Bf6~*bb z)6vl(Rm^gt7?#2~7$0tR-_|cn6Y)!X-v$boRB`c0V#|2z^={>= z1;ZR;cSA*-ovdk>W;@B3JEHr^eh4l$H$VPz|L88irN@Kb$jU-3t7-;~k>2j^ps1jL zP)^pKjv5^*rdA`wa{?gO258mD(97)ccV*qpZ;ucO%^eD6HE>5X!+XtK z$v@^&hLDBifsqkQi3>`B4};=|Zrg^AeIF~uBeXIAeZvc*STulvIQnF}Z z$IGwK{KMIL>8r3ioVQ6`zw5+Vgx~FNi z5KqIXF-I&(-u0Zkuvrrfjd9~%ldC#K8h(5Qf!iW$`Z|g}33R4dHP|aUe{zYQvNOj>&reyfg(=VhkXJ=<8 zB4Yys0|jd(nP2jTs-1zzt81Uq_5^)g{6+d!Bd+>U&C3qhHnb#0GK(bMU-X_!URTeOosH?_>P& zF|2d6D)QJc^*ddo2v8<)(olYN8Htb0A|135m35*YeZ?e7Sef>zOvNtttYfm$Zo4`i zqLZ`JpG4`olh4j>e(U7ToLuR?a3P#pk2-3}izRCU$0zIMg3so$z`Um~7Wgv$u~VeX zoRx(oCBd)OBr%??Z9UUDUyj$>UYHGNf_GtIw z;$puS)s>Z%dV6Nmh9@UeHI3U*QloB1(i+9z9LJVicc=uJ3|ML)^0{Acz^a>p{(=0E z6Y$@49MTEI#cnxV?Aq#L*3`gI?`-LyL3myEKa&ZRSO?T z$>9VYzN^!rtc<>Gc9_~4!9q@sb4%O&^2B+ANDQupe%+x4htp|y%TMX6Fe3f1-7?u= z`z$+6h10~F|5^TRmE^__f=2I>X=yn3aj9wNS~{_N<-IV9n?wYq0VpqQEGkc4#?T@$ zaQ*;wXzfE<(yh5lxyMCC2GKXs?&I^iqzaJpFpZUf3*XoQojt>3Tq?FE8lUct-+0$R zUw_|kNjR-)hx}L~-09$rYP}*k$?zJ|WcGAnWQml^GD(i`|&X!O9oFDQzhL$gt<5)NMD%_tquoYXFQc+FQ zhVv&=6raSf>Ulx}ZsEq#n|BNbBiU;2)=hrD_7ULxCEV#Nfoj8#; z`BLY2x4T0Pk(grxN?ExRSN7zb^RITMp@w*C2}709OpATsppP$fS#>Dx>zk6ZwQzC@ zKq5m=#cM(5BzcBYp^6V2dowFp?DNZ^4Wf(E8~4h8B9KV({lb*)R@jUnysmFBI6ii+<0w~||b%EzQ`7v1ePFMVj6m|{vwhhi9bpWq9X zk9!wPs(Kq8DqY6yiEEBEa)OOTO5d90lOFUHH`#|~`2ut{pOcKSsRD;aP$^JC%pJ&!T^ z+M;5R9ugKFo`=D>J0OuNIsc+X8uW)m-8s-m?~32}Hq)=pC*e-bgWRiFZeR8Q^QChB z?0_d1oG`h4;G8te>q;6i7>VcjILn#7uBr+N418j&`?$Ew7qC1g+#8%KrbzDY?v7?P z=L{gTFvvo6!_qb^xk~7McU)Gg*8Zv z`0lnnxVqj!A{$=6e%;V8Ecj&U>GfkNFprsIWj_ujUoESEU#aRUBGaoREkSWh=Xz%p z^v?`ume_-Lr(DIF{FSCiNG&7(+HLNG?@MwITW+!sb;don`c;)wAgPBN)Lxp87^-nn zh+cgiB6oT-@SpOW*%!XqC5h>Ss@4X1&lMITPuYRHkgN854Qgr&V32rmpIrDstSF9kNTc0)$awP zD!ll_KMILA+0SmBMEp{%){z6xu>tk4rGZ#=_N~>9iHh5Cr**-mM)Y$#v6qn+^tBZ> zg+dWkRL?Z_Y_+@^6BzhO=GLkKt3G(4-LNxYq+XBzx__qnfa~S{M3~gC1>j(k=bpIX zywYY<9a_6$nUQV~3W{W~M~XN@B(k3s%OQ@e=?F!my z)MzE(_KkByXqf+51x_0fo#F}Bhy=A4DT5N*Z7uv-_``st*sgZ}q|Ti9Ndh z@o?ZKkEHAVg$Ii?6HLzXThZGpH!&IR3^^8eab2DMytgfojaqV)b;pg zc$r4I`T%NCM=)AKl$bji#LnR}8l0%`r704Mj@lga)<8H%)a}zEuF%y22n+JJlVHn% zk$Gxj_RmIa)e3fs_0iSNCRR(^{{+f-ImGBk$+(vBa!=RSioZ`~MTwJ?O#v zzsJlZ+fs#8abEa2!jK7{=d*)5l@t^}0o)fl%;JI{$@r%0r&=pY_8GF@;v}EiEm4HS zIvuM9Fr^=xxg$e@DdB4zuAdPGIw2TZeADP>EZ^>S(x=r1Vic&|n_PcJPcAGhfS4Y` z2`G&`3B#20%tn*rez8F~ieGfOezqm}o!j}{6(=YBE#6X~?gw_vsr1DtBPoS)*IM?E zaY^7Zn<`PR8}_|8(aS?;hjBSgVYEtQ&OUm*Fel7!ap%>8F)*;){*--VE<&P&+}9BY zvo|!Kgn3$tOD%8AYgN#dSDJnN@XD{s$*rx08%LBzp9AB0()sEawn#0%%axR}ue!%^ z70cR9+F$NvYMnLBPxOFwT=737-f?Btm!G+Eo4i5J8LTsN4 zY*{xd>pNsp>fKXYq;z)zF_pyFs%JSvc8Ykexbi!Rm?wHbE`^z2eKI`C?!8=T6dFxy zD9zZylFrM>2BHUDeOU>@#j&(LZnC{nZ0|^7GFQ@GT_ioWmhZ^!PE!nOkJ))Xi6BH7 zjaDXcN@0qbpmj8>pr}}86ms0hL$LPI$;h<>eNWn6^^l2cS6hYQQU|ylHN&-C9+dab zC`1=hS)IIg*d4DRN!ik(B5;M-muO=#?2kMAuxeqpxj)yVY*(KvJ0Ap)i;D}DO6?R~ zxH~9kWwx7xsI96iOK84^p=Dcoo=Sg_vdX@Dnz2zv!MFDXIK1-m@^TLsaB&~Sq(HOP zzADVq#kn7eoLu3JKUN~hRwJ0fo!k6aR7M!BPVe;LwGiZf3<*63ypRvwif+1KOLASK5fX`nE|KpTdpkf-jl<7g+ly1H7GG-X_<;S9d{7sRECCnZ zWooe1#7-9YV|-nlX~rdRFdoU*uFJTnj3D_GWW=+`Z|L3eBCHcAIw1 zaO#RXjnNo1q;w!{rV8+ktIR2e!NCGhJ}_ArSSmv-K`h>xoFg-2T3p z1f%c@eIXW}-t>Az}st#-$2zxT0W&sD!PI!e2i7OJ1^J=7<# zM}s@4{Y;!Gz*uCDvZg@xP@u@@c5d%l6{IQ^Q5cF8^<}|FYry-o#oa~|QG4}tWg;zD z>v`wE#8qTcpj8}s?B#yf=NSch`uaXh8A4k{32;w&*Y@w-z{yVSA0d|c{-Zo@BWbez zwrSB!6sXe9AkprArU9qY4eMQ=r~ArZvz~}<9s{-GqJaD8exJVUO%Tpfy*-@iX&>i) zKb)E$ojI<(8hz4eSmevjo{#5(0ky&5_NT_UX5w7SO;U4 zLqW$))U$g4PQdwyEM%(abT9&0pVR4*y?8M3c}u=s*1KWaU3o~2T5pU1SN3l{H!=$NDlXRkIDM6pVK*-y|Gr+_@bjd+mtn z>Di=Wwb+|wN$qwAZ?LExr~FEt`Q1Y<+kSYzU$R6zt02Q?(bGqAX?))@LOMpo& z>&+hyE{AoXK~wvlLCx%k+AUgE>TSa&lB#IuyN$UGaP`GlrPcc4k8Z4kmz+1qT0$8q zw#0|(rj$BfwcmA2lqQ(-Vc#3Kp4Stl4S>3IoF;1y`*XXDZQ+hPipik&i`E^z0cjb$ z41fc0Yr_xMm07uc^b%#O{1njJ|4}PS>@Lt%)iH9$)rt7mCgfkWdwqP^;`LBOA6=P=HOHl5>7a5noHB(G#K{$6-Zy$!gnH%H6^)qG}?hbvH zlzoY2z9PG)Y`$&kwE8wjudM^#E008KMJ!SOEW4UM;}+a9WQz?QkE8^GCEN?guwy>7G16zx8v7VABdWYngo$9YdfqgTi0f5%7V6T6}pS)1q1 zerOW!IE|qJPzn(o>Bj6dY};Q#l7Gb@Oox>QHSYZ*R2lY2M==eVIvGibSrlrut97#f zS>P9x6VR$ptq%3He+Z?ni+&>);k(& zNLN?)cfGo-?$vRWbzBQ55Ws4sD~9mp`B={3=T^zg*aZXlGKZL!-*f&*=86mL3|9DO zLn?`DT+4k!91_^-7*AB_v6k+pISb8&f>(SeaV1lL#d(fCa}?5W!nZ%(ngC~4B1^bR z%|x|E`LUl>u3#%tDw5tD5J^s2#a2CxWw|Fj}W8`A+pMEVSjqYm5mR+YLJnY5)MN0$u=b_S*FYZ-yR%;^YfP3&GnEunJYV;GzoKyp9+` zKqmyQg17>R>c_*d)9MfcxsJr4`2Jsu zAly?m;H(IUhA>qKyM*9r2)jh<0MCPMKJ+(Q4587gRwCfj1md-hM5l|OZdI#;BGmOO z%>Ca1@O5j+gEl$rE;yY3niBAk6Cv^-34Iw5fKXV79>nV#dJt#~=|M^sjS;L11Cd={ z1=$rcHUxV^CioV1|Ele}EXn%-@0UYAfI)%vp$5qE>o`9&*l+>`aCj0lNC4Rznl1nr z&@T}GSBbuvTY#nuh8zGy{TjmM*RgR(&95U3>%$OVbzR@ye}mm2c_Hsu=P82jRRb;o zK-%Q%`gH^wLgg3Cw*2q8g>OaxPPG9r{?dth(cprJ&H8K5PD8|1aiYlqG&BzJ)iw8$ zVL(@rEuQ8D{f*xLJw&nYn*YbR#QG2hw40E`-+<$g#Oq!T*(Tl$;Ot))p#cmG+kWBe zmcG{nVfU?<)_{Qe#*x;2S4AZI^M8-U^|*jM1KLN3?Yd3B4mo_a_J58=Y0;3#zsDkE zF9Z_=*m^8Nu>^V7H?at912ht`J|gi|)&DNT{}GDoTlfDzy#a;Hx`RT~AuzC=_9KTy z=Ij1Ct^)Zol#eD8cL7xoe|1(U5H+COBij1%D>1+kzuB6vGb-Q2A>>35>-Dh+Q9Ak0 z!xDd~ny3si2`HL@EulCBBLx2|Ew0aRtS1F1H$iR*%@+mD|DAjPpZSes(E!a}%`IPI z7RrW@o{+Omz1$Bn_J8!yuWGGNDM4>SUa~%LQUE;iE$GI7p$v%vQU3Q)C!SF32W5R7!~g&Q literal 0 HcmV?d00001 diff --git a/python/gym_jiminy/unit_py/data/cassie_standing_panda3d_6.png b/python/gym_jiminy/unit_py/data/cassie_standing_panda3d_6.png new file mode 100644 index 0000000000000000000000000000000000000000..9b0fea73dbd1f89a0a45458ef9c6442348d16371 GIT binary patch literal 7811 zcmds6c{tQ--=E0RAv&i{WQk5aArm2{jL0cDNTO7Bl6^PA*v4{N>?b>A6lcUtW8cPv zgi*Fh80(m9*-eZwm|@=EOy|^dp6hw9>;3z6U4GZxzu$d-@9%cs_xJvMVy|D*-~WTe z4zsc<9^lR8s^7>U&{0E!%eps%pDmL^AKzo753N$D+Ka>F8yCnI zE{MAQ+1u&ZkY5fyE}*z*|N7e1Dj|pWDXj9Si?RoPllt|iJCAt{G9vdyX~{-;-udb2 zf%Zl6et22WmB8O`R}5A(D0YSrUH^n{tgi>$XJG4z6lDY}#8CaXv4=BS-G(vE)P1~= zV^!;8#0dJlj+;ed5L5cuJfL@z%k(`UP@qh+G-&THb-a-m_XdFuJpLpC;`woG&qdj2 zC`jIA^D#Gi{-9g<~*NIi-6YtzyWUP2dsY{ zIRrHK#^pmnk1yr@?Fn%m5Qygqr%%Jl1M_6`0fM z+X&7?zi>nW-k5kOMj6p$)SID)vyCcWAr{(u$C`0??O{#dCYjoe?YYQgp={_q^jh-+ znJ_gt)urZYIEA6*b3*}JU3x#=`VM}x!_Zt0CXk8T?KE|XC>1?C6rLaxdG>vFCat^E z2eByg*@b=LgSq%ar8+#qr*g*nWwAz&?PgEYyFubiriuBMeW|F_ka3g~3$z!`z)@ba zwiz+9uC=3tQ>Zl3h^0q@U#X2}@>CrfEgzSlBB{JfDz3GKuErzuD9nW3iKA@L8>M|4 z+n`7r*!&Fj8Da~EMqkk#v&2-BER$>YwS2IAI*XfyTP}LndUT#mnv&UNP&Bu*=_nI+ z+Xbx%dofekBdP8-rbO=WfRIZ#MLF&l;Y+BrY7%3o*QZEnEYoSe5ghlHu@i{5F*jfQ z&;k~CteXd&KDR@rfFD{JDtLIAn=92pk+Zmts}>p9TONnTY|MPHiBp@CqHK>vVt89} zPN?6~8zn~;==Wf{75g52-9za)UXwYOY+a)rM16^)2|-59d@(%*5RLLZGo{KEBsc>iozT_*Qidm=ohA=Ywvw-;bo7`ESjiZf^Nai{p+S^Pc>NEvzuNQrs ztUE#WdlLS7sS$@e4}sLy)M)Jf$cUKgmnlZ#*yZgT%cCvX&i8!P;^&Ve| z!)>MJu}&DI$mNBp#{J^swR}*>1GBpu)HU(e*G|wqCI`CE`kF4C`uawjpXnhXA)y9a z>W3GXW_xl&cj$w}XXjH-yjPc$R6F{iRA|y7!@{FLUc&3ce1IjE6njuEBQsM>Oic8s zRu(YD3AdacKCzY6xWR#PD9w>4b?OMZ=%Cjc8qFWm%wn;YmOKyXnI5{ZJy&3Qh)=hy zqC(YhB3F8)SCal-9+jX1N6J70mty6RS=rgK`9;01XC)HbWe}`T3ez^_ghj@T^QAWM z;eLLI%8P4g9P@CrF@@1t5m2k(hD|+@otMW5N4UAY{}**iZ=)A=lZY=Yx|?u>P88yoq z=hJ;&9pT#);`ipRuGb&9f@^C5MsQMkr+zF>#krq3x|LBo!edV2k?R`%n0#^ zaO7=s;O~Gn8$a(FJrw*XG^fx(yxw)CLzR%#>teI#CHmCEqWmj!F#TL%p~Xy;QTej@ ziht4QbMz?+!)AW0J8{c?!mnR%in%*isb_l6;`779bqKtjpmhb6*+^vqUKYguQFM^{ zl+zHNIZUtS{d;nV$FF9tKl_Lk?%Z%`Q1YyQfe z6%}CproDGO^!wU1G&D#dCd0$ftxBGQFTgRH`!bGh%u-9oLmdjWQcuLjzXco}bWwXl z(e47HfD*pQjwk?ko;7~j{drT}?0)quS$wL3iPCTMT@s7v4rN-YX;K-?f-S0(1;6eG z;AdjN_FQJ#FMj&D*E4kYXQ}DP3*2V4nH1_Uy$~x(Keu9v*F5jl)KFEsX=97vEBZPf zov*r5u+=crLrxYN;=&0kN{yIu?QAr<0^CW{627G4ooP{_PPK0|YfTpY?);?-H@0L@~-@f|v<^>`w7H)3-(r(k)(Rt#H zTwZaSVb5;YE9<#Hyt%ph({3iUiTWW5S$)#D?@nkkI7e*!{GeC60*3!aat_q}d7Zyy zdhrh>*}blvo~%yy9h)1@nvZ9xp=-^`yNLsNY+d*Y7LD$&zSLR+Y21#e44VEFo@co| ztri$6N40R=-ifFkT@KJxoFz)#ncM%UMLzEB8R1FU5)Dyha7mfx8M?Smq^A50E9Zp~ z*9d7V=Ya*u=8r9g#i*$~59P4|HnFLZW{a(EXTPPmb%E>5uiHfQ6^1kP$=(cL6Ol7P zzCxqP-inN_LQh9{Ib_fat&QRDOCs1@g<3fWwwGy}l{Bh{kS)$>A~u^%t9N3wh_ts? z09RbniP&XrB_<}GHSRmR%wQU%2tR6>@D2X*sW}iYy~qkgNPH66_uH7t44NL~<8PGu zDj|>9-ngFLM`064^D^m978$6gXFq9@5>*_ibF0b-r!Xd(JwA88c9b5D@bKtt3vld; z;uW}p7%7)5ChJTP`NCe!Wi(ewCgY%$ZaO+!2-X&pHGJGqLC2fS+-f4}&;qjelPFuO zBTtp(QLL4ZYmRnHU@0%`)1S3j2K$w0j%srZB)!q zQn)*%G2F@Gq`_o}Cx47?hEpR|5%sFtK|FBEaK9Y8IWqZ2>>pWyC&@!iW$faY$dQo| zO~fwieX5*K6-s2%e$@e`Jw4cqGSN~)P+z8*C1yVm-7OxpT(^vGo7brte)#mn7?lCh z(9j|prTj^u;(to?C>3t`pe6~Hx{7<_pK`Y&J$qKYKz@N9LdcSi#+xl%nXQz8?9|~} z_>WsIMl?~c{8sPKR3Le)R`ew}+`QcP%*?wnm1R#-?d_NK!N}UGs;csG6{+#(VAbZW zS20)mlm%j%=cFOB5pcMLf_lATho$6P+=Cc}c0Tj|H$}PSee+uDA1=X>rKP1!+Y6Y% z^ucr)lR#}NkL6A6og%+aZ=!~Whtt#3v$KydoRPDQ=sV}K{HuP==f7g|4yKoa!{JaU zl$c%EDzXw2-G|n3mGyhH)A@8QJ8WrOaw%~e2g;r>Sb z_$d?`ZP-_ZK83wsIjj}3?g|`}8Fc2(a?a-7S(x~Pt&52#oA3%>78e(Due}6!qS0$w zjBxhaOeaD7ut=fQKEXoU1DP`$_j}G71HNW3O%LsVbjbrtWnyn(2Ha08E4!q&jiO&w zfF+HOz@|Aq`B-~e~aIfdeniHcuS>Pa$LP0V)< z4D<_+rr}fvir!5Hia&foWdcPZ?{z~957>b7gndWv=@_?{Q`unX?9o4Gk!21E*}1u= zJN?59LsL#LGbB=<5w}O)HiQN&*q`yG+=apVY;MrCeCGH%94GA2roejX!#=o6uQD2< zX$29DrRlh2JGK-N?-Nt)zCI_~PDV@UJ*k&iOgC$!%3=9=%~fXPJMBl{iNjh%pz298 zTX|a;kAp{akCF4`+QPXiU8T^jTJC&a!+66=b7(2N?fK-tPS&PWGPfi%XR-$4FqG!c^fQ45(^=nNv7F25}dcBa~NlO??UO#eswVfnsv_ezWM7vyk7U)ZOv~wn~dDZTxzM^u^Hdm9mGu3 zo1GHAbeo+G4I9&0Pm{K;aB>#FG*1xc+S*^`%-vG>)FTVi_X;*j4mC$t-O@xCYiMUTIM}xhv(jt!zB8er2^S|J$NjDQV^lbQ^;fe~HXM!rb z2YSoIh&1cb$=_!yXW<)YS;3C5vW=c(hIIJdAQ6eWn5-XSEj4Kcl;LxXkvSl}Usdal zmVHTCQAJGHSx4eYC`X8n>u(NulBJtPgYa z4G~eTp(S#ggs!xo?$6bbK+F?zIt{*F`+yY37oC&;^H0@I*!qo=W;cX^Ea?RMDVH4a ze8Z!A!4#&Sm}urHmvjEy`|-)12=$FK-l#M&4Y}cCWsiVkuYN6`bicAC%?wxqrY^hn z()Gx9qvOE0cVnhT!=wdb)6T%+`0KS6L?)6sH99)+d20~Cl3{j_cS(EGB?%s5D+)F;b8UHA^C){UZ>FUi zjedJ(U5h%dyxVY#s*(4n=}gQinZ|8s)5^nvy}AeXHuoyC$Q+|<>tgp+PwN}&xTDK% zc-!_`W;zWo2TGE6YQweh()IdKoOI9Wqi%GO%8S=;01;cp!B=z-$m5}~FP!r##IjQJ zx?TU+G2KIYhQ(;96_Q9yq}xyteq}7z*Yb^k&ijRn;ZJY{JtYKNu?M9xXJMV8$I~u} zL_?>C$d^MuHl2#Rpa)#$KI>ywl@nDCraNhWF})?Q5Z@(T9ccU|_uOGSnxC|TlE2h$)hO{-MrA<=FsPNq>B^@qR)FukPS{2 zhxHufBz3+t_cp!~=@(NE>PJ?S_OmpZA0cwX#06r6PiB(fP$=MxMBFOx@fJm@vA?z` zX#KtnjdYLnVG?2vGQ@2*bM3sOqz4lb0o*6w`T$z$kk^wpq0RaGB@83fmD;=$XU2I z)}~5E$^g`VsccXHG^Xu%)$|Y?sfg0d?fLyuNVSZlW6FtQ^r^UfS-}LA#wMfoAI2;= zPUX|J$CAg!*0oA|T`!xSeZRHDjF4&v&RC$Zmq-K_fLk|@t}$m9Qzp^?UGTJ`VbASG z9iSuJwsLqWK$DQ=Tsf~Al-wmv2aqNAt_PoPc`zM93sqAu2Vtv7Ar21B0HUEmkT+`C zvk>Qfqpv4dC{rK<&X(YYV_`ta9 z6g_^qcM*r0zEoRI$U%R!3`M@IPRf6Gryz*^CJGvL>$m<4^V=oXz|B=O`VC{pu|285 zUq#f%SDQ^l(B^v=o7M+rwF90`_}=mK28t0PO*1!%6fy_7BU_}?zD215^m! zwc!*5bGa7NQ!rtiOO$C$EI(=R)8KvJb~jwDDRps`!gRRJQH$@*!)A#1eRW+bntp+eEvJ&LR9fFc)7*IxT1EYSwDYe&HhXo z$_pmy7f5@F-Ka3weWO@9G6MgkM}BGdtU-{fNMf!o)Q z@!vRR>asC12!X3o%ZBfKE_vkyBr>pDdjy`fbrus!7+uYM&s(GhEMP*1)_vtF5+u)s znpT&rZaAVk6@6$SewDYVdRYj4Te)z>7TXMTxXOF57b_d%) zXYQ7hEN4CN6)g<8CnzyF%CWNB^1u#oH5-j_I`Y)#s%=Jb+yry$TAT6JA{FnuzhZ}% zp;;vn@YKBeVja~Z7+!kY=AQm6g^y@-O-;?na$sQPK}67k&w8}8xw%1#JSzH~8n781 z2DYy|;t&0#LXko9A5n&UsZ`(f4~=v>9XxgNWWP87xd028wU*f!HvR5Ns+`Ggvym7p z*5__q{#>7lPWWVNY^x;TKWX z1TOCk33;RJ`wv#S>6_KZjE$l5HcRwiUuB*eWMfd|2H1sy8S$0*jGEbya1Og;2Qg#MBs1_ zK=c7w9NoUc_*|*n4;*O!GKUfQiu8Zu5V+7j*UM3pCr<+G0*4KG9@#1lntoyRH^%^! zrvO>-^G~3?+&+(m(7-I%KY$09px{2fIVAu4Fb@53=nj{R`Nn$i7%86s$Qe!>SZv?9 zg}<6yPQx@~&tLQiH}v0=;CpEQ$&7p@H@;C7uADTed;Y_}Pr((!4bGky9OF3L33ncD zV)b(98os!GX;AAi4%zX&H#hiSDI;#`a2?M5#hvB5Zozj1{-$`iWY0H?|67Q@XC^oG zxO#CP-zERYL_iq}#DMEtj%2=!IT6t0H{wgy`!3IaC#<+m=Ejc8_I&p_S95O8f9=9? z$iHzxw|)cCiAx-H{hj4snXG>@Pya!MeRs$AQ2moj`kF!9JpKPcPX3>$*E#kc0+>8f UVb!q-kjo$gy=#|CezlMOA9Hz_xBvhE literal 0 HcmV?d00001 diff --git a/python/gym_jiminy/unit_py/data/cassie_standing_panda3d_7.png b/python/gym_jiminy/unit_py/data/cassie_standing_panda3d_7.png new file mode 100644 index 0000000000000000000000000000000000000000..cfeef3d9a3bb95d29a82ae99ab5c2ea2e1fe1465 GIT binary patch literal 6679 zcmds+i9giq_rOQGNTQoIH){za5-nsfE}^X1(?VqEB9g7KjW)h1)NK$N``9IeEMrWe z#9(d-4P(eQ*=9o4F*CpCqkHe~_j~>RgP+%HUOu1CoX>O4bDnd~bDsA+Ja28bL-aRM z7!0=K+}SgBFqps(=p(WPJULT+b_fQOVw^kkyTk1lb3+kNJe|?|zXuT`yu!V_ohar< z_Jo%@O#Fh_a%QiQ;u)h3>&I4xst5CSp|9>!fM@JEd$3(-P*Ge$UBL6*!HR={s!IFs z__+vOj(PZsxASa_XMDWjxB9y#TlAktbN3F2eA{j!GGM*^^;0 zF~2?dEF&+Cl8TAJqF`Hm6nzy0n*R`)gUuJ!3vTt4lr%IPkbrIVtPwLb?9hS<)MtzC ze6lPE6KJOkY`OLO9{FrjcXdTW9W5}_XYh#7Eh8^KCk)$S7bGll)a3!3Vf(b105L^Ey2w6_3@$S(h(w1QfZX zOrTv2ZqKlXYy6dkOVxBYpKM?^~_wm5Tnhwzk2!&~SHY4fTkEjo{&?DDwO;KfKF&S6|HblGb@M^3t>( z^-=DH%a_&Uf}QrGb4>G|qG<0E4)DLbHQplG$O*OEogKY?;hS5{IoSj@DjSm#z$e5`L)t$O%SXYam!_U1tT03pf8-94wQ6MK#{Ww?Jp zVrQ%OOH#0(pP#$C`$Q^1?MUMC5g(FkR$7{-jL4yg6*(kHy3t=Ret{K1aX=iY!;((tBNQG#f?}3)v(g*T#E#Wf3Dg%~#*j_dc^C^6DRd{Lxcjh4N?$9DARu(ycpFRz)>H z;D02BHRfg1CnY8pw9J~%Iac%&lLqIO2ZwpfSi5p3;>g|Y9Tf)4U#<mLw%at4m#7 zJ>l`=>cP1O4<2aUkdu+QQFC!AoHXR0^!PEhVm4Tp*!*p>A>N#bNsdIOgm6bsWxOlOo*Jmy?qW|R;ji@OLIrR9` z;~01KXv_T~rKqCE18)3&XVSEzi7i+neUzwM>;j=1B#d16x191zwY+EiU}_6za+P!FPYY6|3KbOQ(!1tR_RfKQD-ALpjZccKlFYKslE zX&TOUDe8C4OiiQLd$J5xNAAJ#Yiny-sTw_NSBRkr2?-=tNClC@A_9@4^u-D9rXTFu zjqgFe+{Ko83S)`v*LC&FjmTB|m7%arGEXrs1(OsH!+4_kA=ca}FrYq=78ylNcgu~3z`sdG|U*8zC zGq<<5@9F6oLvmZ{v-nGCX=#j??nj8oNwlldq=_xIqH*J%o4Vb6?mkjcgCu{_VV842`QqU)hUhn%CwyIF5Lpc~pDZ-2;CoC0 zX{c20l!Wm5*jv?CyZ=-@Jy+e68MRESudm12)rS@LcXnFp&gmq+uCD$p!%vD_9%>p>~s}b($p0PNSqsN{0MXZ;%_TPfuW3x1%XL+&DcL&Pw8wM(vEQ zTKe}I)EI^);M{}>PSX)PorZ9<3r(J4Y&3-R+f99>x~AGSeDC+Sgp(#(+$`Wx>kSrm z=0k|@$r6mvF>Sl>=$0wTER37^6eEH?)DYs!{s>aLqr8MNj`kltHm>#{-`pB8SB?EN zJ3ITxZDTZfhmd&vW9|*@=sdBBF0U%5QuU)p4ffF@f+aWF+uQ49;dl_R$r9NY{XBBR z!M-F#i|CnNq$0^G@!KmlZkxwlpYPo<9Kc!acaC0aK7!Ywl#Ej-SDlerDn*^+|hTwQG1_oS^oDse?7c-f02dK`NjhTsihs0L3jM@5L zYkNO|5@B?n9U!D6ZfxC!i-+%fotp75Wsl|bQaFFCe&zXuP|LTy*@SZy7*-{3G#&}%b~MFrVwuv zz3_}3$wY9Zsidc*m5Ai1)`qea_f;2=o@#1pk3lKJ$wwTxquw(WEQH96AP?1&xpV!E zlPBSL1gkF3sT5i$3x(ns(3On5xdVb1WFu=}+2a9^Y|H)#dWRMXB%K>N`1*!#Eoe=M zgSoKSG=kWkzcR|1XagiN%Wj(SD6h0moe94Av=i*W z1hpB4NCYZUZuK^c^-NX!9G-=9wvvv`NFh5I)|0Cbv_@zbESoN)Zjje&LHv?Fnz4tF z;p`ECmUA2d)od}=M04wcs5g5~r$ILA$Hzqt-M90Rya_q<#xkaAwjFE{SR%d~Y+zX% zjsGO27bos_rT9du^kF6 zY`XTGC7k)3XvFoJM>BWVa4YL_Wj^9MjIjR!{7;jHL zQr5{N9$v~-X`skSNb6j^d*1fwj$h3{EBSSvo-j4^fx#4Tk8%+`cQ|S)7NY|S*~?sb z%lwxwUp{^Ml>L4YUijiqyL1pr^Z+uiLxS5IlPK!jrlh0_rx4z~d&f|V2n?)LiI!vDX9-Hzk~hBWIYO+pv9{)MS9r81(mVT7-@)!LJdogaJdxYk7fTS!3D%X_ z=cG`dZEg~B>Ftkn91fSBu3M=PRVV0Aulp#S6Ay|}Nr#LnIx{r^y{MP<;)P{YK5as& zmvM@9K-+(o8S^TU;6|R$n28dNk-1g4i7u%A4Ph z7VUcYO`Q6)-X2Zur@sr;ISX>6qHxmSs+Vx@glx%HkA0HTo^ZuvZ@6MvJ{J}kIX79` z#JiFuGa}F>UPAGaQmwcY1iJszU*|X04n!}Vw*jLk_i8!xx^bDMDf&UpVbeNlRrfl8 zH6>nNULC?i=^PLe+vMh~5IheUbE~m2q*5hbjnZK-D=0+;doJkc^78V41$Z^%i4f4_ zq(s1X6g<&FATCw(Un)-lbA?mRqf?BU%@dk^G)gKOYeT}n&Xsej>*}t9mDdU@@!9+< z;rvlWlRj`@W--QWb$5wL7tV)5P@^tt1%c(6#ptC@aG241jvK_@w6 zvocHTq2Bhd9fa;gZ8DjVDO{LiFX;4yK|}cX1L%AC^OyT*6v1>E2j^=f8`yeHb+yB5 zxo=8C$^~LG6A|wheUId%S8w1;OwG(F!k}G51$u#&g8P{H^yyRVC(yc2J4JMgt6GE` zIiiF^Qfu@UJ{;o8y}o?E2W%9>LfN_1a`K6KRAEFWX5nxm3>7ESP!=0={I)-T~d-s!r)jiK0XdA#jmdXG|Wm^QskS3p@TA)qkEVWE*IPe zB+k2|Gr?KPy3l^NiS2H5rW{&NZmkqF(r~<2RT|1A@1si6>5=2HH3x$Vn+N7XrJnlM z4AAM9v&<-qk-@>$PNm80y%S3CB9)V2s%0wip+r)3bug%yv%_R0busTTGcRFTj|$4Z zy*X&vfgAsJ-%{)feYw|WV*r@nF}ezhIpcfgVB)FFB!dBMzc1&N*f{I9+3bdDx$NeK zxnV+A}6Q4Xp9!VXJ8gxbmqQd67Y~3VUEW2dH=h%&lJu^|FH%Eb=o*3A#M7xR^ z7r1ZidT%Sd7?==hf6v@LMF60OSVJsoL_Q55fE1%$ajh~9ejW#Rr8;~)4NkNF`64dd zPJehNIF4CVFBzAP2(GtKG|YKM`eY8DEDI%xiOP%gl1U%a4$qul4u4txr|N(~%){Aw=3@%EszQsfol z7eL3-{1S9=Y7`|UCFm?R6}*nTnu6-`3gRr!X%9y(g4Ucbe1AzRe@=Kqx3lsKDPpCb znVDfXPPx?{V=#8TtorPZt*^e($wM zF1!t$#>!Q!eo;;guU5{ATxeI$lBUz%>Vl3|^Z@?=TleP35ffXz`BurPX5*ys+1TN3 z`zo9+I-C_hVA2vfcX%nL*EEx#IJTt0%T^yWuK3G_!4I)VZz6sqBQ9TGTHh`Gc zk7K0>5N7+Aw{0qhp17zBzPcQf-w)xezYWlN_^$l23K$>IGHWGet5d#k#hY3>QwMQY zmWH0nH)48zf;0L3QoiOFd0XxSIaSMB;W|6`Ckzi4>qRg#6n#e;GIId+?Lq=@hrKayRiF#11zvEEIqu2oV0HiE+m)y)yg% zYLJwd&3K+oQ1p_{B#1dAo}+vO2(S0*l>==i9070I*qk_mn85zlSD+zIMvXUr{i(Ll=&_TI)4xXhu*76=<~bO!R+|L0sFI=l=A88{ zfl?!v*rls~y;a?;aU6lZ`?f;97L(noT6I@R*P+<_d(n2DHy#z=nFNI_2pH(G^dT3R z!$a`@=|_RyO`J&()9_r|egCSYBvn&fe%DVwBh%K%aj*V$fF}zku!%--5r&KkfjQwV z^T=(|KVP>@Mqd*W&$GR-ecR5&2#_ZEldju%&tk@|{Wmu5JU?%}T`cye|J-aC&UYL~ ztu1F-H7wuw*lnhdm(^{-U;0x1vHR}{9NW+P^-fMMw1Pgs1Q#X%y)pm}(^`X1Ft{ 0.1: + if img_diff > IMAGE_DIFF_THRESHOLD: img_obj = Image.fromarray(rgb_array) raw_bytes = io.BytesIO() img_obj.save(raw_bytes, "PNG") raw_bytes.seek(0) print(f"{self.env.robot.name} - {self.env.viewer.backend}:", base64.b64encode(raw_bytes.read())) - self.assertTrue(img_diff < 0.1) + self.assertTrue(img_diff < IMAGE_DIFF_THRESHOLD) # Get the simulation log log_data = self.env.log_data diff --git a/python/jiminy_py/setup.py b/python/jiminy_py/setup.py index c92faaeb5..eb22af578 100644 --- a/python/jiminy_py/setup.py +++ b/python/jiminy_py/setup.py @@ -107,9 +107,12 @@ def finalize_options(self) -> None: # Parser for Jiminy's hardware description file. "toml", # Web-based mesh visualizer used as Viewer's backend. - # 0.19.0 introduces many new features, including loading generic + # 0.0.19 introduces many new features, including loading generic # geometries and jiminy_py viewer releases on it for rendering # collision bodies. + # 0.3.1 updates threejs from 122 to 132, breakin compatibility with the + # old, now deprecated, geometry class used to internally to display + # tile floor. "meshcat>=0.0.19", # Standalone mesh visualizer used as Viewer's backend. # Panda3d>1.10.9 adds support of Nvidia EGL rendering without X11 diff --git a/python/jiminy_py/src/jiminy_py/dynamics.py b/python/jiminy_py/src/jiminy_py/dynamics.py index 07b4c1dc5..967b67dbc 100644 --- a/python/jiminy_py/src/jiminy_py/dynamics.py +++ b/python/jiminy_py/src/jiminy_py/dynamics.py @@ -70,8 +70,8 @@ def velocityXYZQuatToXYZRPY(xyzquat: np.ndarray, However, it is not the case for the linear velocity. .. warning:: - Linear velocity in XYZRPY must be local-world-aligned frame, while - returned linear velocity in XYZQuat is in local frame. + Linear velocity in XYZQuat must be local frame, while returned linear + velocity in XYZRPY is in local-world-aligned frame. """ quat = pin.Quaternion(xyzquat[3:]) rpy = matrixToRpy(quat.matrix()) diff --git a/python/jiminy_py/src/jiminy_py/plot.py b/python/jiminy_py/src/jiminy_py/plot.py index 8c4cd0a9b..fbac743b6 100644 --- a/python/jiminy_py/src/jiminy_py/plot.py +++ b/python/jiminy_py/src/jiminy_py/plot.py @@ -298,6 +298,7 @@ def add_tab(self, plot_method(ax, time, data) if self.tabs_data: self.figure.delaxes(ax) + ax.grid() axes = [ax] # Get unique legend for every subplots diff --git a/python/jiminy_py/src/jiminy_py/viewer/meshcat/index.html b/python/jiminy_py/src/jiminy_py/viewer/meshcat/index.html index eaa05cefe..33d7279ac 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/meshcat/index.html +++ b/python/jiminy_py/src/jiminy_py/viewer/meshcat/index.html @@ -63,7 +63,8 @@ }); } else { - viewer.connect(); + var ws_url = undefined; + viewer.connect(ws_url); } } catch (e) { console.info("Not connected to MeshCat server: ", e); @@ -72,20 +73,25 @@ // Replace the mesh grid by a filled checkerboard, similar to // the one of Gepetto-gui. The paving size is 1m by 1m. var segments = 20; - var geometry = new MeshCat.THREE.PlaneGeometry(20, 20, segments, segments); - var materialEven = new MeshCat.THREE.MeshBasicMaterial( - {color: 0x222233, side: MeshCat.THREE.DoubleSide}); - var materialOdd = new MeshCat.THREE.MeshBasicMaterial( - {color: 0xf2f2fe, side: MeshCat.THREE.DoubleSide}); - var materials = [materialEven, materialOdd] - for (x of [...Array(segments).keys()]) { - for (y of [...Array(segments).keys()]) { - i = x * segments + y; - j = 2 * i; - geometry.faces[j].materialIndex = geometry.faces[j + 1].materialIndex = (x + y) % 2; + var cmap = [new MeshCat.THREE.Color(0x222233), new MeshCat.THREE.Color(0xf2f2fe)]; + var geometry = new MeshCat.THREE.PlaneBufferGeometry( + segments, segments, segments, segments).toNonIndexed(); + var material = new MeshCat.THREE.MeshBasicMaterial( + {vertexColors: true, side: MeshCat.THREE.DoubleSide}); + var colors = []; + for (var x of [...Array(segments).keys()]) { + for (var y of [...Array(segments).keys()]) { + var color = cmap[(x + y) % 2]; + colors.push(color.r, color.g, color.b); + colors.push(color.r, color.g, color.b); + colors.push(color.r, color.g, color.b); + colors.push(color.r, color.g, color.b); + colors.push(color.r, color.g, color.b); + colors.push(color.r, color.g, color.b); } } - var checkerboard = new MeshCat.THREE.Mesh(geometry, materials); + geometry.setAttribute('color', new MeshCat.THREE.Float32BufferAttribute(colors, 3)); + var checkerboard = new MeshCat.THREE.Mesh(geometry, material); viewer.scene_tree.find(["Grid"]).set_object(checkerboard) viewer.scene_tree.find(["Axes", ""]).object.material.linewidth = 2.5 diff --git a/python/jiminy_py/src/jiminy_py/viewer/meshcat/recorder.py b/python/jiminy_py/src/jiminy_py/viewer/meshcat/recorder.py index 5a948b851..6e1a8c8cc 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/meshcat/recorder.py +++ b/python/jiminy_py/src/jiminy_py/viewer/meshcat/recorder.py @@ -68,7 +68,6 @@ async def launch(self) -> Browser: cmd = self.cmd + [ "--enable-webgl", "--use-gl=egl", - "--disable-frame-rate-limit", "--disable-gpu-vsync", "--ignore-gpu-blacklist", "--ignore-certificate-errors", @@ -77,9 +76,10 @@ async def launch(self) -> Browser: "--disable-setuid-sandbox", "--proxy-server='direct://'", "--proxy-bypass-list=*"] + if "--disable-gpu" in cmd: + cmd.remove("--disable-gpu") if not self.dumpio: - options['stdout'] = subprocess.DEVNULL options['stderr'] = subprocess.DEVNULL if sys.platform.startswith('win'): startupflags = subprocess.CREATE_NEW_PROCESS_GROUP @@ -331,7 +331,7 @@ def release(self) -> None: def _send_request(self, request: str, message: Optional[str] = None, - timeout: float = 15.0) -> None: + timeout: float = 20.0) -> None: if not self.is_open: raise RuntimeError( "Meshcat recorder is not open. Impossible to send requests.") diff --git a/python/jiminy_py/src/jiminy_py/viewer/meshcat/server.py b/python/jiminy_py/src/jiminy_py/viewer/meshcat/server.py index c058bdb5d..88209d009 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/meshcat/server.py +++ b/python/jiminy_py/src/jiminy_py/viewer/meshcat/server.py @@ -73,12 +73,13 @@ def handle_web(self, message: str) -> None: self.bridge.websocket_msg.append(message) if len(self.bridge.websocket_msg) == len(self.bridge.websocket_pool) and \ len(self.bridge.comm_msg) == len(self.bridge.comm_pool): + self.is_waiting_ready_msg = False gathered_msg = ",".join( - self.bridge.websocket_msg + self.bridge.comm_msg) + self.bridge.websocket_msg + list(self.bridge.comm_msg.values())) self.bridge.zmq_socket.send(gathered_msg.encode("utf-8")) self.bridge.zmq_stream.flush() - self.bridge.websocket_msg, self.bridge.comm_msg = [], [] - self.is_waiting_ready_msg = False + self.bridge.comm_msg = {} + self.bridge.websocket_msg = [] WebSocketHandler.on_message = handle_web # noqa @@ -103,7 +104,7 @@ def f(port): # Extra buffers for: comm ids and messages self.comm_pool = set() - self.comm_msg = [] + self.comm_msg = {} self.websocket_msg = [] self.is_waiting_ready_msg = False @@ -133,21 +134,22 @@ def wait_for_websockets(self) -> None: def handle_zmq(self, frames: Sequence[bytes]) -> None: cmd = frames[0].decode("utf-8") if cmd == "ready": + self.comm_stream.flush() if not self.websocket_pool and not self.comm_pool: self.zmq_socket.send(b"") msg = umsgpack.packb({"type": "ready"}) - self.is_waiting_ready_msg = True for websocket in self.websocket_pool: websocket.write_message(msg, binary=True) for comm_id in self.comm_pool: self.forward_to_comm(comm_id, msg) + self.is_waiting_ready_msg = True else: super().handle_zmq(frames) def handle_comm(self, frames: Sequence[bytes]) -> None: cmd = frames[0].decode("utf-8") + comm_id = f"{cmd.split(':', 1)[1]}".encode() if cmd.startswith("open:"): - comm_id = f"{cmd.split(':', 1)[1]}".encode() self.send_scene(comm_id=comm_id) self.comm_pool.add(comm_id) if self.is_waiting_ready_msg: @@ -157,19 +159,21 @@ def handle_comm(self, frames: Sequence[bytes]) -> None: # Using `discard` over `remove` to avoid raising exception if # 'comm_id' is not found. It may happend if an old comm is closed # after Jupyter-notebook reset for instance. - comm_id = f"{cmd.split(':', 1)[1]}".encode() self.comm_pool.discard(comm_id) + self.comm_msg.pop(comm_id, None) elif cmd.startswith("data:"): message = f"{cmd.split(':', 2)[2]}" - self.comm_msg.append(message) - if (len(self.websocket_msg) == len(self.websocket_pool) and - len(self.comm_msg) == len(self.comm_pool)): - gathered_msg = ",".join( - self.websocket_msg + self.comm_msg) - self.zmq_socket.send(gathered_msg.encode("utf-8")) - self.zmq_stream.flush() - self.websocket_msg, self.comm_msg = [], [] - self.is_waiting_ready_msg = False + self.comm_msg[comm_id] = message + if self.is_waiting_ready_msg and \ + len(self.websocket_msg) == len(self.websocket_pool) and \ + len(self.comm_msg) == len(self.comm_pool): + self.is_waiting_ready_msg = False + gathered_msg = ",".join( + self.websocket_msg + list(self.comm_msg.values())) + self.zmq_socket.send(gathered_msg.encode("utf-8")) + self.zmq_stream.flush() + self.comm_msg = {} + self.websocket_msg = [] def forward_to_websockets(self, frames: Sequence[bytes]) -> None: super().forward_to_websockets(frames) diff --git a/python/jiminy_py/src/jiminy_py/viewer/meshcat/wrapper.py b/python/jiminy_py/src/jiminy_py/viewer/meshcat/wrapper.py index 130ad2961..1289f316c 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/meshcat/wrapper.py +++ b/python/jiminy_py/src/jiminy_py/viewer/meshcat/wrapper.py @@ -21,15 +21,6 @@ from .recorder import MeshcatRecorder -if interactive_mode() == 1: - # The IO message rate limit has already been increased to 1e6 on Google - # Colab, so no need to throw this warning. - logging.warning( - "You may experience some lags while replaying a simulation.\n" - "Consider increasing the IO message rate limit by adding the " - "extra argument '--NotebookApp.iopub_msg_rate_limit=100000' when " - "executing 'jupyter notebook'.") - if interactive_mode(): # Google colab is using an older version of ipykernel (4.10), which is # not compatible with >= 5.0. The new API is more flexible and enable @@ -130,19 +121,19 @@ def __call__(self, unsafe: bool = False) -> None: msg['header']['msg_type'].startswith('comm_'): # Comm message. Analyzing message content to determine if # it is related to meshcat or not. - if not msg['header']['msg_type'] == 'comm_close': - content = self.__kernel.session.unpack(msg['content']) - data = content['data'] - else: + if msg['header']['msg_type'] == 'comm_close': # All comm_close messages are processed because Google # Colab API does not support sending data on close. - data = "'meshcat:close" + data = "meshcat:close" + else: + content = self.__kernel.session.unpack(msg['content']) + data = content.get('data', '') if isinstance(data, str) and data.startswith('meshcat:'): # Comm message related to meshcat. Processing it right # now and moving to the next message without puting it # back into the queue. tornado.gen.maybe_future(dispatch(*args)) - continue + continue # The message is not related to meshcat comm, so putting it # back in the queue after lowering its priority so that it is @@ -153,11 +144,11 @@ def __call__(self, unsafe: bool = False) -> None: # SHELL_PRIORITY by default. self.__kernel.msg_queue.put_nowait( (SHELL_PRIORITY + 1, t, dispatch, args)) - - # Ensure the eventloop wakes up - self.__kernel.io_loop.add_callback(lambda: None) self.qsize_old = self.__kernel.msg_queue.qsize() + # Ensure the eventloop wakes up + self.__kernel.io_loop.add_callback(lambda: None) + process_kernel_comm = CommProcessor() # Monkey-patch meshcat ViewerWindow 'send' method to process queued comm @@ -181,6 +172,10 @@ def __init__(self, comm_url: str): self.n_comm = 0 self.n_message = 0 + self.__ioloop = None + self.__comm_socket = None + self.__comm_stream = None + def forward_comm_thread(): loop = asyncio.new_event_loop() asyncio.set_event_loop(loop) @@ -191,6 +186,10 @@ def forward_comm_thread(): self.__comm_stream = ZMQStream(self.__comm_socket, self.__ioloop) self.__comm_stream.on_recv(self.__forward_to_ipython) self.__ioloop.start() + self.__ioloop.close() + self.__ioloop = None + self.__comm_socket = None + self.__comm_stream = None self.__thread = threading.Thread(target=forward_comm_thread) self.__thread.daemon = True @@ -208,9 +207,11 @@ def close(self) -> None: self.n_message = 0 self.__kernel.comm_manager.unregister_target( 'meshcat', self.__comm_register) - self.__thread._stop() self.__comm_stream.close(linger=5) self.__comm_socket.close(linger=5) + self.__ioloop.add_callback(lambda: self.__ioloop.stop()) + self.__thread.join() + self.__thread = None def __forward_to_ipython(self, frames: Sequence[bytes]) -> None: comm_id, cmd = frames # There must be always two parts each messages @@ -438,7 +439,9 @@ def add_frame(self) -> None: def capture_frame(self, width: Optional[int] = None, height: Optional[int] = None) -> str: - if not self.recorder.is_open: + if self.recorder.is_open: + self.wait(require_client=False) + else: self.recorder.open() self.wait(require_client=True) return self.recorder.capture_frame(width, height) diff --git a/python/jiminy_py/src/jiminy_py/viewer/panda3d/panda3d_visualizer.py b/python/jiminy_py/src/jiminy_py/viewer/panda3d/panda3d_visualizer.py index 35202392f..bdcd8559b 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/panda3d/panda3d_visualizer.py +++ b/python/jiminy_py/src/jiminy_py/viewer/panda3d/panda3d_visualizer.py @@ -4,6 +4,7 @@ import sys import math import array +import pickle import warnings import xml.etree.ElementTree as ET from datetime import datetime @@ -170,7 +171,7 @@ def make_cone(num_sides: int = 16) -> Geom: """Create a close shaped cone, approximate by a pyramid with regular convex n-sided polygon base. - For reference about refular polygon: + For reference about regular polygon: https://en.wikipedia.org/wiki/Regular_polygon """ # Define vertex format @@ -206,8 +207,53 @@ def make_cone(num_sides: int = 16) -> Geom: prim.add_vertices(i, i + 1, num_sides + 1) prim.add_vertices(i + 1, i, num_sides + 2) + # Create geometry object geom = Geom(vdata) geom.add_primitive(prim) + + return geom + + +def make_height_map(height_map: Callable[ + [np.ndarray], Tuple[float, np.ndarray]], + grid_size: float, + grid_unit: float) -> Geom: + """Create height map. + """ + # Compute grid size and number of vertices + grid_dim = int(np.ceil(grid_size / grid_unit)) + 1 + num_vertices = grid_dim ** 2 + + # Define vertex format + vformat = GeomVertexFormat.get_v3n3t2() + vdata = GeomVertexData('vdata', vformat, Geom.UH_static) + vdata.uncleanSetNumRows(num_vertices) + vertex = GeomVertexWriter(vdata, 'vertex') + normal = GeomVertexWriter(vdata, 'normal') + tcoord = GeomVertexWriter(vdata, 'texcoord') + + # # Add grid points + for x in np.arange(grid_dim) * grid_unit - grid_size / 2.0: + for y in np.arange(grid_dim) * grid_unit - grid_size / 2.0: + height, normal_i = height_map(np.array([x, y, 0.0])) + vertex.addData3(x, y, height) + normal.addData3(*normal_i) + tcoord.addData2(x, y) + + # Make triangles + prim = GeomTriangles(Geom.UH_static) + for j in range(grid_dim): + for i in range(grid_dim - 1): + k = j * grid_dim + i + if j < grid_dim - 1: + prim.add_vertices(k + 1, k, k + grid_dim) + if j > 0: + prim.add_vertices(k, k + 1, k + 1 - grid_dim) + + # Create geometry object + geom = Geom(vdata) + geom.add_primitive(prim) + return geom @@ -687,22 +733,48 @@ def _make_axes(self) -> NodePath: node.set_scale(0.3) return node - def _make_floor(self) -> NodePath: + def _make_floor(self, + height_map: Optional[Callable[ + [np.ndarray], Tuple[float, np.ndarray]]] = None, + grid_unit: float = 0.2) -> NodePath: model = GeomNode('floor') node = self.render.attach_new_node(model) - for xi in range(-10, 11): - for yi in range(-10, 11): - tile = GeomNode(f"tile-{xi}.{yi}") - tile.add_geom(geometry.make_plane(size=(1.0, 1.0))) - tile_path = node.attach_new_node(tile) - tile_path.set_pos((xi, yi, 0.0)) - if (xi + yi) % 2: - tile_path.set_color((0.95, 0.95, 1.0, 1)) - else: - tile_path.set_color((0.13, 0.13, 0.2, 1)) + + if height_map is None: + for xi in range(-10, 11): + for yi in range(-10, 11): + tile = GeomNode(f"tile-{xi}.{yi}") + tile.add_geom(geometry.make_plane(size=(1.0, 1.0))) + tile_path = node.attach_new_node(tile) + tile_path.set_pos((xi, yi, 0.0)) + if (xi + yi) % 2: + tile_path.set_color((0.95, 0.95, 1.0, 1)) + else: + tile_path.set_color((0.13, 0.13, 0.2, 1)) + else: + model.add_geom(make_height_map(height_map, 20.0, grid_unit)) + render_attrib = node.get_state().get_attrib_def( + RenderModeAttrib.get_class_slot()) + node.set_attrib(RenderModeAttrib.make( + RenderModeAttrib.M_filled_wireframe, + 0.5, # thickness + render_attrib.perspective, + (0.7, 0.7, 0.7, 1.0) # wireframe_color + )) + node.set_two_sided(True) + return node + def update_floor(self, + height_map: Optional[Callable[ + [np.ndarray], Tuple[float, np.ndarray]]] = None, + grid_unit: float = 0.2) -> NodePath: + if height_map is not None and not callable(height_map): + height_map = pickle.loads(height_map) + self._floor.remove_node() + self._floor = self._make_floor(height_map, grid_unit) + def append_group(self, root_path: str, remove_if_exists: bool = True, diff --git a/python/jiminy_py/src/jiminy_py/viewer/viewer.py b/python/jiminy_py/src/jiminy_py/viewer/viewer.py index 1e83a1acf..5fae1bd07 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/viewer.py +++ b/python/jiminy_py/src/jiminy_py/viewer/viewer.py @@ -15,6 +15,8 @@ import multiprocessing import xml.etree.ElementTree as ET from copy import deepcopy +from urllib.parse import urlparse +from urllib.request import urlopen from functools import wraps, partial from bisect import bisect_right from threading import RLock @@ -478,8 +480,9 @@ def __init__(self, # no other display cell already opened. The user is # probably expecting a display cell to open in such # cases, but there is no fixed rule. - open_gui_if_parent = interactive_mode() and \ - not Viewer._backend_obj.comm_manager.n_comm + open_gui_if_parent = interactive_mode() and ( + Viewer._backend_obj is None or + not Viewer._backend_obj.comm_manager.n_comm) elif Viewer.backend.startswith('panda3d'): open_gui_if_parent = not interactive_mode() else: @@ -510,6 +513,7 @@ def __init__(self, Viewer._backend_robot_colors.update({ self.robot_name: self.robot_color}) except Exception as e: + self.close() raise RuntimeError( "Impossible to create backend or connect to it.") from e @@ -803,7 +807,7 @@ def open_gui(start_if_needed: bool = False) -> bool: Viewer.__connect_backend(start_if_needed) # If a graphical window is already open, do nothing - if Viewer._has_gui: + if Viewer.has_gui(): return if Viewer.backend in ['gepetto-gui', 'panda3d-qt']: @@ -815,24 +819,28 @@ def open_gui(start_if_needed: bool = False) -> bool: viewer_url = Viewer._backend_obj.gui.url() if interactive_mode(): - import urllib from IPython.core.display import HTML, display # Scrap the viewer html content, including javascript # dependencies - html_content = urllib.request.urlopen( - viewer_url).read().decode() + html_content = urlopen(viewer_url).read().decode() pattern = '' scripts_js = re.findall(pattern % '(.*)', html_content) for file in scripts_js: file_path = os.path.join(viewer_url, file) - js_content = urllib.request.urlopen( - file_path).read().decode() + js_content = urlopen(file_path).read().decode() html_content = html_content.replace(pattern % file, f""" """) + # Provide websocket URL as fallback if needed. It would be + # the case if the environment is not jupyter-notebook nor + # colab but rather japyterlab or vscode for instance. + web_url = f"ws://{urlparse(viewer_url).netloc}" + html_content = html_content.replace( + "var ws_url = undefined;", f'var ws_url = "{web_url}";') + if interactive_mode() == 1: # Embed HTML in iframe on Jupyter, since it is not # possible to load HTML/Javascript content directly. @@ -877,6 +885,13 @@ def open_gui(start_if_needed: bool = False) -> bool: @staticmethod def has_gui() -> bool: if Viewer.is_alive(): + # Make sure the viewer still has gui if necessary + if Viewer.backend == 'meshcat': + comm_manager = Viewer._backend_obj.comm_manager + if comm_manager is not None: + ack = Viewer._backend_obj.wait(require_client=False) + Viewer._has_gui = any([ + msg == "meshcat:ok" for msg in ack.split(",")]) return Viewer._has_gui return False diff --git a/python/jiminy_pywrap/src/Controllers.cc b/python/jiminy_pywrap/src/Controllers.cc index 19ba90a87..d70887d1d 100644 --- a/python/jiminy_pywrap/src/Controllers.cc +++ b/python/jiminy_pywrap/src/Controllers.cc @@ -271,8 +271,10 @@ namespace python boost::noncopyable>("BaseController") .def("reset", &AbstractController::reset, &AbstractControllerWrapper::default_reset, (bp::arg("self"), bp::arg("reset_dynamic_telemetry") = false)) - .def("compute_command", bp::pure_virtual(&AbstractController::computeCommand)) - .def("internal_dynamics", bp::pure_virtual(&AbstractController::internalDynamics)); + .def("compute_command", bp::pure_virtual(&AbstractController::computeCommand), + (bp::arg("self"), "t", "q", "v", "command")) + .def("internal_dynamics", bp::pure_virtual(&AbstractController::internalDynamics), + (bp::arg("self"), "t", "q", "v", "u_custom")); } };