Exercises#

This page contains four take-home exercises that reinforce the concepts from Lecture 11. Each exercise asks you to write code from scratch based on a specification – no starter code is provided.

All files should be created inside your ~/enpm605_ws/src/ workspace in the appropriate packages (frame_demo, tf2_demo, or robot_control_demo).

Exercise 1 – Quaternion Utility Library

Goal

Build a Python module that converts between orientation representations and verify it against scipy.spatial.transform.

Specification

Create the file frame_demo/frame_demo/quat_utils.py that implements the following functions.

  1. ``axis_angle_to_quaternion(axis, angle)``:

    • Input: a 3-element unit vector axis and a rotation angle in radians.

    • Output: a tuple (w, x, y, z) using the half-angle formula:

      \[w = \cos\!\left(\frac{\theta}{2}\right), \quad (x, y, z) = \sin\!\left(\frac{\theta}{2}\right) \cdot \mathbf{u}\]

    • Raise ValueError if the axis is not a unit vector (tolerance: 1e-6).

  2. ``quaternion_to_euler(w, x, y, z)``:

    • Input: a unit quaternion (w, x, y, z).

    • Output: a tuple (roll, pitch, yaw) in radians.

    • Use the standard ZYX (Tait-Bryan) convention.

  3. ``quaternion_multiply(q1, q2)``:

    • Input: two quaternions as (w, x, y, z) tuples.

    • Output: the Hamilton product q1 * q2 as a (w, x, y, z) tuple.

  4. ``normalize_quaternion(w, x, y, z)``:

    • Input: a quaternion (not necessarily unit).

    • Output: the corresponding unit quaternion.

    • Raise ValueError if the magnitude is zero.

Write a main() function that:

  • Computes the quaternion for a \(90°\) rotation about the \(x\)-axis using axis_angle_to_quaternion.

  • Computes the quaternion for a \(90°\) rotation about the \(z\)-axis.

  • Composes them (z-rotation applied first, then x-rotation) using quaternion_multiply.

  • Converts the result to Euler angles.

  • Prints all intermediate and final values.

  • Verifies the result against scipy.spatial.transform.Rotation and prints PASS or FAIL.

Verification

cd ~/enpm605_ws && colcon build --symlink-install --packages-select frame_demo
source install/setup.bash
ros2 run frame_demo quat_utils
Exercise 2 – Static Transform Broadcaster Node

Goal

Write a ROS 2 node that publishes a static transform from base_link to a custom sensor frame using parameters loaded from a YAML file.

Specification

Create frame_demo/frame_demo/sensor_frame_publisher.py.

  1. ``SensorFramePublisher(Node)`` class:

    • __init__: declares the following parameters:

      • parent_frame (string, default "base_link")

      • child_frame (string, default "sensor_link")

      • translation (double array, default [0.0, 0.0, 0.0])

      • rotation_rpy (double array, default [0.0, 0.0, 0.0])

    • Converts the roll-pitch-yaw values to a quaternion using scipy.spatial.transform.Rotation.from_euler("xyz", [r, p, y]).

    • Creates a StaticTransformBroadcaster and publishes the transform once.

    • Logs the published transform at info level.

  2. YAML parameter file config/sensor_frame.yaml:

    /sensor_frame_publisher:
  ros__parameters:
    parent_frame: "base_link"
    child_frame: "camera_link"
    translation: [0.1, 0.0, 0.25]
    rotation_rpy: [0.0, -0.2618, 0.0]

  3. Launch file launch/sensor_frame.launch.py:

    • Starts the sensor_frame_publisher node with the YAML parameter file.

  4. Register the entry point in setup.py and install the config and launch directories.

Expected behavior

  • The node starts, publishes the static transform, and keeps running.

  • ros2 run tf2_ros tf2_echo base_link camera_link shows the transform.

  • ros2 run rqt_tf_tree rqt_tf_tree --force-discover shows base_link → camera_link in the tree.

Verification

cd ~/enpm605_ws && colcon build --symlink-install --packages-select frame_demo
source install/setup.bash

# Terminal 1
ros2 launch frame_demo sensor_frame.launch.py

# Terminal 2
ros2 run tf2_ros tf2_echo base_link camera_link

# Terminal 3
ros2 run rqt_tf_tree rqt_tf_tree --force-discover
Exercise 3 – Transform Listener and Logger

Goal

Write a node that periodically looks up the transform between two frames and logs the position and orientation (as Euler angles).

