notebook_tutorial.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Contact Invariant Optimization\n",
    "\n",
    "This notebook gives an overview of [Contact Invariant Optimization](<https://homes.cs.washington.edu/~todorov/papers/MordatchSCA12.pdf>) (CIO) and an implementation of the method.\n",
    "\n",
    "Solving for manipulation trajectories involving contact is a notoriously difficult optimization problem due to discontinuities introduced by the hybrid space. CIO attempts to solve this issue by smoothing the transitions between contact dynamics and free space dynamics.\n",
    "\n",
    "CIO tries to solve the problem of finding a trajectory which manipulates an object into a desired state. The basis of CIO is the objective function which constrains the desired motion of the manipulated object and the manipulator. CIO attempts to solve for the necessary places to make contact, the forces to apply at these contact points, and the trajectory of the manipulated object and fingers. The paper describes all of the constraints used in the final optimization (L-BFGS). Here is a video of the kinds of manipulation plans CIO is able to find."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from IPython.lib.display import YouTubeVideo\n",
    "YouTubeVideo('Gzt2UoxYfAQ')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# State\n",
    "\n",
    "The manipulated object is a Circle with a position, $p_O$, and linear velocity $\\dot{p}_O$. These make up the state variable for the object, $s_O = [p_O, \\dot{p}_O]$."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from cio.world import Circle\n",
    "from cio.util import Position\n",
    "\"\"\"\n",
    "Position Attributes\n",
    "-----\n",
    "x : float\n",
    "    x position\n",
    "y : float\n",
    "    y position\n",
    "\n",
    "LinearVelocity Attributes\n",
    "-----\n",
    "x : float\n",
    "    linear velocity in x direction\n",
    "y : float\n",
    "    linear velocity in y direction\n",
    "\n",
    "Circle Parameters\n",
    "------\n",
    "radius : float\n",
    "    the radius of the Circle\n",
    "pos : Position\n",
    "    the position of the Circle\n",
    "vel : LinearVelocity, optional\n",
    "    the linear velocity of the Circle, default = LinearVelocity(x=0.0, y=0.0)\n",
    "\"\"\"\n",
    "radius = 5.0\n",
    "manip_obj = Circle(radius, Position(5.0,radius))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The manipulator is represented as a list of $N$ fingers. For finger $j$, the position is $p_j$ and the linear velocity is $\\dot{p}_j$. These make up the hand state variable, $s_H = [p_j \\dot{p}_j]$, $j \\in \\{1,...,N\\}$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "finger0 = Circle(1.0, Position(-5.0, -5.0))\n",
    "finger1 = Circle(1.0, Position(15.0, -5.0))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The contact state for each finger is defined as follows. $f_j$ is the force applied by finger $j$. $r_j^O$ is a position in the object frame which describes where $f_j$ is originating. $r_j$ is $r_j^O$ in the world frame. $c_j \\in [0,1]$ describes the nature of the contact. When $c_j = 1$, the finger should be in contact with the object, and when $c_j = 0$ it is not in contact. The figure below gives a visual for the contact parameters. These make up the contact state variable, $s_{CS} = [f_j, r_j^O c_j]$, $j \\in \\{1,...,N\\}$. In the current implementation we use only positions instead of full poses. The $x$ variables in the image below are full poses.\n",
    "![CIO State](notebook_files/contact_state.png)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from cio.world import Contact\n",
    "from collections import OrderedDict\n",
    "\"\"\"\n",
    "Contact Attributes\n",
    "-----\n",
    "f : tuple[2]\n",
    "    x and y force\n",
    "ro : tuple[2]\n",
    "    position of applied force in the frame of the manipulated object\n",
    "c : float in [0,1]\n",
    "    the probability of being in contact\n",
    "\"\"\"\n",
    "contact_state = OrderedDict([(finger0, Contact(f=(0.0, 0.0), ro=(-7., -7.), c=.5)),\n",
    "                             (finger1, Contact(f=(0.0, 0.0), ro=(7., -7.), c=.5))])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The entire state is:\n",
    "\n",
    "$s = [s_O, s_H, s_{CS}]$\n",
    "\n",
    "The objective is to get the manipulated object to a specific Position."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "goals = [Position(5.0, 20.0)]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The world is then initialized from this initial state."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from cio.world import World\n",
    "\"\"\"\n",
    "World Attributes (set with the following call)\n",
    "-----\n",
    "manip_obj : Circle\n",
    "    the manipulated object\n",
    "fingers : list of Circle\n",
    "    a list of Circle objects representing fingers\n",
    "contact_state : OrderedDict[Circle:Contact]\n",
    "    a dictionary describing the contact state for each finger\n",
    "\"\"\"\n",
    "world = World(manip_obj, [finger0, finger1], contact_state)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Trajectory\n",
    "\n",
    "A trajectory is represented using $K$ keyframe states. $s_0$ is constrained the be the initial state, making the decision variables that are optimized over:\n",
    "$S = [s_k], k \\in [1,K]$\n",
    "\n",
    "The decision variables are initialized to all correspond to the initial state of te system, with some added Gaussian noise.\n",
    "\n",
    "During the optimization we interpolate between these keyframe states to get a smooth trajectory, $s(t)$. Then this smoothed trajectory is used to calculate the cost of the objective function. Different interpolation methods are used for different parts of the state. The positions and velocities use cubic spline interpolation, the forces and $r$ values use linear interpolation, and the contact variables are piece-wise constant.\n",
    "\n",
    "Run the following two cell to generate a .gif which will visualize the initial interpolated trajectory (thanks Tom!!)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "from cio.params import Params\n",
    "from cio.optimization import CIO\n",
    "import IPython\n",
    "\n",
    "p = Params(world) # will discuss the params later, for now use default values\n",
    "\n",
    "# the single argument just generates the trajectory, visualization, and calculates the cost\n",
    "CIO(goals, world, p, single=True) \n",
    "\n",
    "# run the .gif\n",
    "with open('output/initial.gif','rb') as f:\n",
    "    display(IPython.display.Image(data=f.read(), format='png'))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The small red circles are the fingers and the large red circle is the manipulated object. The green circle is the goal position. The black line segments are from finger $j$ to the corresponding $r_j$, and the black arrow from $r_j$ is the applied force of contact $j$. The blue circles are the $r_j$ variables, and the shading represents the $c_j$ value (darker blue is $c_j$ closer to 1)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Objective Cost Terms\n",
    "\n",
    "The objective function is a weighted sum over different objective functions which are each summed over all states, $s(t)$.\n",
    "\n",
    "## Contact-Invariant Cose: $L_{CI}(s_t)$\n",
    "\n",
    "A contact $j$ is active when $c_j$ is close to 1. When this is true, the corresponding gripper must be constrained to be in contact with the object. To calculate this we introduce new variables, $\\pi_O(r_j)$ and $\\pi_j(r_j)$. $\\pi_O(r_j)$ is $r_j$ projected onto finger $j$. $\\pi_j(r_j)$ is $r_j$ projected onto the manipulated object. This objective term aims to minimize the distance between these projections and $r_j$ when $c_j$ is close to 1. They also aim to minimize the rate at which these distances change to dicourage slipping.\n",
    "<img src=\"notebook_files/cis_cost.png\" width=\"440\" height=\"440\" align=\"center\"/>\n",
    "\n",
    "Below is a visualization of a CIO solution which only has $L_{CI}(s)$ as the objective function and $K=10$. In this visualization the $c_j$ values were also constrained to be 0.5. If they had been solved for in the optimization, they would have gone to 0 to minimize $L_{CI}(s)$.\n",
    "![ci_gif](notebook_files/L_ci_vis.gif)\n",
    "\n",
    "## Task Cost: $L_{task}(s_t)$\n",
    "\n",
    "This is the constraint which defines the task goal. First $T_b$ is defined as the time region in which you want the goal enforced. Then $h_b(s)$ is the component of the state which you are driving to drive to the goal, and $h^{star}_{b,t}$ is the desired value of the state components extracted from $h_b(s)$. It simply states that we want to minimize the distance between the desired and final state at some specified time step\n",
    "\n",
    "This term also regularizes the accelerations of the fingers and the object to encourage smoother trajectories. This regularization is parameterized by $\\lambda$.\n",
    "\n",
    "<img src=\"notebook_files/task_cost.png\" width=\"440\" height=\"440\" align=\"center\"/>\n",
    "\n",
    "In the current implementation $h_b(s)$ can either be a desired Position or desired Velocity (this option hasn't been tested) of the object, and $T_b$ is just the final time step in the trajectory.\n",
    "\n",
    "Below is a visualization of a CIO solution which only has $L_{task}(s)$ as the objective function with $K=10$ and $\\lambda=10^{-3}$.\n",
    "![task_gif](notebook_files/L_task_vis.gif)\n",
    "\n",
    "## Physics Violation Cost: $L_{physics}(s_t)$\n",
    "\n",
    "The physics constraint accounts for the fact that forces and moments on the object from the contacts must balance with any change in linear or angular momentum of the object. This term also includes a regularization on the contact forces to keep them small, parameterized by the same $\\lambda$ as before. $f_{ext}$ and $m_{ext}$ in the equation below account for any external forces or moments such as those due to gravity.\n",
    "\n",
    "<img src=\"notebook_files/phys_cost.png\" width=\"440\" height=\"440\" align=\"center\"/>\n",
    "\n",
    "This term also has a friction cone component. For a coefficient of friction, $\\mu$, the contact force $j$ should lie within a friction cone centered at the line between finger $j$ and $r_j$. This friction cone is centered around the black line shown in the visualizations.\n",
    "\n",
    "<img src=\"notebook_files/cone_cost.png\" width=\"440\" height=\"440\" align=\"center\"/>\n",
    "\n",
    "In the current implementation, gravity and angular moment conservation are not incorporated.\n",
    "\n",
    "Below is a visualization of a CIO solution which only has $L_{task}(s)$ and $L_{physics}(s)$ as the objective function and $\\lambda=10^{-3}$,  $\\mu=0.9$. The decision variables were initialized to the values shown in the previous .gif (with the $L_{task}$ cost). If there were only a physics cost then the object would not move.\n",
    "\n",
    "![phys_gif](notebook_files/L_task_phys_vis.gif)\n",
    "\n",
    "## Kinematic Violation: $L_{kinematics}(s_t)$\n",
    "\n",
    "This objective tries to minimize any violations of kinematic constraints, such as joint angles, the distance the fingers can extend from the hand, and collisions between any objects. None of these are implemented in this version. While this permits objects to collide, there is still an objective of making contact between the surfaces of fingers and objects due to the $L_{CI}(s)$ cost term.\n",
    "\n",
    "## Total Objective Function\n",
    "\n",
    "All of these terms are used in the final objective function which sums over time and each individual cost term.\n",
    "\n",
    "<img src=\"notebook_files/final_cost.png\" width=\"240\" height=\"240\" align=\"center\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Optimization\n",
    "\n",
    "CIO makes use of the L-BFGS optimization method and occurs over 2 calls to L-BFGS. The decision variables for the second call are initialized to the resulting values from the first optimization. In each stage the optimization functions are weighted differently. The first stage uses the following weights.\n",
    "\n",
    "<img src=\"notebook_files/stage_0_weights.png\" width=\"540\" height=\"540\" align=\"center\"/>\n",
    "\n",
    "The second stage uses the following weights.\n",
    "\n",
    "<img src=\"notebook_files/stage_1_weights.png\" width=\"540\" height=\"540\" align=\"center\"/>\n",
    "\n",
    "Now you can test it yourself with the following call, and play the resulting .gifs.\n",
    "\n",
    "WARNING: This will take a while to run!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "CIO(goals, world, p)\n",
    "with open('output/stage_0.gif','rb') as f:\n",
    "    display(IPython.display.Image(data=f.read(), format='png'))\n",
    "with open('output/stage_1.gif','rb') as f:\n",
    "    display(IPython.display.Image(data=f.read(), format='png'))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Parameters\n",
    "\n",
    "There are many parameters which can give very different optimization results. The Params() object which was initialized previously can take in arguments to override the default values. Below are the parameters used to initialize a Params() object initialized with their default values. Feel free to directly change them here."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\"\"\"\n",
    "StageWeights Attributes\n",
    "----------\n",
    "w_CI : float\n",
    "    the weight for the L_CI function\n",
    "w_physics : float\n",
    "    the weight for the L_physics function\n",
    "w_kinematics : float\n",
    "    the weight for the L_kinematics function (this parameter doesn't affect anything)\n",
    "w_task : float\n",
    "    the weight for the L_task function\n",
    "\n",
    "Params Parameters\n",
    "----------\n",
    "world : world.World\n",
    "    an object representing the objects in the world\n",
    "K : int, optional\n",
    "    the number of phases used to represent the trajectory\n",
    "delT : float, optional\n",
    "    the time step used to calculate derivates using finite differences\n",
    "delT_phase : float, optional\n",
    "    the length of time a phase lasts\n",
    "mass : float, optional\n",
    "    the mass (kg) of the manipulated object\n",
    "mu : float, optional\n",
    "    the coefficient of friction\n",
    "lamb : float, optional\n",
    "    a regularizer that keeps accelerations and applied forces small\n",
    "stage_weights : list of StageWeights, optional\n",
    "    the list of weights used during each optimization stage. the length of this list represents the number of optimization stages\n",
    "\n",
    "Below are the default values for all of the parameters\n",
    "\"\"\"\n",
    "from cio.params import StageWeights\n",
    "stage_weights=[StageWeights(w_CI=0.1, w_physics=0.1, w_kinematics=0.0, w_task=1.0),\n",
    "                    StageWeights(w_CI=10.**1, w_physics=10.**0, w_kinematics=0., w_task=10.**1)]\n",
    "p = Params(world, K=10, delT=0.05, delT_phase=0.5, mass=1.0,\n",
    "                mu=0.9, lamb=10**-3, stage_weights=stage_weights)\n",
    "CIO(goals, world, p);"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Your Testing\n",
    "\n",
    "Now feel free to play around with CIO. Here are some ideas for the adventurous:\n",
    "- Change the initial world state \n",
    "    - Where the objects are\n",
    "    - The contact state values\n",
    "    - The number of fingers\n",
    "- Change the parameters\n",
    "    - Different stage weights\n",
    "    - Different number of stages\n",
    "    - Different coefficient of friction\n",
    "- Change the goal description\n",
    "    - The goals parameter passed into CIO() is a list of constraints on the world state. Currently it can only take in a constraint on the Position and LinearVelocity of the manipulated object at the final time step, but it isn't too difficult to add more goal types.\n",
    "    - Try sequential goals\n",
    "    - Try constraining the goal velocity\n",
    "- Change other parts of the cost function\n",
    "    - Try to add gravity\n",
    "    \n",
    "The cost functions are all in CIO.py and named the same as above. They take in a time, t, and the world state at time t. See world.World() for what can be accessed in a world state.\n",
    "        "
   ]
  }
 ],
 "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.9.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}