Skip to content

Commit

Permalink
remove dependency on urdfpy, add ipynb example, update installation t…
Browse files Browse the repository at this point in the history 
…o mujoco 3.0
  • Loading branch information
@CarlottaSartore
CarlottaSartore committed Nov 14, 2023
1 parent 51015f9 commit d344200
Show file tree
Hide file tree
Showing 5 changed files with 328 additions and 175 deletions.
19 changes: 11 additions & 8 deletions README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,19 +17,22 @@ Suite of parametrized controller and simulator for codesign of robots.
## Installation
This library depends on

``bipedal-locomotion-framework``
``iDynTree``
``ADAM``
``urdf-modifier``
``mujoco``
``mujoco-python``
``numpy``
-[``casadi``](https://web.casadi.org/)
-[``numpy``](https://numpy.org/)
-[``idyntree``](https://github.com/robotology/idyntree)
-[``bipedal-locomotion-framework``](https://github.com/ami-iit/bipedal-locomotion-framework)
-[``adam-robotics``](https://github.com/ami-iit/ADAM/tree/main)
-[``mujoco``](https://mujoco.org/)
-[``mujoco-python-viewer``](https://github.com/rohanpsingh/mujoco-python-viewer)
-[``matplotlib``](https://matplotlib.org/stable/)
-[``gitpython``](https://github.com/gitpython-developers/GitPython)
-[``urchin``](https://github.com/fishbotics/urchin)

To install you can use the following commands


```
conda install -c conda-forge adam-robotics idyntree bipedal-locomotion-framework mujoco==2.3 mujoco-python==2.3 numpy==1.22 mujoco-python-viewer matplotlib gitpython urchin networkx=3.1 urdf_py
conda install -c conda-forge adam-robotics idyntree bipedal-locomotion-framework mujoco mujoco-python numpy mujoco-python-viewer matplotlib gitpython urchin
pip install --no-deps git+https://github.com/CarlottaSartore/urdf-modifiers.git@scalar_modification
pip install --no-deps -e .
Expand Down
311 changes: 311 additions & 0 deletions examples/mujoco_walking.ipynb
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,311 @@
{
"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 mujoco. "
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# Comodo import\n",
"from comodo.mujocoSimulator.mujocoSimulator import MujocoSimulator\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",
"from git import Repo"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# Getting stickbot urdf file and convert it to string \n",
"\n",
"temp_dir = tempfile.TemporaryDirectory()\n",
"git_url = \"https://github.com/icub-tech-iit/ergocub-gazebo-simulations.git\"\n",
"Repo.clone_from(git_url, temp_dir.name)\n",
"urdf_path_original = temp_dir.name + \"/models/stickBot/model.urdf\"\n",
"\n",
"# Load the URDF file\n",
"tree = ET.parse(urdf_path_original)\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_path_original, save_gazebo_plugin=False\n",
")"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# Define parametric links and controlled joints \n",
" \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",
"\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 set initial position\n",
"mujoco_instance = MujocoSimulator()\n",
"mujoco_instance.load_model(robot_model_init, s=s_des, xyz_rpy=xyz_rpy, kv_motors=kv, Im=Im)\n",
"s, ds, tau = mujoco_instance.get_state()\n",
"t = mujoco_instance.get_simulation_time()\n",
"H_b = mujoco_instance.get_base()\n",
"w_b = mujoco_instance.get_base_velocity()\n",
"mujoco_instance.set_visualize_robot_flag(False)"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# Define the controller parameters and instantiate the controller\n",
"\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",
"\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",
"# Set initial robot state and plan trajectories \n",
"mujoco_instance.step(1)\n",
"\n",
"# Reading the state \n",
"s, ds, tau = mujoco_instance.get_state()\n",
"H_b = mujoco_instance.get_base()\n",
"w_b = mujoco_instance.get_base_velocity()\n",
"t = mujoco_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()\n"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# Set loop variables \n",
"TIME_TH = 20\n",
"\n",
"# Define number of steps\n",
"n_step = int(\n",
" TSID_controller_instance.frequency / mujoco_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",
"\n",
"while t < TIME_TH:\n",
"\n",
" # Reading robot state from simulator\n",
" s, ds, tau = mujoco_instance.get_state()\n",
" energy_i = np.linalg.norm(tau)\n",
" H_b = mujoco_instance.get_base()\n",
" w_b = mujoco_instance.get_base_velocity()\n",
" t = mujoco_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",
" print(\"MPC failed\")\n",
" break\n",
" # Reading new references\n",
" com, dcom, forces_left, forces_right = 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",
" )\n",
"\n",
" # Run control \n",
" succeded_controller = TSID_controller_instance.run()\n",
"\n",
" if not (succeded_controller):\n",
" print(\"Controller failed\")\n",
" break\n",
"\n",
" tau = TSID_controller_instance.get_torque()\n",
" \n",
" # Step the simulator\n",
" mujoco_instance.set_input(tau)\n",
" mujoco_instance.step_with_motors(n_step=n_step, torque=tau)\n",
" counter = counter + 1\n",
"\n",
" if counter == n_step_mpc_tsid:\n",
" counter = 0"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# Closing visualization\n",
"mujoco_instance.close_visualization()"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "comodo_venv",
"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
}
Loading

0 comments on commit d344200

Please sign in to comment.