Skip to content

Commit

Permalink
Merge pull request #9 from ami-iit/adding_ik
Browse files Browse the repository at this point in the history 
Adding inverse kinematic controller
  • Loading branch information
@CarlottaSartore
CarlottaSartore authored May 3, 2024
2 parents 8cbd5f5 + 0e130e3 commit a15d957
Show file tree
Hide file tree
Showing 10 changed files with 1,145 additions and 89 deletions.
2 changes: 1 addition & 1 deletion README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@ To install you can use the following commands


```
conda create -n comododev -c conda-forge adam-robotics idyntree bipedal-locomotion-framework=0.15.0 mujoco mujoco-python numpy mujoco-python-viewer matplotlib urllib3 urchin
conda create -n comododev -c conda-forge adam-robotics idyntree bipedal-locomotion-framework=0.16.0 mujoco mujoco-python numpy mujoco-python-viewer matplotlib urllib3 urchin resolve-robotics-uri-py
conda activate comododev
pip install --no-deps git+https://github.com/CarlottaSartore/urdf-modifiers.git@scalar_modification
pip install --no-deps -e .
Expand Down
55 changes: 28 additions & 27 deletions examples/mujoco_walking.ipynb
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@
"cell_type": "markdown",
"metadata": {},
"source": [
"## Comodo Exmaple \n",
"## Mujoco with TSID and MPC Example \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. "
]
},
Expand All @@ -21,7 +21,7 @@
"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 "
"from comodo.TSIDController.TSIDController import TSIDController"
]
},
{
Expand All @@ -30,7 +30,7 @@
"metadata": {},
"outputs": [],
"source": [
"# General import \n",
"# General import\n",
"import xml.etree.ElementTree as ET\n",
"import numpy as np\n",
"import tempfile\n",
Expand All @@ -43,9 +43,9 @@
"metadata": {},
"outputs": [],
"source": [
"# Getting stickbot urdf file and convert it to string \n",
"# 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",
"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",
Expand All @@ -65,7 +65,7 @@
"metadata": {},
"outputs": [],
"source": [
"# Define parametric links and controlled joints \n",
"# Define parametric links and controlled joints\n",
"legs_link_names = [\"hip_3\", \"lower_leg\"]\n",
"joint_name_list = [\n",
" \"r_shoulder_pitch\",\n",
Expand Down Expand Up @@ -104,11 +104,11 @@
" 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",
"# 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))"
Expand Down Expand Up @@ -136,7 +136,9 @@
"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",
"mujoco_instance.load_model(\n",
" robot_model_init, s=s_des, xyz_rpy=xyz_rpy, kv_motors=kv, Im=Im\n",
")\n",
"s, ds, tau = mujoco_instance.get_state()\n",
"t = mujoco_instance.get_simulation_time()\n",
"H_b = mujoco_instance.get_base()\n",
Expand All @@ -155,7 +157,7 @@
"tsid_parameter = TSIDParameterTuning()\n",
"mpc_parameters = MPCParameterTuning()\n",
"\n",
"# TSID Instance \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",
Expand All @@ -170,10 +172,10 @@
"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",
"# Set initial robot state and plan trajectories\n",
"mujoco_instance.step(1)\n",
"\n",
"# Reading the state \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",
Expand All @@ -182,7 +184,7 @@
"# 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"
"mpc_output = mpc.plan_trajectory()"
]
},
{
Expand All @@ -191,7 +193,7 @@
"metadata": {},
"outputs": [],
"source": [
"# Set loop variables \n",
"# Set loop variables\n",
"TIME_TH = 20\n",
"\n",
"# Define number of steps\n",
Expand All @@ -212,20 +214,19 @@
"metadata": {},
"outputs": [],
"source": [
"# Simulation-control loop \n",
"# Simulation-control loop\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",
"\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",
"\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",
Expand All @@ -236,10 +237,10 @@
" break\n",
"\n",
" # Reading new references\n",
" com, dcom, forces_left, forces_right = mpc.get_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",
"\n",
" # Update references TSID\n",
" TSID_controller_instance.update_task_references_mpc(\n",
" com=com,\n",
" dcom=dcom,\n",
Expand All @@ -251,15 +252,15 @@
" wrenches_right=forces_right,\n",
" )\n",
"\n",
" # Run control \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",
"\n",
" # Step the simulator\n",
" mujoco_instance.set_input(tau)\n",
" mujoco_instance.step_with_motors(n_step=n_step, torque=tau)\n",
Expand Down
Loading

0 comments on commit a15d957

Please sign in to comment.