{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 1.\tWhich learning based grasp model for which scenario?\n",
    "\t\n",
    "In the lecture we discussed VGN, AnyGrasp and GPD as three examples of learning-based grasping model for parallel jaw gripper. For each of the following scenarios, choose the most suitable model. Justify your choice based on what is presented in the lecture slides: consider factors such as sensor modality, real-time capabilities, object specificity, and task goals.\n",
    "\n",
    "a.\tA mobile manipulator with a wrist-mounted RGB-D camera approaches an object that is moving on a conveyor belt. The robot must track and refine the grasp online as it aligns with the object. \n",
    "\n",
    "b.\tA mobile manipulator with a wrist mounted RGB camera that is used for semantic segmentation for generating selective object masks, and LiDAR sensor that provides point clouds navigates home environments to grasp and bring specific objects to the user. The user may change the task mid-way and the robot must adapt and change its target location to grasp the new object. \n",
    "\n",
    "c.\tA 7 dof manipulator arm mounted with a depth-only sensor is fixed in front of a bin with cluttered objects of the same type in a factory. The goal is to clear the bin by removing the object from the bin and placing it on a fixture as fast as possible. \n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 2.\tTarget Driven Action Selection\t\t\n",
    "\n",
    "The user has instructed via an LLM based agent that to a general-purpose service robot (GPSR) equipped with a head mounted RGB-D camera and a 2-finger gripper to retrieve a box of soup can from the shelf quickly and bring it to the kitchen so that they can cook. The GPSR perceives part of a red soup can behind several occluding items on a pantry shelf. It maintains a voxel-based semantic belief map and estimates.\n",
    "\n",
    "The robot can perform one of the following actions:\n",
    "\n",
    "•\tReposition Whole Body (Can be for active viewing, better pushing or better grasping)\n",
    "\n",
    "•\tPush object\n",
    "\n",
    "•\tGrasp object\n",
    "\n",
    " The robot can also perform the action Reposition Head in conjunction with reposition body, push and grasp actions as well. The different actions have the following typical information gain relationship:\n",
    "\n",
    "**IGVb ~ IGMb >> IGMh >> IGVh**\n",
    "\n",
    "•\tIGVb (Information Gain from Reposition Whole Body and View, action length: 1) \n",
    "\n",
    "•\tIGMb (Information Gain from Pushing an Occluding Box and Reposition Whole Body and View, action length: 2)\n",
    "\n",
    "•\tIGMh (Information Gain from Pushing an Occluding Box and Reposition Head and View, action length: 1)\n",
    "\n",
    "•\tIGVh (Information Gain from Repositioning Head for Better View, action length, 1 when independently done but can also be combined with push and grasp action)\n",
    "\n",
    "Consider three different scenarios where robot has an action budget of 1, 2, 3 and 4. How would you sequence the actions for the task of object retrieval? Please note the entire task is considered a failure if the robot fails to grasp the object.  Explain the difference in choices if the task goal was semantic mapping of the shelf. \n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 3.\tFoundation Models \n",
    "\n",
    "For each task below, the robot fails despite using the listed model(s). Identify which one model is most likely responsible, and give one short reason based on lecture concepts.\n",
    "\n",
    "•\t“Put the book on the top shelf” The robot lifts the book but drops it before placing it. (Models used: LLM+VFM+VLA)\n",
    "\n",
    "•\t\"Give me the beer mug.\" The robot hands over a white tea cup instead. (Models used: : VLM+VLA)\n",
    "\n",
    "•\t\"The blue basket has oranges and apples freshly harvested from the glasshouse. Sort all good apples into the red basket.\" The robot moves all apples into the red basket, including bad apples. (Models used: LLM + VFM+ VLA)\n",
    "\n",
    "•\t\"Place the egg into the carton.\" The robot crushes the egg during grasping. (Models used: LMM+VFM+VLA)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 4. Planar Non-Prehensile Pushing: Analytical Model Implementation\n",
    "\n",
    "In this assignment, you will implement and test the analytical model for planar pushing based on the quasi-static framework introduced by Kloss et al. (2017). You will compute motion cones, detect sticking vs. sliding contacts, and predict object motion based on pusher velocity. The assignment also includes a small visualization component to better understand pushing interactions.\n",
    "\n",
    "a. Compute the friction cone boundaries given a surface normal and coefficient of friction. (1 point)\n",
    "\n",
    "b. Compute the torque exerted around the object center given contact point and boundary forces. (1 point)\n",
    "\n",
    "c. Compute the motion cone directions using friction cone and torque values. (1 point)\n",
    "\n",
    "d. Classify whether a given pusher velocity results in sticking or sliding contact. (1 point)\n",
    "\n",
    "e. Compute the resulting object velocity from a pushing action under the correct contact condition. (1 point)\n",
    "\n",
    "f. Run and explain two test scenarios: (2 points)\n",
    "\n",
    "    i. Push at the center vs. push at an offset — how does torque affect motion?\n",
    "    \n",
    "    ii. Show and explain a sliding vs. sticking contact case.\n",
    "\n",
    "## Learning Objectives\n",
    "- Understand the analytical model for planar pushing (quasi-static approximation).\n",
    "- Implement contact type determination (sticking vs. sliding).\n",
    "- Compute object velocity given a pushing action.\n",
    "- Visualize pushing scenarios with depth rendering.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 1. Analytical Model Overview\n",
    "\n",
    "Kloss et al. use Lynch et al.'s quasi-static pushing model. The model takes as input:\n",
    "- Contact point $c$ relative to object center $o$.\n",
    "- Surface normal at contact $n$.\n",
    "- Pusher velocity $u$.\n",
    "- Friction parameters: friction coefficient $\\mu$ and limit‐surface parameter $l$.\n",
    "- Contact indicator $s$ (1 if in contact, 0 otherwise).\n",
    "\n",
    "### 1.1 Friction Cone and Motion Cone\n",
    "1. Compute friction cone half‐angle:\n",
    "$$\\alpha = \\arctan(\\mu)$$\n",
    "\n",
    "2. Boundary forces:\n",
    "$$f_l = R(-\\alpha) \\, n, \\quad f_r = R(\\alpha) \\, n$$\n",
    "where $R(\\theta)$ is a 2D rotation.\n",
    "\n",
    "3. Torques about object center:\n",
    "$$m_b = c'_x f_{b,y} - c'_y f_{b,x}, \\quad b \\in \\{l,r\\}$$\n",
    "\n",
    "4. Boundary effective push velocities (directions):\n",
    "$$v_{p,b} = l^2 f_b + m_b \\, k \\times c'$$\n",
    "\n",
    "5. Determine if $u$ lies inside motion cone spanned by $v_{p,l}, v_{p,r}$. If inside, sticking ($v_p = u$), else sliding toward closer boundary direction:\n",
    "$$v_p = \\frac{u \\cdot n}{v_{p,b} \\cdot n} \\, v_{p,b}$$\n",
    "for the boundary $b$ whose $v_{p,b}$ is closer in direction to $u$.\n",
    "\n",
    "### 1.2 Object Velocity Computation\n",
    "Given $v_p$ and contact point $c'$, the resulting object linear and angular velocities are:\n",
    "$$v_{o_x} = \\frac{(l^2 + c'_x{}^2) \\, v_{p_x} + c'_x c'_y \\, v_{p_y}}{l^2 + c'_x{}^2 + c'_y{}^2}$$\n",
    "$$v_{o_y} = \\frac{(l^2 + c'_y{}^2) \\, v_{p_y} + c'_x c'_y \\, v_{p_x}}{l^2 + c'_x{}^2 + c'_y{}^2}$$\n",
    "$$\\omega = \\frac{c'_x \\, v_{o_y} - c'_y \\, v_{o_x}}{l^2}$$\n",
    "\n",
    "where $l^2$ often denotes $l^2$ from the ellipsoidal approximation.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "from matplotlib.patches import Rectangle\n",
    "from matplotlib.animation import FuncAnimation\n",
    "from IPython.display import HTML\n",
    "\n",
    "# --- BEGIN helper stubs (keep or replace with your own implementations) ---\n",
    "class AttrDict(dict):\n",
    "    \"\"\"Simple wrapper so you can write param_dict['i']['foo'] = ...\"\"\"\n",
    "    def __iadd__(self, other):\n",
    "        self.update(other)\n",
    "        return self\n",
    "\n",
    "def get_rot(theta):\n",
    "    \"\"\"Return a 2×2 rotation matrix by angle theta (radians).\"\"\"\n",
    "    c, s = np.cos(theta), np.sin(theta)\n",
    "    return np.array([[c, -s], [s,  c]])\n",
    "\n",
    "def inside_friction_cone(v_l, v_r, c_dash):\n",
    "    \"\"\"\n",
    "    Minimal check: returns (left, right) = cross(v_l, c_dash), cross(v_r, c_dash).\n",
    "    We declare 'inside' if left < 0 < right.\n",
    "    \"\"\"\n",
    "    a = np.cross(np.append(v_l, 0), np.append(c_dash, 0))\n",
    "    b = np.cross(np.append(v_r, 0), np.append(c_dash, 0))\n",
    "    # Note: both a and b are 3D vectors; the scalar out‐of‐plane is index 2.\n",
    "    return a[2], b[2]\n",
    "\n",
    "# --- END helper stubs ---\n",
    "\n",
    "\n",
    "def predict_next_velocities_single(\n",
    "    contact_point_hand,    # [x, y] in object frame\n",
    "    normal_hand,           # [nx, ny] in object frame (unit normal)\n",
    "    floor_contact,         # tuple: ([x_f, y_f] in object frame, normal_force_f scalar)\n",
    "    object_center,         # [x, y, 0] in world frame (unused here, but kept for signature)\n",
    "    action_obj,            # [v_x, v_y] in object frame (m/s)\n",
    "    debug=False\n",
    "):\n",
    "    \"\"\"\n",
    "    Single‐contact Kloss‐style quasi‐static push. Returns [v_o_x, v_o_y, w_o] in object frame (per dt=1).\n",
    "    \n",
    "    contact_point_hand:  [x, y] of the hand‐contact in the object frame.\n",
    "    normal_hand:         [nx, ny] unit normal, in object frame.\n",
    "    floor_contact:       ([x_f, y_f], N_f), where N_f is the scalar normal force at that floor point.\n",
    "    object_center:       [x, y, 0] (world frame) -- not used to compute c_dash here.\n",
    "    action_obj:          [v_x, v_y] in object frame (m/s).\n",
    "    \"\"\"\n",
    "    #TODO: 1) If no hand contact, object stays still:\n",
    "    \n",
    "    #TODO: 2) Friction coefficient and half‐angle:\n",
    "    \n",
    "\n",
    "    #TODO: 3) Use contact_point_hand directly as c_dash (object frame):\n",
    "   \n",
    "    #TODO: 4) Compute friction‐cone boundary forces f_l, f_r:\n",
    "   \n",
    "\n",
    "    #TODO: 5) Compute torque scalars at left and right boundaries:\n",
    "    \n",
    "\n",
    "    #TODO: 6) Single floor contact → pick l = 1 (any constant works since we scale N_f = 1 below)\n",
    "    #    In 2D with no gravity, we set floor_contact[1] = 1.0, so l = m_max / f_max = 1/1 = 1.\n",
    "   \n",
    "\n",
    "    #TODO: 7) Compute boundary‐mode “push directions” v_p_r, v_p_l (object frame):\n",
    "    \n",
    "    #TODO: 8) Check if action lies inside friction cone annd compute v_p for sliding and sticking modes:\n",
    "    \n",
    "\n",
    "    #TODO: 9) Compute object‐frame velocity using the “ellipsoidal limit‐surface” formula:\n",
    "   \n",
    "\n",
    "    return np.array([v_o_x, v_o_y, w_o]), {}\n",
    "\n",
    "\n",
    "def simulate_push_single(\n",
    "    start_object_pos,     # [x, y] world frame\n",
    "    start_object_rot,     # scalar in radians\n",
    "    contact_pt_obj,       # [x, y] in object frame\n",
    "    normal_obj,           # [nx, ny] in object frame\n",
    "    floor_pt_obj,         # [x, y] in object frame\n",
    "    normal_force_f,       # scalar (here we set = 1.0)\n",
    "    action_world,         # [v_x, v_y] in world frame (cm/s)\n",
    "    dt,                   # time step in seconds\n",
    "    steps,                # number of time steps\n",
    "    debug=False\n",
    "):\n",
    "    \"\"\"\n",
    "    Each time step:\n",
    "      1. Convert action_world → action in object frame (m/s).\n",
    "      2. Call predict_next_velocities_single(...) → [v_o_x, v_o_y, w_o].\n",
    "      3. Update object_pos and obj_rot in world frame.\n",
    "      4. Update pusher_pos in world frame (constant action_world).\n",
    "      5. Compute contact point in world = R(obj_rot) @ contact_pt_obj + obj_pos.\n",
    "    \"\"\"\n",
    "    traj_obj     = np.zeros((steps, 2))\n",
    "    traj_rot     = np.zeros(steps)\n",
    "    traj_push    = np.zeros((steps, 2))\n",
    "    traj_contact = np.zeros((steps, 2))\n",
    "\n",
    "    # Initial states\n",
    "    obj_pos = np.array(start_object_pos, dtype=float)\n",
    "    obj_rot_angle = start_object_rot\n",
    "    # Initial pusher already at contact point in world frame:\n",
    "    #c_world_init = get_rot(obj_rot_angle).dot(contact_pt_obj) + obj_pos\n",
    "    pusher_pos = contact_pt_obj.copy()\n",
    "\n",
    "    for i in range(steps):\n",
    "        #TODO 1) Convert world‐frame action (cm/s) → object‐frame (m/s)\n",
    "        \n",
    "\n",
    "        #TODO 2) Call the corrected single‐contact predictor\n",
    "        \n",
    "        # v_o_obj = [v_o_x, v_o_y, w_o] in object frame\n",
    "       \n",
    "\n",
    "        #TODO: 3) Convert object velocity back to world frame and integrate\n",
    "       \n",
    "        #TODO: 4) Update the pusher: constant velocity in world frame (cm/s→m/s)\n",
    "\n",
    "        #TODO: 5) Recompute the contact point in world frame\n",
    "       \n",
    "\n",
    "    return traj_obj, traj_rot, traj_push, traj_contact\n",
    "\n",
    "\n",
    "# --- Define single‐contact parameters ---\n",
    "contact_pt_obj   = np.array([-0.0, -0.035])   # hand contact 2cm right, 1cm below center\n",
    "normal_obj       = np.array([.0, 1.0])     # hand pushes from the right → normal points left\n",
    "floor_pt_obj     = np.array([0.0, 0.0])      # floor contact at object center (for simplicity)\n",
    "normal_force_f   = 1.0                       # unit “normal” (no gravity → pick 1.0)\n",
    "\n",
    "# Initial object pose (world):\n",
    "object_pos0 = [-0.0, 0.0]  # start at origin\n",
    "object_rot0 = 0.0         # start aligned with world‐x axis\n",
    "\n",
    "# Pusher velocity: 5 cm/s to the right (world frame)\n",
    "action_world = [2.5, 2.5]  # in m/s (0.05 m/s)\n",
    "\n",
    "dt    = 0.05   # 50 ms per time step\n",
    "steps = 100\n",
    "\n",
    "# Run the 50‐step simulation\n",
    "traj_obj, traj_rot, traj_push, traj_contact = simulate_push_single(\n",
    "    object_pos0, object_rot0,\n",
    "    contact_pt_obj, normal_obj,\n",
    "    floor_pt_obj, normal_force_f,\n",
    "    action_world, dt, steps,\n",
    "    debug=False\n",
    ")\n",
    "\n",
    "# --- Set up Matplotlib animation to visualize ---\n",
    "fig, ax = plt.subplots(figsize=(6, 6))\n",
    "ax.set_aspect('equal')\n",
    "ax.set_xlim(-0.3, 0.3)\n",
    "ax.set_ylim(-0.3, 0.3)\n",
    "ax.set_title(\"Planar Push (Single-Contact, Centered)\")\n",
    "\n",
    "# Rectangle patch for the object (10 cm × 5 cm),\n",
    "# with its center at (0,0) instead of lower-left corner:\n",
    "width, height = 0.10, 0.05\n",
    "rect = Rectangle(\n",
    "    (-width/2, -height/2),  # lower-left corner so that center is (0,0)\n",
    "    width,\n",
    "    height,\n",
    "    edgecolor='blue',\n",
    "    facecolor='none',\n",
    "    lw=2\n",
    ")\n",
    "ax.add_patch(rect)\n",
    "\n",
    "# Pusher marker (red) and contact marker (green)\n",
    "pusher_scatter, = ax.plot([], [], 'ro', label='Pusher')\n",
    "contact_scatter, = ax.plot([], [], 'go', label='Contact')\n",
    "\n",
    "# Legend\n",
    "handles, labels = ax.get_legend_handles_labels()\n",
    "by_label = dict(zip(labels, handles))\n",
    "ax.legend(by_label.values(), by_label.keys(), loc='upper left')\n",
    "\n",
    "def update(frame):\n",
    "    # Object pose at this frame\n",
    "    pos = traj_obj[frame]\n",
    "    rot = traj_rot[frame]\n",
    "\n",
    "    # Build a transform: rotate around center, then translate to pos\n",
    "    transf = (\n",
    "        plt.matplotlib.transforms.Affine2D()\n",
    "        .rotate(rot)             \n",
    "        .translate(pos[0], pos[1]) \n",
    "        + ax.transData\n",
    "    )\n",
    "    rect.set_transform(transf)\n",
    "\n",
    "    # Pusher\n",
    "    px, py = traj_push[frame]\n",
    "    pusher_scatter.set_data([px], [py])\n",
    "\n",
    "    # Contact point\n",
    "    cx, cy = traj_contact[frame]\n",
    "    contact_scatter.set_data([cx], [cy])\n",
    "\n",
    "    return rect, pusher_scatter, contact_scatter\n",
    "\n",
    "anim = FuncAnimation(fig, update, frames=steps, interval=100, blit=True)\n",
    "plt.close(fig)\n",
    "HTML(anim.to_jshtml())\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "base",
   "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.9.13"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
