{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from Go2Py.sim.mujoco import Go2Sim\n",
    "import numpy as np"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "robot = Go2Sim()\n",
    "robot.standUpReset()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import mujoco\n",
    "import time\n",
    "q,dq = robot.getJointStates()\n",
    "robot.standUpReset()\n",
    "for i in range(100000):\n",
    "    state = robot.getJointStates()\n",
    "    tau = 20*np.eye(12)@(robot.q0 - state['q']).reshape(12,1)\n",
    "    robot.setCommands(np.zeros(12), np.zeros(12), np.zeros(12), np.zeros(12), tau)\n",
    "    robot.step()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "n = robot.model.nv\n",
    "full_M = np.zeros((n, n))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import mujoco\n",
    "nv = robot.model.nv\n",
    "M = np.zeros((nv, nv))\n",
    "mujoco.mj_fullM(robot.model, M, robot.data.qM)\n",
    "nle = robot.data.qfrc_bias.reshape(nv,1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "id = mujoco.mj_name2id(robot.model, mujoco.mjtObj.mjOBJ_SITE,'FR_foot')\n",
    "nv = robot.model.nv\n",
    "jacp = np.zeros((3, nv))\n",
    "jacr = np.zeros((3, nv))\n",
    "mujoco.mj_jacSite(robot.model, robot.data, jacp, jacr, id)\n",
    "jacp"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "robot.getSiteJacobian('FR_foot')"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "b1-env",
   "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.18"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}