Go2Py_SIM/examples/mujoco_test.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "from Go2Py.sim.mujoco import Go2Sim\n",
    "import numpy as np"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "robot = Go2Sim()\n",
    "robot.standUp()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import mujoco\n",
    "import time\n",
    "q,dq = robot.getJointStates()\n",
    "robot.standUp()\n",
    "for i in range(100000):\n",
    "    q,dq = robot.getJointStates()\n",
    "    tau = 20*np.eye(12)@(robot.q0 - 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": 4,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(array([[ 1.00000000e+00,  0.00000000e+00,  0.00000000e+00,\n",
       "          0.00000000e+00, -3.03599143e-01,  1.39266782e-01,\n",
       "          0.00000000e+00, -3.02750500e-01, -1.51075728e-01,\n",
       "          0.00000000e+00,  0.00000000e+00,  0.00000000e+00,\n",
       "          0.00000000e+00,  0.00000000e+00,  0.00000000e+00,\n",
       "          0.00000000e+00,  0.00000000e+00,  0.00000000e+00],\n",
       "        [ 0.00000000e+00,  1.00000000e+00,  0.00000000e+00,\n",
       "          3.03599143e-01,  0.00000000e+00,  1.94005151e-01,\n",
       "          3.03599143e-01,  5.45551624e-06,  1.35362601e-03,\n",
       "          0.00000000e+00,  0.00000000e+00,  0.00000000e+00,\n",
       "          0.00000000e+00,  0.00000000e+00,  0.00000000e+00,\n",
       "          0.00000000e+00,  0.00000000e+00,  0.00000000e+00],\n",
       "        [ 0.00000000e+00,  0.00000000e+00,  1.00000000e+00,\n",
       "         -1.39266782e-01, -1.94005151e-01,  0.00000000e+00,\n",
       "         -9.27667816e-02, -6.05126024e-04, -1.50144238e-01,\n",
       "          0.00000000e+00,  0.00000000e+00,  0.00000000e+00,\n",
       "          0.00000000e+00,  0.00000000e+00,  0.00000000e+00,\n",
       "          0.00000000e+00,  0.00000000e+00,  0.00000000e+00]]),\n",
       " array([[0.        , 0.        , 0.        , 1.        , 0.        ,\n",
       "         0.        , 1.        , 0.        , 0.        , 0.        ,\n",
       "         0.        , 0.        , 0.        , 0.        , 0.        ,\n",
       "         0.        , 0.        , 0.        ],\n",
       "        [0.        , 0.        , 0.        , 0.        , 1.        ,\n",
       "         0.        , 0.        , 0.99995936, 0.99995936, 0.        ,\n",
       "         0.        , 0.        , 0.        , 0.        , 0.        ,\n",
       "         0.        , 0.        , 0.        ],\n",
       "        [0.        , 0.        , 0.        , 0.        , 0.        ,\n",
       "         1.        , 0.        , 0.00901514, 0.00901514, 0.        ,\n",
       "         0.        , 0.        , 0.        , 0.        , 0.        ,\n",
       "         0.        , 0.        , 0.        ]]))"
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "robot.getSiteJacobian('FR_foot')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "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
}