Adding new tasks, angular_momentum and contact_force_regularisation

README.md

@@ -42,6 +42,17 @@ pip install --no-deps -e .
 ## Usage 
 
 Take a look at the [examples](./examples) folder! 
+To run the examples you will need to install notebook
+```
+conda install notebook
+```
+
+> [!NOTE]  
+> Running the example using the [drake simulator](https://drake.mit.edu/) needs installing the following additional dependencies,
+```
+conda install meshio tqdm
+pip install drake git+https://github.com/akhilsathuluri/odio_urdf.git@comodo
+``` 
 
 ### Maintainer
 

examples/drake_walking.ipynb

@@ -0,0 +1,334 @@
+{
+ "cells": [
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "## Comodo Exmaple \n",
+    "This examples, load a basic robot model (i.e. composed only of basic shapes), modifies the links of such a robot model by elongating the legs, define instances of the TSID (Task Based Inverse Dynamics) and Centroidal MPC  controller and simulate the behavior of the robot using drake.  "
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# Comodo import\n",
+    "from comodo.drakeSimulator.drakeSimulator import DrakeSimulator\n",
+    "from comodo.robotModel.robotModel import RobotModel\n",
+    "from comodo.robotModel.createUrdf import createUrdf\n",
+    "from comodo.centroidalMPC.centroidalMPC import CentroidalMPC\n",
+    "from comodo.centroidalMPC.mpcParameterTuning import MPCParameterTuning\n",
+    "from comodo.TSIDController.TSIDParameterTuning import TSIDParameterTuning\n",
+    "from comodo.TSIDController.TSIDController import TSIDController"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# General  import \n",
+    "import xml.etree.ElementTree as ET\n",
+    "import numpy as np\n",
+    "import tempfile\n",
+    "import urllib.request\n",
+    "import logging"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# set the logging level\n",
+    "logging.getLogger().setLevel(logging.INFO)"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# Getting stickbot urdf file and convert it to string \n",
+    "urdf_robot_file = tempfile.NamedTemporaryFile(mode=\"w+\")\n",
+    "url = 'https://raw.githubusercontent.com/icub-tech-iit/ergocub-gazebo-simulations/master/models/stickBot/model.urdf'\n",
+    "urllib.request.urlretrieve(url, urdf_robot_file.name)\n",
+    "# Load the URDF file\n",
+    "tree = ET.parse(urdf_robot_file.name)\n",
+    "root = tree.getroot()\n",
+    "\n",
+    "# Convert the XML tree to a string\n",
+    "robot_urdf_string_original = ET.tostring(root)\n",
+    "\n",
+    "create_urdf_instance = createUrdf(\n",
+    "    original_urdf_path=urdf_robot_file.name, save_gazebo_plugin=False\n",
+    ")"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# Define parametric links and controlled joints  \n",
+    "legs_link_names = [\"hip_3\", \"lower_leg\"]\n",
+    "joint_name_list = [\n",
+    "    \"r_shoulder_pitch\",\n",
+    "    \"r_shoulder_roll\",\n",
+    "    \"r_shoulder_yaw\",\n",
+    "    \"r_elbow\",\n",
+    "    \"l_shoulder_pitch\",\n",
+    "    \"l_shoulder_roll\",\n",
+    "    \"l_shoulder_yaw\",\n",
+    "    \"l_elbow\",\n",
+    "    \"r_hip_pitch\",\n",
+    "    \"r_hip_roll\",\n",
+    "    \"r_hip_yaw\",\n",
+    "    \"r_knee\",\n",
+    "    \"r_ankle_pitch\",\n",
+    "    \"r_ankle_roll\",\n",
+    "    \"l_hip_pitch\",\n",
+    "    \"l_hip_roll\",\n",
+    "    \"l_hip_yaw\",\n",
+    "    \"l_knee\",\n",
+    "    \"l_ankle_pitch\",\n",
+    "    \"l_ankle_roll\",\n",
+    "]"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# Define the robot modifications\n",
+    "modifications = {}\n",
+    "for item in legs_link_names:\n",
+    "    left_leg_item = \"l_\" + item\n",
+    "    right_leg_item = \"r_\" + item\n",
+    "    modifications.update({left_leg_item: 1.2})\n",
+    "    modifications.update({right_leg_item: 1.2})\n",
+    "# Motors Parameters \n",
+    "Im_arms = 1e-3*np.ones(4) # from 0-4\n",
+    "Im_legs = 1e-3*np.ones(6) # from 5-10\n",
+    "kv_arms = 0.001*np.ones(4) # from 11-14\n",
+    "kv_legs = 0.001*np.ones(6) #from 20\n",
+    "\n",
+    "Im = np.concatenate((Im_arms, Im_arms, Im_legs, Im_legs))\n",
+    "kv = np.concatenate((kv_arms, kv_arms, kv_legs, kv_legs))"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# Modify the robot model and initialize\n",
+    "create_urdf_instance.modify_lengths(modifications)\n",
+    "urdf_robot_string = create_urdf_instance.write_urdf_to_file()\n",
+    "create_urdf_instance.reset_modifications()\n",
+    "robot_model_init = RobotModel(urdf_robot_string, \"stickBot\", joint_name_list)\n",
+    "s_des, xyz_rpy, H_b = robot_model_init.compute_desired_position_walking()"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# Define simulator and initial position\n",
+    "drake_instance = DrakeSimulator()\n",
+    "drake_instance.set_visualize_robot_flag(True)\n",
+    "drake_instance.load_model(robot_model_init, s=s_des, xyz_rpy=xyz_rpy, kv_motors=kv, Im=Im)\n",
+    "s, ds, tau = drake_instance.get_state()\n",
+    "t = drake_instance.get_simulation_time()\n",
+    "H_b = drake_instance.get_base()\n",
+    "# beware of the velocity ordering\n",
+    "w_b = drake_instance.get_base_velocity()"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# Define the controller parameters  and instantiate the controller\n",
+    "# Controller Parameters\n",
+    "tsid_parameter = TSIDParameterTuning()\n",
+    "mpc_parameters = MPCParameterTuning()\n",
+    "\n",
+    "# TSID Instance\n",
+    "TSID_controller_instance = TSIDController(frequency=0.01, robot_model=robot_model_init)\n",
+    "TSID_controller_instance.define_tasks(tsid_parameter)\n",
+    "TSID_controller_instance.set_state_with_base(s, ds, H_b, w_b, t)\n",
+    "\n",
+    "# MPC Instance\n",
+    "step_lenght = 0.1\n",
+    "mpc = CentroidalMPC(robot_model=robot_model_init, step_length=step_lenght)\n",
+    "mpc.intialize_mpc(mpc_parameters=mpc_parameters)\n",
+    "\n",
+    "# Set desired quantities\n",
+    "mpc.configure(s_init=s_des, H_b_init=H_b)\n",
+    "TSID_controller_instance.compute_com_position()\n",
+    "mpc.define_test_com_traj(TSID_controller_instance.COM.toNumPy())\n",
+    "\n",
+    "# TODO: Set initial robot state and plan trajectories \n",
+    "drake_instance.step(1)\n",
+    "\n",
+    "# Reading the state \n",
+    "s, ds, tau = drake_instance.get_state()\n",
+    "H_b = drake_instance.get_base()\n",
+    "w_b = drake_instance.get_base_velocity()\n",
+    "t = drake_instance.get_simulation_time()\n",
+    "\n",
+    "# MPC\n",
+    "mpc.set_state_with_base(s=s, s_dot=ds, H_b=H_b, w_b=w_b, t=t)\n",
+    "mpc.initialize_centroidal_integrator(s=s, s_dot=ds, H_b=H_b, w_b=w_b, t=t)\n",
+    "mpc_output = mpc.plan_trajectory()"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# Set loop variables \n",
+    "TIME_TH = 20.0\n",
+    "\n",
+    "# Define number of steps\n",
+    "n_step = int(\n",
+    "    TSID_controller_instance.frequency / drake_instance.get_simulation_frequency()\n",
+    ")\n",
+    "n_step_mpc_tsid = int(mpc.get_frequency_seconds() / TSID_controller_instance.frequency)\n",
+    "\n",
+    "counter = 0\n",
+    "mpc_success = True\n",
+    "energy_tot = 0.0\n",
+    "succeded_controller = True"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# Simulation-control loop\n",
+    "if  drake_instance.visualize_robot_flag:\n",
+    "    drake_instance.meshcat.StartRecording()\n",
+    "while t < TIME_TH:\n",
+    "    \n",
+    "    # Reading robot state from simulator\n",
+    "    s, ds, tau = drake_instance.get_state()\n",
+    "    energy_i = np.linalg.norm(tau)\n",
+    "    H_b = drake_instance.get_base()\n",
+    "    w_b = drake_instance.get_base_velocity()\n",
+    "    t = drake_instance.get_simulation_time()\n",
+    "\n",
+    "    # Update TSID\n",
+    "    TSID_controller_instance.set_state_with_base(s=s, s_dot=ds, H_b=H_b, w_b=w_b, t=t)\n",
+    "\n",
+    "    # MPC plan \n",
+    "    if counter == 0:\n",
+    "        mpc.set_state_with_base(s=s, s_dot=ds, H_b=H_b, w_b=w_b, t=t)\n",
+    "        mpc.update_references()\n",
+    "        mpc_success = mpc.plan_trajectory()\n",
+    "        mpc.contact_planner.advance_swing_foot_planner()\n",
+    "        if not (mpc_success):\n",
+    "            logging.error(\"MPC failed\")\n",
+    "            break\n",
+    "\n",
+    "    # Reading new references\n",
+    "    com, dcom, forces_left, forces_right, ang_mom = mpc.get_references()\n",
+    "    left_foot, right_foot = mpc.contact_planner.get_references_swing_foot_planner()\n",
+    "\n",
+    "    # Update references TSID \n",
+    "    TSID_controller_instance.update_task_references_mpc(\n",
+    "        com=com,\n",
+    "        dcom=dcom,\n",
+    "        ddcom=np.zeros(3),\n",
+    "        left_foot_desired=left_foot,\n",
+    "        right_foot_desired=right_foot,\n",
+    "        s_desired=np.array(s_des),\n",
+    "        wrenches_left=forces_left,\n",
+    "        wrenches_right=forces_right,\n",
+    "        ang_mom_desired=ang_mom,\n",
+    "    )\n",
+    "\n",
+    "    # Run control \n",
+    "    succeded_controller = TSID_controller_instance.run()\n",
+    "\n",
+    "    if not (succeded_controller):\n",
+    "        logging.error(\"Controller failed\")\n",
+    "        break\n",
+    "\n",
+    "    tau = TSID_controller_instance.get_torque()\n",
+    "\n",
+    "    # Step the simulator\n",
+    "    drake_instance.set_input(tau)\n",
+    "    # drake_instance.step_with_motors(n_step=n_step, torque=tau)\n",
+    "    try:\n",
+    "        drake_instance.step(n_step=n_step)\n",
+    "    except RuntimeError as e:\n",
+    "        logging.error(e)\n",
+    "        break\n",
+    "    counter = counter + 1\n",
+    "\n",
+    "    if counter == n_step_mpc_tsid:\n",
+    "        counter = 0\n",
+    "if  drake_instance.visualize_robot_flag:\n",
+    "    drake_instance.meshcat.StopRecording()\n",
+    "    drake_instance.meshcat.PublishRecording()"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# Closing visualization\n",
+    "drake_instance.close_visualization()"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": []
+  }
+ ],
+ "metadata": {
+  "kernelspec": {
+   "display_name": "walk",
+   "language": "python",
+   "name": "python3"
+  },
+  "language_info": {
+   "codemirror_mode": {
+    "name": "ipython",
+    "version": 3
+   },
+   "file_extension": ".py",
+   "mimetype": "text/x-python",
+   "name": "python",
+   "nbconvert_exporter": "python",
+   "pygments_lexer": "ipython3",
+   "version": "3.10.13"
+  }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 2
+}