{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "from Go2Py.robot.fsm import FSM\n",
    "from Go2Py.robot.model import Go2Model\n",
    "from Go2Py.sim.mujoco import Go2Sim\n",
    "import numpy as np"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "robot = Go2Sim()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[0.3868 0.     0.    ]\n",
      "0.3868\n",
      "0.093\n",
      "0.09550000000000002\n",
      "0.213\n",
      "0.213\n"
     ]
    }
   ],
   "source": [
    "model = Go2Model()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The task space position of the robot feet are:\n",
      "{'base_link': array([0.  , 0.  , 0.34]), 'FR_foot': array([ 0.19400515, -0.142     ,  0.0372495 ]), 'FL_foot': array([0.19811581, 0.142     , 0.03813916]), 'RR_foot': array([-0.19303592, -0.142     ,  0.03357947]), 'RL_foot': array([-0.1897387,  0.142    ,  0.0330523])}\n"
     ]
    }
   ],
   "source": [
    "T0 = np.eye(4) \n",
    "T0[2,-1]=0.34\n",
    "q0 = robot.standing_q\n",
    "x0 = model.forwardKinematics(T0, q0)\n",
    "x0 = {key:x[0:3,-1] for key,x in zip(x0.keys(),x0.values())}\n",
    "print('The task space position of the robot feet are:')\n",
    "print(x0)\n",
    "x0 = np.hstack([x0[key] for key in ['FR_foot', 'FL_foot', 'RR_foot', 'RL_foot']])\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 28,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([ 0.00000000e+00, -2.22044605e-16,  2.22044605e-16,  0.00000000e+00,\n",
       "        2.22044605e-16, -4.44089210e-16,  0.00000000e+00,  1.11022302e-16,\n",
       "        0.00000000e+00,  0.00000000e+00, -1.11022302e-16,  0.00000000e+00])"
      ]
     },
     "execution_count": 28,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "model.inverseKinematics(T0,x0)-q0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 52,
   "metadata": {},
   "outputs": [],
   "source": [
    "dT = 1/50.\n",
    "amplitudes = [0.01*(i+1) for i in range(8)]\n",
    "frequencies = [0.5*(i+1) for i in range(40)]\n",
    "durations = []\n",
    "\n",
    "for f in frequencies:\n",
    "    durations.append(len(amplitudes)*[int(f+1)*(2*np.pi/f)])\n",
    "durations = np.array(durations).reshape(-1).tolist()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 55,
   "metadata": {},
   "outputs": [],
   "source": [
    "x = []\n",
    "for i,a in enumerate(amplitudes):\n",
    "    for j,f in enumerate(frequencies):\n",
    "        for N in range(int(durations[i*j]//dT)):\n",
    "            t = N/durations[i*j]\n",
    "            x_s = a*np.cos(2*np.pi*f*t)\n",
    "            y_s = a*np.sin(2*np.pi*f*t)\n",
    "            x.append(x_s)\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import matplotlib.pyplot as = [] # List of Pose\n",
    "\n",
    "\n",
    "(plan, fraction) = move_group.compute_cartesian_path, eef_step = 0.01, jump_threshold = 0.0)"
   ]
  }
 ],
 "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.8.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}