diff --git a/README.md b/README.md index 8980612..19be085 100644 --- a/README.md +++ b/README.md @@ -41,13 +41,13 @@ make test ## Main classes -This package define 3 main classes representing physical concepts of legged locomotion. Contact Patch, Contact Phase and Contact Sequence. +This package define 4 main classes representing physical concepts of legged locomotion. Contact Model, Contact Patch, Contact Phase and Contact Sequence. -### Contact Patch +A Contact Model define the physical properties of a contact. A Contact Patch define completely a contact between a part of the robot and the environment, it contain a Contact Model. A Contact Phase is defined by a constant set of contacts, it contains one or more Contact Patches. Finally, a Contact Sequence is a sequence of Contact Phases. -A contact patch define the placement (in SE(3), in the world frame) of a contact as long as the friction coefficient for this contact. +### Contact Patch -Future work will also include the type of contact (eg planar, punctual, bilateral ...), a representation of the friction cone or additional information about the contact. +A contact patch define the placement (in SE(3), in the world frame) of a contact between a part of the robot and the environment. It contains a ContactModel. ### Contact Phase diff --git a/include/multicontact-api/bindings/python/scenario/contact-patch.hpp b/include/multicontact-api/bindings/python/scenario/contact-patch.hpp index ec4d742..7bf2c61 100644 --- a/include/multicontact-api/bindings/python/scenario/contact-patch.hpp +++ b/include/multicontact-api/bindings/python/scenario/contact-patch.hpp @@ -19,6 +19,7 @@ template struct ContactPatchPythonVisitor : public boost::python::def_visitor > { typedef typename ContactPatch::Scalar Scalar; typedef typename ContactPatch::SE3 SE3; + typedef typename ContactPatch::ContactModel ContactModel; template void visit(PyClass& cl) const { @@ -26,6 +27,8 @@ struct ContactPatchPythonVisitor : public boost::python::def_visitor(bp::arg("placement"), "Init with a given placement.")) .def(bp::init(bp::args("placement", "friction"), "Init with a given placement and friction coefficient.")) + .def(bp::init(bp::args("placement", "contact_model"), + "Init with a given placement and contact model.")) .def(bp::init(bp::arg("other"), "Copy contructor.")) .add_property("placement", // getter require to use "make_function" to pass the return_internal_reference policy (return ref diff --git a/include/multicontact-api/scenario/contact-patch.hpp b/include/multicontact-api/scenario/contact-patch.hpp index e567bba..35c9376 100644 --- a/include/multicontact-api/scenario/contact-patch.hpp +++ b/include/multicontact-api/scenario/contact-patch.hpp @@ -28,6 +28,9 @@ struct ContactPatchTpl : public serialization::Serializable` defining the set of contact between a specific part of the robot and the environment." + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "True" + ] + }, + "execution_count": 7, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "from multicontact_api import ContactPhase\n", + "\n", + "\n", + "cp = ContactPhase()\n", + "cp.addContact(\"right-feet\", patch2)\n", + "cp.addContact(\"left-feet\", patch3)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "The string argument choice is left to the user and is not required to match anything specifc. However, a good practice when working with a real robot model is to use the name of the frame of the robot creating the contact, as specified in the URDF.\n", + "\n", + "The `ContactPhase` class have several methods to access to the contacts already added:" + ] + }, + { + "cell_type": "code", + "execution_count": 8, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "number of contacts: 2\n", + "effectors in contact: ['right-feet', 'left-feet']\n", + "Is 'right-feet' in contact: True\n", + "Is 'right-hand' in contact: False\n", + "\n", + " Contact placement of the right feet: \n", + " R =\n", + " 0.444451 -0.414554 0.794109\n", + " 0.175408 -0.829041 -0.530964\n", + " 0.878462 0.375281 -0.295751\n", + " p = 0.257742 -0.270431 0.0268018\n", + "\n" + ] + } + ], + "source": [ + "print(\"number of contacts: \", cp.numContacts()) \n", + "print(\"effectors in contact: \", cp.effectorsInContact())\n", + "\n", + "print(\"Is 'right-feet' in contact: \", cp.isEffectorInContact(\"right-feet\"))\n", + "print(\"Is 'right-hand' in contact: \", cp.isEffectorInContact(\"right-hand\"))\n", + "\n", + "# Access the contact patch of a specific contact:\n", + "print(\"\\n Contact placement of the right feet: \\n\", cp.contactPatch(\"right-feet\").placement)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "The `map` can also be retrieved in Python as a Dictionary but this operation add a lot of overhead and should be avoided." + ] + }, + { + "cell_type": "code", + "execution_count": 9, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "effectors in contact: dict_keys(['left-feet', 'right-feet'])\n" + ] + } + ], + "source": [ + "dict_patches = cp.contactPatches()\n", + "print(\"effectors in contact: \", dict_patches.keys())" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "The `ContactPhase` object also store different other optionnal data. For instance, a phase can be defined for a specific time interval:" + ] + }, + { + "cell_type": "code", + "execution_count": 28, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Duration of the phase: 2.5\n" + ] + } + ], + "source": [ + "cp.timeInitial = 1.\n", + "cp.timeFinal = 3.5\n", + "print(\"Duration of the phase: \",cp.duration)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "#### Centroidal data\n", + "Optionnaly, a Contact Phase can store data related to the centroidal dynamic. It store the following initial and final values as public member:" + ] + }, + { + "cell_type": "code", + "execution_count": 11, + "metadata": {}, + "outputs": [], + "source": [ + "# Initial values: the values at the beginning of the phase\n", + "cp.c_init = np.random.rand(3) # CoM position\n", + "cp.dc_init = np.random.rand(3) # CoM velocity\n", + "cp.ddc_init = np.random.rand(3) # CoM acceleration\n", + "cp.L_init = np.random.rand(3) # Angular momentum\n", + "cp.dL_init = np.random.rand(3) # Angular momentum time derivative\n", + "# Final values: the values at the end of the phase:\n", + "cp.c_final = np.random.rand(3)\n", + "cp.dc_final = np.random.rand(3)\n", + "cp.ddc_final = np.random.rand(3)\n", + "cp.L_final = np.random.rand(3)\n", + "cp.dL_final = np.random.rand(3)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "In addition to the initial and final values, a ContactPhase can store a trajectory for all of the following data:\n", + "```Python\n", + "cp.c_t # the CoM trajectory (dimension 3)\n", + "cp.dc_t # the CoM velocity (dimension 3)\n", + "cp.ddc_t # the CoM acceleration (dimension 3)\n", + "cp.L_t # the Angular Momentum (dimension 3)\n", + "cp.dL_t # the Angular Momentum time derivative (dimension 3)\n", + "cp.wrench_t # the centroidal wrench (dimension 6)\n", + "cp.zmp_t # the Zero Moment Point (dimension 3)\n", + "cp.root_t # the base position and orientation (in SE3)\n", + "```\n", + "\n", + "All of this trajectories are represented with objects from the NDCurves library (https://github.com/loco-3d/ndcurves). Example below add a third order polynomial trajectory as the CoM position:" + ] + }, + { + "cell_type": "code", + "execution_count": 12, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "CoM position at t=2. : [1.46198335 2.86437628 1.58724145]\n" + ] + } + ], + "source": [ + "import ndcurves\n", + "from ndcurves import polynomial\n", + "\n", + "coefs = np.random.rand(3, 4) # generate random coefficients for the polynomial: 4 3D points\n", + "# build a 3rdf order polynomial of dimension 3 with random coefficients in a specific time interval:\n", + "c_t = polynomial(coefs, cp.timeInitial, cp.timeFinal) \n", + "\n", + "cp.c_t = c_t\n", + "\n", + "print(\"CoM position at t=2. : \", cp.c_t(2.))" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "#### Whole body data\n", + "\n", + "A Contact Phase can also store data related to the wholebody motion, it store the initial and final wholebody configuration:\n", + "```Python\n", + "cp.q_init\n", + "cp.q_final\n", + "```\n", + "\n", + "As long as the following trajectories:\n", + "```Python\n", + "cp.q_t # joint trajectory\n", + "cp.dq_t # joint velocity trajectory\n", + "cp.ddq_t # joint acceleration trajectory\n", + "cp.tau_t # joint torque trajectory\n", + "```" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "#### Effectors data\n", + "\n", + "The `ContactPhase` class can also store the contact forces and contact normal forces, in a `map` with the effector name as Key. In order to add a contact force trajectory for an effector, this effector must be in contact (see subsection above to add contact to the phase)." + ] + }, + { + "cell_type": "code", + "execution_count": 13, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Contact normal force at t=2. : [2.37947778]\n" + ] + } + ], + "source": [ + "coefs = np.random.rand(1, 4) # generate random coefficients for the polynomial: 4 1D points\n", + "# build a 3rdf order polynomial of dimension 1 with random coefficients in a specific time interval:\n", + "normal_force = polynomial(coefs, cp.timeInitial, cp.timeFinal) \n", + "cp.addContactNormalForceTrajectory(\"right-feet\", normal_force)\n", + "\n", + "print(\"Contact normal force at t=2. :\", cp.contactNormalForce(\"right-feet\")(2.))" + ] + }, + { + "cell_type": "code", + "execution_count": 14, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "True" + ] + }, + "execution_count": 14, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "coefs = np.random.rand(12, 4) # generate random coefficients for the polynomial: 4 12D points\n", + "forces = polynomial(coefs, cp.timeInitial, cp.timeFinal) \n", + "cp.addContactForceTrajectory(\"right-feet\", forces)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "The dimension of the trajectory for the contact forces is not fixed. It is up to the user to decide what is the representation of the forces stored. One of the main representation is to store the 3D force vector at each corner of the feet, leading to a curve of dimension 12. \n", + "\n", + "The `ContactModel` class provide the generator matrix that could be used to map this 12D contact forces to the 6D wrench expressed in the contact frame:" + ] + }, + { + "cell_type": "code", + "execution_count": 15, + "metadata": {}, + "outputs": [ + { + "ename": "ValueError", + "evalue": "matmul: Input operand 1 has a mismatch in its core dimension 0, with gufunc signature (n?,k),(k,m?)->(n?,m?) (size 12 is different from 3)", + "output_type": "error", + "traceback": [ + "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[0;31mValueError\u001b[0m Traceback (most recent call last)", + "\u001b[0;32m\u001b[0m in \u001b[0;36m\u001b[0;34m\u001b[0m\n\u001b[1;32m 1\u001b[0m \u001b[0mcontact_model\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mcp\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mcontactPatch\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m\"right-feet\"\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mcontact_model\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m----> 2\u001b[0;31m \u001b[0mcontact_wrench\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mcontact_model\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mgeneratorMatrix\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m)\u001b[0m \u001b[0;34m@\u001b[0m \u001b[0mcp\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mcontactForce\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m\"right-feet\"\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;36m2.\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m 3\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 4\u001b[0m \u001b[0mprint\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m\"Contact wrench for right-feet at t=2. : \"\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mcontact_wrench\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n", + "\u001b[0;31mValueError\u001b[0m: matmul: Input operand 1 has a mismatch in its core dimension 0, with gufunc signature (n?,k),(k,m?)->(n?,m?) (size 12 is different from 3)" + ] + } + ], + "source": [ + "contact_model = cp.contactPatch(\"right-feet\").contact_model\n", + "contact_wrench = contact_model.generatorMatrix() @ cp.contactForce(\"right-feet\")(2.)\n", + "\n", + "print(\"Contact wrench for right-feet at t=2. : \", contact_wrench)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Finally, the contact phase can also store effector trajectory for the swinging limbs in a `map` with the effector name as Key. The effector cannot be in contact and have an effector trajectory." + ] + }, + { + "cell_type": "code", + "execution_count": 16, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Left hand placement at t=2.5: \n", + " R =\n", + " 1 0 0\n", + " 0 0.587785 -0.809017\n", + " 0 0.809017 0.587785\n", + " p = 2.24 -1.6 -0.3\n", + "\n" + ] + } + ], + "source": [ + "from math import sqrt\n", + "from ndcurves import SE3Curve\n", + "\n", + "# Build two SE3 for the initial/final placement of the feet:\n", + "init_pose = SE3.Identity()\n", + "end_pose = SE3.Identity()\n", + "init_pose.translation = np.array([0.2, -0.7, 0.6])\n", + "end_pose.translation = np.array([3.6, -2.2, -0.9])\n", + "init_pose.rotation = Quaternion.Identity().matrix()\n", + "end_pose.rotation = Quaternion(sqrt(2.) / 2., sqrt(2.) / 2., 0, 0).normalized().matrix()\n", + "# Build a curve in SE3 doing a linear interpolation between this two placements:\n", + "eff_HL = SE3Curve(init_pose, end_pose, cp.timeInitial,cp.timeFinal)\n", + "\n", + "cp.addEffectorTrajectory(\"left-hand\",eff_HL)\n", + "print(\"Left hand placement at t=2.5: \\n\", cp.effectorTrajectory(\"left-hand\").evaluateAsSE3(2.5))" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "\n", + "### Contact Sequence\n", + "\n", + "As soon as a creation or a rupture of contact point occurs the contact set is modified, defining a new contact phase.\n", + "The concatenation of contact phases describes what we name a contact sequence, inside which all the contact phases have their own duration.\n", + "\n", + "A contact sequence is basically a Vector of Contact Phase, with several helper method which can be used to ease the creation of a Contact Sequence. \n", + "\n", + "One can either create a Contact sequence with a know number of contact Phase and correctly set the members of the Contact Phases with: " + ] + }, + { + "cell_type": "code", + "execution_count": 17, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "CoM position at the beginning of the first phase: [0.21871697 0.86272939 0.97332564]\n", + "CoM position at the end of the second phase: [1. 0. 0.85]\n" + ] + } + ], + "source": [ + "from multicontact_api import ContactSequence\n", + "cs = ContactSequence(3)\n", + "\n", + "cs.contactPhases[0] = cp # assign the phase defined below as the first one\n", + "print(\"CoM position at the beginning of the first phase: \", cs.contactPhases[0].c_init)\n", + "\n", + "cp1 = cs.contactPhases[1]\n", + "cp1.c_final = np.array([1., 0. ,0.85])\n", + "print(\"CoM position at the end of the second phase: \", cs.contactPhases[1].c_final)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Or simply append new Contact Phase at the end of the current Contact Sequence:" + ] + }, + { + "cell_type": "code", + "execution_count": 18, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "CoM position at the end of the last phase: [5. 0. 0.85]\n" + ] + } + ], + "source": [ + "cp3 = ContactPhase()\n", + "cp3.c_final = np.array([5, 0, 0.85])\n", + "\n", + "cs.append(cp3)\n", + "print(\"CoM position at the end of the last phase: \", cs.contactPhases[-1].c_final)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "**Helper methods to check a contact sequence**\n", + "\n", + "The ContactSequence class contains several methods to check if the sequence contains some of the optional data, and if they are consistents across all the contact phases. \n", + "This methods should be used in order to check if a ContactSequence object given as input to your algorithm have been correctly initialized with all the data that you are going to use in your algorithm. \n", + "It may also be used to check if your algorithm output consistent data. \n", + "\n", + "Examples of such methods are `haveCentroidalTrajectories` which check that the (c, dc, ddc, L, dL) have been initialized, have the correct duration, and that each trajectories of one phase correctly end at the same value as it begin in the next phase. Here is the list of all such methods:\n", + "\n", + "\n", + "* `haveConsistentContacts` check that there is always one contact change between adjacent phases in the\n", + "sequence and that there isn't any phase without any contact.\n", + "* `haveCOMvalues` Check that the initial and final CoM position values are defined for all phases\n", + "Also check that the initial values of one phase correspond to the final values of the previous ones.\n", + "* `haveAMvalues` Check that the initial and final AM values are defined for all phases\n", + "Also check that the initial values of one phase correspond to the final values of the previous ones.\n", + "* `haveCentroidalValue`s Check that the initial and final CoM position and AM values are defined for all phases\n", + "Also check that the initial values of one phase correspond to the final values of the previous ones.\n", + "* `haveConfigurationsValues` Check that the initial and final configuration are defined for all phases\n", + "Also check that the initial values of one phase correspond to the final values of the previous ones.\n", + "* `haveCOMtrajectories` check that a c, dc and ddc trajectories are defined for each phases\n", + "Also check that the time interval of this trajectories matches the one of the phase\n", + "and that the trajectories start and end and the correct values defined in each phase\n", + "* `haveAMtrajectories` check that a L and dL trajectories are defined for each phases\n", + "Also check that the time interval of this trajectories matches the one of the phase\n", + "and that the trajectories start and end and the correct values defined in each phase\n", + "* `haveCentroidalTrajectories` check that all centroidal trajectories are defined for each phases\n", + "Also check that the time interval of this trajectories matches the one of the phase\n", + "and that the trajectories start and end and the correct values defined in each phase\n", + "* `haveEffectorsTrajectories` check that for each phase preceeding a contact creation,\n", + " an SE3 trajectory is defined for the effector that will be in contact.\n", + "Also check that this trajectory is defined on the time-interval of the phase.\n", + "Also check that the trajectory correctly end at the placement defined for the contact in the next phase.\n", + "If this effector was in contact in the previous phase, it check that the trajectory start at the previous contact\n", + "placement.\n", + "* `haveJointsTrajectories` Check that a q trajectory is defined for each phases\n", + "Also check that the time interval of this trajectories matches the one of the phase\n", + "and that the trajectories start and end and the correct values defined in each phase\n", + "* `haveJointsDerivativesTrajectories` Check that a dq and ddq trajectories are defined for each phases\n", + "Also check that the time interval of this trajectories matches the one of the phase\n", + "and that the trajectories start and end and the correct values defined in each phase\n", + "* `haveJointsTrajectorie`s Check that a joint torque trajectories are defined for each phases\n", + "Also check that the time interval of this trajectories matches the one of the phase\n", + "and that the trajectories start and end and the correct values defined in each phase\n", + "* `haveJointsTrajectories` Check that a contact force trajectory exist for each active contact\n", + "Also check that the time interval of this trajectories matches the one of the phase\n", + "and that the trajectories start and end and the correct values defined in each phase\n", + "* `haveRootTrajectories` check that a root trajectory exist for each contact phases.\n", + "Also check that it start and end at the correct time interval\n", + "* `haveFriction` check that all the contact patch used in the sequence have\n", + "a friction coefficient initialized\n", + "* `haveContactModelDefined` check that all the contact patch have a contact_model defined\n", + "* `haveZMPtrajectories check` that all the contact phases have a zmp trajectory\n", + "\n", + "\n", + "**Helper methods to access Data**\n", + "\n", + "The ContactSequence class also contains methods for easier access to the data contained in the ContactPhase vector. For example, `phaseAtTime` or `phaseIdAtTime` can be used to access a specific ContactPhase at a given time.\n", + "`getAllEffectorsInContact` output all the effectors used to create contact during the sequence. \n", + "\n", + "Finally, methods exists to return the complete trajectory along the contact sequence, concatenating the trajectories of each phases (eg. `concatenateCtrajectories` return the complete c(t) trajectory for all the contact sequence). More details on this can be found on the Third notebook: load_from_file. \n" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Going further\n", + "\n", + "Several helper methods have been added to the ContactSequence class to ease the contact creation process. See the next notebook for more information about this.\n", + "\n", + "As said in the introduction, all the classes of this package can be serialized. The third notebook shows how to load a ContactSequence serialized and how to access some of the data, eg. plotting the centroidal trajectory or displaying the wholebody motion stored. " + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.6.9" + } + }, + "nbformat": 4, + "nbformat_minor": 4 +} diff --git a/notebooks/2_contact_sequence_creation.ipynb b/notebooks/2_contact_sequence_creation.ipynb new file mode 100644 index 0000000..12b99c7 --- /dev/null +++ b/notebooks/2_contact_sequence_creation.ipynb @@ -0,0 +1,668 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Creation of a contact sequence\n", + "\n", + "In this second notebook, we will manually create a contact sequence from a predefined gait. Then, we will add some centroidal data to the contact sequence and export it. \n", + "\n", + "## Contact plan\n", + "\n", + "In this section we will create a contact sequence for a bipedal robot, with a gait alterning double support phases and single support phases, for a simple walking motion. \n", + "\n", + "First, we need to create the first contact phase: a phase with both feet in contact with a flat floor at `z=0`. All the values here are taken for the robot Talos." + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "First phase: \n", + " Contact phase defined for t \\in [-1;-1]\n", + "Conecting (c0,dc0,ddc0,L0,dL0) = \n", + "0 0 0 0 0\n", + "0 0 0 0 0\n", + "0 0 0 0 0\n", + "to (c0,dc0,ddc0,L0,dL0) = \n", + "0 0 0 0 0\n", + "0 0 0 0 0\n", + "0 0 0 0 0\n", + "Effectors in contact 2 : \n", + "______________________________________________\n", + "Effector leg_right_sole_fix_joint contact patch:\n", + "Placement:\n", + " R =\n", + "1 0 0\n", + "0 1 0\n", + "0 0 1\n", + " p = 0 -0.085 0\n", + "\n", + "ContactModel : ContactType: 0, mu: -1\n", + "Number of contact points: 1, positions: \n", + "0\n", + "0\n", + "0\n", + "\n", + "\n", + "Has contact force trajectory : 0\n", + "Has contact normal force trajectory : 0\n", + "______________________________________________\n", + "Effector leg_left_sole_fix_joint contact patch:\n", + "Placement:\n", + " R =\n", + "1 0 0\n", + "0 1 0\n", + "0 0 1\n", + " p = 0 0.085 0\n", + "\n", + "ContactModel : ContactType: 0, mu: -1\n", + "Number of contact points: 1, positions: \n", + "0\n", + "0\n", + "0\n", + "\n", + "\n", + "Has contact force trajectory : 0\n", + "Has contact normal force trajectory : 0\n", + "\n" + ] + } + ], + "source": [ + "# Import the required lib\n", + "import numpy as np\n", + "from pinocchio import SE3\n", + "from multicontact_api import ContactType, ContactModel, ContactPatch, ContactPhase, ContactSequence\n", + "\n", + "#Define the name of the contacts. \n", + "# As explained in the previous notebook, a good practice is to use the names of the frames as defined in the urdf\n", + "rf_name = 'leg_right_sole_fix_joint'\n", + "lf_name = 'leg_left_sole_fix_joint'\n", + "\n", + "OFFSET_Y = 0.085 # the position along the y axis of the feet in the reference configuration\n", + "\n", + "# Create a contact phase:\n", + "p0 = ContactPhase()\n", + "\n", + "# Define the placement of each contact:\n", + "placement_rf = SE3.Identity()\n", + "placement_lf = SE3.Identity()\n", + "translation_rf = np.array([0, -OFFSET_Y, 0])\n", + "translation_lf = np.array([0, OFFSET_Y, 0])\n", + "placement_rf.translation = translation_rf\n", + "placement_lf.translation = translation_lf\n", + "\n", + "# Add both contacts to the contact phase:\n", + "p0.addContact(rf_name, ContactPatch(placement_rf))\n", + "p0.addContact(lf_name, ContactPatch(placement_lf))\n", + "\n", + "print(\"First phase: \\n\", p0)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "As you can see in the print, a lot of data in this contact phase are undefined, we will fill this data.\n", + "Now we can create an empty contact sequence and set this phase as the first one of the sequence:" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Current size of the sequence : 1\n" + ] + } + ], + "source": [ + "cs = ContactSequence()\n", + "cs.append(p0)\n", + "print(\"Current size of the sequence : \", cs.size())" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Now we can add more phases to define the walking motion. The final contact plan will consist of 5 steps for each leg of 20cm forward (with the first and last step only 10cm), thus moving the robot of 1m forward. Let's create the first step, remenber that in our formulation there should only be one contact variation (creation OR removing) between each adjacent phases, one step is thus two contact phases: single support and double support." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "number of contact phases in the contact sequence : 3\n", + "# Right feet contact of phase 0: True\n", + " R =\n", + "1 0 0\n", + "0 1 0\n", + "0 0 1\n", + " p = 0 -0.085 0\n", + "\n", + "# Left feet contact of phase 0: True\n", + " R =\n", + "1 0 0\n", + "0 1 0\n", + "0 0 1\n", + " p = 0 0.085 0\n", + "\n", + "# Right feet contact of phase 1: False\n", + "# Left feet contact of phase 1: True\n", + " R =\n", + "1 0 0\n", + "0 1 0\n", + "0 0 1\n", + " p = 0 0.085 0\n", + "\n", + "# Right feet contact of phase 2: True\n", + " R =\n", + "1 0 0\n", + "0 1 0\n", + "0 0 1\n", + " p = 0.1 -0.085 0\n", + "\n", + "# Left feet contact of phase 2: True\n", + " R =\n", + "1 0 0\n", + "0 1 0\n", + "0 0 1\n", + " p = 0 0.085 0\n", + "\n" + ] + } + ], + "source": [ + "# First, create a new phase where we break the right feet contact:\n", + "p1 = ContactPhase(p0) # copy the previous double support phase\n", + "p1.removeContact(rf_name)\n", + "\n", + "# Now, add it to the sequence:\n", + "cs.append(p1)\n", + "\n", + "# Then, create the second double support phase by creating a new contact with the right foot:\n", + "placement_rf = SE3.Identity()\n", + "translation_rf[0] = 0.1 # move 10cm along the x axis\n", + "placement_rf.translation = translation_rf\n", + "\n", + "p2 = ContactPhase(p1) # copy the previous phase\n", + "p2.addContact(rf_name, ContactPatch(placement_rf))\n", + "\n", + "# Now, add it to the sequence:\n", + "cs.append(p2)\n", + "\n", + "# Lets print the result:\n", + "print(\"number of contact phases in the contact sequence : \", cs.size())\n", + "\n", + "#first phase:\n", + "print(\"# Right feet contact of phase 0: \", cs.contactPhases[0].isEffectorInContact(rf_name))\n", + "print(cs.contactPhases[0].contactPatch(rf_name).placement)\n", + "print(\"# Left feet contact of phase 0: \", cs.contactPhases[0].isEffectorInContact(lf_name))\n", + "print(cs.contactPhases[0].contactPatch(lf_name).placement)\n", + "#second phase:\n", + "print(\"# Right feet contact of phase 1: \", cs.contactPhases[1].isEffectorInContact(rf_name))\n", + "print(\"# Left feet contact of phase 1: \", cs.contactPhases[1].isEffectorInContact(lf_name))\n", + "print(cs.contactPhases[1].contactPatch(lf_name).placement)\n", + "#Third phase:\n", + "print(\"# Right feet contact of phase 2: \", cs.contactPhases[2].isEffectorInContact(rf_name))\n", + "print(cs.contactPhases[2].contactPatch(rf_name).placement)\n", + "print(\"# Left feet contact of phase 2: \", cs.contactPhases[2].isEffectorInContact(lf_name))\n", + "print(cs.contactPhases[2].contactPatch(lf_name).placement)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "As expected we now have a contact sequence with 3 phases: a double support, a single support and a double support. The code above is quite verbose, fortunately several helper methods exist to achieve the same result easier. Lets create the second step with the left foot with this helpers:" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Current contact sequence size: 5\n" + ] + } + ], + "source": [ + "# This method add a new contact phase to the sequence, \n", + "# copy the contacts from the previous phase exept the one specified\n", + "cs.breakContact(lf_name) \n", + "\n", + "translation_lf[0] = 0.2 # move 20cm forward\n", + "placement_lf.translation = translation_lf\n", + "\n", + "# This method add a new contact phase to the sequence, \n", + "# copy the contacts from the previous phase and add the one specified\n", + "cs.createContact(lf_name, ContactPatch(placement_lf))\n", + "\n", + "print(\"Current contact sequence size: \", cs.size())" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "For the next steps, we will use another helper usefull for gaited motion. This method is used to \"reposition\" a contact and automatically add the intermediate contact phase with the broken contact. " + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Final contact sequence size: 23\n", + "Right foot position at the end of the motion: \n", + " [ 1. -0.085 0. ]\n", + "Left foot position at the end of the motion: \n", + " [1. 0.085 0. ]\n" + ] + } + ], + "source": [ + "# First define the step length:\n", + "displacement = SE3.Identity()\n", + "displacement.translation = np.array([0.2, 0, 0]) # 20cm forward\n", + "\n", + "for _ in range(4):\n", + " cs.moveEffectorOf(rf_name, displacement)\n", + " cs.moveEffectorOf(lf_name, displacement)\n", + "\n", + "# add the last step of only 10cm to end the motion with both feet side by side:\n", + "displacement.translation = np.array([0.1, 0, 0])\n", + "cs.moveEffectorOf(rf_name, displacement)\n", + "\n", + "print(\"Final contact sequence size: \", cs.size())\n", + "print(\"Right foot position at the end of the motion: \\n\",\n", + " cs.contactPhases[-1].contactPatch(rf_name).placement.translation)\n", + "print(\"Left foot position at the end of the motion: \\n\",\n", + " cs.contactPhases[-1].contactPatch(lf_name).placement.translation)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "At this point the contact sequence define a consistent contact plan, we can check this with the following method:" + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "True" + ] + }, + "execution_count": 6, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "cs.haveConsistentContacts()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "This method check that there is no discontinuities between the phases, and that there is always 1 contact variation between each phases.\n", + "\n", + "\n", + "### Additionnal data\n", + "\n", + "#### Contact model\n", + "\n", + "The Contact phases created do not specify any contact model (see the previous notebook for more information about this). We can check that the contact model are indeed not defined with the following code:\n" + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Friction coefficient defined for all contacts: False\n", + "Contact models defined for all contacts: False\n" + ] + } + ], + "source": [ + "print(\"Friction coefficient defined for all contacts: \", cs.haveFriction())\n", + "print(\"Contact models defined for all contacts: \", cs.haveContactModelDefined())" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + " We can create a contact model specific to the robot feet and assign it to all the contact patches of all phases with this code:" + ] + }, + { + "cell_type": "code", + "execution_count": 8, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Friction coefficient defined for all contacts: True\n", + "Contact models defined for all contacts: True\n" + ] + } + ], + "source": [ + "# Create a contact model with a friction coefficient of 0.5 and the PLANAR type\n", + "contact_model = ContactModel(0.5, ContactType.CONTACT_PLANAR)\n", + "\n", + "# Define 4 contacts points at the corners of a rectangle:\n", + "contact_model.num_contact_points = 4\n", + "\n", + "lx = 0.2 / 2. # half size of the feet along x axis\n", + "ly = 0.13 / 2. # half size of the feet along y axis\n", + "\n", + "contact_points = np.zeros([3,4])\n", + "contact_points[0, :] = [-lx, -lx, lx, lx]\n", + "contact_points[1, :] = [-ly, ly, -ly, ly]\n", + "contact_model.contact_points_positions = contact_points\n", + "\n", + "# Now, add this model to all patches of all phases:\n", + "\n", + "for phase in cs.contactPhases:\n", + " for ee_name in phase.effectorsInContact():\n", + " phase.contactPatch(ee_name).contact_model = contact_model\n", + "\n", + " \n", + "print(\"Friction coefficient defined for all contacts: \", cs.haveFriction())\n", + "print(\"Contact models defined for all contacts: \", cs.haveContactModelDefined())" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "#### Phase duration\n", + "\n", + "As explained in the previous notebook, a contact phase may be defined on a specific time interval. We can check if this is correctly defined for all the phases and that the timings are consistent with the following method:" + ] + }, + { + "cell_type": "code", + "execution_count": 9, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "False" + ] + }, + "execution_count": 9, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "cs.haveTimings()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "The code below set the duration for all the phases, depending on the number of contacts of this phase. We are going to define long phase duration here because in the next section we are going to generate a quasi-static centroidal reference." + ] + }, + { + "cell_type": "code", + "execution_count": 10, + "metadata": { + "scrolled": true + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Contact sequence have consistent timings: True\n" + ] + } + ], + "source": [ + "DURATION_SS = 2. # duration of the single support phases\n", + "DURATION_DS = 4. # duration of the double support phases\n", + "\n", + "for i, phase in enumerate(cs.contactPhases):\n", + " if i == 0:\n", + " phase.timeInitial = 0.\n", + " else:\n", + " # Set the initial time as the final time of the previous phase\n", + " phase.timeInitial = cs.contactPhases[i-1].timeFinal\n", + " # set the duration of the phase based on the number of contacts\n", + " if phase.numContacts() == 1:\n", + " phase.duration = DURATION_SS\n", + " elif phase.numContacts() == 2:\n", + " phase.duration = DURATION_DS\n", + " else:\n", + " raise RuntimeError(\"Incorrect number of contacts for the phase \" + str(i))\n", + " \n", + "# Check that the timings are correctly set :\n", + "print(\"Contact sequence have consistent timings: \", cs.haveTimings())" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Centroidal data\n", + "\n", + "Now that the contact sequence correctly define a contact plan, we are going to store centroidal data to the sequence: the center of mass position, velocity and acceleration trajectory. \n", + "As this notebook is about the multicontact_api package and not about centroidal trajectory optimization, we are going to use really simple and quasi-static trajectory:\n", + "\n", + "During the single support phases, the CoM is fixed above the center of the support polygon. During the double support phases, the CoM will go from the previous support polygon to the next one in a straight line (starting and ending with a null velocity and acceleration).\n", + "\n", + "First, we need to compute the initial and final CoM position for each phases, the CoM velocity and acceleration are initialized to 0 by defaut so we do not need to modify it here." + ] + }, + { + "cell_type": "code", + "execution_count": 11, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Final CoM position: [1. 0. 0.85]\n" + ] + } + ], + "source": [ + "# Define the CoM height:\n", + "COM_HEIGHT = 0.85\n", + "\n", + "\n", + "for i, phase in enumerate(cs.contactPhases):\n", + " if i == 0:\n", + " # Define the initial CoM position:\n", + " cs.contactPhases[0].c_init = np.array([0, 0, COM_HEIGHT])\n", + " elif phase.numContacts() == 1:\n", + " # Single support phase: set the CoM position above the feet in contact:\n", + " com = phase.contactPatch(phase.effectorsInContact()[0]).placement.translation\n", + " com[2] += COM_HEIGHT\n", + " phase.c_init = com\n", + " # The CoM is not moving during single support phase, so we set the same position as the final point\n", + " phase.c_final = com\n", + " # Set the final point of the previous double support phase to be the same as this position\n", + " cs.contactPhases[i-1].c_final = com\n", + " elif phase.numContacts() == 2:\n", + " # Double support phase: \n", + " # set the initial CoM position to be equal to the final position of the previous phase\n", + " phase.c_init = cs.contactPhases[i-1].c_final\n", + " else:\n", + " raise RuntimeError(\"Incorrect number of contacts for the phase \" + str(i))\n", + " \n", + "# For the final phase: set the final position between the feets:\n", + "com = (phase.contactPatch(rf_name).placement.translation + phase.contactPatch(lf_name).placement.translation) / 2.\n", + "com[2] += COM_HEIGHT\n", + "phase.c_final = com\n", + " \n", + "print(\"Final CoM position: \",cs.contactPhases[-1].c_final)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "We can check that the position for all the phases have been set and that the initial position of each phase match the final position of the previous phase like this:" + ] + }, + { + "cell_type": "code", + "execution_count": 12, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "True" + ] + }, + "execution_count": 12, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "cs.haveCentroidalValues()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Now we can generate trajectories for each phases. For the single support phases we will use Constant trajectories. For the double support phases we will use fifth order polynomials that connect the initial and final position and have a null initial and final velocity and acceleration. \n", + "\n", + "As explained in the previous notebook, this trajectories must be represented with objects from the `NDCurves` package.\n" + ] + }, + { + "cell_type": "code", + "execution_count": 15, + "metadata": { + "scrolled": true + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Contact sequence have consistent CoM trajectories: True\n" + ] + } + ], + "source": [ + "from ndcurves import polynomial\n", + "\n", + "for phase in cs.contactPhases:\n", + " if phase.numContacts() == 1:\n", + " # Single support phase: build a constant trajectory at the CoM position:\n", + " phase.c_t = polynomial(phase.c_init, phase.timeInitial, phase.timeFinal) # Build a degree 0 polynomial curve\n", + " # Compute the derivate of this curve and store it in the phase\n", + " phase.dc_t = phase.c_t.compute_derivate(1)\n", + " phase.ddc_t = phase.dc_t.compute_derivate(1)\n", + " elif phase.numContacts() == 2:\n", + " # Double support phase: build a minJerk trajectory (5th degree) between the initial and final CoM position\n", + " phase.c_t = polynomial.MinimumJerk(phase.c_init, phase.c_final, phase.timeInitial, phase.timeFinal)\n", + " phase.dc_t = phase.c_t.compute_derivate(1)\n", + " phase.ddc_t = phase.dc_t.compute_derivate(1)\n", + " else:\n", + " raise RuntimeError(\"Incorrect number of contacts.\")\n", + "\n", + "\n", + "# Check that all the phases have CoM trajectories:\n", + "print(\"Contact sequence have consistent CoM trajectories: \", cs.haveCOMtrajectories())" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Conclusion\n", + "\n", + "In this notebook we saw how to manually build a `ContactSequence` from scratch. Adding several phases with contact placement and time interval. Then adding CoM trajectories for each phases. \n", + "\n", + "In the next notebook we will se how to use the data inside a contact sequence, plot it and display it on a 3D viewer. \n", + "\n", + "The last code below show how to export the ContactSequence build in this notebook to a file. " + ] + }, + { + "cell_type": "code", + "execution_count": 16, + "metadata": {}, + "outputs": [], + "source": [ + "# Serialize the contact sequence\n", + "cs.saveAsBinary(\"notebook2.cs\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.6.9" + } + }, + "nbformat": 4, + "nbformat_minor": 4 +} diff --git a/notebooks/3_load_from_file.ipynb b/notebooks/3_load_from_file.ipynb new file mode 100644 index 0000000..ad69912 --- /dev/null +++ b/notebooks/3_load_from_file.ipynb @@ -0,0 +1,2338 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Introduction\n", + "In this notebook, we will load serialized `ContactSequence` files from the \"examples\" directory. See the Readme file of this directory for more information about this files: https://github.com/loco-3d/multicontact-api/tree/master/examples\n", + "\n", + "First we will plot various data stored in the file like CoM trajectory or contact placement, then we will visualize the wholebody motion.\n", + "\n", + "## Displaying the CoM trajectory\n", + "\n", + "The first step is to load the ContactSequence from a file. Feel free to change the name of the file loaded to any of the files in the example folder, but do not use the files without any suffix as they do not store any centroidal data. " + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Number of contact phases: 23\n", + "Duration of the motion: 35.00000000000026\n" + ] + } + ], + "source": [ + "import ndcurves\n", + "from pinocchio import SE3\n", + "from multicontact_api import ContactSequence\n", + "cs = ContactSequence()\n", + "cs.loadFromBinary(\"../examples/walk_20cm_REF.cs\")\n", + "\n", + "print(\"Number of contact phases: \", cs.size())\n", + "print(\"Duration of the motion: \", cs.contactPhases[-1].timeFinal)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Now we are going to plot the CoM position and it's derivatives for the complete motion. As seen in the previous notebook, each contact phases of the sequence store the trajectoires for this specific time interval, in order to plot the traejctories for the complete motion we should iterate over all the phases. \n", + "\n", + "Fortunately, there is several helper methods that you can use to concatenate the trajectories of all the phases in a single `NDCurves` object which can be manipulated easily. " + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "c_t = cs.concatenateCtrajectories()\n", + "dc_t = cs.concatenateDCtrajectories()\n", + "ddc_t = cs.concatenateDDCtrajectories()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Then, let's create an helper method to discretize any NDCurves object at the desired frequency:" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [], + "source": [ + "def discretize_curve(curve,dt):\n", + " \"\"\"\n", + " Discretize the given curve at the given dt\n", + " return the result as an array (one column per discret point)\n", + " In case where the time interval of the curve is not a multiple of dt, the last point is still included\n", + " This mean that the timestep between the two last points may be less than dt\n", + " :param curve: a curve object, require operator (), min() and max()\n", + " :param dt: the discretization step\n", + " :return: an array of shape (curve.dim(), numPoints) and an array corresponding to the timeline\n", + " \"\"\"\n", + " numPoints = round((curve.max() - curve.min()) / dt ) + 1\n", + " res = np.zeros([curve.dim(), numPoints])\n", + " timeline = np.zeros(numPoints)\n", + " t = curve.min()\n", + " for i in range(numPoints):\n", + " res[:, i] = curve(t)\n", + " timeline[i] = t\n", + " t += dt\n", + " if t > curve.max():\n", + " t = curve.max()\n", + " return res, timeline" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "The first simple plot will display the x-y position of the CoM:" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [ + { + "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 (fig.ratio !== 1) {\n", + " fig.send_message('set_dpi_ratio', { dpi_ratio: fig.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", + " this.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", + " // Apply a ponyfill if ResizeObserver is not implemented by browser.\n", + " if (this.ResizeObserver === undefined) {\n", + " if (window.ResizeObserver !== undefined) {\n", + " this.ResizeObserver = window.ResizeObserver;\n", + " } else {\n", + " var obs = _JSXTOOLS_RESIZE_OBSERVER({});\n", + " this.ResizeObserver = obs.ResizeObserver;\n", + " }\n", + " }\n", + "\n", + " this.resizeObserverInstance = new this.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 * fig.ratio);\n", + " canvas.setAttribute('height', height * fig.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 (fig.ws.readyState == 1 && width != 0 && height != 0) {\n", + " fig.request_resize(width, height);\n", + " }\n", + " }\n", + " });\n", + " this.resizeObserverInstance.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'] / fig.ratio;\n", + " var y0 = (fig.canvas.height - msg['y0']) / fig.ratio;\n", + " var x1 = msg['x1'] / fig.ratio;\n", + " var y1 = (fig.canvas.height - msg['y1']) / fig.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 / fig.ratio,\n", + " fig.canvas.height / fig.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 * this.ratio;\n", + " var y = canvas_pos.y * this.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", + "\n", + "///////////////// REMAINING CONTENT GENERATED BY embed_js.py /////////////////\n", + "// prettier-ignore\n", + "var _JSXTOOLS_RESIZE_OBSERVER=function(A){var t,i=new WeakMap,n=new WeakMap,a=new WeakMap,r=new WeakMap,o=new Set;function s(e){if(!(this instanceof s))throw new TypeError(\"Constructor requires 'new' operator\");i.set(this,e)}function h(){throw new TypeError(\"Function is not a constructor\")}function c(e,t,i,n){e=0 in arguments?Number(arguments[0]):0,t=1 in arguments?Number(arguments[1]):0,i=2 in arguments?Number(arguments[2]):0,n=3 in arguments?Number(arguments[3]):0,this.right=(this.x=this.left=e)+(this.width=i),this.bottom=(this.y=this.top=t)+(this.height=n),Object.freeze(this)}function d(){t=requestAnimationFrame(d);var s=new WeakMap,p=new Set;o.forEach((function(t){r.get(t).forEach((function(i){var r=t instanceof window.SVGElement,o=a.get(t),d=r?0:parseFloat(o.paddingTop),f=r?0:parseFloat(o.paddingRight),l=r?0:parseFloat(o.paddingBottom),u=r?0:parseFloat(o.paddingLeft),g=r?0:parseFloat(o.borderTopWidth),m=r?0:parseFloat(o.borderRightWidth),w=r?0:parseFloat(o.borderBottomWidth),b=u+f,F=d+l,v=(r?0:parseFloat(o.borderLeftWidth))+m,W=g+w,y=r?0:t.offsetHeight-W-t.clientHeight,E=r?0:t.offsetWidth-v-t.clientWidth,R=b+v,z=F+W,M=r?t.width:parseFloat(o.width)-R-E,O=r?t.height:parseFloat(o.height)-z-y;if(n.has(t)){var k=n.get(t);if(k[0]===M&&k[1]===O)return}n.set(t,[M,O]);var S=Object.create(h.prototype);S.target=t,S.contentRect=new c(u,d,M,O),s.has(i)||(s.set(i,[]),p.add(i)),s.get(i).push(S)}))})),p.forEach((function(e){i.get(e).call(e,s.get(e),e)}))}return s.prototype.observe=function(i){if(i instanceof window.Element){r.has(i)||(r.set(i,new Set),o.add(i),a.set(i,window.getComputedStyle(i)));var n=r.get(i);n.has(this)||n.add(this),cancelAnimationFrame(t),t=requestAnimationFrame(d)}},s.prototype.unobserve=function(i){if(i instanceof window.Element&&r.has(i)){var n=r.get(i);n.has(this)&&(n.delete(this),n.size||(r.delete(i),o.delete(i))),n.size||r.delete(i),o.size||cancelAnimationFrame(t)}},A.DOMRectReadOnly=c,A.ResizeObserver=s,A.ResizeObserverEntry=h,A}; // eslint-disable-line\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", + " fig.cell_info[0].output_area.element.on(\n", + " 'cleared',\n", + " { fig: fig },\n", + " fig._remove_fig_handler\n", + " );\n", + "};\n", + "\n", + "mpl.figure.prototype.handle_close = function (fig, msg) {\n", + " var width = fig.canvas.width / fig.ratio;\n", + " fig.cell_info[0].output_area.element.off(\n", + " 'cleared',\n", + " fig._remove_fig_handler\n", + " );\n", + " fig.resizeObserverInstance.unobserve(fig.canvas_div);\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 / this.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 (event) {\n", + " var fig = event.data.fig;\n", + " if (event.target !== this) {\n", + " // Ignore bubbled events from children.\n", + " return;\n", + " }\n", + " fig.close_ws(fig, {});\n", + "};\n", + "\n", + "mpl.figure.prototype._root_extra_style = function (el) {\n", + " el.style.boxSizing = 'content-box'; // override notebook setting of border-box.\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": [ + "" + ] + }, + "metadata": {}, + "output_type": "display_data" + }, + { + "data": { + "text/html": [ + "" + ], + "text/plain": [ + "" + ] + }, + "metadata": {}, + "output_type": "display_data" + }, + { + "data": { + "text/plain": [ + "[]" + ] + }, + "execution_count": 4, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "import numpy as np\n", + "import matplotlib\n", + "%matplotlib notebook\n", + "import matplotlib.pyplot as plt\n", + "\n", + "# Discretize the curve\n", + "DT = 0.01\n", + "c_k = discretize_curve(c_t, DT)[0]\n", + "\n", + "# Create a new plot\n", + "fig = plt.figure(\"CoM position(xy)\")\n", + "plt.xlabel(\"x position (m)\")\n", + "plt.ylabel(\"y position (m)\")\n", + "plt.axis('equal')\n", + "plt.grid(True)\n", + "plt.plot(c_k[0,:], c_k[1,:]) # plot the CoM position along x in function of the y position" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "The previous plot coul be improved by adding the contact positions. Lets display all the contacts as small circles. " + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "metadata": {}, + "outputs": [], + "source": [ + "colors = [\"g\", \"r\"] # define the colors used for the circles\n", + "circle_radius = 0.03 \n", + "effector_list = cs.getAllEffectorsInContact()\n", + "ax = fig.gca()\n", + "\n", + "# Iterate for all phases\n", + "for p in cs.contactPhases:\n", + " for ee_name in p.effectorsInContact():\n", + " # plot x for the center of the feets contact,\n", + " # and a circle of 3cm of radius around it :\n", + " pos = p.contactPatch(ee_name).placement.translation\n", + " color = colors[effector_list.index(ee_name)]\n", + " plt.plot(pos[0], pos[1], marker=\"x\", markersize=5, color=color)\n", + " circle_r = plt.Circle((pos[0], pos[1]), circle_radius, color=color, fill=False)\n", + " ax.add_artist(circle_r)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "One last addition to this plot could be to add the ZMP trajectory (as a black-dashed line):" + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "[]" + ] + }, + "execution_count": 7, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "zmp_t = cs.concatenateZMPtrajectories()\n", + "zmp_k = discretize_curve(zmp_t, DT)[0]\n", + "plt.plot(zmp_k[0,:], zmp_k[1,:], color = 'k', linestyle='dashed')" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "For a second plot, we are going to display the CoM position and it's two derivatives in a 3*3 plot. The lines are the position, velocity and acceleration and the column the x, y, z axis.\n" + ] + }, + { + "cell_type": "code", + "execution_count": 8, + "metadata": {}, + "outputs": [ + { + "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 (fig.ratio !== 1) {\n", + " fig.send_message('set_dpi_ratio', { dpi_ratio: fig.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", + " this.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", + " // Apply a ponyfill if ResizeObserver is not implemented by browser.\n", + " if (this.ResizeObserver === undefined) {\n", + " if (window.ResizeObserver !== undefined) {\n", + " this.ResizeObserver = window.ResizeObserver;\n", + " } else {\n", + " var obs = _JSXTOOLS_RESIZE_OBSERVER({});\n", + " this.ResizeObserver = obs.ResizeObserver;\n", + " }\n", + " }\n", + "\n", + " this.resizeObserverInstance = new this.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 * fig.ratio);\n", + " canvas.setAttribute('height', height * fig.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 (fig.ws.readyState == 1 && width != 0 && height != 0) {\n", + " fig.request_resize(width, height);\n", + " }\n", + " }\n", + " });\n", + " this.resizeObserverInstance.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'] / fig.ratio;\n", + " var y0 = (fig.canvas.height - msg['y0']) / fig.ratio;\n", + " var x1 = msg['x1'] / fig.ratio;\n", + " var y1 = (fig.canvas.height - msg['y1']) / fig.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 / fig.ratio,\n", + " fig.canvas.height / fig.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 * this.ratio;\n", + " var y = canvas_pos.y * this.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", + "\n", + "///////////////// REMAINING CONTENT GENERATED BY embed_js.py /////////////////\n", + "// prettier-ignore\n", + "var _JSXTOOLS_RESIZE_OBSERVER=function(A){var t,i=new WeakMap,n=new WeakMap,a=new WeakMap,r=new WeakMap,o=new Set;function s(e){if(!(this instanceof s))throw new TypeError(\"Constructor requires 'new' operator\");i.set(this,e)}function h(){throw new TypeError(\"Function is not a constructor\")}function c(e,t,i,n){e=0 in arguments?Number(arguments[0]):0,t=1 in arguments?Number(arguments[1]):0,i=2 in arguments?Number(arguments[2]):0,n=3 in arguments?Number(arguments[3]):0,this.right=(this.x=this.left=e)+(this.width=i),this.bottom=(this.y=this.top=t)+(this.height=n),Object.freeze(this)}function d(){t=requestAnimationFrame(d);var s=new WeakMap,p=new Set;o.forEach((function(t){r.get(t).forEach((function(i){var r=t instanceof window.SVGElement,o=a.get(t),d=r?0:parseFloat(o.paddingTop),f=r?0:parseFloat(o.paddingRight),l=r?0:parseFloat(o.paddingBottom),u=r?0:parseFloat(o.paddingLeft),g=r?0:parseFloat(o.borderTopWidth),m=r?0:parseFloat(o.borderRightWidth),w=r?0:parseFloat(o.borderBottomWidth),b=u+f,F=d+l,v=(r?0:parseFloat(o.borderLeftWidth))+m,W=g+w,y=r?0:t.offsetHeight-W-t.clientHeight,E=r?0:t.offsetWidth-v-t.clientWidth,R=b+v,z=F+W,M=r?t.width:parseFloat(o.width)-R-E,O=r?t.height:parseFloat(o.height)-z-y;if(n.has(t)){var k=n.get(t);if(k[0]===M&&k[1]===O)return}n.set(t,[M,O]);var S=Object.create(h.prototype);S.target=t,S.contentRect=new c(u,d,M,O),s.has(i)||(s.set(i,[]),p.add(i)),s.get(i).push(S)}))})),p.forEach((function(e){i.get(e).call(e,s.get(e),e)}))}return s.prototype.observe=function(i){if(i instanceof window.Element){r.has(i)||(r.set(i,new Set),o.add(i),a.set(i,window.getComputedStyle(i)));var n=r.get(i);n.has(this)||n.add(this),cancelAnimationFrame(t),t=requestAnimationFrame(d)}},s.prototype.unobserve=function(i){if(i instanceof window.Element&&r.has(i)){var n=r.get(i);n.has(this)&&(n.delete(this),n.size||(r.delete(i),o.delete(i))),n.size||r.delete(i),o.size||cancelAnimationFrame(t)}},A.DOMRectReadOnly=c,A.ResizeObserver=s,A.ResizeObserverEntry=h,A}; // eslint-disable-line\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", + " fig.cell_info[0].output_area.element.on(\n", + " 'cleared',\n", + " { fig: fig },\n", + " fig._remove_fig_handler\n", + " );\n", + "};\n", + "\n", + "mpl.figure.prototype.handle_close = function (fig, msg) {\n", + " var width = fig.canvas.width / fig.ratio;\n", + " fig.cell_info[0].output_area.element.off(\n", + " 'cleared',\n", + " fig._remove_fig_handler\n", + " );\n", + " fig.resizeObserverInstance.unobserve(fig.canvas_div);\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 / this.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 (event) {\n", + " var fig = event.data.fig;\n", + " if (event.target !== this) {\n", + " // Ignore bubbled events from children.\n", + " return;\n", + " }\n", + " fig.close_ws(fig, {});\n", + "};\n", + "\n", + "mpl.figure.prototype._root_extra_style = function (el) {\n", + " el.style.boxSizing = 'content-box'; // override notebook setting of border-box.\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": [ + "" + ] + }, + "metadata": {}, + "output_type": "display_data" + }, + { + "data": { + "text/html": [ + "" + ], + "text/plain": [ + "" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "labels = [\"x (m)\", \"y (m)\", \"z (m)\",\n", + " \"dx (m/s)\", \"dy (m/s)\", \"dz (m/s)\",\n", + " \"ddx (m/s^2)\", \"ddy (m/s^2)\", \"ddz (m/s^2)\"]\n", + "colors = ['r', 'g', 'b']\n", + "\n", + "# create a figure with 3*3 subplots\n", + "fig, ax = plt.subplots(3, 3)\n", + "fig.suptitle(\"COM trajectories\")\n", + "\n", + "# discretize the CoM trajectories and it's derivatives:\n", + "c_k, timeline = discretize_curve(c_t, DT)\n", + "dc_k = discretize_curve(dc_t, DT)[0]\n", + "ddc_k = discretize_curve(ddc_t, DT)[0]\n", + "\n", + "# stack the values for easier access in the loops:\n", + "values = np.vstack([c_k, dc_k, ddc_k])\n", + "\n", + "for i in range(3): # line = pos,vel,acc\n", + " for j in range(3): # col = x,y,z\n", + " ax_sub = ax[i, j]\n", + " ax_sub.plot(timeline.T, values[i * 3 + j, :].T, color=colors[j])\n", + " ax_sub.set_xlabel('time (s)')\n", + " ax_sub.set_ylabel(labels[i * 3 + j])\n", + " ax_sub.yaxis.grid()\n" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Finally, we can add to the previous plot small vertical line showing the contact changes timings. " + ] + }, + { + "cell_type": "code", + "execution_count": 9, + "metadata": {}, + "outputs": [], + "source": [ + "for i in range(3): # line = pos,vel,acc\n", + " for j in range(3): # col = x,y,z\n", + " ax_sub = ax[i, j]\n", + " # Add vertical line at each contact change time\n", + " for phase in cs.contactPhases: \n", + " ax_sub.axvline(phase.timeFinal, linestyle=\"-.\", color='k')" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Displaying a joint trajectory\n", + "\n", + "For this section you will need two additionnal depencies: the simplifed Talos model and the Meshcat 3D visualizer:\n", + "\n", + "* install the binary package `robotpkg-py3\\*-example-robot-data` (see http://robotpkg.openrobots.org/robotpkg-wip.html if you haven't configured robotpkg on your system yet\n", + "* `pip3 install --user meshcat`\n", + "\n", + "First, let's load the contact sequence from the file. Feel free to change the name of the file loaded to any of the files in the example folder, but be sure to use the `_WB` suffix: the other files do not store joint trajectories." + ] + }, + { + "cell_type": "code", + "execution_count": 10, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Number of contact phases: 23\n", + "Duration of the motion: 35.00000000000026\n" + ] + } + ], + "source": [ + "from multicontact_api import ContactSequence\n", + "cs = ContactSequence()\n", + "cs.loadFromBinary(\"../examples/walk_20cm_WB.cs\")\n", + "\n", + "print(\"Number of contact phases: \", cs.size())\n", + "print(\"Duration of the motion: \", cs.contactPhases[-1].timeFinal)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Then let's load the robot model in Pinocchio and initialize the viewer:" + ] + }, + { + "cell_type": "code", + "execution_count": 11, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "You can open the visualizer by visiting the following URL:\n", + "http://127.0.0.1:7000/static/\n" + ] + } + ], + "source": [ + "from ipywidgets import interact\n", + "from example_robot_data import load\n", + "from pinocchio.visualize import MeshcatVisualizer\n", + "\n", + "robot = load('talos')\n", + "viz = MeshcatVisualizer(robot.model, robot.collision_model, robot.visual_model)\n", + "viz.initViewer(loadModel=True)\n", + "viz.viewer.jupyter_cell()\n", + "\n", + "q0 = robot.q0\n", + "viz.display(q0)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "After the initialization you should get a link to open meshcat in your browser, and you should see Talos standing in it's reference configuration. \n", + "\n", + "Now we are simply going to retrieve the complete joint trajectory from the file (see the previous section), discretize it and send the configurations to the viewer." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Warning : display not real time ! choose a greater time step for the display.\n" + ] + } + ], + "source": [ + "import ndcurves\n", + "import time\n", + "\n", + "# concatenate the joint trajectories of each phases in a single curve object\n", + "q_t = cs.concatenateQtrajectories()\n", + "DT = 0.04 # 25 FPS\n", + "\n", + "t = 0.\n", + "t_end = q_t.max()\n", + "\n", + "while t <= t_end:\n", + " t_start = time.time() \n", + " viz.display(q_t(t)) # Display the current configuration\n", + " elapsed = time.time() - t_start # Save the time required to display one configuration\n", + " if elapsed > DT:\n", + " print(\"Warning : display not real time ! choose a greater time step for the display.\")\n", + " else:\n", + " time.sleep(DT - elapsed)\n", + " t += DT" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.6.9" + } + }, + "nbformat": 4, + "nbformat_minor": 4 +} diff --git a/unittest/python/scenario.py b/unittest/python/scenario.py index 4b46cd9..208943b 100644 --- a/unittest/python/scenario.py +++ b/unittest/python/scenario.py @@ -289,6 +289,20 @@ def test_constructor_with_arguments(self): self.assertTrue(cp.friction == 0.9) self.assertTrue(cp.placement == p) + def test_constructor_with_contact_model(self): + cm = ContactModel(0.5, ContactType.CONTACT_PLANAR) + cm.num_contact_points = 4 + p = SE3() + p.setRandom() + cp = ContactPatch(p, cm) + self.assertTrue(cp.friction == 0.5) + self.assertTrue(cp.placement == p) + self.assertTrue(cp.contact_model.num_contact_points == 4) + + # check that the value have been copied and it's not the same pointer anymore : + cm.num_contact_points = 6 + self.assertTrue(cp.contact_model.num_contact_points == 4) + def test_operator_equal(self): cp1 = ContactPatch() cp2 = ContactPatch() diff --git a/unittest/scenario.cpp b/unittest/scenario.cpp index c5fc643..10b393c 100644 --- a/unittest/scenario.cpp +++ b/unittest/scenario.cpp @@ -469,6 +469,15 @@ BOOST_AUTO_TEST_CASE(contact_patch) { BOOST_CHECK(cp2.placement() == p); BOOST_CHECK(cp2.friction() == 0.9); + // constructor with contact_model + ContactModel cm(0.5, ContactType::CONTACT_PLANAR); + cm.num_contact_points(4); + ContactPatch cp_with_model(p, cm); + BOOST_CHECK(cp_with_model.placement() == p); + BOOST_CHECK(cp_with_model.friction() == 0.5); + BOOST_CHECK(cp_with_model.m_contact_model.num_contact_points() == 4); + + // check comparison operator BOOST_CHECK(cp1 != cp2); ContactPatch cp3(p, 0.9);