Merge pull request #1 from duckietown/mooc2022

braitenberg/packages/braitenberg_agent/agent.py

@@ -25,8 +25,8 @@
 # TODO edit this Config class ! Play with different gain and const values
 @dataclass
 class BraitenbergAgentConfig:
-    gain: float = 0.9
-    const: float = 0.0
+    gain: float = 0.3
+    const: float = 0.5
 
 
 class BraitenbergAgent:

braitenberg/packages/solution/connections.py

@@ -4,19 +4,12 @@
 
 
 def get_motor_left_matrix(shape: Tuple[int, int]) -> np.ndarray:
-    # TODO: write your function instead of this one
     res = np.zeros(shape=shape, dtype="float32")
-    # these are random values
-    res[100:150, 100:150] = 1
-    res[300:, 200:] = 1
-    # ---
+    res[:, 0 : int(shape[1] / 2)] = 1
     return res
 
 
 def get_motor_right_matrix(shape: Tuple[int, int]) -> np.ndarray:
-    # TODO: write your function instead of this one
     res = np.zeros(shape=shape, dtype="float32")
-    # these are random values
-    res[100:150, 100:300] = -1
-    # ---
+    res[:, int(shape[1] / 2) : shape[1]] = 1
     return res

braitenberg/packages/solution/preprocessing.py

@@ -1,8 +1,8 @@
 import cv2
 import numpy as np
 
-lower_hsv = np.array([171, 140, 100])
-upper_hsv = np.array([179, 200, 255])
+lower_hsv = np.array([0.0001, 80, 40])
+upper_hsv = np.array([25.7, 255, 255])
 
 
 def preprocess(image_rgb: np.ndarray) -> np.ndarray:

modcon/notebooks/01-Representations/pose_representation.ipynb

@@ -134,7 +134,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 1,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -145,20 +145,31 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 2,
    "metadata": {},
-   "outputs": [],
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "1.0471975511965976\n",
+      "[[ 0.5       -0.8660254  2.       ]\n",
+      " [ 0.8660254  0.5        3.       ]\n",
+      " [ 0.         0.         1.       ]]\n"
+     ]
+    }
+   ],
    "source": [
     "# Type your answer here\n",
     "# Tip 1: always express your angles in radians!\n",
     "# Tip 2: you can go from degrees to radians through the np.deg2rad() function\n",
     "\n",
-    "theta = None\n",
+    "theta = np.deg2rad(60)\n",
     "\n",
     "p = np.array([\n",
-    "    [None, None, None],\n",
-    "    [None, None, None],\n",
-    "    [None, None, None]\n",
+    "    [np.cos(theta), -np.sin(theta), 2],\n",
+    "    [np.sin(theta), np.cos(theta), 3],\n",
+    "    [0, 0, 1]\n",
     "])\n",
     "\n",
     "print(theta)\n",
@@ -226,7 +237,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 3,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -242,24 +253,48 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 4,
    "metadata": {},
-   "outputs": [],
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "[1.71809221 0.50260604]\n"
+     ]
+    }
+   ],
    "source": [
     "# Let's find the answer!\n",
-    "\n",
     "# Step 1. Convert degrees to radians\n",
+    "rot_in_rad_a = np.deg2rad(duckie_or_g) # (np.pi * duckie_or_g) / 180.\n",
+    "rot_in_rad_b = np.deg2rad(obstacle_angle) # (np.pi * obstacle_angle) / 180.\n",
     "\n",
     "# Step 2. Transformation matrix from origin (o) to Duckiebot (a)\n",
     "\n",
+    "T_oa = np.array(([np.cos(rot_in_rad_a),-np.sin(rot_in_rad_a),duckie_pos_g[0]],\n",
+    "                 [np.sin(rot_in_rad_a),np.cos(rot_in_rad_a),duckie_pos_g[1]],\n",
+    "                 [0, 0, 1]))\n",
+    "\n",
     "# Step 3. Transformation matrix from Duckiebot (a) to obstacle (b)\n",
     "\n",
+    "dx = obstacle_dist_to_duckie * np.cos(rot_in_rad_b)\n",
+    "dy = obstacle_dist_to_duckie * np.sin(rot_in_rad_b)\n",
+    "T_ab =  np.array(([np.cos(rot_in_rad_b),-np.sin(rot_in_rad_b),dx],\n",
+    "                 [np.sin(rot_in_rad_b),np.cos(rot_in_rad_b),dy],\n",
+    "                 [0, 0, 1]))\n",
+    "\n",
     "# Step 4. Use the above to compute the transformation from origin (o) to obstacle (b)\n",
     "\n",
+    "T_ob = np.dot(T_oa,T_ab) # T_ob = (T_oa)(T_ab)\n",
+    "\n",
     "# Step 5. Extract the information requested from the transformation matrix\n",
     "\n",
+    "obstacle_x_g = T_ob[0,2]\n",
+    "obstacle_y_g = T_ob[1,2]\n",
+    "\n",
     "# Write your answer (position of obstacle in global frame) here instead of \"None, None\" \n",
-    "obstacle_pos_g = np.array([None, None])    \n",
+    "obstacle_pos_g = np.array([obstacle_x_g, obstacle_y_g])    \n",
     "print(obstacle_pos_g)"
    ]
   },
