Skip to content

Commit

Permalink
Merge pull request #57 from ami-iit/ik-example
Browse files Browse the repository at this point in the history 
Update examples
  • Loading branch information
@Giulero
Giulero authored Dec 12, 2023
2 parents 21a7fa8 + 7595aca commit 5c24e22
Show file tree
Hide file tree
Showing 2 changed files with 323 additions and 4,919 deletions.
309 changes: 309 additions & 0 deletions examples/ik.ipynb
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,309 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"id": "view-in-github",
"colab_type": "text"
},
"source": [
"<a href=\"https://colab.research.google.com/github/ami-iit/ADAM/blob/ik-example/examples/ik.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "MmkmgG_gGWVi"
},
"source": [
"## Install Mujoco, adam, and mediapy.\n",
"\n",
"Download also mujoco-menagerie for the panda model and the urdf needed from adam.\n",
"Set some stuff for the visualization."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "4YLBIn1QLEqx"
},
"outputs": [],
"source": [
"!pip install mujoco\n",
"!pip install adam-robotics\n",
"!pip install -q mediapy\n",
"!git clone https://github.com/google-deepmind/mujoco_menagerie.git\n",
"!wget https://raw.githubusercontent.com/bulletphysics/bullet3/master/examples/pybullet/gym/pybullet_data/franka_panda/panda.urdf\n",
"# Graphics and plotting.\n",
"print('Installing mediapy:')\n",
"!command -v ffmpeg >/dev/null || (apt update && apt install -y ffmpeg)\n",
"\n",
"# Configure MuJoCo to use the EGL rendering backend (requires GPU)\n",
"print('Setting environment variable to use GPU rendering:')\n",
"%env MUJOCO_GL=egl\n"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "Dz4cHPy2Gtmq"
},
"source": [
"## Import packages"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "ztJxnbhaLEqz"
},
"outputs": [],
"source": [
"import mujoco\n",
"import mediapy as media\n",
"from adam.casadi import KinDynComputations\n",
"import numpy as np\n",
"import casadi as cs\n"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "2zw4FO-IGxdR"
},
"source": [
"## Import the panda scene in mujoco"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "iEBl8tgoLEq0"
},
"outputs": [],
"source": [
"# load scene from xml\n",
"model = mujoco.MjModel.from_xml_path(\"mujoco_menagerie/franka_emika_panda/scene.xml\")\n"
]
},
{
"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!"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "gupN7s3CLEq0"
},
"outputs": [],
"source": [
"joints_name_list = ['panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4', 'panda_joint5', 'panda_joint6', 'panda_joint7', 'panda_joint8']\n",
"\n",
"kindyn = KinDynComputations(urdfstring=\"panda.urdf\", joints_name_list=joints_name_list)\n"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "g5LX5kQAKwaM"
},
"source": [
"## A wrapper interface with mujoco"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "XpniQY18LEq0"
},
"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",
"\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 get_qpos(self):\n",
" # get the joint positions\n",
" return self.data.qpos[:]\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",
"\n",
" def step(self):\n",
" # step the simulation\n",
" mujoco.mj_step(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"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "wuY9hqdlD3Vo"
},
"source": [
"# Model IK\n",
"\n",
"A simple IK with damped least squares and manipulability regularization."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "r8w2TWr6LEq1"
},
"outputs": [],
"source": [
"# setup inverse kinematics\n",
"\n",
"\n",
"w_H_ee = kindyn.forward_kinematics_fun(\"panda_hand\")\n",
"J = kindyn.jacobian_fun(\"panda_hand\")\n",
"\n",
"class InverseKinematics:\n",
" def __init__(self, w_H_ee, J):\n",
" self.q_cs = cs.SX.sym(\"joint_positions\", 8)\n",
" self.q_dot_cs = cs.SX.sym(\"joint_velocities\", 8)\n",
" w_H_b = np.eye(4)\n",
" self.w_p_ee = cs.Function(\"w_H_ee\", [self.q_cs], [w_H_ee(w_H_b, self.q_cs)[:3, 3]])\n",
" self.J = cs.Function(\"J\", [self.q_cs], [J(w_H_b, self.q_cs)[:3, 6:]])\n",
" manipulability = cs.sqrt(cs.det(self.J(self.q_cs) @ self.J(self.q_cs).T))\n",
" # use casadi tools to compute the gradient of the manipulability\n",
" q_dot_manipulability = cs.jacobian(manipulability, self.q_cs).T\n",
" self.q_dot_manipulability = cs.Function(\"q_dot_manipulability\", [self.q_cs], [q_dot_manipulability])\n",
"\n",
" def __call__(self, q, w_p_ee_desired):\n",
" w_p_ee = self.w_p_ee(q)\n",
" ee_error = w_p_ee_desired - w_p_ee\n",
" J = self.J(q)\n",
" K_p = 2\n",
" N = self.null_space_projection(J)\n",
" q_dot_bias = self.q_dot_manipulability(q)\n",
" # damped least squares\n",
" damping_factor = 1e-2\n",
" damped_pinv = np.linalg.inv(J.T @ J + damping_factor * np.eye(8)) @ J.T\n",
" q_dot = damped_pinv @ (K_p * ee_error) + N @ q_dot_bias\n",
" return np.array(q_dot).squeeze()\n",
"\n",
" def null_space_projection(self, J):\n",
" return np.eye(8) - np.linalg.pinv(J) @ J\n",
"\n",
"ik = InverseKinematics(w_H_ee, J)\n"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "Hf-Uq8PWFy6v"
},
"source": [
"# Simulation loop\n",
"\n",
"We set the joint velocities as control input in Mujoco.\n",
"We retrieve the joint positions as measurement from Mujoco and set them as feedback for the IK.\n"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "zG_pcqOfLEq1"
},
"outputs": [],
"source": [
"# start mujoco simulation along with control\n",
"duration = 10 # (seconds)\n",
"framerate = 100 # (Hz)\n",
"\n",
"# Simulate and display video.\n",
"\n",
"des_ee_pos_numeric = np.array([0.4, 1.0, 0.4])\n",
"\n",
"frames = []\n",
"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",
" i += 1\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",
" sol_q_dot = ik(q0_numeric, des_ee_pos_numeric)\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",
" 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",
" 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",
" 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",
"\n",
"media.show_video(frames, fps=framerate)\n"
]
}
],
"metadata": {
"colab": {
"provenance": [],
"include_colab_link": true
},
"kernelspec": {
"display_name": "adam_env",
"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.11"
}
},
"nbformat": 4,
"nbformat_minor": 0
}
Loading

0 comments on commit 5c24e22

Please sign in to comment.