Specification

Create frame_demo/frame_demo/frame_logger.py.

  1. ``FrameLogger(Node)`` class:

    • __init__: declares two parameters:

      • target_frame (string, default "odom")

      • source_frame (string, default "base_link")

    • Creates a Buffer and a TransformListener.

    • Creates a timer at 1 Hz.

  2. Timer callback _timer_callback(self):

    • Calls self._tf_buffer.lookup_transform(target, source, rclpy.time.Time(), timeout=Duration(seconds=0.1)).

    • Extracts the translation (x, y, z).

    • Converts the quaternion to Euler angles using scipy.spatial.transform.Rotation.

    • Logs translation and Euler angles (degrees) at info level.

    • Catches TransformException and logs a warning instead.

Expected behavior

  • With the ROSbot simulation running, the node logs the robot’s pose in the odom frame once per second.

  • Moving the robot (e.g., with teleop_twist_keyboard) shows the logged values changing.

Verification

# Terminal 1
ros2 launch rosbot_gazebo empty_world.launch.py

# Terminal 2
cd ~/enpm605_ws && colcon build --symlink-install --packages-select frame_demo
source install/setup.bash
ros2 run frame_demo frame_logger

# Terminal 3
ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -p stamped:=true
Exercise 4 – Proportional Go-to-Goal Controller

Goal

Implement a two-phase proportional controller that drives a differential-drive robot to a goal pose (position + final heading) using odometry feedback.

Specification

Create robot_control_demo/robot_control_demo/goto_goal.py.

  1. ``GoToGoal(Node)`` class:

    • __init__: declares the following parameters with defaults:

      • goal_x (double, 3.0)

      • goal_y (double, 2.0)

      • goal_heading (double, 1.57) — desired final yaw in radians

      • k_rho (double, 0.5) — linear gain

      • k_alpha (double, 1.0) — angular gain (drive phase)

      • k_heading (double, 1.5) — angular gain (rotate phase)

      • position_tolerance (double, 0.05) — meters

      • heading_tolerance (double, 0.05) — radians

    • Creates a publisher on /cmd_vel (geometry_msgs/msg/TwistStamped).

    • Subscribes to /odometry/filtered (nav_msgs/msg/Odometry).

    • Creates a timer at 20 Hz for the control loop.

    • Tracks the current phase: DRIVE or ROTATE.

  2. Odometry callback _odom_callback(self, msg):

    • Extracts x, y from msg.pose.pose.position.

    • Extracts yaw from the quaternion using scipy.spatial.transform.Rotation.

  3. Control loop _control_loop(self):

    • DRIVE phase:

      • Compute distance error: \(\rho = \sqrt{(x_g - x)^2 + (y_g - y)^2}\)

      • Compute heading error to goal: \(\alpha = \text{atan2}(y_g - y, x_g - x) - \psi\)

      • Normalize \(\alpha\) to \([-\pi, \pi]\).

      • Set linear.x = k_rho * rho and angular.z = k_alpha * alpha.

      • Clamp linear velocity to [0, 0.5] m/s and angular velocity to [-1.0, 1.0] rad/s.

      • If \(\rho <\) position_tolerance, switch to ROTATE phase.

    • ROTATE phase:

      • Compute heading error: \(e_\psi = \psi_{\text{goal}} - \psi\)

      • Normalize to \([-\pi, \pi]\).

      • Set linear.x = 0.0 and angular.z = k_heading * e_psi.

      • Clamp angular velocity to [-1.0, 1.0] rad/s.

      • If \(|e_\psi| <\) heading_tolerance, publish a zero-velocity command, log "Goal reached!", and stop the timer.

  4. Register the entry point in setup.py.

Expected behavior

  • The robot drives toward the goal position with a smooth curved trajectory (simultaneous linear + angular motion).

  • Once within position_tolerance of the goal, the robot stops translating and rotates in place to the desired heading.

  • Velocities decrease as the robot approaches the goal.

Verification

# Terminal 1
ros2 launch rosbot_gazebo empty_world.launch.py

# Terminal 2
cd ~/enpm605_ws && colcon build --symlink-install --packages-select robot_control_demo
source install/setup.bash
ros2 run robot_control_demo goto_goal --ros-args \
    -p goal_x:=3.0 -p goal_y:=2.0 -p goal_heading:=1.57

# Optional: visualize in RViz2 with Odometry display on /odometry/filtered

This Page