Skip to content

Commit

Permalink
Fix collision checker exercise
Browse files Browse the repository at this point in the history
  • Loading branch information
@nicolaloi
nicolaloi committed Nov 11, 2023
1 parent b7e78c6 commit f9a3662
Show file tree
Hide file tree
Showing 3 changed files with 131 additions and 31 deletions.
6 changes: 4 additions & 2 deletions collision-checker/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,9 @@ While this checker will only function in-simulation, it should give you a good i
The boilerplate code contained in this folder is a fully functional (albeit wrong) solution. Currently, the solution submits random guesses as to whether the robot collides with the environment or not.
You can try to evaluate right away to see how it works.

This learning experience is provided by the Duckietown team and can be run on Duckiebots. Visit us at the
**Note: This is a code-only exercise: you don't need the Duckiebot**.

This learning experience is provided by the Duckietown team. Visit us at the
[Duckietown Website](https://www.duckietown.com) for more learning materials, documentation, and demos.

For guided setup instructions, lecture content, and more related to this LX, see the [Self Driving Cars with
Expand Down Expand Up @@ -84,7 +86,7 @@ Follow the instructions on the notebook and work through the notebooks in sequen
We suggest you evaluate your work locally before submitting your solution.
You can do so by running the following command,

dts code evaluate
dts code evaluate --challenge lx22-collision-check-vali

This should take a few minutes.
This is not supposed to be an interactive process: just let it run, and when you return,
Expand Down
140 changes: 120 additions & 20 deletions collision-checker/notebooks/01-Collision-Checker/collision_checker.ipynb
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,13 @@
"\n",
"As part of this exercise, you will write your very own collision checker. While this checker will only function in-simulation, it should give you a good idea of the complexity associated with detecting collisions in the real world.\n",
"\n",
"We have defined a few data structures that you will use in this task."
"**Note: This is a code-only exercise: you don't need the Duckiebot**.\n",
"\n",
"We have defined a few data structures that you will use in this task.\n",
"\n",
"You will be working in the [`collision_checker.py`][file] file.\n",
"\n",
"[file]: ../../packages/collision_checker/collision_checker.py"
]
},
{
Expand All @@ -35,16 +41,16 @@
"make up the robot's body. Therefore, both `environment` and `body` are lists of `PlacedPrimitive`s. However, in\n",
"the validation tests, the robot list will only contain one `PlacedPrimitive`.\n",
"\n",
"A `PlacedPrimitive` is a pair of a `FriendlyPose` and a `Primitive`, or a pose and a shape. Note that `theta_deg` in\n",
"`FriendlyPose` starts at zero in the positive x-axis direction and ends at 359 degrees, moving in a counter-clockwise \n",
"direction.\n",
"A `PlacedPrimitive` is a pair of a `FriendlyPose` and a `Primitive`, or a pose and a shape (for this exercise, you do not need to consider the `motion` and `appearance` attributes). Note that `theta_deg` in `FriendlyPose` starts at zero in the positive x-axis direction and ends at 359 degrees, moving in a counter-clockwise direction.\n",
"\n",
"\n",
"```python\n",
"@dataclass\n",
"class PlacedPrimitive:\n",
" pose: FriendlyPose\n",
" primitive: Primitive\n",
" motion: Optional[Motion] = None\n",
" appearance: Optional[Appearance] = None\n",
" \n",
" \n",
"@dataclass\n",
Expand All @@ -56,14 +62,13 @@
"\n",
"A `FriendlyPose` is a handy pose representation containing a $(x,y)$ coordinate along with an angle. How friendly!\n",
"\n",
"A `Primitive` is either a `Rectangle` or a `Circle`. A circle's shape needs only be defined\n",
"by a `radius`, while a Rectangle is defined by four values:\n",
" - `xmax` is the distance from the pose point to its side in the positive x direction (if theta_deg in `FriendlyPose` is zero, this side is on the right of the pose point).\n",
" - `xmin` is the same, but in the negative x direction\n",
" - `ymax` is the distance from the center point to its side in the positive y direction (if theta_deg in `FriendlyPose` is zero, this side is on the top of the pose point).\n",
" - `ymin` is the same, but in the negative y direction\n",
"A `Primitive` is either a `Circle` or a `Rectangle`. A `Circle`'s shape needs only be defined by a `radius`, while a `Rectangle` is defined by four values:\n",
" - `xmax` is the distance from the pose point to its side in the positive x direction (if `theta_deg` in `FriendlyPose` is zero, this side is on the right of the pose point);\n",
" - `xmin` is the same, but in the negative x direction;\n",
" - `ymax` is the distance from the center point to its side in the positive y direction (if `theta_deg` in `FriendlyPose` is zero, this side is on the top of the pose point);\n",
" - `ymin` is the same, but in the negative y direction.\n",
" \n",
"`xmax`, `xmin`, `ymax`, and `ymin` are all given with respect to the robot/obstacle's coordinate system, and not the world coordinate system. Therefore, the theta_deg value of the `FriendlyPose` affects the rotation of the Rectangle.\n",
"`xmax`, `xmin`, `ymax`, and `ymin` are all given with respect to the robot/obstacle's coordinate system, and not the world coordinate system. Therefore, the `theta_deg` value of the `FriendlyPose` affects the rotation of the `Rectangle`.\n",
"\n",
"```python\n",
"@dataclass\n",
Expand All @@ -82,9 +87,9 @@
"\n",
"So, we represents shapes as the union of rototranslated `Rectangle`s and `Circle`s.\n",
"\n",
"The class `CollisionChecker` in `collision_checker.py` first receives a message `MapDefinition` to define the environment and robot shape. It also contains a default pose for the robot (0, 0). Then, it recieves a sequence of `CollisionCheckQuery`s. The query contains a new pose for the robot, which you will need to cross-reference against the `MapDefinition` to detect collisions. \n",
"The class `CollisionChecker` in [collision_checker.py](../../packages/collision_checker/collision_checker.py) first receives a message `MapDefinition` to define the environment and robot shape. It also contains a default pose for the robot (0, 0). Then, it receives a sequence of `CollisionCheckQuery`s. The query contains a new pose for the robot, which you will need to cross-reference against the `MapDefinition` to detect collisions. \n",
"\n",
"Therefore, the `CollisionChecker` must take the new pose from the `CollisionCheckQuery`, combine it with the default pose already contained the the robot's `PlacedPrimitive`s, and then see if it collides with any part of the environment. The result of this will go into a `CollisionCheckResult`. The `CollisionCheckResult` contains only a boolean: true means that it is in collision, false means that it is not in collision."
"Therefore, the `CollisionChecker` must take the new pose from the `CollisionCheckQuery`, combine it with the default pose already contained in the robot's `PlacedPrimitive`s, and then see if it collides with any part of the environment. The result of this will go into a `CollisionCheckResult`. The `CollisionCheckResult` contains only a boolean: `True` means that it is in collision, `False` means that it is not in collision."
]
},
{
Expand Down Expand Up @@ -155,21 +160,21 @@
"\n",
"Now, the intersection of unions is a union of intersection:\n",
"\n",
" [rp1 ∩ (wc1 ∪ wc2 ∪ ...)] ∪ [rp2 ∩ (wc1 ∪ wc2 ∪ ...)] ∪ ...\n",
" [rp1 ∩ (obj1 ∪ obj2 ∪ ...)] ∪ [rp2 ∩ (obj1 ∪ obj2 ∪ ...)] ∪ ...\n",
"\n",
"The above shows that you have to check whether any primitive of the robot collides with environment.\n",
"\n",
"Further expanding the first term we obtain:\n",
"\n",
" [rp1 ∩ (obj1 ∪ obj2 ∪ ...)] = (rp1 ∩ obj1) ∪ (rp2 ∩ obj2) ∪ ...\n",
" [rp1 ∩ (obj1 ∪ obj2 ∪ ...)] = (rp1 ∩ obj1) ∪ (rp1 ∩ obj2) ∪ ...\n",
"\n",
"This shows that in the end, you can reduce to problem to checking pairwise intersection of `PlacedPrimitives`. \n",
"Therefore, using *decomposition*, we have simplified the problem of \"Does the robot collide with \n",
"the environment?\" to asking \"Does this part of the robot collide with this environmental object?\". We ask\n",
"this second question multiple times for each query. If the answer to this second question is ever yes, then \n",
"we know that the robot collides with the environment.\n",
"\n",
"This tip has already been partially implemented in `collision_checker.py`.\n",
"This tip has already been partially implemented in [collision_checker.py](../../packages/collision_checker/collision_checker.py).\n",
"In other words...\n",
"\n",
"```python\n",
Expand All @@ -195,8 +200,6 @@
"\n",
"where `RT()` rototranslates a primitive by a pose. Also note that for each query the robot changes pose. Let's call this pose `Q`. Note that we have:\n",
"\n",
"Note that we have\n",
"\n",
" robot at pose Q = RT(Q * pose1, primitive1) ∪ RT(Q * pose2, primitive1) ∪ ... \n",
"\n",
"where `Q * pose` represent matrix multiplication.\n",
Expand All @@ -214,9 +217,9 @@
"- `Rectangle` vs `Rectangle`\n",
"\n",
"\n",
"Note that without loss of generality you can get to the point where you have one primitive at the origin (You put one primitive in the coordinate frame of the other). How would you go about it?\n",
"Note that without loss of generality you can get to the point where you have one primitive at the origin (you put one primitive in the coordinate frame of the other). How would you go about it?\n",
"\n",
"`Circle` vs `Circle` is easy: two circles intersects if the distance of the centers is less than the sum of the radii. (The validation tests don't actually ever use a circle shape on a robot, so this case may seem unncessary, but it's useful to leave it in for learning purposes).\n",
"`Circle` vs `Circle` is easy: two circles intersects if the distance of the centers is less than the sum of the radii (the validation tests don't actually ever use a circle shape on a robot, so this case may seem unncessary, but it's useful to leave it in for learning purposes).\n",
"\n",
"For the others, you have to think about it... Use your robotic mind to engineer a solution!\n",
"\n",
Expand Down Expand Up @@ -252,6 +255,103 @@
"\n",
"- There are subtle issues regarding the approximations you are making. What exactly does a pixel represent? is it a point, or is it an area? is this an optimistic or pessimistic approximation? The semantics of painting is unclear. "
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## 💻 Test your collision checker in the simulator\n",
"\n",
"At the moment, there is no VNC simulator for this exercise. Running `dts code workbench --sim` will produce the error \n",
"```bash \n",
"dts : Recipe must contain a 'assets/environment' directory\n",
"```"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## 🚙 Test the collision checker on your Duckiebot\n",
"\n",
"At the moment, the exercise cannot be tested on the real Duckiebot. Running `dts code workbench -b ROBOT_NAME` will produce an error."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Local evaluation and remote submission of your homework exercise\n",
"\n",
"\n",
"## Local evaluation\n",
"\n",
"1. Open a terminal, navigate to the exercise folder and run:\n",
"\n",
"\n",
" dts code evaluate --challenge lx22-collision-check-vali\n",
" \n",
"\n",
"2. The evaluation output is saved locally at the end of the evaluation process. \n",
"\n",
"## Remote submission\n",
"\n",
"You can submit your agent for evaluation by: \n",
"\n",
"1. Opening a terminal on your computer, navigating to the exercise folder and running:\n",
"\n",
"\n",
" dts code submit\n",
" \n",
"\n",
"2. The result of the submission can be visualize on the AIDO challenges website:\n",
"\n",
"After some processing, you should see something like this:\n",
"\n",
"```\n",
"\n",
"~ ## Challenge lx22-collision-check-vali - MOOC - Collision checking\n",
"~ \n",
"~ Track this submission at:\n",
"~ \n",
"~ https://challenges.duckietown.org/v4/humans/submissions/SUBMISSION-NUMBER\n",
"~ \n",
"~ You can follow its fate using:\n",
"~ \n",
"~ $ dts challenges follow --submission SUBMISSION-NUMBER\n",
"~ \n",
"~ You can speed up the evaluation using your own evaluator:\n",
"~ \n",
"~ $ dts challenges evaluator --submission SUBMISSION-NUMBER\n",
"~ \n",
"~ For more information, see the manual at https://docs.duckietown.org/daffy/AIDO/out/\n",
"~ \n",
"~\n",
"~ ## Challenge lx22-collision-check-test - MOOC - Collision checking\n",
"~ \n",
"~ Track this submission at:\n",
"~ \n",
"~ https://challenges.duckietown.org/v4/humans/submissions/SUBMISSION-NUMBER\n",
"~ \n",
"~ You can follow its fate using:\n",
"~ \n",
"~ $ dts challenges follow --submission SUBMISSION-NUMBER\n",
"~ \n",
"~ You can speed up the evaluation using your own evaluator:\n",
"~ \n",
"~ $ dts challenges evaluator --submission SUBMISSION-NUMBER\n",
"~ \n",
"~ For more information, see the manual at https://docs.duckietown.org/daffy/AIDO/out/\n",
"~ \n",
"\n",
"\n",
"~ \n",
"~ Note that the additional 1 challenges (lx22-collision-check-vali) are required checks\n",
"~ before running the code on the challenges you chose (lx22-collision-check-test).\n",
"~ \n",
"\n",
"```"
]
}
],
"metadata": {
Expand Down
16 changes: 7 additions & 9 deletions collision-checker/packages/collision_checker/collision_checker.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,14 +38,14 @@ def check_collision(
) -> bool:
# This is just some code to get you started, but you don't have to follow it exactly

# You can start by rototranslating the robot_body by the robot_pose
# TODO you can start by rototranslating the robot_body by the robot_pose
rototranslated_robot: List[PlacedPrimitive] = []
# == WRITE ME ==

# Then, call check_collision_list to see if the robot collides with the environment
collided = check_collision_list(rototranslated_robot, environment)

# return a random choice
# TODO return the status of the collision
# for now let's return a random guess
return random.uniform(0, 1) > 0.5


Expand All @@ -63,17 +63,15 @@ def check_collision_list(
def check_collision_shape(a: PlacedPrimitive, b: PlacedPrimitive) -> bool:
# This is just some code to get you started, but you don't have to follow it exactly

# This is just some code to get you started, but you don't have to follow it exactly
# TODO check if the two primitives are colliding
if isinstance(a.primitive, Circle) and isinstance(b.primitive, Circle):
...
# == WRITE ME ==
if isinstance(a.primitive, Rectangle) and isinstance(b.primitive, Circle):
...
# == WRITE ME ==
if isinstance(a.primitive, Rectangle) and isinstance(b.primitive, Rectangle):
...
# == WRITE ME ==
...
# for now let's return a random guess

return ...
# TODO return the status of the collision
# for now let's return a random guess
return random.uniform(0, 1) > 0.5

0 comments on commit f9a3662

Please sign in to comment.