Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Adding inverse kinematic controller #9

Merged
merged 17 commits into from
May 3, 2024
Merged

Adding inverse kinematic controller #9

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
17 commits
Select commit Hold shift + click to select a range
c09bca0
adding ik with example
CarlottaSartore Jan 10, 2024
dce7199
update bipedal version
CarlottaSartore Jan 10, 2024
25ada23
added automatic definition of collision and visualization names in urdf
CarlottaSartore Apr 3, 2024
7837853
adding compute resulting branch and computing total resulting wrench …
CarlottaSartore Apr 10, 2024
72eb2a1
added automatic parsing of meshes
CarlottaSartore Apr 10, 2024
a54ed82
modified installation, reading mesh info only if mesh exist
CarlottaSartore Apr 10, 2024
b387a8d
clean up and re-formatting
CarlottaSartore Apr 16, 2024
56ff269
adding map joints mujoco, work in progresss
CarlottaSartore Apr 16, 2024
074f93f
fix joint order for setting control mujoco
CarlottaSartore Apr 17, 2024
9cd1637
set wip example
CarlottaSartore Apr 17, 2024
da04511
Update examples/mujoco_walking_position.ipynb
CarlottaSartore Apr 17, 2024
82da526
Update examples/mujoco_walking_position.ipynb
CarlottaSartore Apr 17, 2024
92b4dde
Update examples/mujoco_walking_position.ipynb
CarlottaSartore Apr 17, 2024
58aab49
Update examples/mujoco_walking_position.ipynb
CarlottaSartore Apr 17, 2024
6090ad0
change the name to kinematics, remove hardoced minimum weigth, minor …
CarlottaSartore Apr 17, 2024
5e1463e
remove comment leftover
CarlottaSartore Apr 30, 2024
0e130e3
remove delta w.r.t. terrain
CarlottaSartore Apr 30, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion README.md
View file
Open in desktop
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
CarlottaSartore marked this conversation as resolved.
Show resolved Hide resolved
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
Open in desktop
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