{
  "metadata": {
    "kernelspec": {
      "display_name": "Python 3 (ipykernel)",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.8.10"
    }
  },
  "nbformat_minor": 5,
  "nbformat": 4,
  "cells": [
    {
      "id": "8fbc3b83-e598-419b-bb34-f323cd31ab31",
      "cell_type": "markdown",
      "source": "## Humanoid Robotics\n## Assignment 9\nDue Thursday, July 3rd, before class.\n## Legged Robots Locomotion\n\n---\n### Task 1: Robot Kinematics\n\nIn the lecture, we discussed the limitations of Euler angles and how quaternions help overcome them.\n\n---\n\n#### a) Gimbal lock\n\nWhat is gimbal lock, and how does it arise in Euler angle parameterization? Can changing the rotation order avoid this singularity?\n\n---\n\n#### b) Quaternion\nA quaternion is a 4D vector representing rotation. Why does a quaternion need to be normalized to represent a valid rotation? What does the quaternion conjugate represent physically?\n\n---\n\n#### c) Rotation with Quaternion\nIn this task, you will implement quaternion-based rotation using concepts from the lecture in Python. You will need to:\n- Convert an **axis-angle representation** into a **quaternion**.\n- Implement **quaternion multiplications** to apply rotation.\n- Use quaternion algebra to rotate a 3D vector in four different test cases.\n- **Verify your implementation** by comparing the result of the quaternion-based rotation with the result from a corresponding rotation matrix (using Rodrigues’ formula) for each test case.\n",
      "metadata": {}
    },
    {
      "id": "135e1c99-1980-4110-9e2b-2d914f441d9e",
      "cell_type": "code",
      "source": "import numpy as np\n\ndef cross_operator(axis):\n    return np.array([[ 0      , -axis[2],  axis[1]],\n                     [ axis[2],  0      , -axis[0]],\n                     [-axis[1],  axis[0],  0     ]])\n\ndef axis_angle_to_rot_matrix(axis, angle):\n    # Returns a rotation matrix for a given axis-angle representation \n    # This is Rodrigues' rotation formula. (Lecture slide #26)\n    axis = axis / np.linalg.norm(axis)\n    rot_matrix = (np.eye(3) + np.sin(angle) * cross_operator(axis) \n                  + (1 - np.cos(angle)) * np.dot(cross_operator(axis), cross_operator(axis)))\n    return rot_matrix\n\ndef axis_angle_to_quaternion(axis, angle):\n    #\n    #\n    # Your Code Comes Here\n    #\n    #\n\n# Other functions: Write necessary functions to perfrom rotation action with quaternion (Lecture siles #24-30)\n    #\n    #\n    # Your Code Comes Here\n    #\n    #",
      "metadata": {},
      "outputs": [],
      "execution_count": null
    },
    {
      "id": "0a9b9dac-85b9-4af7-bd36-80ea5c986a9a",
      "cell_type": "code",
      "source": "vec = np.array([5.0, 2.0, 3.0])  # Example vector to be rotated",
      "metadata": {},
      "outputs": [],
      "execution_count": null
    },
    {
      "id": "9e6255cb-ebb7-445d-8bf3-b61e893f84bc",
      "cell_type": "code",
      "source": "# 1. Test case for 90-degree rotation around x-axis\n#\n#\n# Your Code Comes Here\n#\n#",
      "metadata": {},
      "outputs": [],
      "execution_count": 11
    },
    {
      "id": "5c398870-47b7-408a-bd19-44ace3011c52",
      "cell_type": "code",
      "source": "# 2. Test case for 90-degree rotation around y-axis\n#\n#\n# Your Code Comes Here\n#\n#",
      "metadata": {},
      "outputs": [],
      "execution_count": 9
    },
    {
      "id": "d6ebd5fb-c692-4c07-8b48-fb88b3aa1f76",
      "cell_type": "code",
      "source": "# 3. Test case for 90-degree rotation around z-axis\n#\n#\n# Your Code Comes Here\n#\n#",
      "metadata": {},
      "outputs": [],
      "execution_count": null
    },