Introduction to Robotics — KTH DD2410

This repository contains my solutions to the programming assignments for the Introduction to Robotics course at KTH. Each assignment focuses on a core robotics competency, from inverse kinematics to autonomous mission planning.

---

Table of Contents

• Assignment 2 — Inverse Kinematics
• Assignment 3 — Motion Planning
• Assignment 4 — Mapping
• Assignment 5 — Mobile Robot Project

---

Assignment 2 — Inverse Kinematics

File: IK_functions.py
Submitted to: Kattis

This assignment implements the Inverse Kinematics (IK) algorithm for two different robots, moving their joints so that the end-effector follows a desired path.

Part 1 — SCARA Robot (Grade E)

A 3-DOF SCARA robot with an analytic IK solution. The robot has two revolute joints (q1, q2) and one prismatic joint (q3). The function scara_IK(point) receives a desired position (x, y, z) and returns the corresponding joint values (q1, q2, q3).

To visualize:

ros2 launch kinematics_assignment scara_launch.py

Part 2 — KUKA Robot (Grade C)

A 7-DOF KUKA robot where the complexity makes an analytic solution infeasible. The function kuka_IK(point, R, joint_positions) implements an iterative (Jacobian-based) IK algorithm. Inputs are the desired end-effector position, a 3×3 target rotation matrix, and the current joint positions.

To visualize:

ros2 launch kinematics_assignment kuka_launch.py

Kattis scoring:

Score	Meaning
≥ 20	Grade E (SCARA solved)
22	Grade C (both robots solved)

---

Assignment 3 — Motion Planning

File: planning.py
Submitted to: Kattis

This assignment implements a motion planner for a Dubins car with the following discrete-time dynamics:

$$x_{t+1} = x_t + \cos(\theta_t)$$ $$y_{t+1} = y_t + \sin(\theta_t)$$ $$\theta_{t+1} = \theta_t + \tan(\phi_t)$$

The state is $(x, y, \theta)$ — position and heading — and the sole control input is the steering angle $\phi \in [-\pi/4,\ \pi/4]$.

The solution(car) function returns:

• controls — a list of steering angles
• times — the times at which each control is applied (len(controls) == len(times) - 1)

The planner drives the car from an initial position to a target position while avoiding obstacles and staying within bounds. The simulation stops when the car is within 1.5 m of the target.

Grades

Task	Requirement
E	Navigate with circular obstacles (≥ 60 pts on Kattis)
C	Navigate with line obstacles (66 pts on Kattis)

To evaluate locally:

# From within the dubins directory
python3 main.py        # run all test cases
python3 main.py -p     # show plot
python3 main.py -v     # verbose trajectory info

---

Assignment 4 — Mapping

File: mapping.py
Submitted to: Kattis

This assignment implements occupancy grid mapping — building a 2D map of the environment from scratch using laser scan data and robot pose information.

The core function update_map(self, grid_map, pose, scan) processes incoming LaserScan and PoseStamped messages to update the occupancy grid.

Part 1 — Occupancy Mapping (Grade E)

Mark cells as occupied based on laser scan endpoints:

1. Convert ranges and bearings to coordinates in the laser frame
2. Transform coordinates to the map frame
3. Convert to grid indices using int()
4. Mark occupied cells via add_to_map()

Part 2 — Free Space & C-Space (Grade C)

Extends the E-part with:

• Free space clearing — use raytrace(start, end) to mark cells between the robot and each scan endpoint as free
• Partial map updates — fill in OccupancyGridUpdate to only send the changed rectangular region instead of the full map
• C-space inflation — expand each obstacle by the robot's radius in inflate_map() to enable point-robot path planning

Running locally (ROS2)

# Terminal 1 — launch RViz
ros2 launch mapping_assignment play.launch.py

# Terminal 2 — run mapping node
ros2 run mapping_assignment main.py

# Terminal 3 — play a rosbag
ros2 bag play <path-to-bagfile>

Rosbags are located in mapping_assignment/bags/. Reference maps for comparison are in mapping_assignment/correct_maps/.

---

Assignment 5 — Mobile Robot Project

File: BT.py
Presented to: TA (oral presentation)

This project integrates the previous assignments into a fully autonomous Turtlebot mission planner running in a Gazebo/RViz simulation. The robot must navigate to a series of goals using a State Machine (SM) or Behavior Tree (BT).

Missions

Task E — Reach Goals (no mandatory sensing)

A state machine (sm_students.py) that:

1. Activates the robot
2. Requests a goal and navigates to it
3. Checks if the goal is reached; handles unreachable goals
4. Repeats until no new goals arrive
5. Deactivates the robot

Task C — Reach Goals with Sensor Fusion

A behavior tree (bt_students.py) equivalent to the E-level SM but with real sensor input (odometry, laser scan) for dynamic obstacle avoidance. No hardcoded movements — the TA may change goal positions during evaluation.

Task A — Full Autonomy (sensing + navigation + localization)

Extends the C-level BT with:

• Exploration of an unknown environment (builds on Assignment 4)
• AMCL-based localization — the robot must know its pose in the arena using particle filter convergence
• Kidnap recovery — detects when the robot is displaced and re-localizes
• Goal validation — checks if goal is in obstacle space before navigating

Launching the simulation

# Export robot model
export TURTLEBOT3_MODEL=burger

# Terminal 1 — Gazebo + RViz
ros2 launch irob_assignment_5 simulator.launch.py

# Terminal 2 — run the SM or BT node
ros2 run irob_assignment_5 BT_students.py

For setup and dependencies, see the assignment repository.

Bonus Points

Grade	Deadline (Oct 10, 17:00)	Bonus
E	✓	+3 pts to final exam
C	✓	+5 pts to final exam
A	✓	+7 pts to final exam

---

Setup

Prerequisites

• ROS2 Foxy
• Python 3 + NumPy
• matplotlib (for planning visualization)

General ROS2 workspace setup

source /opt/ros/foxy/setup.bash
mkdir -p ros2_ws/src
cd ros2_ws
colcon build
source install/setup.bash

Add to ~/.bashrc to avoid common issues:

export ROS_LOCALHOST_ONLY=1
export ROS_DOMAIN_ID=11
export LC_NUMERIC="en_US.UTF-8"