2_robot_models.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Introduction to `robotoc` \n",
    "# 2: Robot Models"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## The goal of this chapter\n",
    "Here, we learn how to create robot models (`robotoc::Robot`) from URDF packages.  \n",
    "Note that, `robotoc::Robot` is just a wrapper of [`Pinocchio`](https://github.com/stack-of-tasks/pinocchio)'s API.  \n",
    "So, if you want to dive in deeper, please check [`Pinocchio`'s documentation](https://stack-of-tasks.github.io/pinocchio/)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## What is URDF?\n",
    "URDF (universal robot description format) describes the robot.  \n",
    "We need very little knowledge on URDF as long as we use off-the-shelf URDF files with `robotoc`.  \n",
    "So don't worry even if you do not know well about URDF!  \n",
    "You can see off-the-shelf URDF packages in this repository's `urdf` folder.    \n",
    "As we can see there, a URDF package is composed of urdf file (`~.urdf`) and mesh files (e.g., `~.stl`, `~.dae`, or `~.obj`)   \n",
    "The urdf file describes the tree structure of the multi-link robot and mesh files contain visual and collision information.   \n",
    "If you'd like to know more, please see, e.g., the ROS tutorials http://wiki.ros.org/urdf/Tutorials.  "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Simplest example: a robot manipulator `iiwa14`\n",
    "\n",
    "First, we construct `robotoc.Robot` (`robotoc::Robot` in C++) with a simple example.  \n",
    "Note that C++'s `robotoc::Robot` has more rich methods than the following Python counterpart."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import robotoc\n",
    "model_info = robotoc.RobotModelInfo()\n",
    "model_info.urdf_path = \"urdf/iiwa_description/urdf/iiwa14.urdf\"\n",
    "iiwa14 = robotoc.Robot(model_info)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can see the information of the robot as "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print(iiwa14)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The printed information is as follows:\n",
    "- `dimq`: the dimension of the configuration\n",
    "- `dimv`: the dimension of the generalized velocity (not always the same as `dimq`)\n",
    "- `dimu`: the dimension of the input torque\n",
    "- `dim_passive`: the dimension of the passive joints including the floating base\n",
    "- `frames`: frames of the robot. you can use to design the cost function and constraints on the end-effector.\n",
    "- `joints`: each joint info.\n",
    "- `effort limit`: limits of joint torques\n",
    "- `velocity limit`: limits of joint velocities\n",
    "- `lower position limit`, `upper position limit`: limits of joint positions\n",
    "\n",
    "You can individually get some of them as"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "iiwa14.dimq()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "iiwa14.dimv()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "iiwa14.dimu()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "iiwa14.dim_passive()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "iiwa14.joint_effort_limit()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "iiwa14.joint_velocity_limit()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "iiwa14.lower_joint_position_limit()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "iiwa14.upper_joint_position_limit()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Next, let's visualize them with random joint veocities."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "steps = 100\n",
    "dt = 0.1\n",
    "q = []\n",
    "q.append(iiwa14.generate_feasible_configuration())\n",
    "for i in range(steps):\n",
    "    v = np.random.randn(iiwa14.dimv())\n",
    "    q.append(iiwa14.integrate_configuration(q[-1], v, dt))\n",
    "\n",
    "time_discretization = robotoc.TimeDiscretization(T=dt*steps, N=steps)\n",
    "    \n",
    "viewer = robotoc.utils.TrajectoryViewer(model_info=model_info, viewer_type='meshcat')\n",
    "viewer.display(time_discretization, q)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can also call simple forward kinematics with the robot object."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "end_effector_frame = 'iiwa_link_ee_kuka'\n",
    "for e in q:\n",
    "    iiwa14.forward_kinematics(e)\n",
    "    print('q: ', e)\n",
    "    print('frame position: ', iiwa14.frame_position(end_effector_frame))\n",
    "    print('frame rotation: ', iiwa14.frame_rotation(end_effector_frame))\n",
    "    print('frame placement (SE3): ', iiwa14.frame_placement(end_effector_frame))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Quadrupedal robot `ANYmal`\n",
    "\n",
    "Second, we try a quadruped robot example.  \n",
    "The quadrupedal robot typically involves **four point contacts**.  So we specify the names of the contact frames.  \n",
    "We also specify the time-step parameter (or gain parameters) of the Baumgarte's stabilization method for acceleration-level rigid contact constraints (detail is found, e.g., in [this paper](https://www.researchgate.net/publication/234610391_A_Parametric_Study_on_the_Baumgarte_Stabilization_Method_for_Forward_Dynamics_of_Constrained_Multibody_Systems)).  \n",
    "The appropriate parameter depends on the time step of MPC formulation.  So some numerical experimentation is necessary.   \n",
    "In my experience, 2~10 times of time step of MPC work well.\n",
    "Here, we set it as 0.04.\n",
    "These information of each contact is summarized in `robotoc.ContactModelInfo` (`robotoc::ContactModelInfo` in C++) class, e.g.."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "baumgarte_time_step = 0.04\n",
    "robotoc.ContactModelInfo('FL_foot', baumgarte_time_step)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The robot has a **Floating base**. \n",
    "The model info such as the **URDF path**, **contact info**, and the **base type (floating base or fixed base)** are specified in `robotoc.RobotModelInfo` (`robotoc::RobotModelInfo` in C++), which represents the settings of a robot model."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "model_info = robotoc.RobotModelInfo()\n",
    "model_info.urdf_path = 'urdf/anymal_b_simple_description/urdf/anymal.urdf'\n",
    "model_info.base_joint_type = robotoc.BaseJointType.FloatingBase\n",
    "baumgarte_time_step = 0.04\n",
    "model_info.point_contacts = [robotoc.ContactModelInfo('LF_FOOT', baumgarte_time_step),\n",
    "                             robotoc.ContactModelInfo('LH_FOOT', baumgarte_time_step),\n",
    "                             robotoc.ContactModelInfo('RF_FOOT', baumgarte_time_step),\n",
    "                             robotoc.ContactModelInfo('RH_FOOT', baumgarte_time_step)]\n",
    "anymal = robotoc.Robot(model_info)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can see the detail of the robot model as "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print(anymal)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can also display the robot.  "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "steps = 100\n",
    "dt = 0.1\n",
    "q = []\n",
    "q.append(anymal.generate_feasible_configuration())\n",
    "q[0][0:3] = np.zeros(3)\n",
    "for i in range(steps):\n",
    "    v = np.concatenate([0.1*np.random.randn(6), np.random.randn(anymal.dimu())])\n",
    "    q.append(anymal.integrate_configuration(q[-1], v, dt))\n",
    "\n",
    "time_discretization = robotoc.TimeDiscretization(T=dt*steps, N=steps)\n",
    "\n",
    "viewer = robotoc.utils.TrajectoryViewer(model_info=model_info, viewer_type='meshcat')\n",
    "viewer.display(time_discretization, q)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Humanoid robot `iCub`\n",
    "\n",
    "Third, we try a humanoid robot example.  \n",
    "Humanoid robots typically involve **two surface contacts**.  \n",
    "We construct the robot model as the quadrupedal case as"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "model_info = robotoc.RobotModelInfo()\n",
    "model_info.urdf_path = 'urdf/icub_description/urdf/icub.urdf'\n",
    "model_info.base_joint_type = robotoc.BaseJointType.FloatingBase\n",
    "baumgarte_time_step = 0.05\n",
    "model_info.surface_contacts = [robotoc.ContactModelInfo('l_sole', baumgarte_time_step),\n",
    "                               robotoc.ContactModelInfo('r_sole', baumgarte_time_step)]\n",
    "icub = robotoc.Robot(model_info)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can see the detail of the robot model as "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print(icub)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "and display the robot"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "steps = 100\n",
    "dt = 0.1\n",
    "q = []\n",
    "q.append(icub.generate_feasible_configuration())\n",
    "q[0][0:3] = np.zeros(3)\n",
    "for i in range(steps):\n",
    "    v = np.concatenate([0.1*np.random.randn(6), np.random.randn(icub.dimu())])\n",
    "    q.append(icub.integrate_configuration(q[-1], v, dt))\n",
    "\n",
    "time_discretization = robotoc.TimeDiscretization(T=dt*steps, N=steps)\n",
    "    \n",
    "viewer = robotoc.utils.TrajectoryViewer(model_info=model_info, viewer_type='meshcat')\n",
    "viewer.display(time_discretization, q)"
   ]
  }
 ],
 "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.8.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}