
    {
      "id": "d5b31cb4-d781-44fd-8cf1-a5c68c334679",
      "cell_type": "code",
      "source": "# 4. Test case for rotation by an arbitrary angle around an arbitrary axis\n#\n#\n# Your Code Comes Here\n#\n#",
      "metadata": {},
      "outputs": [],
      "execution_count": null
    },
    {
      "id": "af7d00ec-7289-4a92-9b90-df7f73c04f77",
      "cell_type": "markdown",
      "source": "### Task 2: Robot Control\nIn class, we discussed the trade-offs between **position control** and **force control**. Below are three robot control scenarios. For each one, answer which control strategy is more suitable? Justify your choice clearly, referring to task properties (e.g., contact, compliance, sensing, dynamics).\n\n- **a)** Peg in the hole assembly: a 6-DoF robotic manipulator must insert a cylindrical peg into a tight-fitting hole during an automated assembly process. The hole may slightly misalign due to manufacturing tolerances. \n\n---\n- **b)** Legged robot traversing uneven terrain: a quadruped robot walks over rocky terrain outdoors. The robot must adapt its motion to variable surface contact, like slipping rocks or yielding grass, while maintaining balance. \n\n---\n- **c)** High-speed pick-and-place in a factory line: a robotic arm repeatedly moves items from one conveyor belt to another with high precision. Timing and accuracy are critical.\n\n---",
      "metadata": {}
    },
    {
      "id": "48a0bac6-306a-416a-86f4-394eabbe8a04",
      "cell_type": "markdown",
      "source": "### Task3: Base State Estimation with Linear Kalman Filter\nIn the lecture, we discussed how **nonlinear motion and measurement models** can be used with an **Extended Kalman Filter (EKF)** to jointly estimate the base position, velocity, and orientation of a legged robot. However, EKFs have some drawbacks; therefore, as a simpler alternative, this exercise implements a **linear Kalman Filter (KF)** for estimating **base position and linear velocity** based on the approach in **(Section III)** of the paper below:\n\nFlayols et al., **Experimental evaluation of simple estimators for humanoid robots**, Humanoids, 2017.\n\n### Task 3.a) \n\n1. What are the **drawbacks of using an Extended Kalman Filter (EKF)** for base state estimation in legged robots? \n\n2. Under what **assumptions** can we design a **linear Kalman Filter (KF)** to estimate the robot's base position and linear velocity? \n\n3. In this linear KF setting, what are the **inputs to the filter?** Specify which sensors provide the prediction (process model) input and which measurements are used in the correction (measurement model). \n\n---\n### Kalman Filter\nIn the following tasks we develop a KF class in Python, and we will apply the method to a **quadruped robot**, which means we have **four foot contact points**.\n\n#### Goal\nImplement and run a linear KF to estimate the robot’s **base position and linear velocity** from known:\n- IMU acceleration data\n- Robot base orientation\n- Foot positions relative to the base (calculated from forward kinematics)\n- Contact array (whether each foot is in contact with the ground)\n#### Data Description\nThe provided dataset includes simulated sensor and ground truth signals for a quadruped robot. All data is stored in plain `.dat` files and is sampled at **1 kHz (dt = 0.001 sec)**. Each column corresponds to a single time step, and **N** is the number of time steps.\n\nThe following files are included:\n\n- `true_base_position.dat`  \n  Shape: `(3, N)` — Ground-truth base position `[x, y, z]` in world frame\n\n- `true_base_velocity.dat`  \n  Shape: `(3, N)` — Ground-truth base linear velocity `[vx, vy, vz]` in world frame\n\n- `base_quat.dat`  \n  Shape: `(4, N)` — Orientation of the base as a quaternion `[x, y, z, w]`\n\n- `imu_acc.dat`  \n  Shape: `(3, N)` — Accelerometer measurement in the **base frame** (without gravity)\n\n- `relative_feet_position.dat`  \n  Shape: `(12, N)` — Stacked relative foot positions in the **base frame** (this is calculated from forward kinematics of the leg)   \n  Format: `[FL, FR, RL, RR]` where each foot contributes 3 values `[x, y, z]`\n\n- `contact_array.dat`  \n  Shape: `(4, N)` — Binary contact flags for each foot  \n  Format: `1` = in contact, `0` = not in contact  \n  Order: `[FL, FR, RL, RR]`\n\n---\n### Task 3.b) Compute KF Matrices\nSince the process model is linear time invariant in the linear Kalman Filter, the corresponding matrix remain constant over time. **Precompute** the system matrix **`F`** in the constructor of the Kalman Filter class (e.g., `__init__()` method). The measurement matrix **`H`** is depending on which feet are in contact and it should be computed in the `update_step()` function.\n\n---\n### Task 3.c) Implement the Prediction Step\nComplete the `prediction_step()` function in your Kalman Filter class. Use the motion model described in **Equation (6)** of the paper to integrate the motion model over time.\n**Note:** The IMU acceleration (`imu_acc`) is provided in the base frame. \n\n---\n\n### Task 3.d) Implement the Measurement model in the Update Step\nComplete the `update_step()` function in your Kalman Filter class.\nWhen a foot is in contact with the ground, it provides a measurement of the base position relative to that foot. \nFocus on implementing the measurement model described in **Equations (7), (8)** of the paper.\n\n---\n### Task 3.e) Apply the Kalman Filter Update\n\nIn the `update_step()` function, compute the **Kalman gain** and apply the update equations for:\n- The **state estimate**  \n- The ** error covariance matrix**\n\nUse the standard Kalman Filter update equations.\n\n---\n\n### Task 3.f) Visualize the Results\nPlot the **estimated base position** and **velocity** obtained from the Kalman Filter compared to the **ground truth**.\n\n- Use different colors for estimated and ground truth values.\n- Ensure to label the axes and add a legend for clarity.\n---",
      "metadata": {}
    },