Skip to content

Commit

Permalink
Remove unused imports and fix f-strings
Browse files Browse the repository at this point in the history
  • Loading branch information
@flferretti
flferretti committed Oct 11, 2024
1 parent 376e3f8 commit e68b68a
Show file tree
Hide file tree
Showing 25 changed files with 139 additions and 156 deletions.
2 changes: 1 addition & 1 deletion docs/conf.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,7 +59,7 @@
templates_path = ["_templates"]
exclude_patterns = []

html_title = f"adam"
html_title = "adam"


# -- Options for HTML output -------------------------------------------------
Expand Down
201 changes: 104 additions & 97 deletions examples/mpc-ik.ipynb
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,20 +66,17 @@
"import mediapy as media\n",
"from adam.casadi import KinDynComputations\n",
"import numpy as np\n",
"import casadi as cs\n",
"import distutils.util\n",
"import os\n",
"import subprocess"
"import casadi as cs"
]
},
{
"cell_type": "markdown",
"source": [
"## Import the panda scene in mujoco"
],
"metadata": {
"id": "2zw4FO-IGxdR"
}
},
"source": [
"## Import the panda scene in mujoco"
]
},
{
"cell_type": "code",
Expand All @@ -90,22 +87,22 @@
"outputs": [],
"source": [
"# load scene from xml\n",
"model = mujoco.MjModel.from_xml_path(\"mujoco_menagerie/franka_emika_panda/scene.xml\")\n"
"model = mujoco.MjModel.from_xml_path(\"mujoco_menagerie/franka_emika_panda/scene.xml\")"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "CZMO7PsmKUB6"
},
"source": [
"## Import urdf in adam\n",
"\n",
"Set the commanded joint list and impor the urdf in adam.\n",
"\n",
"For now I have to use a separate urdf for adam.\n",
"An importer for a mujoco model could be an idea for the future!"
],
"metadata": {
"id": "CZMO7PsmKUB6"
}
]
},
{
"cell_type": "code",
Expand All @@ -115,19 +112,28 @@
},
"outputs": [],
"source": [
"joints_name_list = ['panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4', 'panda_joint5', 'panda_joint6', 'panda_joint7', 'panda_joint8']\n",
"joints_name_list = [\n",
" \"panda_joint1\",\n",
" \"panda_joint2\",\n",
" \"panda_joint3\",\n",
" \"panda_joint4\",\n",
" \"panda_joint5\",\n",
" \"panda_joint6\",\n",
" \"panda_joint7\",\n",
" \"panda_joint8\",\n",
"]\n",
"\n",
"kindyn = KinDynComputations(urdfstring=\"panda.urdf\", joints_name_list=joints_name_list)\n"
"kindyn = KinDynComputations(urdfstring=\"panda.urdf\", joints_name_list=joints_name_list)"
]
},
{
"cell_type": "markdown",
"source": [
"## A wrapper interface with mujoco"
],
"metadata": {
"id": "g5LX5kQAKwaM"
}
},
"source": [
"## A wrapper interface with mujoco"
]
},
{
"cell_type": "code",
Expand All @@ -138,50 +144,51 @@
"outputs": [],
"source": [
"class MujocoWrapper:\n",
" # a simple wrapper to use mujoco as a simulator\n",
" def __init__(self, model, joints_list=None):\n",
" self.model = model\n",
" self.data = mujoco.MjData(model)\n",
" self.renderer = mujoco.Renderer(self.model)\n",
" # a simple wrapper to use mujoco as a simulator\n",
" def __init__(self, model, joints_list=None):\n",
" self.model = model\n",
" self.data = mujoco.MjData(model)\n",
" self.renderer = mujoco.Renderer(self.model)\n",
"\n",
" def set_qpos(self, qpos):\n",
" # set the joint positions\n",
" self.data.qpos[:] = qpos\n",
" mujoco.mj_forward(self.model, self.data)\n",
"\n",
" def set_qpos(self, qpos):\n",
" # set the joint positions\n",
" self.data.qpos[:] = qpos\n",
" mujoco.mj_forward(self.model, self.data)\n",
" def get_qpos(self):\n",
" # get the joint positions\n",
" return self.data.qpos[:]\n",
"\n",
" def get_qpos(self):\n",
" # get the joint positions\n",
" return self.data.qpos[:]\n",
" def render(self):\n",
" # render the scene and return the frame\n",
" mujoco.mj_forward(self.model, self.data)\n",
" self.renderer.update_scene(self.data)\n",
" return self.renderer.render()\n",
"\n",
" def render(self):\n",
" # render the scene and return the frame\n",
" mujoco.mj_forward(self.model, self.data)\n",
" self.renderer.update_scene(self.data)\n",
" return self.renderer.render()\n",
" def step(self):\n",
" # step the simulation\n",
" mujoco.mj_step(self.model, self.data)\n",
"\n",
" def step(self):\n",
" # step the simulation\n",
" mujoco.mj_step(self.model, self.data)\n",
" def set_qvel(self, qvel):\n",
" # set the joint velocities\n",
" self.data.qvel[:] = qvel\n",
" mujoco.mj_forward(self.model, self.data)\n",
"\n",
" def set_qvel(self, qvel):\n",
" # set the joint velocities\n",
" self.data.qvel[:] = qvel\n",
" mujoco.mj_forward(self.model, self.data)\n",
"\n",
"wrapper = MujocoWrapper(model)\n"
"wrapper = MujocoWrapper(model)"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "wuY9hqdlD3Vo"
},
"source": [
"# Model Inverse Kinematics as an MPC\n",
"\n",
"An MPC is maybe not the best way to solve an IK problem.\n",
"I just want to show how to use the casadi interface."
],
"metadata": {
"id": "wuY9hqdlD3Vo"
}
]
},
{
"cell_type": "code",
Expand All @@ -197,13 +204,13 @@
"# casadi opti stack\n",
"opti = cs.Opti()\n",
"\n",
"N = 10 # number of control intervals\n",
"dt = 0.05 # time step\n",
"N = 10 # number of control intervals\n",
"dt = 0.05 # time step\n",
"\n",
"# joints variables\n",
"q = opti.variable(8, N+1)\n",
"q = opti.variable(8, N + 1)\n",
"q_dot = opti.variable(8, N)\n",
"w_H_b = np.eye(4) # base of the manipulator as identity matrix\n",
"w_H_b = np.eye(4) # base of the manipulator as identity matrix\n",
"\n",
"# set the desidered end-effector position as a parameter\n",
"# it will be set later when the mpc is solved at each iteration\n",
Expand All @@ -217,7 +224,7 @@
"\n",
"for i in range(N):\n",
" # integration - forward euler\n",
" opti.subject_to(q[:, i+1] == q[:, i] + q_dot[:, i] * dt)\n",
" opti.subject_to(q[:, i + 1] == q[:, i] + q_dot[:, i] * dt)\n",
" # bounds on the joint velocities\n",
" opti.subject_to(opti.bounded(-5, q_dot[:, i], 5))\n",
" # running cost\n",
Expand All @@ -232,13 +239,16 @@
"opti.minimize(target_cost + velocities_cost)\n",
"\n",
"# set the solver\n",
"p_opts = {\"expand\": True, 'ipopt.print_level': 0, 'print_time': 0, 'ipopt.sb': 'yes'}\n",
"p_opts = {\"expand\": True, \"ipopt.print_level\": 0, \"print_time\": 0, \"ipopt.sb\": \"yes\"}\n",
"s_opts = {\"max_iter\": 100, \"print_level\": 0}\n",
"opti.solver(\"ipopt\", p_opts, s_opts)\n"
"opti.solver(\"ipopt\", p_opts, s_opts)"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "Hf-Uq8PWFy6v"
},
"source": [
"# Simulation loop\n",
"\n",
Expand All @@ -247,10 +257,7 @@
"\n",
"On the notebook it is a bit slow.\n",
"To run it real time set OMP_NUM_THREADS=1 on your laptop!"
],
"metadata": {
"id": "Hf-Uq8PWFy6v"
}
]
},
{
"cell_type": "code",
Expand All @@ -275,45 +282,49 @@
"mujoco.mj_resetData(wrapper.model, wrapper.data)\n",
"i = 0\n",
"while wrapper.data.time < duration:\n",
" wrapper.step()\n",
" if len(frames) < wrapper.data.time * framerate:\n",
" # you do not need to set the desired ee position every time step\n",
" # you can set it only when you want to change it\n",
" opti.set_value(des_ee_pos, des_ee_pos_numeric)\n",
" q0_numeric = wrapper.get_qpos()\n",
" # remove the last joint since they are not controlled\n",
" q0_numeric = q0_numeric[:-1]\n",
" # set the initial condition\n",
" opti.set_value(q0, q0_numeric)\n",
" sol = opti.solve()\n",
" # take the q_dot solution at the first time step and extend with the last joint to 0\n",
" sol_q_dot = sol.value(q_dot)[:, 0]\n",
" sol_q_dot = np.concatenate((sol_q_dot, np.zeros(1)))\n",
" wrapper.set_qvel(sol_q_dot)\n",
" pixels = wrapper.render()\n",
" frames.append(pixels)\n",
" # set the solution as the initial condition for the next time step\n",
" opti.set_initial(q, sol.value(q))\n",
" opti.set_initial(q_dot, sol.value(q_dot))\n",
" i += 1\n",
" if wrapper.data.time > 2:\n",
" # change the desired ee position\n",
" des_ee_pos_numeric = np.array([2.0, 0.0, 0.2])\n",
" opti.set_value(des_ee_pos, des_ee_pos_numeric)\n",
" if wrapper.data.time > 4:\n",
" # change the desired ee position\n",
" des_ee_pos_numeric = np.array([0.0, 0.5, 0.4])\n",
" opti.set_value(des_ee_pos, des_ee_pos_numeric)\n",
" if wrapper.data.time > 6:\n",
" # change the desired ee position\n",
" des_ee_pos_numeric = np.array([0.0, -0.6, 0.6])\n",
" opti.set_value(des_ee_pos, des_ee_pos_numeric)\n",
" wrapper.step()\n",
" if len(frames) < wrapper.data.time * framerate:\n",
" # you do not need to set the desired ee position every time step\n",
" # you can set it only when you want to change it\n",
" opti.set_value(des_ee_pos, des_ee_pos_numeric)\n",
" q0_numeric = wrapper.get_qpos()\n",
" # remove the last joint since they are not controlled\n",
" q0_numeric = q0_numeric[:-1]\n",
" # set the initial condition\n",
" opti.set_value(q0, q0_numeric)\n",
" sol = opti.solve()\n",
" # take the q_dot solution at the first time step and extend with the last joint to 0\n",
" sol_q_dot = sol.value(q_dot)[:, 0]\n",
" sol_q_dot = np.concatenate((sol_q_dot, np.zeros(1)))\n",
" wrapper.set_qvel(sol_q_dot)\n",
" pixels = wrapper.render()\n",
" frames.append(pixels)\n",
" # set the solution as the initial condition for the next time step\n",
" opti.set_initial(q, sol.value(q))\n",
" opti.set_initial(q_dot, sol.value(q_dot))\n",
" i += 1\n",
" if wrapper.data.time > 2:\n",
" # change the desired ee position\n",
" des_ee_pos_numeric = np.array([2.0, 0.0, 0.2])\n",
" opti.set_value(des_ee_pos, des_ee_pos_numeric)\n",
" if wrapper.data.time > 4:\n",
" # change the desired ee position\n",
" des_ee_pos_numeric = np.array([0.0, 0.5, 0.4])\n",
" opti.set_value(des_ee_pos, des_ee_pos_numeric)\n",
" if wrapper.data.time > 6:\n",
" # change the desired ee position\n",
" des_ee_pos_numeric = np.array([0.0, -0.6, 0.6])\n",
" opti.set_value(des_ee_pos, des_ee_pos_numeric)\n",
"\n",
"media.show_video(frames, fps=framerate)\n"
"media.show_video(frames, fps=framerate)"
]
}
],
"metadata": {
"colab": {
"include_colab_link": true,
"provenance": []
},
"kernelspec": {
"display_name": "adam_env",
"language": "python",
Expand All @@ -330,10 +341,6 @@
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.10.11"
},
"colab": {
"provenance": [],
"include_colab_link": true
}
},
"nbformat": 4,
Expand Down
3 changes: 2 additions & 1 deletion src/adam/casadi/computations.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,9 +2,10 @@
# This software may be modified and distributed under the terms of the
# GNU Lesser General Public License v2.1 or any later version.

