panda_test__01-30-22.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "bcf40afd",
   "metadata": {},
   "source": [
    "# Imports"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "cd0b830f",
   "metadata": {},
   "outputs": [],
   "source": [
    "import sys"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6770ec4c",
   "metadata": {},
   "outputs": [],
   "source": [
    "sys.path.append(\"../\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4c80eaca",
   "metadata": {},
   "outputs": [],
   "source": [
    "import config\n",
    "import plant.manipulator as manipulator\n",
    "import constants\n",
    "import plant.simulation as simulation\n",
    "from ctrl.impedance_generators.setpoint_generators.circle import CircularSetpointGenerator"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f5c03ddd",
   "metadata": {},
   "outputs": [],
   "source": [
    "import pydrake\n",
    "from pydrake.all import (\n",
    "    MultibodyPlant, RotationMatrix, RigidTransform, RollPitchYaw, Meshcat, MeshcatVisualizerParams, DiagramBuilder,\n",
    "    MeshcatVisualizerCpp\n",
    ")\n",
    "from pydrake.common.eigen_geometry import Quaternion_"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a176b730",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from numpy import array"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d3ec89a6",
   "metadata": {},
   "outputs": [],
   "source": [
    "from collections import namedtuple"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6b957ca1",
   "metadata": {},
   "outputs": [],
   "source": [
    "from visualization import AddMeshcatTriad"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8b45a440",
   "metadata": {},
   "outputs": [],
   "source": [
    "import panda.panda_config"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "39a15d7c",
   "metadata": {},
   "source": [
    "# Setup"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "93fc12ad",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4c811fc4",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "49a73aec",
   "metadata": {},
   "outputs": [],
   "source": [
    "meshcat = Meshcat()\n",
    "web_url = meshcat.web_url()\n",
    "\n",
    "builder = pydrake.systems.framework.DiagramBuilder()\n",
    "\n",
    "# Physical parameters\n",
    "plant, scene_graph = pydrake.multibody.plant.AddMultibodyPlantSceneGraph(builder, time_step=config.DT)\n",
    "manipulator.addArm(\n",
    "    plant,\n",
    "    scene_graph=scene_graph,\n",
    "    m_M=constants.nominal_sys_consts.m_M,\n",
    "    r=constants.nominal_sys_consts.r,\n",
    "    mu=constants.nominal_sys_consts.mu\n",
    ")\n",
    "plant.Finalize()\n",
    "\n",
    "meshcat_params = MeshcatVisualizerParams()\n",
    "vis = MeshcatVisualizerCpp.AddToBuilder(\n",
    "    builder, scene_graph.get_query_output_port(), meshcat, meshcat_params)\n",
    "\n",
    "diagram = builder.Build()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e0bd5312",
   "metadata": {},
   "outputs": [],
   "source": [
    "diagram_context = diagram.CreateDefaultContext()\n",
    "plant_context = plant.GetMyContextFromRoot(diagram_context)\n",
    "vis_context = vis.GetMyContextFromRoot(diagram_context)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4eaf1990",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "552bdaa5",
   "metadata": {},
   "outputs": [],
   "source": [
    "def convert_config(config_dict):\n",
    "    pref = \"panda_joint\"\n",
    "    out = []\n",
    "    for i in range(7):\n",
    "        out.append(config_dict[pref + str(i+1)])\n",
    "    return out"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f39af1d7",
   "metadata": {},
   "outputs": [],
   "source": [
    "def convert_pose_dict(pos_dict):\n",
    "    pos = pos_dict['position']\n",
    "    quat_ = pos_dict['orientation']\n",
    "    quat = Quaternion_[float](w=quat_.w, x=quat_.x, y=quat_.y, z=quat_.z)\n",
    "    return pos, quat"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "4d318a14",
   "metadata": {
    "tags": []
   },
   "source": [
    "# Evaluate single data point positions"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "0c5ed546",
   "metadata": {},
   "source": [
    "## Set $q_0$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fc8c0c06",
   "metadata": {},
   "outputs": [],
   "source": [
    "q0 = manipulator.neutral_q"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "86271404",
   "metadata": {},
   "outputs": [],
   "source": [
    "plant.SetPositions(plant_context, q0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1da03ed7",
   "metadata": {},
   "outputs": [],
   "source": [
    "quaternion = namedtuple(\"quaternion\", \"w x y z\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2b101717",
   "metadata": {},
   "source": [
    "## Evaluate panda hand position"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b54f426a",
   "metadata": {},
   "outputs": [],
   "source": [
    "X = plant.EvalBodyPoseInWorld(plant_context, plant.GetBodyByName(\"panda_hand\"))\n",
    "X"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "eb60e5f8",
   "metadata": {},
   "outputs": [],
   "source": [
    "quat = X.rotation().ToQuaternion()\n",
    "print(\"x: {}\\ny: {}\\nz: {}\\nw: {}\".format(\n",
    "    quat.x(),\n",
    "    quat.y(),\n",
    "    quat.z(),\n",
    "    quat.w()\n",
    "))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d0f08e62",
   "metadata": {},
   "source": [
    "## Evaluate end effector position"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "08d31278",
   "metadata": {},
   "outputs": [],
   "source": [
    "X = plant.EvalBodyPoseInWorld(plant_context, plant.GetBodyByName(\"main_end_effector_body\"))\n",
    "X"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e308cbba",
   "metadata": {},
   "outputs": [],
   "source": [
    "quat = X.rotation().ToQuaternion()\n",
    "print(\"x: {}\\ny: {}\\nz: {}\\nw: {}\".format(\n",
    "    quat.x(),\n",
    "    quat.y(),\n",
    "    quat.z(),\n",
    "    quat.w()\n",
    "))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "777eb74e",
   "metadata": {
    "tags": []
   },
   "source": [
    "# Calculate offset transform"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "627e784b",
   "metadata": {},
   "source": [
    "## Raw inputs"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "cc960325",
   "metadata": {},
   "outputs": [],
   "source": [
    "q_1_dict = {'panda_joint1': 0.5920717059436604, 'panda_joint2': -1.1923560818855956, \n",
    "            'panda_joint3': -0.9708204979467376, 'panda_joint4': -2.7506264015373474, \n",
    "            'panda_joint5': -0.029033820947907442, 'panda_joint6': 2.930613038685587, \n",
    "            'panda_joint7': -0.02070499410846963}\n",
    "q_1 = convert_config(q_1_dict)\n",
    "\n",
    "endpoint_dict_1 = {\n",
    "    'position': array([ 0.42532115, -0.14015937,  0.48573156]),\n",
    "    'orientation': quaternion(-0.154862189461281, 0.786106686594126, 0.138303084147821, 0.582173716756831)}\n",
    "pos_1, quat_1 = convert_pose_dict(endpoint_dict_1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a1e39fea",
   "metadata": {},
   "outputs": [],
   "source": [
    "q_2_dict = {\n",
    "    'panda_joint1': -0.34472184209238016, 'panda_joint2': 0.9388866558695357,\n",
    "    'panda_joint3': 0.9880253082911173, 'panda_joint4': -0.7271104281800026,\n",
    "    'panda_joint5': -0.0012154511641710997, 'panda_joint6': 3.7257856980155495,\n",
    "    'panda_joint7': 0.8182791094515057}\n",
    "q_2 = convert_config(q_2_dict)\n",
    "\n",
    "endpoint_dict_2 = {\n",
    "    'position': array([ 0.79588991, -0.09137514,  0.7548383 ]),\n",
    "    'orientation': quaternion(-0.314319532035134, 0.410839113601719, -0.265100815312353, 0.813717403179319)}\n",
    "pos_2, quat_2 = convert_pose_dict(endpoint_dict_2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "bdab1503",
   "metadata": {},
   "outputs": [],
   "source": [
    "q_3_dict = {'panda_joint1': -0.19806813592241537, 'panda_joint2': -1.4399713259579843,\n",
    "            'panda_joint3': 1.1155892525723103, 'panda_joint4': -3.06285333814119,\n",
    "            'panda_joint5': 0.2876149759428815, 'panda_joint6': 1.8047770896460589,\n",
    "            'panda_joint7': 0.9565096986823611}\n",
    "q_3 = convert_config(q_3_dict)\n",
    "\n",
    "endpoint_dict_3 = {\n",
    "    'position': array([0.2077336 , 0.00865574, 0.27209928]), \n",
    "    'orientation': quaternion(0.391679109975748, 0.914522326921643, -0.0711655063304001, 0.0719156386272558)}\n",
    "pos_3, quat_3 = convert_pose_dict(endpoint_dict_3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "20fb63ba",
   "metadata": {},
   "outputs": [],
   "source": [
    "q_4_dict = {'panda_joint1': 0.13736649785340663, 'panda_joint2': -1.511650379851549, \n",
    "            'panda_joint3': -0.13755869549826572, 'panda_joint4': -1.5583121646141103, \n",
    "            'panda_joint5': 0.2808405818541844, 'panda_joint6': 0.6858634739555333, \n",
    "            'panda_joint7': 0.8222335470252566}\n",
    "q_4 = convert_config(q_4_dict)\n",
    "\n",
    "endpoint_dict_4 = {\n",
    "    'position': array([-0.1855927 , -0.01979181,  0.70970675]),\n",
    "    'orientation': quaternion(-0.140598016690583, 0.928419729813831, 0.152047280259162, 0.308464953537479)}\n",
    "pos_4, quat_4 = convert_pose_dict(endpoint_dict_4)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1d3e4b3d",
   "metadata": {},
   "outputs": [],
   "source": [
    "neutral_quat = Quaternion_[float](\n",
    "    w=8.86055883171457e-05,\n",
    "    x=0.999999994645199,\n",
    "    y=-4.8259523485426e-05,\n",
    "    z=-2.30145461781558e-05\n",
    ")\n",
    "neutral_q = manipulator.neutral_q\n",
    "neutral_pos = [3.06918839e-01, 2.42611677e-05, 4.86808120e-01]"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b22c11ae",
   "metadata": {},
   "source": [
    "## Sanity check visuals"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "09378e3b",
   "metadata": {},
   "outputs": [],
   "source": [
    "plant.SetPositions(plant_context, starting_q)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "44254a8f",
   "metadata": {},
   "outputs": [],
   "source": [
    "vis.Publish(vis_context)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2c6a7a78-3231-40cc-b901-94ad0b501f53",
   "metadata": {},
   "outputs": [],
   "source": [
    "traj_poses = np.load(\"panda/poses.npz\", allow_pickle=True)[\"poses\"]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3166a467-4b1c-4031-9327-12b621feb404",
   "metadata": {},
   "outputs": [],
   "source": [
    "traj_poses[0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "60dc6029-672e-485b-9925-034db34763df",
   "metadata": {},
   "outputs": [],
   "source": [
    "i=0\n",
    "quat = Quaternion_(\n",
    "    traj_poses[i]['orientation'].w,\n",
    "    traj_poses[i]['orientation'].x,\n",
    "    traj_poses[i]['orientation'].y,\n",
    "    traj_poses[i]['orientation'].z\n",
    ")\n",
    "pose = traj_poses[0]['position']"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2d78ec86",
   "metadata": {},
   "outputs": [],
   "source": [
    "X_P_SP = RigidTransform(quaternion=quat, p=pose)\n",
    "X_W_SP = manipulator.X_W_panda.multiply(X_P_SP)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d7382d1d",
   "metadata": {},
   "outputs": [],
   "source": [
    "AddMeshcatTriad(meshcat, \"end_effector_pose\", X_PT=X_W_SP)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "27416a96",
   "metadata": {},
   "source": [
    "## Calculate offset"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0af3bec4",
   "metadata": {},
   "outputs": [],
   "source": [
    "qs = [neutral_q, q_1, q_2, q_3, q_4]\n",
    "\n",
    "quaternions = [\n",
    "    neutral_quat, quat_1, quat_2, quat_3, quat_4\n",
    "]\n",
    "\n",
    "positions = [\n",
    "    neutral_pos, pos_1, pos_2, pos_3, pos_4\n",
    "]\n",
    "\n",
    "assert(len(positions) == len(quaternions))\n",
    "assert(len(positions) == len(qs))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "82430208",
   "metadata": {},
   "outputs": [],
   "source": [
    "X_pred_trues = []\n",
    "for (q, quat, pos) in zip(qs, quaternions, positions):\n",
    "    X_panda_true = RigidTransform(\n",
    "        p=pos, quaternion=quat\n",
    "    )\n",
    "    X_W_true = manipulator.X_W_panda.multiply(X_panda_true)\n",
    "    \n",
    "    plant.SetPositions(plant_context, q)\n",
    "    X_W_pred = plant.EvalBodyPoseInWorld(plant_context, plant.GetBodyByName(\"main_end_effector_body\"))\n",
    "    \n",
    "    X_pred_true = X_W_pred.inverse().multiply(X_W_true)\n",
    "    X_pred_trues.append(X_pred_true)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "88516903",
   "metadata": {},
   "outputs": [],
   "source": [
    "for X_pred_true in X_pred_trues:\n",
    "    rpy = RollPitchYaw(X_pred_true.rotation()).vector()\n",
    "    xyz = X_pred_true.translation()\n",
    "    print(\"x: {:.5f}\\ty: {:.5f}\\tz: {:.5f}\\t\\tr: {:.5f}\\tp: {:.5f}\\ty: {:.5f}\".format(\n",
    "        xyz[0], xyz[1], xyz[2], rpy[0], rpy[1], rpy[2]\n",
    "    ))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b58202ac-b256-47c3-84a6-f413107d698d",
   "metadata": {},
   "source": [
    "# Double check conversion"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f93bdef8-e6c4-4e3e-b666-251128675a13",
   "metadata": {},
   "source": [
    "## Get setpoint corresponding to neutral in simulation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8ab31af6-c388-493c-864c-06232e6fc52d",
   "metadata": {},
   "outputs": [],
   "source": [
    "plant.SetPositions(plant_context, manipulator.neutral_q)\n",
    "# Pose from world to setpoint in software\n",
    "X_W_SP_SW = plant.EvalBodyPoseInWorld(plant_context, plant.GetBodyByName(\"main_end_effector_body\"))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4e0cfbd0-f0b2-4060-bd03-a9c998fe9ba3",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Pose from panda to setpoint in hardware\n",
    "X_panda_SP_HW = panda.panda_config.X_panda_SP_hw(X_W_SP_SW)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "bb718beb-32cb-4a16-8754-871d48952ffd",
   "metadata": {},
   "outputs": [],
   "source": [
    "X_panda_SP_HW"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "33bcf51f-65b2-48da-8c89-14f9c547509f",
   "metadata": {},
   "source": [
    "## Get setpoint corresponding to neutral in hardware"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "fa77db39-732b-43ed-bc7c-239c50a962b4",
   "metadata": {},
   "source": [
    "Result of `arm.endpoint_pose()`:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c187a8d5-0735-4b47-8371-53a6c8b50409",
   "metadata": {},
   "outputs": [],
   "source": [
    "X_panda_SP_HW__real = {\n",
    "    'position': np.array([3.06952304e-01, 3.95072413e-05, 4.86853821e-01]),\n",
    "    'orientation': np.array([-7.89707360659434e-05, 0.999999906462373, 0.00036403278825529, 0.000219815826069973])\n",
    "}"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d4b6251c-4e33-446b-b930-552c74987df6",
   "metadata": {},
   "source": [
    "## Compare"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "dc214348-f9a2-4ed3-8a44-e6667c2487d0",
   "metadata": {},
   "outputs": [],
   "source": [
    "def drake_quat_to_vec(quat):\n",
    "    return np.array([quat.w(), quat.x(), quat.y(), quat.z()])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1740661f-7f21-4c4c-89ec-9500b4951b2d",
   "metadata": {},
   "outputs": [],
   "source": [
    "print(\"delta x: {:.5f}\\ndelta y: {:.5f}\\ndelta z: {:.5f}\".format(\n",
    "    *(X_panda_SP_HW.translation() - X_panda_SP_HW__real[\"position\"])))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "73d7671d-f950-4e79-87e1-d7b4bdf59986",
   "metadata": {},
   "outputs": [],
   "source": [
    "print(\"delta x: {:.5f}\\ndelta y: {:.5f}\\ndelta z: {:.5f}\".format(\n",
    "    *(drake_quat_to_vec(X_panda_SP_HW.rotation().ToQuaternion()) - X_panda_SP_HW__real[\"orientation\"])))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7dc94e1e-d0d6-45ae-a726-f1c58a1ace07",
   "metadata": {
    "tags": [],
    "toc-hr-collapsed": true
   },
   "source": [
    "# Test a few candidate points"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "0cb9dab8-7e5e-4015-96d5-0bd3ba309a06",
   "metadata": {},
   "source": [
    "## Starting position"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "a8689791-a7f5-45e7-856a-eba560503827",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Desired $q$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2d572914-4e27-4f36-a9e3-3c2f296032b4",
   "metadata": {},
   "outputs": [],
   "source": [
    "starting_q = q = [\n",
    "    -1.90831538,\n",
    "    -0.74013834,\n",
    "    2.29209901,\n",
    "    -2.58333082,\n",
    "    4.52633766,\n",
    "    0.60032527,\n",
    "    -3.10678351\n",
    "]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "36ca719f-bbef-4f46-84b8-9d5ae633e990",
   "metadata": {},
   "outputs": [],
   "source": [
    "starting_q[4]-np.pi*2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9f298a0c-69c7-4778-b0bb-c7bbc7ce6cc0",
   "metadata": {},
   "outputs": [],
   "source": [
    "plant.SetPositions(plant_context, q)\n",
    "X_W_SP_SW = plant.EvalBodyPoseInWorld(plant_context, plant.GetBodyByName(\"main_end_effector_body\"))\n",
    "X_panda_SP_HW = panda.panda_config.X_panda_SP_hw(X_W_SP_SW)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8f62b93b-12d3-4772-9d80-60438aa2e9cb",
   "metadata": {},
   "outputs": [],
   "source": [
    "vis.Publish(vis_context)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7ec86d7d-7f54-4906-9eca-7a087c26a713",
   "metadata": {},
   "outputs": [],
   "source": [
    "X_panda_SP_HW.translation()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2ebd589d-974f-48cc-a369-5830f731bf83",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Relevant end effector location"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8994cbfe-c88d-4eda-9ac4-5972c124b2d5",
   "metadata": {},
   "outputs": [],
   "source": [
    "quat_ = X_panda_SP_HW.rotation().ToQuaternion()\n",
    "print(\"w: {}\\nx: {}\\ny: {}\\nz: {}\".format(quat_.w(), quat_.x(), quat_.y(), quat_.z()))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "37e824ed-6cc7-46cd-b7e9-f27f30062862",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Approximate real $q$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "af1f684e-bb9f-4911-8856-8cf86e590f11",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "id": "0a3aa0d1-e278-4530-a2f4-6b1130e5e062",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Approximate true end effector location"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5337db07-0280-412f-8bc3-c991a3906df2",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "id": "bc8c6c48-d381-46d5-9099-bd280642e84c",
   "metadata": {
    "tags": []
   },
   "source": [
    "## `t = end_time * 0.1`"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ec326a88-815f-4b0c-bdc4-873b046c49b0",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Desired $q$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "53744bef-a5ee-4bd0-bdc0-03770f722b7e",
   "metadata": {},
   "outputs": [],
   "source": [
    "t1_q = q = [\n",
    "    -2.1146478801194806,\n",
    "    -0.5741377797591646,\n",
    "    2.421334309172595,\n",
    "    -2.6520921048533364,\n",
    "    4.296990530955979,\n",
    "    0.4557018857881668,\n",
    "    -3.2641926011421214,\n",
    "]"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "820f1d30-f20f-45ba-a709-c044c4f0ee93",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Approximate real $q$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "930eecfb-f98e-4af2-a452-7ae8302aa869",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "id": "476cb8ca-1b51-4f8c-9927-924780258e69",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Approximate true end effector location"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "45fb2f0b-e159-4d79-a0cf-c59579e2de07",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "id": "7dc96d57-bafb-4f0f-900e-ce55502e4f4d",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Relevant end effector location"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "18c28744-2ac7-47e6-a4ec-bd2f17feb368",
   "metadata": {},
   "outputs": [],
   "source": [
    "plant.SetPositions(plant_context, q)\n",
    "X_W_SP_SW = plant.EvalBodyPoseInWorld(plant_context, plant.GetBodyByName(\"main_end_effector_body\"))\n",
    "X_panda_SP_HW = panda.panda_config.X_panda_SP_hw(X_W_SP_SW)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "87f6924c-7b21-4540-a8b9-f15321e726be",
   "metadata": {},
   "outputs": [],
   "source": [
    "X_panda_SP_HW.translation()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "04e47899-a65a-43e2-9692-75728925cde1",
   "metadata": {},
   "outputs": [],
   "source": [
    "quat_ = X_panda_SP_HW.rotation().ToQuaternion()\n",
    "print(\"w: {}\\nx: {}\\ny: {}\\nz: {}\".format(quat_.w(), quat_.x(), quat_.y(), quat_.z()))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "54cdaf00-5541-4d0d-80d6-2a12170d6bc3",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Approximate real $q$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "714eb219-8db7-4d33-b5b4-ea62a16bd80e",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "id": "a3372483-5299-4027-a3ed-7ab87cdbed0e",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Approximate true end effector location"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2c208a62-2e07-43d4-971e-1ab9e82f3b15",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "id": "47da530f-dc24-46cc-96d3-bc402b0fbaca",
   "metadata": {
    "tags": []
   },
   "source": [
    "## `t = end_time * 0.2`"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "db889118-f126-4211-8171-116489f7d65f",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Desired $q$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "86e8d4db-3446-4e3e-9d3c-a8e2c0337ee5",
   "metadata": {},
   "outputs": [],
   "source": [
    "t2_q = q = [\n",
    "    -1.9923999435056001,\n",
    "    -0.3649667563328254,\n",
    "    2.2852638372758345,\n",
    "    -2.834742287573601,\n",
    "    3.93642474172192,\n",
    "    0.5108014249814706,\n",
    "    -3.709962619794525,\n",
    "]"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "cc7a7a83-b981-4aba-8b66-d9e4d484eff8",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Relevant end effector location"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4772f28c-0862-422e-b5c2-91d0faea79a6",
   "metadata": {},
   "outputs": [],
   "source": [
    "plant.SetPositions(plant_context, q)\n",
    "X_W_SP_SW = plant.EvalBodyPoseInWorld(plant_context, plant.GetBodyByName(\"main_end_effector_body\"))\n",
    "X_panda_SP_HW = panda.panda_config.X_panda_SP_hw(X_W_SP_SW)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "999e9355-6a27-48fb-8f19-41e577283680",
   "metadata": {},
   "outputs": [],
   "source": [
    "X_panda_SP_HW.translation()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d13b060f-22fd-4291-adc1-b4e06d7c373f",
   "metadata": {},
   "outputs": [],
   "source": [
    "quat_ = X_panda_SP_HW.rotation().ToQuaternion()\n",
    "print(\"w: {}\\nx: {}\\ny: {}\\nz: {}\".format(quat_.w(), quat_.x(), quat_.y(), quat_.z()))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "368ee72e-a236-40fe-8c3c-435da1b85463",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Approximate real $q$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "66b511c7-3dad-4b88-a4be-4cacfbd105f2",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "id": "30d3d750-34ce-4cb8-8b44-8385b592374c",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Approximate true end effector location"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "56435a94-7b5f-4908-9a2e-cb0ad74480f4",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "id": "1783048c-b2b1-4b34-ab89-f03749d4a0fa",
   "metadata": {
    "tags": []
   },
   "source": [
    "## `t = end_time * 0.3`"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "21d8a8e1-732f-48af-8314-e205d517206c",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Desired $q$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "90ff9d04-32e7-4764-86e8-30bd6416f75b",
   "metadata": {},
   "outputs": [],
   "source": [
    "t3_q = q = [\n",
    "    -1.3669552018880975,\n",
    "    -0.0493623881307237,\n",
    "    1.508231366013591,\n",
    "    -2.985729614242096,\n",
    "    3.3298860836292303,\n",
    "    0.6243204293551693,\n",
    "    -4.314615141139232,\n",
    "]"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d6614633-2a26-429d-9ade-9de350c15d9c",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Relevant end effector location"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "640fb00e-47a9-4af0-b066-759cd512e10b",
   "metadata": {},
   "outputs": [],
   "source": [
    "plant.SetPositions(plant_context, q)\n",
    "X_W_SP_SW = plant.EvalBodyPoseInWorld(plant_context, plant.GetBodyByName(\"main_end_effector_body\"))\n",
    "X_panda_SP_HW = panda.panda_config.X_panda_SP_hw(X_W_SP_SW)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "35212a72-b894-4e04-9584-7b96c48f8ac4",
   "metadata": {},
   "outputs": [],
   "source": [
    "X_panda_SP_HW.translation()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c9bda853-1771-43a3-aa7d-3810abc6639a",
   "metadata": {},
   "outputs": [],
   "source": [
    "quat_ = X_panda_SP_HW.rotation().ToQuaternion()\n",
    "print(\"w: {}\\nx: {}\\ny: {}\\nz: {}\".format(quat_.w(), quat_.x(), quat_.y(), quat_.z()))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2f782de7-00ce-49d1-810d-07717c7466f6",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Approximate real $q$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "859784cd-af65-46ff-90a2-7d4e9c184de1",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "id": "211a822d-97c7-40b2-b65f-9bdd82b23907",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Approximate true end effector location"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b810b023-b51d-48ed-9c34-6cf894e57b75",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "id": "42a48c1d-71a6-4a08-8a21-cdb6d7d6973b",
   "metadata": {
    "tags": []
   },
   "source": [
    "## `t = end_time * 0.4`"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e7842e83-ee9a-4b2f-8307-cb79c5f43d1d",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Desired $q$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "796d39bd-36f0-4004-96bc-4840d6d8eabd",
   "metadata": {},
   "outputs": [],
   "source": [
    "t4_q = q = [\n",
    "    -0.8343593302307178,\n",
    "    -0.5113238160486787,\n",
    "    1.15297441506536,\n",
    "    -3.036362269755852,\n",
    "    3.729384595352719,\n",
    "    1.105484178182587,\n",
    "    -4.152855198300328,\n",
    "]"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9112bfa6-a500-486f-9824-6e386998777e",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Relevant end effector location"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "516b85e0-8d70-4891-b44d-95ad23af2b0e",
   "metadata": {},
   "outputs": [],
   "source": [
    "plant.SetPositions(plant_context, q)\n",
    "X_W_SP_SW = plant.EvalBodyPoseInWorld(plant_context, plant.GetBodyByName(\"main_end_effector_body\"))\n",
    "X_panda_SP_HW = panda.panda_config.X_panda_SP_hw(X_W_SP_SW)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "75d80706-af25-4ecf-8c7e-0b51994c6687",
   "metadata": {},
   "outputs": [],
   "source": [
    "X_panda_SP_HW.translation()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "78d697b9-2c4e-402f-bc6b-e7e850e6f961",
   "metadata": {},
   "outputs": [],
   "source": [
    "quat_ = X_panda_SP_HW.rotation().ToQuaternion()\n",
    "print(\"w: {}\\nx: {}\\ny: {}\\nz: {}\".format(quat_.w(), quat_.x(), quat_.y(), quat_.z()))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "662e57ca-d409-43af-918a-85520fb2ffba",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Approximate real $q$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "114b545b-175f-4496-bc64-f781035e5d7a",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "id": "b959a806-8477-42e6-800f-aee6ae00dab6",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Approximate true end effector location"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8bbf6764-c57e-446e-9570-a2e51ffc6b4d",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "id": "d5504417-a8da-49fb-a93a-19e27aab3e0d",
   "metadata": {
    "tags": []
   },
   "source": [
    "## `t = end_time * 0.5`"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "4be82d0e-60c9-4af1-aa5e-12c198f7a4b4",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Desired $q$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fca5dd72-16b1-4b72-81b5-1121fc64fb80",
   "metadata": {},
   "outputs": [],
   "source": [
    "t5_q = q = [\n",
    "    -0.6739151219982793,\n",
    "    -0.8307647774224596,\n",
    "    1.0289509907170764,\n",
    "    -3.056506301627166,\n",
    "    3.7113407918053385,\n",
    "    1.5572295627007329,\n",
    "    -4.24757843211469,\n",
    "]"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "73c73f43-6920-4de1-80a5-495e266dbe49",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Relevant end effector location"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "996d06b5-697f-432b-8335-96a584afa6b1",
   "metadata": {},
   "outputs": [],
   "source": [
    "plant.SetPositions(plant_context, q)\n",
    "X_W_SP_SW = plant.EvalBodyPoseInWorld(plant_context, plant.GetBodyByName(\"main_end_effector_body\"))\n",
    "X_panda_SP_HW = panda.panda_config.X_panda_SP_hw(X_W_SP_SW)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7d253294-71a8-4f81-b689-e8deb10ce7ed",
   "metadata": {},
   "outputs": [],
   "source": [
    "X_panda_SP_HW.translation()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "857e9df9-bc30-4700-acc0-6182d3049dd1",
   "metadata": {},
   "outputs": [],
   "source": [
    "quat_ = X_panda_SP_HW.rotation().ToQuaternion()\n",
    "print(\"w: {}\\nx: {}\\ny: {}\\nz: {}\".format(quat_.w(), quat_.x(), quat_.y(), quat_.z()))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b436abc7-7547-4387-9e46-5347d1bcd466",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Approximate real $q$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2e445839-4295-4fc9-89fd-66dda68d3831",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "id": "3332e7fb-eb5e-443c-baad-ff08c8c72428",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Approximate true end effector location"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6a3b91f8-20ae-4ba0-8c41-d4ec7e803a4d",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "id": "2fd7fdd0-2b0e-42a5-ba34-91b6f2116399",
   "metadata": {
    "tags": []
   },
   "source": [
    "## `t = end_time * 0.6`"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "a5602cda-46f4-4842-996f-882cb868c30a",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Desired $q$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0cd23fd5-9c10-43b3-98e2-6dfc596e6fab",
   "metadata": {},
   "outputs": [],
   "source": [
    "t6_q = q = [\n",
    "    -0.6374298041563494,\n",
    "    -1.051983578537568,\n",
    "    0.9552403158630598,\n",
    "    -3.0243824117891935,\n",
    "    3.593330117640885,\n",
    "    1.9333055716922152,\n",
    "    -4.325931578828055,\n",
    "]"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "66d32680-a06e-48d9-a3f8-68e53a05d1e0",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Relevant end effector location"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ac7d81c1-a982-4793-8f43-20009040c58f",
   "metadata": {},
   "outputs": [],
   "source": [
    "plant.SetPositions(plant_context, q)\n",
    "X_W_SP_SW = plant.EvalBodyPoseInWorld(plant_context, plant.GetBodyByName(\"main_end_effector_body\"))\n",
    "X_panda_SP_HW = panda.panda_config.X_panda_SP_hw(X_W_SP_SW)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2e441d32-8616-4052-91ea-5de377722ed8",
   "metadata": {},
   "outputs": [],
   "source": [
    "X_panda_SP_HW.translation()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "914c34fa-3f53-4a17-b78a-6a796f83177d",
   "metadata": {},
   "outputs": [],
   "source": [
    "quat_ = X_panda_SP_HW.rotation().ToQuaternion()\n",
    "print(\"w: {}\\nx: {}\\ny: {}\\nz: {}\".format(quat_.w(), quat_.x(), quat_.y(), quat_.z()))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "835d5527-02ca-4472-a94c-3a5125f1e3c6",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Approximate real $q$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b31241f8-0cde-4182-80ed-f271ee381382",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "id": "8d8dca33-33ed-4eac-b7bc-519aaeec60e7",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Approximate true end effector location"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fe2bc677-cde1-45c6-b9b6-97375a5e8034",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "id": "17a1b3a3-5c78-496c-9dcf-ee40ad8dff0a",
   "metadata": {
    "tags": []
   },
   "source": [
    "## `t = end_time * 0.7`"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "71c64626-7c22-4d20-be3a-6d7cfaa49ef0",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Desired $q$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9bc76ace-c7ed-4eb5-ac90-c8f4ef82c5d7",
   "metadata": {},
   "outputs": [],
   "source": [
    "t7_q = q = [\n",
    "    -0.6985304670801346,\n",
    "    -1.2241447167059634,\n",
    "    0.9516565461351049,\n",
    "    -2.945916103740626,\n",
    "    3.421652194372958,\n",
    "    2.2480143731924884,\n",
    "    -4.298986656717967,\n",
    "]"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "fec7e495-6d92-4266-8b61-5d62fd95bca0",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Relevant end effector location"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "54616e74-3193-4796-b011-c352b7787042",
   "metadata": {},
   "outputs": [],
   "source": [
    "plant.SetPositions(plant_context, q)\n",
    "X_W_SP_SW = plant.EvalBodyPoseInWorld(plant_context, plant.GetBodyByName(\"main_end_effector_body\"))\n",
    "X_panda_SP_HW = panda.panda_config.X_panda_SP_hw(X_W_SP_SW)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "968efd01-808b-48d2-a39f-ad4fdc99e1d6",
   "metadata": {},
   "outputs": [],
   "source": [
    "X_panda_SP_HW.translation()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ee3b8188-8703-45a7-9f41-5b4d52c805e3",
   "metadata": {},
   "outputs": [],
   "source": [
    "quat_ = X_panda_SP_HW.rotation().ToQuaternion()\n",
    "print(\"w: {}\\nx: {}\\ny: {}\\nz: {}\".format(quat_.w(), quat_.x(), quat_.y(), quat_.z()))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "0ac986b4-d553-4708-b442-9dd6008b1a0c",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Approximate real $q$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a0cef6e2-a270-4cf9-b607-28a9387d8436",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "id": "63cba08f-4cae-4b1f-b134-e84a304a9d56",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Approximate true end effector location"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "71467693-26ee-4340-b1fe-7e9c46beaf31",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "id": "a0f7601e-1c1e-4ab0-8282-703e43f04fd4",
   "metadata": {
    "tags": []
   },
   "source": [
    "## `t = end_time * 0.8`"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "236dd821-be0f-4ad1-8495-ecc30d3536a6",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Desired $q$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "35e04d44-195c-47a8-b1d6-67401e7d92f1",
   "metadata": {},
   "outputs": [],
   "source": [
    "t8_q = q = [\n",
    "    -0.791058996058301,\n",
    "    -1.3340007077373213,\n",
    "    0.9761776658876062,\n",
    "    -2.8500472338816167,\n",
    "    3.166709249486946,\n",
    "    2.459191495106102,\n",
    "    -4.167089374724142,\n",
    "]"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "32b6c17d-ae5b-447c-a1e5-9ff00f8df767",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Relevant end effector location"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d0204dc0-11f7-4f61-b22b-22b6dc817d30",
   "metadata": {},
   "outputs": [],
   "source": [
    "plant.SetPositions(plant_context, q)\n",
    "X_W_SP_SW = plant.EvalBodyPoseInWorld(plant_context, plant.GetBodyByName(\"main_end_effector_body\"))\n",
    "X_panda_SP_HW = panda.panda_config.X_panda_SP_hw(X_W_SP_SW)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fe0472bb-2d22-4ca8-95af-ebffbe5ab85d",
   "metadata": {},
   "outputs": [],
   "source": [
    "X_panda_SP_HW.translation()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c439a96c-fa88-4e60-8ee4-dd7632749f97",
   "metadata": {},
   "outputs": [],
   "source": [
    "quat_ = X_panda_SP_HW.rotation().ToQuaternion()\n",
    "print(\"w: {}\\nx: {}\\ny: {}\\nz: {}\".format(quat_.w(), quat_.x(), quat_.y(), quat_.z()))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "db6e4e69-565a-406b-a9f4-0053c838531f",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Approximate real $q$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "aedd7eb3-6c56-41dd-87aa-82d4ef070cdd",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "id": "ebc9ee10-f80c-4edd-9be0-a1e3f1f95180",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Approximate true end effector location"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c81eb3c5-76a8-4f66-b481-e90b544def6d",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "id": "9311e906-ec8d-4f43-a8c5-0529877858cf",
   "metadata": {
    "tags": []
   },
   "source": [
    "## `t = end_time * 0.9`"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "fcbca73c-2c2b-403d-bf73-ee6dbf81ccd7",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Desired $q$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "67dfe42a-a2d9-44b4-9f61-9b114495a07d",
   "metadata": {},
   "outputs": [],
   "source": [
    "t9_q = q = [\n",
    "    -0.8688123684145203,\n",
    "    -1.3838866414859068,\n",
    "    0.9972242249030413,\n",
    "    -2.7682700094431887,\n",
    "    2.842237591892566,\n",
    "    2.558597770370147,\n",
    "    -3.987070127648293,\n",
    "]"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f879d104-401a-4dd8-abf1-d08185a32525",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Relevant end effector location"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a60a68e7-46dd-40ef-b623-893127c7e8c5",
   "metadata": {},
   "outputs": [],
   "source": [
    "plant.SetPositions(plant_context, q)\n",
    "X_W_SP_SW = plant.EvalBodyPoseInWorld(plant_context, plant.GetBodyByName(\"main_end_effector_body\"))\n",
    "X_panda_SP_HW = panda.panda_config.X_panda_SP_hw(X_W_SP_SW)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "27630022-1262-4262-8d2d-8cd53c08dd80",
   "metadata": {},
   "outputs": [],
   "source": [
    "X_panda_SP_HW.translation()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "610f3290-eba9-4581-9716-686b82fcf128",
   "metadata": {},
   "outputs": [],
   "source": [
    "quat_ = X_panda_SP_HW.rotation().ToQuaternion()\n",
    "print(\"w: {}\\nx: {}\\ny: {}\\nz: {}\".format(quat_.w(), quat_.x(), quat_.y(), quat_.z()))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8e88c13e-31f5-4738-99e0-d0331b171a9d",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Approximate real $q$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5e045263-0971-4708-a8c6-993e5c92d067",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "id": "e191a56c-e6b1-4efa-a65e-70958bcaf433",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Approximate true end effector location"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4e1a054f-6026-4711-ae2c-008eee61d033",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "id": "cd9f0bbe-905e-416f-bbea-66c247033385",
   "metadata": {
    "tags": []
   },
   "source": [
    "## `t = end_time`"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "6ff0c59d-9f3d-45da-a275-145d5bfa4d10",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Desired $q$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fc93cb24-0f0d-4d9c-9aa4-ecc6178af6dc",
   "metadata": {},
   "outputs": [],
   "source": [
    "t_end_q = q = [\n",
    "    -0.9404502731817744,\n",
    "    -1.4239662327751528,\n",
    "    1.0110803170708464,\n",
    "    -2.717567209177025,\n",
    "    2.510573875661588,\n",
    "    2.5518798052855085,\n",
    "    -3.7958517758859918,\n",
    "]"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "602c360e-57f7-4bbf-803b-86d5f0454ca9",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Relevant end effector location"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f613f519-cacf-4d72-af01-0addb1813706",
   "metadata": {},
   "outputs": [],
   "source": [
    "plant.SetPositions(plant_context, q)\n",
    "X_W_SP_SW = plant.EvalBodyPoseInWorld(plant_context, plant.GetBodyByName(\"main_end_effector_body\"))\n",
    "X_panda_SP_HW = panda.panda_config.X_panda_SP_hw(X_W_SP_SW)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a9ce7b1a-4bcb-4730-a41f-78b3ff8c32c8",
   "metadata": {},
   "outputs": [],
   "source": [
    "X_panda_SP_HW.translation()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9e2e1dde-a593-449d-81cd-f3b1cdde5293",
   "metadata": {},
   "outputs": [],
   "source": [
    "quat_ = X_panda_SP_HW.rotation().ToQuaternion()\n",
    "print(\"w: {}\\nx: {}\\ny: {}\\nz: {}\".format(quat_.w(), quat_.x(), quat_.y(), quat_.z()))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "98d8f15f-1144-4462-bce0-27514fd3e81d",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Approximate real $q$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e93e54d4-1eed-4836-a667-5424f2f899e0",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "id": "155da739-d183-4bd0-9759-ef5ddcae293a",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Approximate true end effector location"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "29590f6a-e6bd-4969-975c-4575c830e5f1",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "id": "5bbb4e06",
   "metadata": {
    "tags": []
   },
   "source": [
    "# Generate trajectory"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2c6fd91e-e496-4e21-a224-8a347784fd96",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "id": "3a98f371",
   "metadata": {},
   "source": [
    "## Sanity check speed"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5731f9f4",
   "metadata": {},
   "outputs": [],
   "source": [
    "end_time = 180\n",
    "desired_radius=constants.nominal_sys_consts.w_L/2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a5e43e80",
   "metadata": {},
   "outputs": [],
   "source": [
    "setpoint_gen = CircularSetpointGenerator(\n",
    "    desired_radius=desired_radius, \n",
    "    end_time=end_time,\n",
    "    sys_consts=constants.nominal_sys_consts\n",
    ")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0a3b64b4",
   "metadata": {},
   "outputs": [],
   "source": [
    "angular_vel = np.pi/end_time\n",
    "tangential_vel = angular_vel*desired_radius\n",
    "tangential_vel"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b3781faa",
   "metadata": {},
   "outputs": [],
   "source": [
    "times = np.arange(0, end_time, 0.1)\n",
    "x0s = np.zeros((len(times), 6))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1e11a435",
   "metadata": {},
   "outputs": [],
   "source": [
    "for i, t in enumerate(times):\n",
    "    x0s[i,:] = setpoint_gen._calc_x0(t)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "09e203ec",
   "metadata": {},
   "outputs": [],
   "source": [
    "positions = x0s[:,3:]\n",
    "rpys = x0s[:,:3]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b5f7dafc",
   "metadata": {},
   "outputs": [],
   "source": [
    "quaternions = np.zeros((len(times), 4))\n",
    "for i, t in enumerate(times):\n",
    "    quat = RollPitchYaw(rpys[0,:]).ToQuaternion()\n",
    "    quaternions[i,0] = quat.w()\n",
    "    quaternions[i,1] = quat.x()\n",
    "    quaternions[i,2] = quat.y()\n",
    "    quaternions[i,3] = quat.z()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1957b0de",
   "metadata": {},
   "outputs": [],
   "source": [
    "q_ = quat_(quaternions[0,0], quaternions[0,1], quaternions[0,2], quaternions[0,3])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5710a527-33a6-4b2f-8d71-6dd36492df25",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0ac00fa4-821d-4efc-9d9d-75a4cc0d05a0",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "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.9"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}