exp1_box_circulation_libfranka_gripper_no_circ.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 1. Set up communication with the FR3"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 1.1 Set up communication with the arm"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Interface Running...\n"
     ]
    }
   ],
   "source": [
    "%load_ext autoreload\n",
    "%autoreload 2\n",
    "import sys \n",
    "from FR3Py.robot.interface import FR3Real\n",
    "robot = FR3Real(robot_id='fr3')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "{'q': array([ 7.20597975e-04, -7.84720396e-01,  2.86792980e-04, -2.35597345e+00,\n",
       "         6.72882415e-04,  1.57114048e+00,  7.84918573e-01]),\n",
       " 'dq': array([-6.05203587e-04, -2.22595737e-04,  2.18287203e-04,  5.75445679e-06,\n",
       "         3.54091411e-04,  2.87853852e-04,  1.75911721e-04]),\n",
       " 'T': array([-0.11178899, -4.39721823, -0.72402036, 22.94101334,  0.67399824,\n",
       "         2.11484504,  0.1268256 ]),\n",
       " 'M': array([[ 4.93210072e-01, -9.14834068e-03,  4.91578980e-01,\n",
       "         -1.17950473e-02,  6.15759797e-02, -3.07076350e-03,\n",
       "          1.29280006e-03],\n",
       "        [-9.14834068e-03,  1.54931631e+00,  4.68171005e-03,\n",
       "         -7.02521209e-01, -1.29722412e-02, -2.24928174e-02,\n",
       "         -1.54792050e-03],\n",
       "        [ 4.91578980e-01,  4.68171005e-03,  9.96165670e-01,\n",
       "         -2.58413563e-02,  7.51428537e-02, -3.85934726e-03,\n",
       "          3.24994312e-03],\n",
       "        [-1.17950473e-02, -7.02521209e-01, -2.58413563e-02,\n",
       "          9.73361732e-01,  2.55906336e-02,  1.11776515e-01,\n",
       "         -1.71185927e-04],\n",
       "        [ 6.15759797e-02, -1.29722412e-02,  7.51428537e-02,\n",
       "          2.55906336e-02,  2.35421516e-02,  2.32744815e-03,\n",
       "          8.05558523e-04],\n",
       "        [-3.07076350e-03, -2.24928174e-02, -3.85934726e-03,\n",
       "          1.11776515e-01,  2.32744815e-03,  3.89451368e-02,\n",
       "         -6.75649122e-04],\n",
       "        [ 1.29280006e-03, -1.54792050e-03,  3.24994312e-03,\n",
       "         -1.71185927e-04,  8.05558523e-04, -6.75649122e-04,\n",
       "          2.17287172e-03]]),\n",
       " 'G': array([ 5.42101086e-19, -4.08923829e+00, -6.10953057e-01,  2.29256979e+01,\n",
       "         7.48789557e-01,  2.24356861e+00,  5.87708147e-05]),\n",
       " 'C': array([ 3.29779364e-08,  1.33247113e-07,  2.15360350e-07, -6.43364160e-08,\n",
       "         4.29255786e-09, -7.07137713e-09,  8.35112574e-10])}"
      ]
     },
     "execution_count": 2,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Print the arm state\n",
    "robot.getJointStates()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 1.2 Set up communication with the gripper"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "{'width': 0.023304443806409836, 'max_width': 0.08006539940834045, 'is_grasped': False, 'temperature': 52, 'time (ms)': 185}\n"
     ]
    }
   ],
   "source": [
    "from fr3_gripper import Gripper, GripperState\n",
    "gripper = Gripper(\"192.168.123.250\")\n",
    "\n",
    "# Initialize the gripper\n",
    "gripper.homing()\n",
    "\n",
    "# Print the gripper state\n",
    "gripper_state = gripper.readOnce()\n",
    "print(gripper_state)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 2. Define robot task parameters"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 2.1 Load PinocchioModel and define various parameters"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from FR3Py.robot.model_collision_avoidance import PinocchioModel\n",
    "import time\n",
    "\n",
    "pin_robot = PinocchioModel()\n",
    "joint_lb = np.array([-2.3093, -1.5133, -2.4937, -2.7478, -2.48, 0.8521, -2.6895])\n",
    "joint_ub = np.array([2.3093, 1.5133, 2.4937, -0.4461, 2.48, 4.2094, 2.6895])\n",
    "torque_lb = np.array([-87, -87, -87, -87, -12, -12, -12])\n",
    "torque_ub = np.array([87, 87, 87, 87, 12, 12, 12])\n",
    "joint_acc_lb = np.array([-10, -10, -10, -10, -10, -10, -10])\n",
    "joint_acc_ub = np.array([10, 10, 10, 10, 10, 10, 10])\n",
    "v_EE_lb = np.array([-0.15, -0.15, -0.15])\n",
    "v_EE_ub = np.array([0.15, 0.15, 0.15])\n",
    "static_friction = np.array([0.8, 2.0, 0.5, 2.0, 1.3, 1.0, 0.5])\n",
    "\n",
    "\n",
    "n_joints = 7\n",
    "n_controls = 7\n",
    "q_bar = 0.5*(joint_ub + joint_lb)\n",
    "delta_M = np.diag([0.2, 0.25, 0.25, 0.25, 0.25, 0.25, 0.2])\n",
    "\n",
    "ball_gravity = np.array([0, 0, -9.81, 0, 0, 0])*0.042\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 2.2 Define the obstacles using scaling functions and setup problems"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Obstacle parameters\n",
    "obstacle_config = {\n",
    "    \"obs1\":{\n",
    "        \"type\": \"polytope3d\",\n",
    "        \"vertices\": [[0.226, 0.293, 0.28], [0.234, 0.287, 0.28], [0.416, 0.543, 0.28], [0.424, 0.537, 0.28], [0.226, 0.293, 0.0], [0.234, 0.287, 0.0], [0.416, 0.543, 0.0], [0.424, 0.537, 0.0]],\n",
    "        \"pos\": [0.0, 0.0, 0.0],\n",
    "        \"quat\": [0.0, 0.0, 0.0, 1.0]\n",
    "    },\n",
    "\n",
    "    \"obs2\":{\n",
    "        \"type\": \"polytope3d\",\n",
    "        \"vertices\": [[0.417, 0.536, 0.28], [0.423, 0.544, 0.28], [0.047, 0.826, 0.28], [0.053, 0.834, 0.28], [0.417, 0.536, 0.0], [0.423, 0.544, 0.0], [0.047, 0.826, 0.0], [0.053, 0.834, 0.0]],\n",
    "        \"pos\": [0.0, 0.0, 0.0],\n",
    "        \"quat\": [0.0, 0.0, 0.0, 1.0]\n",
    "    },\n",
    "\n",
    "    \"obs3\":{\n",
    "        \"type\": \"polytope3d\",\n",
    "        \"vertices\": [[0.054, 0.827, 0.28], [0.046, 0.833, 0.28], [-0.136, 0.577, 0.28], [-0.144, 0.583, 0.28], [0.054, 0.827, 0.0], [0.046, 0.833, 0.0], [-0.136, 0.577, 0.0], [-0.144, 0.583, 0.0]],\n",
    "        \"pos\": [0.0, 0.0, 0.0],\n",
    "        \"quat\": [0.0, 0.0, 0.0, 1.0]\n",
    "    },\n",
    "\n",
    "    \"obs4\":{\n",
    "        \"type\": \"polytope3d\",\n",
    "        \"vertices\": [[-0.137, 0.584, 0.28], [-0.143, 0.576, 0.28], [0.233, 0.294, 0.28], [0.227, 0.286, 0.28], [-0.137, 0.584, 0.0], [-0.143, 0.576, 0.0], [0.233, 0.294, 0.0], [0.227, 0.286, 0.0]],\n",
    "        \"pos\": [0.0, 0.0, 0.0],\n",
    "        \"quat\": [0.0, 0.0, 0.0, 1.0]\n",
    "    }\n",
    "\n",
    "}"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "/workspaces/HOCBF-FR3-Experiments/cores/obstacle_collections/polytope_collection.py:13: ClusterWarning: scipy.cluster: The symmetric non-negative hollow observation matrix looks suspiciously like an uncondensed distance matrix\n",
      "  Z = linkage(dist_matrix, method='single')\n"
     ]
    }
   ],
   "source": [
    "from cores.utils.bounding_shape_coef_mj import BoundingShapeCoef\n",
    "from cores.obstacle_collections.polytope_collection import PolytopeCollection\n",
    "import scalingFunctionsHelperPy as sfh\n",
    "import HOCBFHelperPy as hh\n",
    "from scipy.spatial.transform import Rotation\n",
    "import multiprocessing\n",
    "from liegroups import SO3\n",
    "from cores.utils.proxsuite_utils import init_proxsuite_qp\n",
    "from cores.utils.rotation_utils import get_quat_from_rot_matrix\n",
    "\n",
    "# Load the bounding shape coefficients\n",
    "BB_coefs = BoundingShapeCoef()\n",
    "selected_BBs = [\"HAND_BB\", \"LINK7_BB\", \"LINK6_BB\"]\n",
    "n_selected_BBs = len(selected_BBs)\n",
    "\n",
    "# Obstacle\n",
    "n_polytope = len(obstacle_config)\n",
    "obs_col = PolytopeCollection(3, n_polytope, obstacle_config)\n",
    "\n",
    "# CBF parameters\n",
    "obstacle_kappa = 80\n",
    "\n",
    "robot_SFs = []\n",
    "for (i, bb_key) in enumerate(selected_BBs):\n",
    "    ellipsoid_quadratic_coef = BB_coefs.coefs[bb_key]\n",
    "    SF_rob = sfh.Ellipsoid3d(True, ellipsoid_quadratic_coef, np.zeros(3))\n",
    "    robot_SFs.append(SF_rob)\n",
    "\n",
    "polytope_SFs = []\n",
    "for (i, obs_key) in enumerate(obs_col.face_equations.keys()):\n",
    "    A_obs_np = obs_col.face_equations[obs_key][\"A\"]\n",
    "    b_obs_np = obs_col.face_equations[obs_key][\"b\"]\n",
    "    obs_kappa = obstacle_kappa\n",
    "    SF_obs = sfh.LogSumExp3d(False, A_obs_np, b_obs_np, obs_kappa)\n",
    "    polytope_SFs.append(SF_obs)\n",
    "    \n",
    "hyperplane_SFs = []\n",
    "SF_hyperplane = sfh.Hyperplane3d(False, np.array([0,0,1]), 0.0)\n",
    "hyperplane_SFs.append(SF_hyperplane)\n",
    "\n",
    "# Create the problem collection\n",
    "n_threads = min(max(multiprocessing.cpu_count() -1, 1), 10)\n",
    "probs = hh.Problem3dCollection(n_threads)\n",
    "for i in range(len(selected_BBs)):\n",
    "    SF_rob = robot_SFs[i]\n",
    "    frame_id = i\n",
    "    for (j, obs_key) in enumerate(obs_col.face_equations.keys()):\n",
    "        SF_obs = polytope_SFs[j]\n",
    "        vertices = obs_col.face_equations[obs_key][\"vertices_in_world\"]\n",
    "        prob = hh.EllipsoidAndLogSumExp3dPrb(SF_rob, SF_obs, vertices)\n",
    "        probs.addProblem(prob, frame_id)\n",
    "\n",
    "    for j in range(len(hyperplane_SFs)):\n",
    "        SF_obs = hyperplane_SFs[j]\n",
    "        prob = hh.EllipsoidAndHyperplane3dPrb(SF_rob, SF_obs)\n",
    "        probs.addProblem(prob, frame_id)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 3. Control the arm to starting point"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Rotate link 1 to reach preparation configuration\n",
    "T = 5\n",
    "q_d = np.array([np.pi/3, -np.pi/4, 0, -np.pi/4*3, 0, np.pi/2 , np.pi/4])\n",
    "\n",
    "\n",
    "Kp_joint = np.diag([1, 1, 1, 1, 4, 2, 1])*1.5e2\n",
    "Kd_joint = np.diag([2, 2, 2, 2, 2, 2, 2])*1e2\n",
    "t_start = time.time()\n",
    "while time.time() - t_start < T:\n",
    "    robot_info = robot.getJointStates()\n",
    "    q = robot_info['q'] # shape (7,)\n",
    "    dq = robot_info['dq'] # shape (7,)\n",
    "    M = robot_info['M'] + delta_M # shape (7,7)\n",
    "    G = robot_info['G'] # shape (7,)\n",
    "    Coriolis = robot_info['C'] # shape (7,)\n",
    "\n",
    "    q_pin = 0.025*np.ones(9)\n",
    "    dq_pin = np.zeros(9)\n",
    "    q_pin[0:n_joints] = q # shape (9,)\n",
    "    dq_pin[0:n_joints] = dq # shape (9,)\n",
    "\n",
    "    pin_info = pin_robot.getInfo(q_pin, dq_pin)\n",
    "    P_EE = pin_info[\"P_EE\"]\n",
    "    R_EE = pin_info[\"R_EE\"]\n",
    "\n",
    "    W = np.diag(1.0/(joint_ub-joint_lb))\n",
    "    eq = W @ (q - q_d)\n",
    "    deq = W @ dq\n",
    "    ddq_nominal = - Kd_joint @ deq - Kp_joint @ eq\n",
    "\n",
    "    ddq = ddq_nominal\n",
    "\n",
    "    tau = M @ ddq + Coriolis + static_friction * np.tanh(dq)\n",
    "\n",
    "    tau = np.clip(tau, torque_lb, torque_ub)\n",
    "    robot.setCommands(tau)\n",
    "\n",
    "robot.setCommands(np.zeros_like(tau))\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 4. Collision avoidance without circulation"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 4.1 Define the CBF-QP and various parameters"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [],
   "source": [
    "from cores.utils.trajectory_utils import PositionTrapezoidalTrajectory, OrientationTrapezoidalTrajectory\n",
    "\n",
    "# Define proxuite problem\n",
    "n_obstacle = n_polytope + 1\n",
    "n_CBF = n_selected_BBs*n_obstacle\n",
    "n_controls = 7\n",
    "n_in = n_controls + n_CBF + 3\n",
    "cbf_qp = init_proxsuite_qp(n_v=n_controls, n_eq=0, n_in=n_in)\n",
    "\n",
    "# Define CBF parameters\n",
    "alpha0 = 1.03\n",
    "gamma1 = 20\n",
    "gamma2 = 20\n",
    "compensation = 0\n",
    "\n",
    "# Define tracking gains\n",
    "K_p_pos = np.diag([100,100,100])*0.9\n",
    "K_d_pos = np.diag([50,50,50])*0.5\n",
    "\n",
    "K_p_rot = np.diag([200,200,200])*1.8\n",
    "K_d_rot = np.diag([100,100,100])*0.4\n",
    "\n",
    "Kp_joint = 20*np.diag([1, 1, 1, 1, 1, 1, 1])\n",
    "Kd_joint = 10*np.diag([1, 1, 1, 1, 1, 1, 1])\n",
    "\n",
    "# Define the record\n",
    "q_list = []\n",
    "dq_list = []\n",
    "ddq_list = []\n",
    "tau_list = []\n",
    "all_h_list = []\n",
    "time_per_loop_list = []\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 4.2 Move from the starting point to pre-grasping pose "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {},
   "outputs": [],
   "source": [
    "t_final = 6\n",
    "\n",
    "# Get current pose\n",
    "robot_info = robot.getJointStates()\n",
    "q = robot_info['q'] # shape (7,)\n",
    "dq = robot_info['dq'] # shape (7,)\n",
    "\n",
    "q_pin = 0.30*np.ones(9)\n",
    "dq_pin = np.zeros(9)\n",
    "q_pin[0:n_joints] = q # shape (9,)\n",
    "dq_pin[0:n_joints] = dq # shape (9,)\n",
    "\n",
    "pin_info = pin_robot.getInfo(q_pin, dq_pin)\n",
    "P_EE = pin_info[\"P_EE\"]\n",
    "R_EE = pin_info[\"R_EE\"]\n",
    "\n",
    "# Translational trajectory\n",
    "P_EE_start = P_EE.copy()\n",
    "P_EE_pre_grasping = np.array([0.15, 0.62, 0.10])\n",
    "\n",
    "via_points = np.array([P_EE_start, P_EE_pre_grasping])\n",
    "target_time = np.array([0, t_final])\n",
    "Ts = 0.01\n",
    "traj_position = PositionTrapezoidalTrajectory(via_points, target_time, T_antp=0.2, Ts=Ts)\n",
    "\n",
    "# Rotational trajectory\n",
    "R_EE_start  = R_EE.copy()\n",
    "\n",
    "roll = np.pi\n",
    "pitch = 0\n",
    "yaw = np.pi/3\n",
    "R_EE_pre_grasping = Rotation.from_euler('xyz', [roll, pitch, yaw]).as_matrix()\n",
    "\n",
    "orientations = np.array([R_EE_start, R_EE_pre_grasping])\n",
    "target_time = np.array([0, t_final])\n",
    "traj_orientation = OrientationTrapezoidalTrajectory(orientations, target_time, Ts=Ts)\n",
    "\n",
    "t_start = time.time()\n",
    "while time.time() - t_start < t_final + 1:\n",
    "    time_loop_start = time.time()\n",
    "    t = time.time() - t_start\n",
    "    traj_pos, traj_pos_dt, traj_pos_dtdt = traj_position.get_traj_and_ders(t)\n",
    "    traj_ori, traj_ori_dt, traj_ori_dtdt = traj_orientation.get_traj_and_ders(t)\n",
    "\n",
    "    robot_info = robot.getJointStates()\n",
    "    q = robot_info['q'] # shape (7,)\n",
    "    dq = robot_info['dq'] # shape (7,)\n",
    "    M = robot_info['M'] + delta_M # shape (7,7)\n",
    "    G = robot_info['G'] # shape (7,)\n",
    "    Coriolis = robot_info['C'] # shape (7,)\n",
    "\n",
    "    q_pin = 0.025*np.ones(9)\n",
    "    dq_pin = np.zeros(9)\n",
    "    q_pin[0:n_joints] = q # shape (9,)\n",
    "    dq_pin[0:n_joints] = dq # shape (9,)\n",
    "\n",
    "    pin_info = pin_robot.getInfo(q_pin, dq_pin)\n",
    "\n",
    "    P_EE = pin_info[\"P_EE\"]\n",
    "    R_EE = pin_info[\"R_EE\"]\n",
    "    J_EE = pin_info[\"J_EE\"][:,0:n_joints] # shape (6,7)\n",
    "    dJdq_EE = pin_info[\"dJdq_EE\"] # shape (6,)\n",
    "    v_EE = J_EE @ dq # shape (6,)\n",
    "\n",
    "    # Primary obejctive: tracking control\n",
    "    e_pos = P_EE - traj_pos # shape (3,)\n",
    "    e_pos_dt = v_EE[:3] - traj_pos_dt # shape (3,)\n",
    "    v_dt = traj_pos_dtdt - K_p_pos @ e_pos - K_d_pos @ e_pos_dt\n",
    "\n",
    "    e_rot = SO3(R_EE @ traj_ori.T).log() # shape (3,)\n",
    "    e_rot_dt = v_EE[3:] - traj_ori_dt # shape (3,)\n",
    "    omega_dt = traj_ori_dtdt -K_p_rot @ e_rot - K_d_rot @ e_rot_dt\n",
    "\n",
    "    v_EE_dt_desired = np.concatenate([v_dt, omega_dt])\n",
    "    S = J_EE\n",
    "    S_pinv = S.T @ np.linalg.pinv(S @ S.T + 0.01* np.eye(S.shape[0]))\n",
    "    S_null = (np.eye(len(q)) - S_pinv @ S)\n",
    "    ddq_task = S_pinv @ (v_EE_dt_desired - dJdq_EE)\n",
    "\n",
    "    # Secondary objective: encourage the joints to remain close to the initial configuration\n",
    "    W = np.diag(1.0/(joint_ub-joint_lb))\n",
    "    q_bar = 1/2*(joint_ub+joint_lb)\n",
    "    e_joint = W @ (q - q_bar)\n",
    "    e_joint_dot = W @ dq\n",
    "    ddq_nominal = ddq_task + S_null @ (- Kp_joint @ e_joint - Kd_joint @ e_joint_dot)\n",
    "\n",
    "    # CBF\n",
    "    C = np.zeros([n_in, n_controls])\n",
    "    lb = np.zeros(n_in)\n",
    "    ub = np.zeros(n_in)\n",
    "    CBF_tmp = np.zeros(n_CBF)\n",
    "    phi1_tmp = np.zeros(n_CBF)\n",
    "    phi2_tmp = np.zeros(n_CBF)\n",
    "\n",
    "    all_P_np = np.zeros([n_selected_BBs, 3])\n",
    "    all_quat_np = np.zeros([n_selected_BBs, 4])\n",
    "    all_J_np = np.zeros([n_selected_BBs, 6, 7])\n",
    "    all_dJdq_np = np.zeros([n_selected_BBs, 6])\n",
    "\n",
    "    for (ii, bb_key) in enumerate(selected_BBs):\n",
    "        all_P_np[ii, :] = pin_info[\"P_\"+bb_key]\n",
    "        all_J_np[ii, :, :] = pin_info[\"J_\"+bb_key][:,:7]\n",
    "        all_quat_np[ii, :] = get_quat_from_rot_matrix(pin_info[\"R_\"+bb_key])\n",
    "        all_dJdq_np[ii, :] = pin_info[\"dJdq_\"+bb_key][:7]\n",
    "\n",
    "    all_h_np, _, _, all_phi1_np, all_actuation_np, all_lb_np, all_ub_np = \\\n",
    "        probs.getCBFConstraints(dq, all_P_np, all_quat_np, all_J_np, all_dJdq_np, alpha0, gamma1, gamma2, compensation)\n",
    "    \n",
    "    # CBF-QP constraints\n",
    "    C[0:n_CBF,:] = all_actuation_np\n",
    "    lb[0:n_CBF] = all_lb_np\n",
    "    ub[0:n_CBF] = all_ub_np\n",
    "    CBF_tmp = all_h_np\n",
    "    phi1_tmp = all_phi1_np\n",
    "\n",
    "    g = -ddq_nominal\n",
    "\n",
    "    C[n_CBF:n_CBF+n_controls,:] = np.eye(n_controls)\n",
    "    lb[n_CBF:n_CBF+n_controls] = joint_acc_lb[:7]\n",
    "    ub[n_CBF:n_CBF+n_controls] = joint_acc_ub[:7]\n",
    "\n",
    "    h_v_lb = v_EE[0:3] - v_EE_lb\n",
    "    h_v_ub = v_EE_ub - v_EE[0:3]\n",
    "    C[n_CBF+n_controls:n_CBF+n_controls+3, :] = J_EE[0:3,:]\n",
    "    lb[n_CBF+n_controls:n_CBF+n_controls+3] = -20*h_v_lb - dJdq_EE[0:3]\n",
    "    ub[n_CBF+n_controls:n_CBF+n_controls+3] = 20*h_v_ub - dJdq_EE[0:3]\n",
    "\n",
    "    cbf_qp.update(g=g, C=C, l=lb, u=ub)\n",
    "    cbf_qp.solve()\n",
    "    ddq = cbf_qp.results.x\n",
    "\n",
    "    # ddq = ddq_nominal\n",
    "    tau = M @ ddq + Coriolis + static_friction * np.tanh(dq)\n",
    "\n",
    "    tau = np.clip(tau, -20, 20)\n",
    "    robot.setCommands(tau)\n",
    "    time_loop_end = time.time()\n",
    "\n",
    "    q_list.append(q.copy())\n",
    "    dq_list.append(dq.copy())\n",
    "    ddq_list.append(ddq.copy())\n",
    "    tau_list.append(tau.copy())\n",
    "    all_h_list.append(all_h_np.copy())\n",
    "    time_per_loop_list.append(time_loop_end - time_loop_start)\n",
    "\n",
    "robot.setCommands(np.zeros_like(tau))\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 4.3 Grasp the object"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "True"
      ]
     },
     "execution_count": 21,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "t_final = 2\n",
    "\n",
    "# Get current pose\n",
    "robot_info = robot.getJointStates()\n",
    "q = robot_info['q'] # shape (7,)\n",
    "dq = robot_info['dq'] # shape (7,)\n",
    "\n",
    "q_pin = 0.30*np.ones(9)\n",
    "dq_pin = np.zeros(9)\n",
    "q_pin[0:n_joints] = q # shape (9,)\n",
    "dq_pin[0:n_joints] = dq # shape (9,)\n",
    "\n",
    "pin_info = pin_robot.getInfo(q_pin, dq_pin)\n",
    "P_EE = pin_info[\"P_EE\"]\n",
    "R_EE = pin_info[\"R_EE\"]\n",
    "\n",
    "# Translational trajectory\n",
    "P_EE_start = P_EE.copy()\n",
    "P_EE_grasping = np.array([0.15, 0.62, 0.02])\n",
    "\n",
    "via_points = np.array([P_EE_start, P_EE_grasping])\n",
    "target_time = np.array([0, t_final])\n",
    "Ts = 0.01\n",
    "traj_position = PositionTrapezoidalTrajectory(via_points, target_time, T_antp=0.2, Ts=Ts)\n",
    "\n",
    "# Rotational trajectory\n",
    "R_EE_start  = R_EE.copy()\n",
    "\n",
    "roll = np.pi\n",
    "pitch = 0\n",
    "yaw = np.pi/3\n",
    "R_EE_pre_grasping = Rotation.from_euler('xyz', [roll, pitch, yaw]).as_matrix()\n",
    "\n",
    "orientations = np.array([R_EE_start, R_EE_pre_grasping])\n",
    "target_time = np.array([0, t_final])\n",
    "traj_orientation = OrientationTrapezoidalTrajectory(orientations, target_time, Ts=Ts)\n",
    "\n",
    "t_start = time.time()\n",
    "while time.time() - t_start < t_final + 1:\n",
    "    time_loop_start = time.time()\n",
    "    t = time.time() - t_start\n",
    "    traj_pos, traj_pos_dt, traj_pos_dtdt = traj_position.get_traj_and_ders(t)\n",
    "    traj_ori, traj_ori_dt, traj_ori_dtdt = traj_orientation.get_traj_and_ders(t)\n",
    "\n",
    "    robot_info = robot.getJointStates()\n",
    "    q = robot_info['q'] # shape (7,)\n",
    "    dq = robot_info['dq'] # shape (7,)\n",
    "    M = robot_info['M'] + delta_M # shape (7,7)\n",
    "    G = robot_info['G'] # shape (7,)\n",
    "    Coriolis = robot_info['C'] # shape (7,)\n",
    "\n",
    "    q_pin = 0.025*np.ones(9)\n",
    "    dq_pin = np.zeros(9)\n",
    "    q_pin[0:n_joints] = q # shape (9,)\n",
    "    dq_pin[0:n_joints] = dq # shape (9,)\n",
    "\n",
    "    pin_info = pin_robot.getInfo(q_pin, dq_pin)\n",
    "\n",
    "    P_EE = pin_info[\"P_EE\"]\n",
    "    R_EE = pin_info[\"R_EE\"]\n",
    "    J_EE = pin_info[\"J_EE\"][:,0:n_joints] # shape (6,7)\n",
    "    dJdq_EE = pin_info[\"dJdq_EE\"] # shape (6,)\n",
    "    v_EE = J_EE @ dq # shape (6,)\n",
    "\n",
    "    # Primary obejctive: tracking control\n",
    "    e_pos = P_EE - traj_pos # shape (3,)\n",
    "    e_pos_dt = v_EE[:3] - traj_pos_dt # shape (3,)\n",
    "    v_dt = traj_pos_dtdt - K_p_pos @ e_pos - K_d_pos @ e_pos_dt\n",
    "\n",
    "    e_rot = SO3(R_EE @ traj_ori.T).log() # shape (3,)\n",
    "    e_rot_dt = v_EE[3:] - traj_ori_dt # shape (3,)\n",
    "    omega_dt = traj_ori_dtdt -K_p_rot @ e_rot - K_d_rot @ e_rot_dt\n",
    "\n",
    "    v_EE_dt_desired = np.concatenate([v_dt, omega_dt])\n",
    "    S = J_EE\n",
    "    S_pinv = S.T @ np.linalg.pinv(S @ S.T + 0.01* np.eye(S.shape[0]))\n",
    "    S_null = (np.eye(len(q)) - S_pinv @ S)\n",
    "    ddq_task = S_pinv @ (v_EE_dt_desired - dJdq_EE)\n",
    "\n",
    "    # Secondary objective: encourage the joints to remain close to the initial configuration\n",
    "    W = np.diag(1.0/(joint_ub-joint_lb))\n",
    "    q_bar = 1/2*(joint_ub+joint_lb)\n",
    "    e_joint = W @ (q - q_bar)\n",
    "    e_joint_dot = W @ dq\n",
    "    ddq_nominal = ddq_task + S_null @ (- Kp_joint @ e_joint - Kd_joint @ e_joint_dot)\n",
    "\n",
    "    # CBF\n",
    "    C = np.zeros([n_in, n_controls])\n",
    "    lb = np.zeros(n_in)\n",
    "    ub = np.zeros(n_in)\n",
    "    CBF_tmp = np.zeros(n_CBF)\n",
    "    phi1_tmp = np.zeros(n_CBF)\n",
    "    phi2_tmp = np.zeros(n_CBF)\n",
    "\n",
    "    all_P_np = np.zeros([n_selected_BBs, 3])\n",
    "    all_quat_np = np.zeros([n_selected_BBs, 4])\n",
    "    all_J_np = np.zeros([n_selected_BBs, 6, 7])\n",
    "    all_dJdq_np = np.zeros([n_selected_BBs, 6])\n",
    "\n",
    "    for (ii, bb_key) in enumerate(selected_BBs):\n",
    "        all_P_np[ii, :] = pin_info[\"P_\"+bb_key]\n",
    "        all_J_np[ii, :, :] = pin_info[\"J_\"+bb_key][:,:7]\n",
    "        all_quat_np[ii, :] = get_quat_from_rot_matrix(pin_info[\"R_\"+bb_key])\n",
    "        all_dJdq_np[ii, :] = pin_info[\"dJdq_\"+bb_key][:7]\n",
    "\n",
    "    all_h_np, _, _, all_phi1_np, all_actuation_np, all_lb_np, all_ub_np = \\\n",
    "        probs.getCBFConstraints(dq, all_P_np, all_quat_np, all_J_np, all_dJdq_np, alpha0, gamma1, gamma2, compensation)\n",
    "    \n",
    "    # CBF-QP constraints\n",
    "    C[0:n_CBF,:] = all_actuation_np\n",
    "    lb[0:n_CBF] = all_lb_np\n",
    "    ub[0:n_CBF] = all_ub_np\n",
    "    CBF_tmp = all_h_np\n",
    "    phi1_tmp = all_phi1_np\n",
    "\n",
    "    g = -ddq_nominal\n",
    "\n",
    "    C[n_CBF:n_CBF+n_controls,:] = np.eye(n_controls)\n",
    "    lb[n_CBF:n_CBF+n_controls] = joint_acc_lb[:7]\n",
    "    ub[n_CBF:n_CBF+n_controls] = joint_acc_ub[:7]\n",
    "\n",
    "    h_v_lb = v_EE[0:3] - v_EE_lb\n",
    "    h_v_ub = v_EE_ub - v_EE[0:3]\n",
    "    C[n_CBF+n_controls:n_CBF+n_controls+3, :] = J_EE[0:3,:]\n",
    "    lb[n_CBF+n_controls:n_CBF+n_controls+3] = -20*h_v_lb - dJdq_EE[0:3]\n",
    "    ub[n_CBF+n_controls:n_CBF+n_controls+3] = 20*h_v_ub - dJdq_EE[0:3]\n",
    "\n",
    "    cbf_qp.update(g=g, C=C, l=lb, u=ub)\n",
    "    cbf_qp.solve()\n",
    "    ddq = cbf_qp.results.x\n",
    "\n",
    "    # ddq = ddq_nominal\n",
    "    tau = M @ ddq + Coriolis + static_friction * np.tanh(dq)\n",
    "\n",
    "    tau = np.clip(tau, -20, 20)\n",
    "    robot.setCommands(tau)\n",
    "    time_loop_end = time.time()\n",
    "\n",
    "    q_list.append(q.copy())\n",
    "    dq_list.append(dq.copy())\n",
    "    ddq_list.append(ddq.copy())\n",
    "    tau_list.append(tau.copy())\n",
    "    all_h_list.append(all_h_np.copy())\n",
    "    time_per_loop_list.append(time_loop_end - time_loop_start)\n",
    "\n",
    "robot.setCommands(np.zeros_like(tau))\n",
    "\n",
    "# Grasp the object\n",
    "gripper_width = 0.04  # m\n",
    "speed = 0.05  # m / s\n",
    "force = 10  # N\n",
    "epsilon_inner = 0.01  # m\n",
    "epsilon_outer = 0.01  # m\n",
    "gripper.grasp(gripper_width, speed, force, epsilon_inner, epsilon_outer)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 4.4 Move out of the box"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {},
   "outputs": [],
   "source": [
    "t_1 = 2\n",
    "t_2 = 12\n",
    "t_final = 22\n",
    "\n",
    "# Get current pose\n",
    "robot_info = robot.getJointStates()\n",
    "q = robot_info['q'] # shape (7,)\n",
    "dq = robot_info['dq'] # shape (7,)\n",
    "\n",
    "q_pin = 0.30*np.ones(9)\n",
    "dq_pin = np.zeros(9)\n",
    "q_pin[0:n_joints] = q # shape (9,)\n",
    "dq_pin[0:n_joints] = dq # shape (9,)\n",
    "\n",
    "pin_info = pin_robot.getInfo(q_pin, dq_pin)\n",
    "P_EE = pin_info[\"P_EE\"]\n",
    "R_EE = pin_info[\"R_EE\"]\n",
    "\n",
    "# Translational trajectory\n",
    "P_EE_start = P_EE.copy()\n",
    "P_EE_pre_grasping = np.array([0.15, 0.64, 0.10])\n",
    "P_EE_end = np.array([0.50, 0.25, 0.08])\n",
    "\n",
    "via_points = np.array([P_EE_start, P_EE_pre_grasping, P_EE_end, P_EE_end])\n",
    "target_time = np.array([0, t_1, t_2, t_final])\n",
    "Ts = 0.01\n",
    "traj_position = PositionTrapezoidalTrajectory(via_points, target_time, T_antp=0.2, Ts=Ts)\n",
    "\n",
    "# Rotational trajectory\n",
    "R_EE_start  = R_EE.copy()\n",
    "\n",
    "roll = np.pi\n",
    "pitch = 0\n",
    "yaw = np.pi/3\n",
    "R_EE_pre_grasping = Rotation.from_euler('xyz', [roll, pitch, yaw]).as_matrix()\n",
    "\n",
    "roll = np.pi\n",
    "pitch = 0\n",
    "yaw = np.pi/3\n",
    "R_EE_end = Rotation.from_euler('xyz', [roll, pitch, yaw]).as_matrix()\n",
    "\n",
    "orientations = np.array([R_EE_start, R_EE_pre_grasping, R_EE_end, R_EE_end])\n",
    "target_time = np.array([0, t_1, t_2, t_final])\n",
    "traj_orientation = OrientationTrapezoidalTrajectory(orientations, target_time, Ts=Ts)\n",
    "\n",
    "t_start = time.time()\n",
    "while time.time() - t_start < t_final + 1:\n",
    "    time_loop_start = time.time()\n",
    "    t = time.time() - t_start\n",
    "    traj_pos, traj_pos_dt, traj_pos_dtdt = traj_position.get_traj_and_ders(t)\n",
    "    traj_ori, traj_ori_dt, traj_ori_dtdt = traj_orientation.get_traj_and_ders(t)\n",
    "\n",
    "    robot_info = robot.getJointStates()\n",
    "    q = robot_info['q'] # shape (7,)\n",
    "    dq = robot_info['dq'] # shape (7,)\n",
    "    M = robot_info['M'] + delta_M # shape (7,7)\n",
    "    G = robot_info['G'] # shape (7,)\n",
    "    Coriolis = robot_info['C'] # shape (7,)\n",
    "\n",
    "    q_pin = 0.025*np.ones(9)\n",
    "    dq_pin = np.zeros(9)\n",
    "    q_pin[0:n_joints] = q # shape (9,)\n",
    "    dq_pin[0:n_joints] = dq # shape (9,)\n",
    "\n",
    "    pin_info = pin_robot.getInfo(q_pin, dq_pin)\n",
    "\n",
    "    P_EE = pin_info[\"P_EE\"]\n",
    "    R_EE = pin_info[\"R_EE\"]\n",
    "    J_EE = pin_info[\"J_EE\"][:,0:n_joints] # shape (6,7)\n",
    "    dJdq_EE = pin_info[\"dJdq_EE\"] # shape (6,)\n",
    "    v_EE = J_EE @ dq # shape (6,)\n",
    "\n",
    "    # Primary obejctive: tracking control\n",
    "    e_pos = P_EE - traj_pos # shape (3,)\n",
    "    e_pos_dt = v_EE[:3] - traj_pos_dt # shape (3,)\n",
    "    v_dt = traj_pos_dtdt - K_p_pos @ e_pos - K_d_pos @ e_pos_dt\n",
    "\n",
    "    e_rot = SO3(R_EE @ traj_ori.T).log() # shape (3,)\n",
    "    e_rot_dt = v_EE[3:] - traj_ori_dt # shape (3,)\n",
    "    omega_dt = traj_ori_dtdt -K_p_rot @ e_rot - K_d_rot @ e_rot_dt\n",
    "\n",
    "    v_EE_dt_desired = np.concatenate([v_dt, omega_dt])\n",
    "    S = J_EE\n",
    "    S_pinv = S.T @ np.linalg.pinv(S @ S.T + 0.01* np.eye(S.shape[0]))\n",
    "    S_null = (np.eye(len(q)) - S_pinv @ S)\n",
    "    ddq_task = S_pinv @ (v_EE_dt_desired - dJdq_EE)\n",
    "\n",
    "    # Secondary objective: encourage the joints to remain close to the initial configuration\n",
    "    W = np.diag(1.0/(joint_ub-joint_lb))\n",
    "    q_bar = 1/2*(joint_ub+joint_lb)\n",
    "    e_joint = W @ (q - q_bar)\n",
    "    e_joint_dot = W @ dq\n",
    "    ddq_nominal = ddq_task + S_null @ (- Kp_joint @ e_joint - Kd_joint @ e_joint_dot)\n",
    "\n",
    "    # CBF\n",
    "    C = np.zeros([n_in, n_controls])\n",
    "    lb = np.zeros(n_in)\n",
    "    ub = np.zeros(n_in)\n",
    "    CBF_tmp = np.zeros(n_CBF)\n",
    "    phi1_tmp = np.zeros(n_CBF)\n",
    "    phi2_tmp = np.zeros(n_CBF)\n",
    "\n",
    "    all_P_np = np.zeros([n_selected_BBs, 3])\n",
    "    all_quat_np = np.zeros([n_selected_BBs, 4])\n",
    "    all_J_np = np.zeros([n_selected_BBs, 6, 7])\n",
    "    all_dJdq_np = np.zeros([n_selected_BBs, 6])\n",
    "\n",
    "    for (ii, bb_key) in enumerate(selected_BBs):\n",
    "        all_P_np[ii, :] = pin_info[\"P_\"+bb_key]\n",
    "        all_J_np[ii, :, :] = pin_info[\"J_\"+bb_key][:,:7]\n",
    "        all_quat_np[ii, :] = get_quat_from_rot_matrix(pin_info[\"R_\"+bb_key])\n",
    "        all_dJdq_np[ii, :] = pin_info[\"dJdq_\"+bb_key][:7]\n",
    "\n",
    "    all_h_np, _, _, all_phi1_np, all_actuation_np, all_lb_np, all_ub_np = \\\n",
    "        probs.getCBFConstraints(dq, all_P_np, all_quat_np, all_J_np, all_dJdq_np, alpha0, gamma1, gamma2, compensation)\n",
    "    \n",
    "    # CBF-QP constraints\n",
    "    C[0:n_CBF,:] = all_actuation_np\n",
    "    lb[0:n_CBF] = all_lb_np\n",
    "    ub[0:n_CBF] = all_ub_np\n",
    "    CBF_tmp = all_h_np\n",
    "    phi1_tmp = all_phi1_np\n",
    "\n",
    "    g = -ddq_nominal\n",
    "\n",
    "    C[n_CBF:n_CBF+n_controls,:] = np.eye(n_controls)\n",
    "    lb[n_CBF:n_CBF+n_controls] = joint_acc_lb[:7]\n",
    "    ub[n_CBF:n_CBF+n_controls] = joint_acc_ub[:7]\n",
    "\n",
    "    h_v_lb = v_EE[0:3] - v_EE_lb\n",
    "    h_v_ub = v_EE_ub - v_EE[0:3]\n",
    "    C[n_CBF+n_controls:n_CBF+n_controls+3, :] = J_EE[0:3,:]\n",
    "    lb[n_CBF+n_controls:n_CBF+n_controls+3] = -20*h_v_lb - dJdq_EE[0:3]\n",
    "    ub[n_CBF+n_controls:n_CBF+n_controls+3] = 20*h_v_ub - dJdq_EE[0:3]\n",
    "\n",
    "    cbf_qp.update(g=g, C=C, l=lb, u=ub)\n",
    "    cbf_qp.solve()\n",
    "    ddq = cbf_qp.results.x\n",
    "\n",
    "    # ddq = ddq_nominal\n",
    "    tau = M @ ddq + Coriolis + static_friction * np.tanh(dq) + J_EE.T @ ball_gravity\n",
    "\n",
    "    tau = np.clip(tau, -20, 20)\n",
    "    robot.setCommands(tau)\n",
    "    time_loop_end = time.time()\n",
    "\n",
    "    q_list.append(q.copy())\n",
    "    dq_list.append(dq.copy())\n",
    "    ddq_list.append(ddq.copy())\n",
    "    tau_list.append(tau.copy())\n",
    "    all_h_list.append(all_h_np.copy())\n",
    "    time_per_loop_list.append(time_loop_end - time_loop_start)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 4.5 Drop the object"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "True"
      ]
     },
     "execution_count": 23,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "robot.setCommands(np.zeros_like(tau))\n",
    "gripper_width = 0.08\n",
    "speed = 0.05\n",
    "gripper.move(gripper_width, speed)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 5. Save the data"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {},
   "outputs": [],
   "source": [
    "import pickle\n",
    "from datetime import datetime\n",
    "import os\n",
    "\n",
    "now = datetime.now()\n",
    "formatted_date_time = now.strftime(\"%Y-%m-%d-%H-%M-%S\")\n",
    "\n",
    "directory = f\"exp1_results/no_circ_{formatted_date_time}\"\n",
    "\n",
    "# Create the directory if it doesn't exist\n",
    "if not os.path.exists(directory):\n",
    "    os.makedirs(directory)\n",
    "\n",
    "# Define the file path\n",
    "file_path = os.path.join(directory, \"data.pickle\")\n",
    "\n",
    "# Data to be saved\n",
    "data = {\n",
    "    \"q\": q_list,\n",
    "    \"dq\": dq_list,\n",
    "    \"ddq\": ddq_list,\n",
    "    \"tau\": tau_list,\n",
    "    \"all_h\": all_h_list,\n",
    "    \"time_per_loop\": time_per_loop_list\n",
    "}\n",
    "\n",
    "# Save the data using pickle\n",
    "with open(file_path, \"wb\") as f:\n",
    "    pickle.dump(data, f)\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "<matplotlib.collections.LineCollection at 0x71ab742c17d0>"
      ]
     },
     "execution_count": 25,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "image/png": "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",
      "text/plain": [
       "<Figure size 640x480 with 1 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "\n",
    "t = np.cumsum(time_per_loop_list)\n",
    "all_h = np.array(all_h_list)\n",
    "plt.plot(t, np.min(all_h, axis = 1))\n",
    "plt.hlines(0, t[0], t[-1], colors='r', linestyles='dashed')"
   ]
  }
 ],
 "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.10.13"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}