[L4] Adding challenge 1.

Author: Murilo Marinho

basic_lessons/lesson4_exercise_answers.ipynb

@@ -46,8 +46,8 @@
   {
    "metadata": {
     "ExecuteTime": {
-     "end_time": "2026-02-20T14:06:45.056822Z",
-     "start_time": "2026-02-20T14:06:45.042313Z"
+     "end_time": "2026-02-20T15:10:25.892287Z",
+     "start_time": "2026-02-20T15:10:25.874186Z"
     }
    },
    "cell_type": "code",
@@ -56,7 +56,7 @@
     "import numpy as np"
    ],
    "outputs": [],
-   "execution_count": 13
+   "execution_count": 63
   },
   {
    "metadata": {},
@@ -94,8 +94,8 @@
   {
    "metadata": {
     "ExecuteTime": {
-     "end_time": "2026-02-20T14:06:45.072929Z",
-     "start_time": "2026-02-20T14:06:45.058627Z"
+     "end_time": "2026-02-20T15:10:25.906372Z",
+     "start_time": "2026-02-20T15:10:25.892785Z"
     }
    },
    "cell_type": "code",
@@ -154,7 +154,118 @@
      ]
     }
    ],
-   "execution_count": 14
+   "execution_count": 64
+  },
+  {
+   "metadata": {},
+   "cell_type": "markdown",
+   "source": [
+    "# Challenge 1\n",
+    "\n",
+    "To solve this challenge, we notice the patterns in the computation. You can change to the same link values of exercise a to test that this is correct."
+   ]
+  },
+  {
+   "metadata": {
+    "ExecuteTime": {
+     "end_time": "2026-02-20T15:10:25.918860Z",
+     "start_time": "2026-02-20T15:10:25.907159Z"
+    }
+   },
+   "cell_type": "code",
+   "source": [
+    "# Consider manipulator DoFs as the length of the following lists.\n",
+    "# Consider it as the configuration space of the RRR...RRR robot\n",
+    "q = [pi/4.0, -pi/8.0, pi/12.0, -pi/3.0] # Increase length of q if you'd like to check\n",
+    "l = [1, 1, 1, 1] # l must be same size of q\n",
+    "\n",
+    "if len(q) != len(l):\n",
+    "    raise Exception(\"q and l are not the same length\")\n",
+    "\n",
+    "def c_n(q, n_frame):\n",
+    "    \"\"\"\n",
+    "    Get cos(q_0 + q_1 + ... + q_n).\n",
+    "    \"\"\"\n",
+    "    q_sum = 0\n",
+    "    c_result = 0\n",
+    "    for i in range(n_frame+1):\n",
+    "        qi = q[i]\n",
+    "        q_sum += qi # First will be q_0, then q_0 + q_1, then...\n",
+    "        c_result = cos(q_sum) # First will be cos(q_0), then cos(q_0 + q_1), then...\n",
+    "    return c_result\n",
+    "\n",
+    "def s_n(q, n_frame):\n",
+    "    \"\"\"\n",
+    "    Get sin(q_0 + q_1 + ... + q_n).\n",
+    "    \"\"\"\n",
+    "    q_sum = 0\n",
+    "    s_result = 0\n",
+    "    for i in range(n_frame+1):\n",
+    "        qi = q[i]\n",
+    "        q_sum += qi # First will be q_0, then q_0 + q_1, then...\n",
+    "        s_result = sin(q_sum) # First will be sin(q_0), then sin(q_0 + q_1), then...\n",
+    "    return s_result\n",
+    "\n",
+    "def px_n(q, l, n_frame):\n",
+    "    \"\"\"\n",
+    "    Get l_0*cos(q_0) + l_1*cos(q_0 + q_1) + ... + l_n*cos(q_0 + q_1 + ... + q_n).\n",
+    "    \"\"\"\n",
+    "    px = 0\n",
+    "    for i in range(n_frame + 1):\n",
+    "        li = l[i]\n",
+    "        px += li * c_n(q, i)\n",
+    "    return px\n",
+    "\n",
+    "def py_n(q, l, n_frame):\n",
+    "    \"\"\"\n",
+    "    Get l_0*sin(q_0) + l_1*sin(q_0 + q_1) + ... + l_n*sin(q_0 + q_1 + ... + q_n).\n",
+    "    \"\"\"\n",
+    "    py = 0\n",
+    "    for i in range(n_frame + 1):\n",
+    "        li = l[i]\n",
+    "        py += li * s_n(q, i)\n",
+    "    return py\n",
+    "\n",
+    "\n",
+    "def j_n(q, l, n_frame):\n",
+    "    \"\"\"\n",
+    "    Construct the n-th column of the Jacobian matrix.\n",
+    "    \"\"\"\n",
+    "    N = len(q)\n",
+    "\n",
+    "    pnx = px_n(q, l, n_frame-1) # starts at 0\n",
+    "    pny = py_n(q, l, n_frame-1) # starts at 0\n",
+    "    pNx = px_n(q, l, N-1)\n",
+    "    pNy = py_n(q, l, N-1)\n",
+    "\n",
+    "    px = pNx - pnx\n",
+    "    py = pNy - pny\n",
+    "\n",
+    "    jn = np.array(\n",
+    "        [[-py],\n",
+    "          [px],\n",
+    "          [1]]\n",
+    "    )\n",
+    "    return jn\n",
+    "\n",
+    "# Jacobian\n",
+    "J = j_n(q, l, 0)\n",
+    "for i in range(1,len(q)):\n",
+    "    J = np.hstack((J, j_n(q, l, i))) # We stack the columns horizontally\n",
+    "print(J)\n"
+   ],
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "[[-1.31586821 -0.60876143 -0.226078    0.38268343]\n",
+      " [ 3.34821919  2.64111241  1.71723287  0.92387953]\n",
+      " [ 1.          1.          1.          1.        ]]\n"
+     ]
+    }
+   ],
+   "execution_count": 65
   }
  ],
  "metadata": {