
    {
      "id": "3eff6d38-c15e-43d4-8dd4-386243747437",
      "cell_type": "code",
      "source": "import numpy as np\nfrom scipy.spatial.transform import Rotation as R\nimport matplotlib.pyplot as plt\nimport os\n\nclass FloatingBaseKF:\n    def __init__(self):\n        self.dt = 0.001 # Sampling time in seconds\n        self.gravity = np.array([0, 0, -9.81])\n        self.num_feet = 4  # Number of feet for a quadruped robot\n        self.n_x = 6 + 3 * self.num_feet # Number of state variables\n        self.x = np.zeros(self.n_x)\n        \n        # Task 3.b) Compute F matrix for the Kalman filter\n        #\n        #\n        # Your Code Comes Here\n        #\n        #\n        \n        # Initialize the covariance matrices with correct dimensions\n        self.P = np.eye(self.n_x) * 1e-6\n\n    def init_state(self, feet_pos_base):\n        self.x[2]= 0.33\n        base_pose = np.array([0.0, 0.0, self.x[2]])\n        for i in range(self.num_feet):\n            self.x[3*i+6:3*i+9] = feet_pos_base[3*i:3*(i+1)] + base_pose\n        \n    def prediction_step(self, acc_body, R_bn):\n        # Task 3.c) Integrate the linear motion model\n        #\n        #\n        # Your Code Comes Here\n        #\n        #\n\n        # Calculate the \"a priori error covariance matrix\" with linerized process model\n        Q = np.zeros((self.n_x, self.n_x))\n        Q[3:6, 3:6] = np.eye(3) * 1e-4  # base velocity\n        Q[6:, 6:] = np.eye(3*self.num_feet) * 1e1  # foot position noise\n        self.Q = Q\n        self.P = self.F @ self.P @ self.F.T + self.Q\n\n    def update_step(self, R_bn, feet_pos_base, cnt_array):\n        num_feet_in_cnt = np.sum(cnt_array)\n        y_measured = np.zeros(3*num_feet_in_cnt)\n        y_predicted = np.zeros(3*num_feet_in_cnt)\n\n        # Task 3.b) Compute H matrix for the Kalman filter\n        #\n        #\n        # Your Code Comes Here\n        #\n        #\n        \n        # Task 3.d) Implement the measurement model\n        #\n        #\n        # Your Code Comes Here\n        #\n        #\n        \n        residual_error = y_measured - y_predicted\n        \n        self.R = np.eye(3*num_feet_in_cnt) * 1e-5  # Measurement noise covariance\n        # Task 3.e) Apply the Kalman Filter update, \n        # This includes: compute Kalman gain, update the state estimate and error covariance matrix\n        #\n        #\n        # Your Code Comes Here\n        #\n        #        \n\n        \n    def run_kalman_filter(self, acc_body, R_bn, feet_pos_base, cnt_array):\n        self.prediction_step(acc_body, R_bn)\n        self.update_step(R_bn, feet_pos_base, cnt_array)\n    \n    def get_state(self):\n        return self.x.copy()",
      "metadata": {},
      "outputs": [],
      "execution_count": 11
    },