Add gravity compensation

flferretti · Dec 22, 2023 · 20a45f6 · 20a45f6
1 parent ef7c161
commit 20a45f6
Show file tree

Hide file tree

Showing 2 changed files with 171 additions and 9 deletions.
diff --git a/examples/MUJOCO_LOG.TXT b/examples/MUJOCO_LOG.TXT
@@ -0,0 +1,156 @@
+Fri Dec 22 12:09:55 2023
+WARNING: Inertia matrix is too close to singular at DOF 0. Check model. Time = 0.0000.
+
+Fri Dec 22 12:12:17 2023
+WARNING: Inertia matrix is too close to singular at DOF 0. Check model. Time = 0.0000.
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
+Fri Dec 22 12:12:17 2023
+WARNING: Linesearch objective is not convex
+
diff --git a/examples/PD_controller.ipynb b/examples/PD_controller.ipynb
@@ -115,7 +115,7 @@
     "!{sys.executable} -m pip install -U -q mujoco\n",
     "print('Installing mediapy:')\n",
     "!command -v ffmpeg >/dev/null || (apt update && apt install -y ffmpeg)\n",
-    "!pip install -q mediapy\n",
+    "!{sys.executable} -m pip install -q mediapy\n",
     "\n",
     "import mediapy as media\n",
     "import tempfile\n",
@@ -207,9 +207,8 @@
     "  return mujoco_jointpos\n",
     " \n",
     "\n",
-    "\n",
     "# To get a good camera location, you can use \"Copy camera\" functionality in MuJoCo GUI\n",
-    "mj_model = load_mujoco_model_with_camera(model_urdf_string, [2.507, 1.822, 1.867], [-0.588, 0.809, 0.000, -0.170, -0.124, 0.978])\n",
+    "mj_model = load_mujoco_model_with_camera(model_urdf_string, [3.954, 3.533, 2.343], [-0.594, 0.804, -0.000, -0.163, -0.120, 0.979])\n",
     "renderer = mujoco.Renderer(mj_model, height=480, width=640)\n",
     "\n",
     "def get_image(camera, mujocojointpos) -> np.ndarray:\n",
@@ -241,7 +240,7 @@
     "\n",
     "sim_images = []\n",
     "timestep = 0.01\n",
-    "for _ in range(200):\n",
+    "for _ in range(300):\n",
     "    sim_images.append(get_image(\"side\", from_jaxsim_to_mujoco_pos(np.array(model.joint_positions()), mj_model, model)))\n",
     "    model.integrate(t0=0.0, tf=timestep, integrator_type=IntegratorType.EulerSemiImplicit)\n",
     "\n",
@@ -254,7 +253,12 @@
    "source": [
     "Let's now define the PD controller. We will use the following equations:\n",
     "\n",
-    "\\begin{align} \\tau &= K_p \\left( q_d - q \\right) + K_d \\left( \\dot{q}_d - \\dot{q} \\right) \\end{align}"
+    "\\begin{align} \n",
+    "\\mathbf{M}\\ddot{s} + \\underbrace{\\mathbf{C}\\dot{s} + \\mathbf{G}}_{\\mathbf{H}} = \\tau \\\\\n",
+    "\\tau = \\mathbf{H} - \\mathbf{K}_p(s - s_d) - \\mathbf{K}_d(\\dot{s} - \\dot{s}_d)\n",
+    "\\end{align}\n",
+    "\n",
+    "where $\\mathbf{M}$ is the mass matrix, $\\mathbf{C}$ is the Coriolis matrix, $\\mathbf{G}$ is the gravity vector, $\\mathbf{K}_p$ is the proportional gain matrix, $\\mathbf{K}_d$ is the derivative gain matrix, $s$ is the position vector, $\\dot{s}$ is the velocity vector, $\\ddot{s}$ is the acceleration vector, and $s_d$ and $\\dot{s}_d$ are the desired position and velocity vectors, respectively."
    ]
   },
   {
@@ -264,14 +268,16 @@
    "outputs": [],
    "source": [
     "# Define the PD gains\n",
-    "KP = 30.0\n",
-    "KD = 4.0\n",
+    "KP = 10.0\n",
+    "KD = 6.0\n",
     "\n",
+    "# Compute the gravity compensation term\n",
+    "H = model.free_floating_bias_forces()[6:]\n",
     "\n",
     "def pd_controller(\n",
     "    q: jax.Array, q_d: jax.Array, q_dot: jax.Array, q_dot_d: jax.Array\n",
     ") -> jax.Array:\n",
-    "    return KP * (q_d - q) + KD * (q_dot_d - q_dot)"
+    "    return H + KP * (q_d - q) + KD * (q_dot_d - q_dot)"
    ]
   },
   {
@@ -290,7 +296,7 @@
     "sim_images = []\n",
     "timestep = 0.01\n",
     "\n",
-    "for _ in range(200):\n",
+    "for _ in range(300):\n",
     "    sim_images.append(get_image(\"side\", from_jaxsim_to_mujoco_pos(np.array(model.joint_positions()), mj_model, model)))\n",
     "    model.set_joint_generalized_force_targets(\n",
     "        forces=pd_controller(\n",