@@ -296,7 +331,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 5,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -310,24 +345,39 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 6,
    "metadata": {},
-   "outputs": [],
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "[ 0.49497475 -0.21213203]\n"
+     ]
+    }
+   ],
    "source": [
     "# Let's send the Duckiebot to the correct place!\n",
-    "\n",
     "# Convert degrees to radians\n",
+    "rot_in_rad_a = (np.pi * duckie_or_g) / 180.\n",
     "\n",
     "# Write the transformation matrix from origin (o) to the duckie (a)\n",
+    "T_oa = np.array(([np.cos(rot_in_rad_a),-np.sin(rot_in_rad_a),duckie_pos_g[0]],\n",
+    "                 [np.sin(rot_in_rad_a),np.cos(rot_in_rad_a),duckie_pos_g[1]],\n",
+    "                 [0, 0, 1])) # Transformation matrix from origin (o) to the duckie (a)\n",
     "\n",
-    "# Write the transformation matrix from origin (o) to the obstacle (b)\n",
+    "# Write the transformation matrix from origin (o) to the obstacle (b)                \n",
+    "T_ob = np.array(([1, 0, obstacle_pos_g[0]],\n",
+    "                 [0, 1, obstacle_pos_g[1]],\n",
+    "                 [0, 0, 1])) # Transformation matrix from origin (o) to the obstacle (b)\n",
     "\n",
     "# Leverage the known properties of transformation matrices and basics of linear algebra to find your answer\n",
+    "T_ab = np.dot(np.linalg.inv(T_oa),T_ob)\n",
     "\n",
-    "# Extract the obstacle position from the transformation matrix\n",
     "\n",
-    "obstacle_x_a = None\n",
-    "obstacle_y_a = None\n",
+    "# Extract the obstacle position from the transformation matrix\n",
+    "obstacle_x_a = T_ab[0,2]\n",
+    "obstacle_y_a = T_ab[1,2]\n",
     "\n",
     "# Place your answer here instead of None, None (position of obstacle in global frame)\n",
     "obstacle_pos_r = np.array([obstacle_x_a, obstacle_y_a])    \n",

modcon/notebooks/01-Representations/solutions_pose_representation.ipynb

@@ -18,7 +18,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 2,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -29,9 +29,20 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 3,
    "metadata": {},
-   "outputs": [],
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "1.0471975511965976\n",
+      "[[ 0.5       -0.8660254  2.       ]\n",
+      " [ 0.8660254  0.5        3.       ]\n",
+      " [ 0.         0.         1.       ]]\n"
+     ]
+    }
+   ],
    "source": [
     "# Type your answer here\n",
     "# Tip 1: always express your angles in radians!\n",
@@ -58,7 +69,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 4,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -72,9 +83,17 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 5,
    "metadata": {},
-   "outputs": [],
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "[1.71809221 0.50260604]\n"
+     ]
+    }
+   ],
    "source": [
     "# Let's find the answer!\n",
     "# Step 1. Convert degrees to radians\n",
@@ -118,7 +137,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 6,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -131,9 +150,17 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 7,
    "metadata": {},
-   "outputs": [],
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "[ 0.49497475 -0.21213203]\n"
+     ]
+    }
+   ],
    "source": [
     "# Let's send the Duckiebot to the correct place!\n",
     "# Convert degrees to radians\n",

modcon/notebooks/03-Wheel-Encoders-Tutorial/wheel_encoders_tutorial.ipynb

@@ -185,9 +185,40 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 1,
    "metadata": {},
-   "outputs": [],
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "The received message is :\n",
+      "\n",
+      "header: \n",
+      "  seq: 372\n",
+      "  stamp: \n",
+      "    secs: 1618436796\n",
+      "    nsecs:  55785179\n",
+      "  frame_id: \"agent/left_wheel_axis\"\n",
+      "data: 4\n",
+      "resolution: 135\n",
+      "type: 1\n",
+      "\n",
+      "N. of ticks : 4\n",
+      "Total ticks : 135\n"
+     ]
+    },
+    {
+     "data": {
+      "text/plain": [
+       "<tests.unit_test.UnitTestMessage at 0x7fdd44d549d0>"
+      ]
+     },
+     "execution_count": 1,
+     "metadata": {},
+     "output_type": "execute_result"
+    }
+   ],
    "source": [
     "import sys\n",
     "sys.path.append('../')\n",