A_pinocchio_cheat_notebook.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "2a07c6b1-dfcc-4d35-a14e-504c52cff6b8",
   "metadata": {},
   "source": [
    "## Pinocchio library\n",
    "Let us look at what is inside !"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "13107d41-2c29-455b-bdd4-1ce99fadf3cf",
   "metadata": {},
   "outputs": [],
   "source": [
    "import pinocchio as pin"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8e5dea6e-7192-4a8a-a89e-53236477b812",
   "metadata": {},
   "outputs": [],
   "source": [
    "dir(pin)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d05b0138-e374-4a8b-bd6c-303eeafd3006",
   "metadata": {},
   "source": [
    "Here are some comments about the pinocchio functions:"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "69e29d56-e9cb-423b-bad0-e360ab180f19",
   "metadata": {},
   "source": [
    "## Library meta info\n",
    "- `PINOCCHIO_MAJOR_VERSION`\n",
    "- `PINOCCHIO_MINOR_VERSION`\n",
    "- `PINOCCHIO_PATCH_VERSION`\n",
    "\n",
    "### `KinematicLevel`: Constant to identify level of variable kinematic POSITION, VELOCITY, ACCELERATION\n",
    "- `POSITION`\n",
    "- `VELOCITY`\n",
    "- `ACCELERATION`:\n",
    "\n",
    "### `ArgumentPosition`:\n",
    "- `ARGX`: precise differentiation with respect to chosen variable\n",
    "\n",
    "### Geometry_type\n",
    "- `VISUAL`: \n",
    "- `COLLISION`:\n",
    "\n",
    "### Ref type for twist and jacobian\n",
    "- `WORLD`: constant to specify to express lie algebra vector of SE(3) as world twist\n",
    "- `LOCAL`: constant to specify to express lie algebra vector of SE(3) as local twist $[v,\\omega]$ in local frame (cf TP 3)\n",
    "- `LOCAL_WORLD_ALIGNED`: constant to specify to express lie algebra vector of SE(3) as local twist $[v,\\omega]$ in world frame (cf TP 3)\n",
    "\n",
    "### Dependency\n",
    "- `WITH_CPPAD`:\n",
    "- `WITH_HPP_FCL`:\n",
    "- `WITH_HPP_FCL_BINDINGS`:\n",
    "- `WITH_OPENMP`:\n",
    "- `WITH_URDFDOM`:\n",
    "\n",
    "## Mathematics\n",
    "### Canonic lie group operation\n",
    "- `exp3`: exp on SO(3)\n",
    "- `exp6`: exp on SE(3)\n",
    "- `log3`: log on SO(3)\n",
    "- `log6`: log on SE(3)\n",
    "\n",
    "- `Jexp3`: differential of exp on SO(3)\n",
    "- `Jexp6`: differential of exp on SE(3)\n",
    "- `Jlog3`: differential of log on SO(3)\n",
    "- `Jlog6`: differential of log on SE(3)\n",
    "--> verify\n",
    "\n",
    "### Base mathematical object for Lie groups\n",
    "- `Quaternion`: hamilton representation of a rotation dimnum = 4\n",
    "- `AngleAxis`: angle axis representation of a rotation dimnum = 6\n",
    "- `SE3`: Homoegenous 4x4 matric with R bloc and translation bloc\n",
    "- `SE3ToXYZQUAT`: Convert to the translation bloc, quat repr of the R bloc\n",
    "- `XYZQUATToSE3`: Reverse transformation\n",
    "- `Motion`: Lie algebra for se(3), cross which is the equivalent of Lie bracket in vector coord.\n",
    "- `Force`: Dual space of motion\n",
    "- `Inertia`: a 6x6 matrix representing a dot product on se(3). With the right adjoint transformation (such that frame match COM and principal axis) it is a diagonal of the form (m, m, m mx, my, mz) with m the mass, and the principal moments. It is a body in a mechanical POV. Implicitely it is a left invariant riemannian metric on SE(3), defining a connection allowing for proper derivation. For a frame, we call acceleration the derivative of the left maurer cartan form of the velocity. Then we can link it to proper derivation using Lie group theory\n",
    "\n",
    "- `skew`: Operation on angular part of motion\n",
    "- `skewSquare`: Use because close form faster than matrix multiplication.\n",
    "- `unSkew`: We have the adjoint action of element of SE(3) on elements of Motion.\n",
    "\n",
    "## Modeling\n",
    "### `Model`: Identifying the relation between joint, frames and body (in dynamic perspective). It creates an underlying Lie group as cartesian product of dof of joints for the config space. And each frame leave in SE(3). Model also incorporate physic quantities such as intertia and so on for dynamic algorithms.\n",
    "- `ModelPool`:\n",
    "- `appendModel`:\n",
    "- `buildModelFromUrdf`:\n",
    "- `buildModelFromXML`:\n",
    "- `buildModelsFromUrdf`:\n",
    "\n",
    "### `LieGroup` formalism for configuration space as cartesian product of special Lie group, it is implicit within a model.\n",
    "- `name`: identification\n",
    "- `nq`: dimension of numerical vector used to represent a configuration\n",
    "- `nv`: dimension of numerical vector to represent a tangent vector. It is the dimension of the group ! It often rely on a Lie algebra transformation of the tangent vector and a basis of the Lie Algebra.\n",
    "- `neutral`: the identity q\n",
    "- `normalize`: make the configuration q complient\n",
    "- `randomConfiguration`: implement the uniform sampling on the configuration space\n",
    "- `isNormalized`:\n",
    "- `isSameConfiguration`:\n",
    "- `integrate`: generalization of exp map with any starting q, and v in algebra implicitly being a tangent vec\n",
    "- `loadReferenceConfigurations`:\n",
    "- `loadReferenceConfigurationsFromXML`:\n",
    "- `difference`: generalization of log map with any starting q\n",
    "- `dIntegrate`: differential of the general exp, using the general notation\n",
    "- `dDifference`: differential of the general log\n",
    "#### Special more complexe operation\n",
    "- `dIntegrateTransport`: XXX\n",
    "- `dIntegrate_dq`: XXX\n",
    "- `dIntegrate_dv`: XXX\n",
    "#### Canonic distance on the configuration space\n",
    "- `distance`: canonical (left / right / bi-invariant) metric on the Lie group. If the group admit a Riemann and Cartan connection it is the norm of the log\n",
    "- `squaredDistance`: the same but we keep the square\n",
    "- `interpolate`: interpole using the exp/log map of the Lie group. It is a geodesic (auto //) for any cartan connection, and a metric geodesic iif there is a Riemann / Cartan connection\n",
    "--> verify\n",
    "\n",
    "### `Frame`: frame of the model are non parametric transfo of some joint placement\n",
    "- `ReferenceFrame`: the fixed frame\n",
    "- `FrameType`:\n",
    "- `BODY`:\n",
    "- `JOINT`:\n",
    "- `FIXED_JOINT`:\n",
    "- `OP_FRAME`:\n",
    "\n",
    "### `JointModel`: the parametric part of the model, each joint is a mapping from a part of q to local SE(3) so it encompass frame and lie stuff. You can add joint to model.\n",
    "- `JointModelComposite`:\n",
    "- `JointModelFreeFlyer`:\n",
    "- `JointModelMimic<JointModelRX>`:\n",
    "- `JointModelMimic<JointModelRY>`:\n",
    "- `JointModelMimic<JointModelRZ>`:\n",
    "- `JointModelPX`:\n",
    "- `JointModelPY`:\n",
    "- `JointModelPZ`:\n",
    "- `JointModelPlanar`:\n",
    "- `JointModelPrismaticUnaligned`:\n",
    "- `JointModelRUBX`:\n",
    "- `JointModelRUBY`:\n",
    "- `JointModelRUBZ`:\n",
    "- `JointModelRX`:\n",
    "- `JointModelRY`:\n",
    "- `JointModelRZ`:\n",
    "- `JointModelRevoluteUnaligned`:\n",
    "- `JointModelRevoluteUnboundedUnaligned`:\n",
    "- `JointModelSpherical`:\n",
    "- `JointModelSphericalZYX`:\n",
    "- `JointModelTranslation`:\n",
    "\n",
    "### `GeometryModel`: Geometry model in addition of model. It is the link between frames, joint and body (in geometry perspective). A model with some geometry object attach to some frames. It use hppFcl\n",
    "- `GeometryPool`:\n",
    "- `CollisionPair`: To specify which pairs should be checked\n",
    "- `MeshLoader`:\n",
    "- `setGeometryMeshScales`:\n",
    "Can be use for visual or collision. Usually a robot have a visual and a collision model, one for display, the other for GJK-EPA use where we enable data collection.\n",
    "If the geometry object are different for visual and collision checking, we can use only one.\n",
    "- `buildGeomFromUrdf`:\n",
    "- `buildGeomFromUrdfString`:\n",
    "\n",
    "### `RobotWrapper`: Wrap a model, a visual model, a geom model and all pinocchio functions that acts on it. See details below.\n",
    "\n",
    "\n",
    "## Data container\n",
    "- `Data`: Store the value of data given model and config (with derivatives)\n",
    "- `createDatas`:\n",
    "\n",
    "### `JointData`: Following the joint model, able to store the global placement data of the joint frame\n",
    "- `JointDataComposite`:\n",
    "- `JointDataFreeFlyer`:\n",
    "- `JointDataMimic<JointDataRX>`:\n",
    "- `JointDataMimic<JointDataRY>`:\n",
    "- `JointDataMimic<JointDataRZ>`:\n",
    "- `JointDataPX`:\n",
    "- `JointDataPY`:\n",
    "- `JointDataPZ`:\n",
    "- `JointDataPlanar`:\n",
    "- `JointDataPrismaticUnaligned`:\n",
    "- `JointDataRUBX`:\n",
    "- `JointDataRUBY`:\n",
    "- `JointDataRUBZ`:\n",
    "- `JointDataRX`:\n",
    "- `JointDataRY`:\n",
    "- `JointDataRZ`:\n",
    "- `JointDataRevoluteUnaligned`:\n",
    "- `JointDataRevoluteUnboundedUnalignedTpl`:\n",
    "- `JointDataSpherical`:\n",
    "- `JointDataSphericalZYX`:\n",
    "- `JointDataTranslation`:\n",
    "\n",
    "### `GeometryData`\n",
    "- `GeometryData`: The data container for computation using geometry model.\n",
    "\n",
    "## Algorithms\n",
    "The signature of algorithm often look like `pin.fun(model, data, [model_spe, data_spe,] [q, [v, [a]]], **kwargs)`.\n",
    "Some function update part of data (update), some use data to compute quantities (compute) and some only request data (get).\n",
    "\n",
    "### For collision\n",
    "When geometry model is used for collision, one can deal with collision pair and so on.\n",
    "- `CollisionResult`:\n",
    "- `computeCollision`:\n",
    "- `computeCollisions`:\n",
    "- `removeCollisionPairs`:\n",
    "- `removeCollisionPairsFromXML`:\n",
    "- `Contact`: Object to wrap a contact out of GJK-EPA, come from enabling contact in geom model\n",
    "- `DistanceResult`: Same for distance result\n",
    "\n",
    "### For frames\n",
    "- `updateFramePlacements`: difuse using model and data, the joint data into the other frames data, then we can get the following quantities\n",
    "- *Update*\n",
    "- `updateFramePlacement`:\n",
    "- `updateFramePlacements`:\n",
    "- `updateGlobalPlacements`:\n",
    "- *Get*\n",
    "- `getFrameAcceleration`: \n",
    "- `getFrameAccelerationDerivatives`:\n",
    "- `getFrameJacobian`:\n",
    "- `getFrameJacobianTimeVariation`:\n",
    "- `getFrameVelocity`:\n",
    "- `getFrameVelocityDerivatives`:\n",
    "- `getFrameClassicalAcceleration`:\n",
    "\n",
    "### For geometry\n",
    "- `updateGeometryPlacements`: diffuse the data of model into geom model.\n",
    "\n",
    "### For model mass matrix\n",
    "- `cholesky`: efficient way to manipulate sparse big square matrix of inertia.\n",
    "-- `decompose`:\n",
    "-- `solve`:\n",
    "-- `computeMinv`:\n",
    "\n",
    "### Forward Kinematics\n",
    "- `forwardKinematics`: update data of joint global placement\n",
    "- `computeForwardKinematicsDerivatives`:\n",
    "\n",
    "### Forward dynamics\n",
    "- `forwardDynamics`:\n",
    "- `aba`:\n",
    "- `computeABADerivatives`:\n",
    "- `crba`:\n",
    "- `ccrba`:\n",
    "- `dccrba`:\n",
    "- `centerOfMass`:\n",
    "- `getCenterOfMassVelocityDerivatives`:\n",
    "- `computeCoriolisMatrix`:\n",
    "- `getCoriolisMatrix`:\n",
    "- `nonLinearEffects`:\n",
    "- `nle`:\n",
    "- `kineticEnergy`:\n",
    "- `computeKineticEnergy`:\n",
    "- `potentialEnergy`:\n",
    "- `computePotentialEnergy`:\n",
    "\n",
    "### Inverse dynamic\n",
    "- `rnea`:\n",
    "- `computeRNEADerivatives`:\n",
    "\n",
    "### Centroïdal\n",
    "- `computeCentroidalDynamics`:\n",
    "- `computeCentroidalDynamicsDerivatives`:\n",
    "- `getCentroidalDynamicsDerivatives`:\n",
    "- `computeCentroidalMap`:\n",
    "- `computeCentroidalMapTimeVariation`:\n",
    "- `computeCentroidalMomentum`:\n",
    "- `computeCentroidalMomentumTimeVariation`:\n",
    "\n",
    "*What are the derivatives ? Which link to jacobian ? Lie algebra generalized ?*\n",
    "### Cinematic jacobian\n",
    "- `computeFrameJacobian`:\n",
    "- `computeJointJacobian`:\n",
    "- `computeJointJacobians`:\n",
    "- `computeJointJacobiansTimeVariation`:\n",
    "- `getJacobianSubtreeCenterOfMass`:\n",
    "- `getJointJacobian`:\n",
    "- `getJointJacobianTimeVariation`:\n",
    "- `frameJacobian`:\n",
    "- `frameJacobianTimeVariation`:\n",
    "- `jacobianCenterOfMass`:\n",
    "- `jacobianSubtreeCenterOfMass`:\n",
    "- `jacobianSubtreeCoMJacobian`:\n",
    "- `jointJacobian`:\n",
    "\n",
    "### Other dynamic algorithm (constrained forward kinematic)\n",
    "- `computeGeneralizedGravity`:\n",
    "- `computeGeneralizedGravityDerivatives`:\n",
    "- `computeKKTContactDynamicMatrixInverse`:\n",
    "- `computeMinverse`:\n",
    "- `computeStaticTorque`:\n",
    "- `computeStaticTorqueDerivatives`:\n",
    "- `computeSubtreeMasses`:\n",
    "- `computeTotalMass`:\n",
    "- `getAcceleration`:\n",
    "- `getClassicalAcceleration`:\n",
    "- `getJointAccelerationDerivatives`:\n",
    "- `getJointVelocityDerivatives`:\n",
    "- `getKKTContactDynamicMatrixInverse`:\n",
    "- `framesForwardKinematics`:\n",
    "- `getVelocity`:\n",
    "- `impulseDynamics`:\n",
    "\n",
    "### Regressor for robot calibration:\n",
    "- `bodyRegressor`:\n",
    "- `computeFrameKinematicRegressor`:\n",
    "- `computeJointKinematicRegressor`:\n",
    "- `computeJointTorqueRegressor`:\n",
    "- `jointBodyRegressor`:\n",
    "- `computeStaticRegressor`:\n",
    "- `frameBodyRegressor`:\n",
    "\n",
    "### Collections\n",
    "- `StdMap_String_VectorXd`:\n",
    "- `StdVec_Bool`:\n",
    "- `StdVec_CollisionPair`:\n",
    "- `StdVec_CollisionResult`:\n",
    "- `StdVec_Contact`:\n",
    "- `StdVec_Data`:\n",
    "- `StdVec_DistanceResult`:\n",
    "- `StdVec_Double`:\n",
    "- `StdVec_Force`:\n",
    "- `StdVec_Frame`:\n",
    "- `StdVec_GeometryData`:\n",
    "- `StdVec_GeometryModel`:\n",
    "- `StdVec_GeometryObject`:\n",
    "- `StdVec_Index`:\n",
    "- `StdVec_IndexVector`:\n",
    "- `StdVec_Inertia`:\n",
    "- `StdVec_Int`:\n",
    "- `StdVec_JointModelVector`:\n",
    "- `StdVec_Matrix6x`:\n",
    "- `StdVec_Motion`:\n",
    "- `StdVec_SE3`:\n",
    "- `StdVec_StdString`:\n",
    "- `StdVec_Vector3`:\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7db47554-504e-4aad-aabf-5d7b45edd9ec",
   "metadata": {},
   "source": [
    "Yes it is hudge !!! But you can find the documentation [here](https://gepettoweb.laas.fr/doc/stack-of-tasks/pinocchio/master/doxygen-html/). And here is a nice cheat sheet :"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9d21e6cb-49f0-4787-a874-6f3f75061be8",
   "metadata": {},
   "outputs": [],
   "source": [
    "from IPython.display import IFrame\n",
    "IFrame(\"./pinocchio_cheat_sheet.pdf\", width=1200, height=1200)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "69c4d48a-4773-42fb-baa3-1a7b5fe079c6",
   "metadata": {},
   "source": [
    "## Prototypical example of pinocchio usages\n",
    "### Build the models"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "149bd83e-5fa2-4c87-bf29-5c8ba5890d2c",
   "metadata": {},
   "outputs": [],
   "source": [
    "import pinocchio as pin\n",
    "import numpy as np\n",
    "import hppfcl\n",
    "from pinocchio.utils import rotate\n",
    "\n",
    "\n",
    "model = pin.Model()\n",
    "collision_model = pin.GeometryModel()\n",
    "visual_model = pin.GeometryModel()\n",
    "\n",
    "# Add a floating joint\n",
    "# joint_id = model.addJoint(\n",
    "#     0,  # Parent joint number, 0 is universe\n",
    "#     pin.JointModelFreeFlyer(),  # Joint type\n",
    "#     pin.SE3.Identity(),  # Placement relative to parent joint frame\n",
    "#     'main_joint'  # Name\n",
    "#     max_effort=1000 * np.ones(6),  # Limites\n",
    "#     max_velocity=1000 * np.ones(6),\n",
    "#     min_config=np.array([-1, -1, -1, 0., 0., 0., 1.]),\n",
    "#     max_config=np.array([1, 1, 1, 0., 0., 0., 1.]),\n",
    "# )\n",
    "joint_id = model.addJoint(\n",
    "    0,\n",
    "    pin.JointModelFreeFlyer(),\n",
    "    pin.SE3.Identity(),\n",
    "    joint_name='first_joint',\n",
    "    max_effort=1000 * np.ones(6),\n",
    "    max_velocity=1000 * np.ones(6),\n",
    "    min_config=np.array([-1, -1, -1, 0., 0., 0., 1.]),\n",
    "    max_config=np.array([1, 1, 1, 0., 0., 0., 1.]),\n",
    ")\n",
    "\n",
    "# We attach a cylinder to it, in the referential, the base is in 0 and it is along x axis\n",
    "M_cyl = 3.\n",
    "h = 0.2\n",
    "r = 0.02\n",
    "com = np.array([h/2, 0, 0])  # Where com will be place in joint frame\n",
    "moment_inertia = np.diag([\n",
    "    1/2*M_cyl*r**2,\n",
    "    1/12*M_cyl*h**2 + 1/4*M_cyl*r**2,\n",
    "    1/12*M_cyl*h**2 + 1/4*M_cyl*r**2\n",
    "])  # moment inertia matrix of a cylinder\n",
    "\n",
    "# Add the body as dynamic quantity in the model\n",
    "# model.appendBodyToJoint(\n",
    "#     joint_id,  # Joint Id\n",
    "#     pin.Inertia(M_cyl, com, moment_inertia),  # Inertia matrix with mass, com, moments in express in body frame\n",
    "#     pin.SE3.Identity()  # transformation from joint frame to body frame\n",
    "# )\n",
    "model.appendBodyToJoint(\n",
    "    joint_id,\n",
    "    pin.Inertia(M_cyl, com, moment_inertia),\n",
    "    pin.SE3.Identity()\n",
    ")\n",
    "\n",
    "# Add the body as geometric quantity in the collision_model\n",
    "# geom = pin.GeometryObject(\n",
    "#     'main_colision_shape',  # Name\n",
    "#     joint_id,  # joint id\n",
    "#     hppfcl.Cylinder(r, h),  # Hpp shape\n",
    "#     pin.SE3(pin.SE3(rotate('y',np.pi/2), np.array([h/2,0,0])))  # Position of mesh in joint frame, here canonically cylinder is along z, we rotate.\n",
    "# )\n",
    "geom_col = pin.GeometryObject(\n",
    "    'world/first_col_shape',\n",
    "    joint_id,\n",
    "    hppfcl.Cylinder(r, h),\n",
    "    pin.SE3(pin.SE3(rotate('y',np.pi/2), np.array([h/2,0,0])))\n",
    ")\n",
    "collision_model.addGeometryObject(geom_col)\n",
    "\n",
    "# But visually it looks like two cylinder with a ball inside\n",
    "geom_viz1 = pin.GeometryObject(\n",
    "    'world/first_viz_shape_p1',\n",
    "    joint_id,\n",
    "    hppfcl.Cylinder(r, h/2 - r),\n",
    "    pin.SE3(rotate('y',np.pi/2), np.array([(h/2 - r)/2,0,0]))\n",
    ")\n",
    "geom_viz1.meshColor = np.array([1., 0., 0., 1.])\n",
    "geom_viz2 = pin.GeometryObject(\n",
    "    'world/first_viz_shape_p2',\n",
    "    joint_id,\n",
    "    hppfcl.Cylinder(r, h/2 - r),\n",
    "    pin.SE3(rotate('y',np.pi/2), np.array([(3*h/2 + r)/2,0,0]))\n",
    ")\n",
    "geom_viz2.meshColor = np.array([1., 0., 0., 1.])\n",
    "geom_viz3 = pin.GeometryObject(\n",
    "    'world/first_viz_shape_p3',\n",
    "    joint_id,\n",
    "    hppfcl.Sphere(r),\n",
    "    pin.SE3(np.eye(3), np.array([h/2,0,0]))\n",
    ")\n",
    "geom_viz3.meshColor = np.array([0., 1., 0., 1.])\n",
    "visual_model.addGeometryObject(geom_viz1)\n",
    "visual_model.addGeometryObject(geom_viz2)\n",
    "visual_model.addGeometryObject(geom_viz3)\n",
    "\n",
    "# Now let us add another joint at the end of the cylinder\n",
    "joint_id_2 = model.addJoint(\n",
    "    joint_id,\n",
    "    pin.JointModelRY(),\n",
    "    pin.SE3(np.eye(3), np.array([h,0,0])),\n",
    "    'second_joint',\n",
    "    max_effort=np.array([1000]),\n",
    "    max_velocity=np.array([1000]),\n",
    "    min_config=np.array([-np.pi]),\n",
    "    max_config=np.array([np.pi]),\n",
    "\n",
    ")\n",
    "model.appendBodyToJoint(\n",
    "    joint_id_2,\n",
    "    pin.Inertia(M_cyl, com, moment_inertia),\n",
    "    pin.SE3.Identity()\n",
    ")\n",
    "# Here visual and collision coincide\n",
    "geom_colviz_2 = pin.GeometryObject(\n",
    "    'world/second_colviz_shape',\n",
    "    joint_id_2,\n",
    "    hppfcl.Cylinder(r, h),\n",
    "    pin.SE3(pin.SE3(rotate('y',np.pi/2), np.array([h/2,0,0])))\n",
    ")\n",
    "collision_model.addGeometryObject(geom_colviz_2)\n",
    "geom_colviz_2.meshColor = np.array([1., 0., 0., 1.])\n",
    "visual_model.addGeometryObject(geom_colviz_2)\n",
    "\n",
    "# And a third one\n",
    "joint_id_3 = model.addJoint(\n",
    "    joint_id_2,\n",
    "    pin.JointModelRY(),\n",
    "    pin.SE3(np.eye(3), np.array([h,0,0])),\n",
    "    'third_joint',\n",
    "    max_effort=np.array([1000]),\n",
    "    max_velocity=np.array([1000]),\n",
    "    min_config=np.array([-np.pi]),\n",
    "    max_config=np.array([np.pi]),\n",
    ")\n",
    "model.appendBodyToJoint(\n",
    "    joint_id_3,\n",
    "    pin.Inertia(M_cyl, com, moment_inertia),\n",
    "    pin.SE3.Identity()\n",
    ")\n",
    "# Here visual and collision coincide\n",
    "geom_colviz_3 = pin.GeometryObject(\n",
    "    'world/third_colviz_shape',\n",
    "    joint_id_3,\n",
    "    hppfcl.Cylinder(r, h),\n",
    "    pin.SE3(pin.SE3(rotate('y',np.pi/2), np.array([h/2,0,0])))\n",
    ")\n",
    "collision_model.addGeometryObject(geom_colviz_3)\n",
    "geom_colviz_3.meshColor = np.array([1., 0., 0., 1.])\n",
    "visual_model.addGeometryObject(geom_colviz_3)\n",
    "\n",
    "# But we can have object that are not in chain, tree is alowed !\n",
    "joint_id_bis = model.addJoint(\n",
    "    0,\n",
    "    pin.JointModelFreeFlyer(),\n",
    "    pin.SE3(np.eye(3), np.array([0,0.1,0])),\n",
    "    'other_joint',\n",
    "    max_effort=1000 * np.ones(6),\n",
    "    max_velocity=1000 * np.ones(6),\n",
    "    min_config=np.array([-1, -1, -1, 0., 0., 0., 1.]),\n",
    "    max_config=np.array([1, 1, 1, 0., 0., 0., 1.]),\n",
    ")\n",
    "\n",
    "# We attach a ball to it, in the referential, the base is in 0 and it is along x axis\n",
    "M_ball = 3.\n",
    "r_ball = 0.05\n",
    "com_ball = np.array([0, 0, 0])  # Where com will be place in joint frame\n",
    "moment_inertia_ball = M_ball * np.eye(3)  # moment inertia matrix of a cylinder\n",
    "\n",
    "model.appendBodyToJoint(\n",
    "    joint_id_bis,\n",
    "    pin.Inertia(M_ball, com_ball, moment_inertia_ball),\n",
    "    pin.SE3.Identity()\n",
    ")\n",
    "\n",
    "geom_col_other = pin.GeometryObject(\n",
    "    'world/other_colviz_shape',\n",
    "    joint_id_bis,\n",
    "    hppfcl.Sphere(2*r),\n",
    "    pin.SE3(pin.SE3(rotate('y',np.pi/2), np.array([h/2,0,0])))\n",
    ")\n",
    "collision_model.addGeometryObject(geom_col_other)\n",
    "geom_colviz_3.meshColor = np.array([0., 1., 1., 1.])\n",
    "visual_model.addGeometryObject(geom_col_other)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "22d90c5d-f0ae-420a-b87a-e6313ce16db8",
   "metadata": {},
   "source": [
    "#### Manipulate placement mathematical objects"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "177c84b2-1f63-4c42-9dcf-fbc84b98d5d1",
   "metadata": {},
   "outputs": [],
   "source": [
    "a = pin.SE3(pin.SE3(rotate('y',np.pi/2), np.array([h/2,0,0])))\n",
    "b = pin.SE3ToXYZQUAT(a)\n",
    "c = pin.Motion(np.array([1., 1., 1., 2., 2., 2.]))\n",
    "d = pin.Motion(np.array([1., 2., 3., -1., -2., -3.]))\n",
    "e = c.cross(d)  # Generalized cross product\n",
    "f = c.se3Action(a)\n",
    "f2 = a.act(c)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1db3bb9c-1bbc-46e8-b904-0cb31d22337d",
   "metadata": {},
   "source": [
    "And a lot more ! See the documentation"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2484ad70-5984-4ce6-92b3-24de4542b254",
   "metadata": {},
   "source": [
    "### Add the collision information to the collision model"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "db387ce3-7967-4ff9-9028-0b0f145cf3d4",
   "metadata": {},
   "outputs": [],
   "source": [
    "pairs = [\n",
    "    ['first_col_shape', 'third_colviz_shape'],\n",
    "    ['other_colviz_shape', 'first_col_shape'],\n",
    "    ['other_colviz_shape', 'second_colviz_shape'],\n",
    "    ['other_colviz_shape', 'third_colviz_shape']\n",
    "]\n",
    "for (n1,n2) in pairs:\n",
    "    collision_model.addCollisionPair(\n",
    "        pin.CollisionPair(\n",
    "            collision_model.getGeometryId(f'world/{n1}'),\n",
    "            collision_model.getGeometryId(f'world/{n2}')\n",
    "        )\n",
    "    )"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c9df2e0a-4a7a-432a-b9d2-741a35b6e6bd",
   "metadata": {},
   "source": [
    "### Create a visualizer"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ea529625-413b-4be2-b67d-0b24694776ad",
   "metadata": {},
   "outputs": [],
   "source": [
    "from pinocchio.visualize import MeshcatVisualizer\n",
    "import meshcat\n",
    "\n",
    "vizualizer = MeshcatVisualizer(model, collision_model, visual_model)\n",
    "vizualizer.initViewer(loadModel=True, viewer=meshcat.Visualizer())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ca546ca9-8526-4c36-b461-422470436fa2",
   "metadata": {},
   "outputs": [],
   "source": [
    "vizualizer.display(pin.neutral(model))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b3279f62-ce03-41c8-8e46-2c85d28d3297",
   "metadata": {},
   "outputs": [],
   "source": [
    "vizualizer.display(pin.randomConfiguration(model))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "08955457-a6b5-421a-9e80-5ac6d93df392",
   "metadata": {},
   "source": [
    "## Use of algorithm"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "92d21296-07b9-48bf-a884-cad263580d90",
   "metadata": {},
   "outputs": [],
   "source": [
    "data = model.createData()\n",
    "collision_data = collision_model.createData()\n",
    "visual_data = visual_model.createData()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "cb4c6271-58a2-4d6f-996a-74b87b9a3cfc",
   "metadata": {},
   "outputs": [],
   "source": [
    "q = pin.randomConfiguration(model)\n",
    "v = np.random.rand(model.nv)\n",
    "a = np.random.rand(model.nv)\n",
    "tau = np.random.rand(model.nv)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "075bf373-ac81-4a4f-97c4-0aab9ce2ea24",
   "metadata": {},
   "source": [
    "### EG1: Compute collision"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7687daf2-8740-4e79-9836-8131127ab043",
   "metadata": {},
   "outputs": [],
   "source": [
    "res = pin.computeCollisions(model, data, collision_model, collision_data, q, False)  # Use q to fill the data with value\n",
    "res = pin.computeDistances(model, data, collision_model, collision_data, q)  # Only compute what is missing because it is the same q"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "239d8599-a7e9-4b24-aace-8fd4c5b34e2e",
   "metadata": {},
   "source": [
    "### EG2: Forwards kinematic and jacobian"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "15ff5858-949d-4f29-b9fe-244d1e62768d",
   "metadata": {},
   "outputs": [],
   "source": [
    "pin.forwardKinematics(model, data, q)\n",
    "placement_of_joint_3 = data.oMi[3]  # Now it is in data "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "df8bfc1b-803f-4bb5-8c12-ee5900a396d4",
   "metadata": {},
   "outputs": [],
   "source": [
    "placement_of_joint_3"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "852ce367-7bd9-4ea4-8dc9-a137b4e96eb4",
   "metadata": {},
   "source": [
    "For jacobian we need to compute and get"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "abbc3148-c39c-406d-b641-45879319cd9e",
   "metadata": {},
   "outputs": [],
   "source": [
    "pin.computeJointJacobian(model, data, q, 3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "69252878-7a06-49ec-a494-25c16ddf7692",
   "metadata": {},
   "outputs": [],
   "source": [
    "jacobian_of_joint_3 = pin.getJointJacobian(model, data, 3, pin.LOCAL)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "22656b49-ca84-4ae1-afc1-71baa61c1a5b",
   "metadata": {},
   "source": [
    "### EG3: Dynamics"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3ec9c8dc-e077-4acb-baee-93852e545d19",
   "metadata": {},
   "outputs": [],
   "source": [
    "a_esti = pin.rnea(model, data, q, v, tau)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "df44a4bf-56e4-41e5-9a56-7b9bb22d553d",
   "metadata": {},
   "outputs": [],
   "source": [
    "tau_esti = pin.aba(model, data, q, v, a)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "76fb236a-5329-442c-b476-b34b258b4526",
   "metadata": {},
   "source": [
    "## Take away"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5c46633f-c95d-4008-809d-73a40c618d80",
   "metadata": {},
   "source": [
    "Pinocchio has its own logic, you will discover its use and usefullness through the different TPs. There is a lot to do in it and there is a lot of mathematics in it.\n",
    "\n",
    "A lot of code are already done to save you some time and let you focus on the main aspects."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "61428ced-5402-4b21-bc0c-b904824ef4bd",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.9.13"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}