
    {
      "id": "aac32abf-2079-4f25-addc-1828ca774905",
      "cell_type": "code",
      "source": "# Read the data\ncwd = os.getcwd()\npath = os.path.join(cwd, \"files/\")\ntrue_base_position = np.loadtxt(path+\"true_base_position.dat\", delimiter='\\t', dtype=np.float32) # Shape: (3,N)\ntrue_base_velocity = np.loadtxt(path+\"true_base_velocity.dat\", delimiter='\\t', dtype=np.float32) # Shape: (3,N)\nbase_quat = np.loadtxt(path+\"base_quat.dat\", delimiter='\\t', dtype=np.float32) # Shape: (4,N)\nimu_acc = np.loadtxt(path+\"imu_acc.dat\", delimiter='\\t', dtype=np.float32) # Shape: (3,N)\nrelative_feet_position = np.loadtxt(path+\"relative_feet_position.dat\", delimiter='\\t', dtype=np.float32) # Shape: (12,N)\ncontact_array = np.loadtxt(path+\"contact_array.dat\", delimiter='\\t', dtype=np.float32) # Shape: (4,N)\ncontact_array = contact_array.astype(np.int8)",
      "metadata": {},
      "outputs": [],
      "execution_count": 12
    },
    {
      "id": "617d4984-6c45-4377-958c-a5f5e2f7f0ab",
      "cell_type": "code",
      "source": "num_samples = true_base_velocity.shape[1]\nkf = FloatingBaseKF()\nest_base_pos = []\nest_base_vel = []\n\n# Run the Kalman filter for each sample\nfor idx in range(num_samples):\n    if idx == 0:\n        # Initialization\n        kf.init_state(relative_feet_position[:, idx])\n    \n    # Read the sensor data\n    acc_local = imu_acc[:, idx]\n    r = R.from_quat(base_quat[:, idx])\n    Rwb = r.as_matrix()\n    rel_feet_pos = relative_feet_position[:, idx]\n    cnt_arr = contact_array[:, idx]\n\n    # Run the filter\n    kf.run_kalman_filter(acc_local, Rwb, rel_feet_pos, cnt_arr)\n    est_base_pos.append(kf.get_state()[0:3])\n    est_base_vel.append(kf.get_state()[3:6])\n\n# Save the estimated base position and velocity\nest_base_pos = np.vstack(est_base_pos).T # Shape: (3,N)\nest_base_vel = np.vstack(est_base_vel).T # Shape: (3,N)",
      "metadata": {},
      "outputs": [],
      "execution_count": null
    },
    {
      "id": "c736e1b5-c6e7-4d93-8765-3ce95b2db73c",
      "cell_type": "code",
      "source": "# Task 3.f) Plot the results for estimated and true base position\n#\n#\n# Your Code Comes Here\n#\n#",
      "metadata": {},
      "outputs": [],
      "execution_count": null
    },
    {
      "id": "0d7aa356-1212-4bbb-9f9e-40d6bbed6d2e",
      "cell_type": "code",
      "source": "# Task 3.f) Plot the results for estimated and true base velocity\n#\n#\n# Your Code Comes Here\n#\n#",
      "metadata": {},
      "outputs": [],
      "execution_count": null
    }
  ]
}