from typing import Union

import casadi as cs
import numpy as np
from typing import Union

from adam.casadi.casadi_like import SpatialMath
from adam.core import RBDAlgorithms
Expand Down
2 changes: 1 addition & 1 deletion src/adam/model/__init__.py
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
from .abc_factories import Joint, Link, ModelFactory, Inertial, Pose
from .abc_factories import Inertial, Joint, Link, ModelFactory, Pose
from .model import Model
from .std_factories.std_joint import StdJoint
from .std_factories.std_link import StdLink
Expand Down
6 changes: 3 additions & 3 deletions src/adam/model/conversions/idyntree.py
View file
Original file line number Diff line number Diff line change
@@ -1,11 +1,11 @@
from typing import List

import idyntree.bindings
import numpy as np
import urdf_parser_py.urdf
from typing import List


from adam.model.abc_factories import Joint, Link
from adam.model.model import Model
from adam.model.abc_factories import Link, Joint


def to_idyntree_solid_shape(
Expand Down
2 changes: 1 addition & 1 deletion src/adam/model/std_factories/std_link.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
import urdf_parser_py.urdf

from adam.core.spatial_math import SpatialMath
from adam.model import Link, Inertial, Pose
from adam.model import Inertial, Link, Pose


class StdLink(Link):
Expand Down
5 changes: 3 additions & 2 deletions src/adam/model/std_factories/std_model.py
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,8 @@
import os
import pathlib
from typing import List
import xml.etree.ElementTree as ET
import os
from typing import List

import urdf_parser_py.urdf

from adam.core.spatial_math import SpatialMath
Expand Down
Loading

0 comments on commit e68b68a

Please sign in to comment.