2024-10-02 07:13:03 +08:00
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"RL policy based on the [SoloParkour: Constrained Reinforcement Learning for Visual Locomotion from Privileged Experience](https://arxiv.org/abs/2409.13678). "
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Flat Ground"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Test In Simulation"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {},
2024-10-03 03:09:24 +08:00
"outputs": [
{
2024-10-04 03:22:46 +08:00
"name": "stdout",
"output_type": "stream",
"text": [
"pygame 2.6.1 (SDL 2.28.4, Python 3.10.12)\n",
"Hello from the pygame community. https://www.pygame.org/contribute.html\n"
2024-10-03 03:09:24 +08:00
]
}
],
2024-10-02 07:13:03 +08:00
"source": [
"from Go2Py.robot.fsm import FSM\n",
2024-10-04 03:22:46 +08:00
"from Go2Py.robot.remote import KeyboardRemote, XBoxRemote\n",
2024-10-02 07:13:03 +08:00
"from Go2Py.robot.safety import SafetyHypervisor\n",
"from Go2Py.sim.mujoco import Go2Sim\n",
"from Go2Py.control.cat import *\n",
"import torch"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {},
2024-10-04 03:22:46 +08:00
"outputs": [],
2024-10-02 07:13:03 +08:00
"source": [
2024-10-10 04:08:54 +08:00
"robot = Go2Sim(dt = 0.001)"
2024-10-02 07:13:03 +08:00
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {},
"outputs": [],
"source": [
2024-10-04 03:22:46 +08:00
"remote = KeyboardRemote() # XBoxRemote() # KeyboardRemote()\n",
2024-10-02 07:13:03 +08:00
"robot.sitDownReset()\n",
"safety_hypervisor = SafetyHypervisor(robot)"
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {},
"outputs": [],
"source": [
"class CaTController:\n",
" def __init__(self, robot, remote, checkpoint):\n",
" self.remote = remote\n",
" self.robot = robot\n",
" self.policy = Policy(checkpoint)\n",
" self.command_profile = CommandInterface()\n",
" self.agent = CaTAgent(self.command_profile, self.robot)\n",
2024-10-03 03:09:24 +08:00
" self.hist_data = {}\n",
2024-10-02 07:13:03 +08:00
"\n",
" def init(self):\n",
" self.obs = self.agent.reset()\n",
" self.policy_info = {}\n",
" self.command_profile.yaw_vel_cmd = 0.0\n",
" self.command_profile.x_vel_cmd = 0.0\n",
" self.command_profile.y_vel_cmd = 0.0\n",
"\n",
" def update(self, robot, remote):\n",
2024-10-10 04:08:54 +08:00
" if not hasattr(self, \"obs\"):\n",
" self.init()\n",
2024-10-04 03:22:46 +08:00
" commands = remote.getCommands()\n",
" self.command_profile.yaw_vel_cmd = -commands[2]\n",
" self.command_profile.x_vel_cmd = max(commands[1] * 0.5, -0.3)\n",
" self.command_profile.y_vel_cmd = -commands[0]\n",
"\n",
2024-10-02 07:13:03 +08:00
" action = self.policy(self.obs, self.policy_info)\n",
2024-10-03 03:09:24 +08:00
" self.obs, self.ret, self.done, self.info = self.agent.step(action)\n",
" for key, value in self.info.items():\n",
" if key in self.hist_data:\n",
" self.hist_data[key].append(value)\n",
" else:\n",
" self.hist_data[key] = [value]"
2024-10-02 07:13:03 +08:00
]
},
{
"cell_type": "code",
2024-10-10 04:08:54 +08:00
"execution_count": 23,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"{'q': array([-0.00773064, 0.80945404, -1.57698752, 0.00496356, 0.79965704,\n",
" -1.58089532, -0.02255189, 0.72332114, -1.5933249 , 0.02528539,\n",
" 0.71221541, -1.59029358]),\n",
" 'dq': array([-0.00111779, -0.00300811, -0.0018302 , 0.00127752, -0.00336594,\n",
" -0.00191152, 0.00043393, 0.00364245, 0.00186821, -0.00060087,\n",
" 0.00334342, 0.00158933]),\n",
" 'tau_est': array([[ 1.16278188],\n",
" [-4.66026962],\n",
" [ 2.45516649],\n",
" [-0.74817567],\n",
" [-4.80651708],\n",
" [ 2.18850108],\n",
" [ 3.3815474 ],\n",
" [ 6.51408279],\n",
" [ 8.59169141],\n",
" [-3.79109617],\n",
" [ 6.29146412],\n",
" [ 8.69463319]])}"
]
},
"execution_count": 23,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"robot.getJointStates()"
]
},
{
"cell_type": "code",
"execution_count": 9,
2024-10-02 07:13:03 +08:00
"metadata": {},
"outputs": [
{
2024-10-04 03:22:46 +08:00
"name": "stdout",
2024-10-02 07:13:03 +08:00
"output_type": "stream",
"text": [
2024-10-10 04:08:54 +08:00
"Exported model has been tested with ONNXRuntime, and the result looks good!\n",
2024-10-04 03:22:46 +08:00
"p_gains: [20. 20. 20. 20. 20. 20. 20. 20. 20. 20. 20. 20.]\n"
2024-10-02 07:13:03 +08:00
]
},
{
2024-10-04 03:22:46 +08:00
"name": "stderr",
2024-10-02 07:13:03 +08:00
"output_type": "stream",
"text": [
2024-10-10 04:08:54 +08:00
"/home/Go2py/Go2Py/control/cat.py:100: FutureWarning: You are using `torch.load` with `weights_only=False` (the current default value), which uses the default pickle module implicitly. It is possible to construct malicious pickle data which will execute arbitrary code during unpickling (See https://github.com/pytorch/pytorch/blob/main/SECURITY.md#untrusted-models for more details). In a future release, the default value for `weights_only` will be flipped to `True`. This limits the functions that could be executed during unpickling. Arbitrary objects will no longer be allowed to be loaded via this mode unless they are explicitly allowlisted by the user via `torch.serialization.add_safe_globals`. We recommend you start setting `weights_only=True` for any use case where you don't have full control of the loaded file. Please open an issue on GitHub for any issues related to this experimental feature.\n",
2024-10-04 03:22:46 +08:00
" actor_sd = torch.load(checkpoint_path, map_location=\"cpu\")\n"
2024-10-02 07:13:03 +08:00
]
2024-10-10 04:08:54 +08:00
},
{
"name": "stdout",
"output_type": "stream",
"text": [
"[ 0.00918089 0.63971192 -1.48127012 -0.01194117 0.66272462 -1.50595971\n",
" 0.02226374 0.53299185 -1.39982229 -0.01784526 0.52746472 -1.37062384] [[0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0.]]\n"
]
2024-10-02 07:13:03 +08:00
}
],
"source": [
"from Go2Py import ASSETS_PATH \n",
"import os\n",
2024-10-10 04:08:54 +08:00
"#checkpoint_path = os.path.join(ASSETS_PATH, 'checkpoints/SoloParkour/go2_flat.pt')\n",
"#checkpoint_path = os.path.join(ASSETS_PATH, 'checkpoints/SoloParkour/foot_contact_rate_2_max_e1500_06-23-26-56.pt')\n",
"#checkpoint_path = os.path.join(ASSETS_PATH, 'checkpoints/SoloParkour/trainparamsconfigmax_epochs1500_taskenvlearnlimitsfoot_contact_force_rate60_soft_07-20-22-43.pt')\n",
"#checkpoint_path = os.path.join(ASSETS_PATH, 'checkpoints/SoloParkour/taskenvlearnconstraints_CaTfeetAirTimeConstraint020_taskenvlearnconstraints_CaTfeetMaxAirTimeConstraint025_08-03-50-21.pt')\n",
"checkpoint_path = os.path.join(ASSETS_PATH, 'checkpoints/SoloParkour/vel_deadzone_02_09-21-04-26.pt')\n",
2024-10-02 07:13:03 +08:00
"controller = CaTController(robot, remote, checkpoint_path)\n",
2024-10-10 04:08:54 +08:00
"decimation = 20\n",
2024-10-04 03:22:46 +08:00
"fsm = FSM(robot, remote, safety_hypervisor, control_dT=decimation * robot.dt, user_controller_callback=controller.update)"
2024-10-02 07:13:03 +08:00
]
},
{
"cell_type": "code",
2024-10-10 04:08:54 +08:00
"execution_count": 6,
2024-10-02 07:13:03 +08:00
"metadata": {},
"outputs": [],
"source": [
2024-10-04 03:22:46 +08:00
"remote.x_vel_cmd=0.0\n",
"remote.y_vel_cmd=0.0\n",
"remote.yaw_vel_cmd = 0.0"
2024-10-02 07:13:03 +08:00
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Pressing `u` on the keyboard will make the robot stand up. This is equivalent to the `L2+A` combo of the Go2 builtin state machine. After the the robot is on its feet, pressing `s` will hand over the control the RL policy. This action is equivalent to the `start` key of the builtin controller. When you want to stop, pressing `u` again will act similarly to the real robot and locks it in standing mode. Finally, pressing `u` again will command the robot to sit down."
]
},
{
"cell_type": "code",
2024-10-10 04:08:54 +08:00
"execution_count": 6,
2024-10-02 07:13:03 +08:00
"metadata": {},
"outputs": [],
"source": [
"fsm.close()"
]
},
2024-10-04 03:22:46 +08:00
{
"cell_type": "code",
2024-10-10 04:08:54 +08:00
"execution_count": 7,
2024-10-04 03:22:46 +08:00
"metadata": {},
2024-10-10 04:08:54 +08:00
"outputs": [
{
"data": {
"image/png": "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
"text/plain": [
"<Figure size 1500x500 with 3 Axes>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"import matplotlib.pyplot as plt\n",
"# Assuming 'controller.hist_data[\"torques\"]' is a dictionary with torque profiles\n",
"torques = np.array(controller.hist_data[\"body_linear_vel\"])[:, 0, :, 0]\n",
"\n",
"# Number of torque profiles\n",
"torque_nb = torques.shape[1]\n",
"\n",
"# Number of rows needed for the grid, with 3 columns per row\n",
"n_cols = 3\n",
"n_rows = int(np.ceil(torque_nb / n_cols))\n",
"\n",
"# Create the figure and axes for subplots\n",
"fig, axes = plt.subplots(n_rows, n_cols, figsize=(15, 5 * n_rows))\n",
"\n",
"# Flatten the axes array for easy indexing (in case of multiple rows)\n",
"axes = axes.flatten()\n",
"\n",
"# Plot each torque profile\n",
"for i in range(torque_nb):\n",
" axes[i].plot(np.arange(torques.shape[0]) * robot.dt * decimation, torques[:, i])\n",
" axes[i].set_title(f'Torque {i+1}')\n",
" axes[i].set_xlabel('Time')\n",
" axes[i].set_ylabel('Torque Value')\n",
" axes[i].grid(True)\n",
"\n",
"# Remove any empty subplots if torque_nb is not a multiple of 3\n",
"for j in range(torque_nb, len(axes)):\n",
" fig.delaxes(axes[j])\n",
"\n",
"# Adjust layout\n",
"plt.tight_layout()\n",
"plt.savefig(\"torque_profile.png\")\n",
"plt.show()"
]
2024-10-04 03:22:46 +08:00
},
{
"cell_type": "code",
2024-10-10 04:08:54 +08:00
"execution_count": 8,
2024-10-04 03:22:46 +08:00
"metadata": {},
"outputs": [
{
"data": {
2024-10-10 04:08:54 +08:00
"image/png": "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
2024-10-04 03:22:46 +08:00
"text/plain": [
"<Figure size 1500x2000 with 12 Axes>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"import matplotlib.pyplot as plt\n",
"# Assuming 'controller.hist_data[\"torques\"]' is a dictionary with torque profiles\n",
"torques = np.array(controller.hist_data[\"torques\"])\n",
"\n",
"# Number of torque profiles\n",
"torque_nb = torques.shape[1]\n",
"\n",
"# Number of rows needed for the grid, with 3 columns per row\n",
"n_cols = 3\n",
"n_rows = int(np.ceil(torque_nb / n_cols))\n",
"\n",
"# Create the figure and axes for subplots\n",
"fig, axes = plt.subplots(n_rows, n_cols, figsize=(15, 5 * n_rows))\n",
"\n",
"# Flatten the axes array for easy indexing (in case of multiple rows)\n",
"axes = axes.flatten()\n",
"\n",
"# Plot each torque profile\n",
"for i in range(torque_nb):\n",
2024-10-10 04:08:54 +08:00
" axes[i].plot(np.arange(torques.shape[0]) * robot.dt * decimation, torques[:, i])\n",
2024-10-04 03:22:46 +08:00
" axes[i].set_title(f'Torque {i+1}')\n",
" axes[i].set_xlabel('Time')\n",
" axes[i].set_ylabel('Torque Value')\n",
2024-10-10 04:08:54 +08:00
" axes[i].grid(True)\n",
2024-10-04 03:22:46 +08:00
"\n",
"# Remove any empty subplots if torque_nb is not a multiple of 3\n",
"for j in range(torque_nb, len(axes)):\n",
" fig.delaxes(axes[j])\n",
"\n",
"# Adjust layout\n",
"plt.tight_layout()\n",
"plt.savefig(\"torque_profile.png\")\n",
"plt.show()"
]
},
{
"cell_type": "code",
2024-10-10 04:08:54 +08:00
"execution_count": 9,
2024-10-04 03:22:46 +08:00
"metadata": {},
"outputs": [
{
"data": {
2024-10-10 04:08:54 +08:00
"image/png": "iVBORw0KGgoAAAANSUhEUgAABc8AAAfGCAYAAADBfkZbAAAAOXRFWHRTb2Z0d2FyZQBNYXRwbG90bGliIHZlcnNpb24zLjkuMiwgaHR0cHM6Ly9tYXRwbG90bGliLm9yZy8hTgPZAAAACXBIWXMAAA9hAAAPYQGoP6dpAAEAAElEQVR4nOy9eZwdVZ3+/9Td7+29k04nISELexAIomJkETQScAH8DW4zIwOKC36ZGcFxwUFAREEHkVFRHBRRxgVnXEeRLRDWsIbIloSE7Et3pzu99+271u+Pe8+pc2/fpc5S1Us+79erX4Tu231u1a2qU/V8nvN8LNu2bRAEQRAEQRAEQRAEQRAEQRAEwQlM9hsgCIIgCIIgCIIgCIIgCIIgiKkGiecEQRAEQRAEQRAEQRAEQRAEUQaJ5wRBEARBEARBEARBEARBEARRBonnBEEQBEEQBEEQBEEQBEEQBFEGiecEQRAEQRAEQRAEQRAEQRAEUQaJ5wRBEARBEARBEARBEARBEARRBonnBEEQBEEQBEEQBEEQBEEQBFEGiecEQRAEQRAEQRAEQRAEQRAEUQaJ5wRBEARBEARBEARBEARBEARRBonnBDHNufPOO2FZFrZv3z7Zb8U427dvh2VZuPPOO1293rIsXHvttZ6+J4IgCIJQgeZrB5qvCYIgiKkKzdcONF8TRAESzwliisAm6eeee863MV999VVce+21rm8Mrr32WliWxb8SiQSWLVuGq666CkNDQ96+2SL33HPPlJvA9+3bhy996Us488wz0dTUBMuysGbNmsl+WwRBEIQH0Hztjqk4X69evRof+9jHcOSRRyKRSGDp0qW45JJLsG/fvsl+awRBEIRhaL52x1Scrx999FGce+65WLhwIWKxGObOnYuzzz4bTzzxxGS/NeIgJTTZb4AgCD0++tGP4sMf/jCi0aj077766qv46le/ijPOOAOLFy92/Xs//OEP0djYiJGREdx///34+te/joceeghPPPEELMuSfh/VWLRoEZLJJMLhMP/ePffcg1tvvbXiBJ9MJhEK+X9Z27RpE775zW/iiCOOwHHHHYe1a9f6/h4IgiCIqQ3N1w6TNV9/8YtfxIEDB/CBD3wARxxxBLZu3Yrvf//7+POf/4z169dj7ty5vr8ngiAIYmpB87XDZM3Xr732GgKBAD796U9j7ty56O/vx3//93/j9NNPx1/+8hecffbZvr8n4uCGxHOCmOYEg0EEg0Ffx7zgggswe/ZsAMCnP/1p/N3f/R1+97vf4amnnsKKFSuMjWNZFmKxmOvXy7zWJCeddBL6+vrQ3t6O//3f/8UHPvCBSXkfBEEQxNSF5muHyZqvb775Zpx66qkIBJzFt2effTbe/va34/vf/z6uv/76SXlfBEEQxNSB5muHyZqvL7nkElxyySUl3/vMZz6DpUuX4pZbbiHxnPAdim0hiCnMQw89hNNOOw0NDQ1obW3Feeedhw0bNpS8plIm2+LFi/He974Xjz/+ON7ylrcgFoth6dKl+PnPf17ye0zkPfPMM/lSMZW4kXe84x0AgG3btgEARkdH8bnPfQ4LFy5ENBrFUUcdhZtuugm2bZf83gMPPIBTTz0Vra2taGxsxFFHHYUvf/nL/OflmWwXXXQRbr31VgAoWd7GqJTJ9sILL+Ccc85Bc3MzGhsb8c53vhNPPfVUxX34xBNP4IorrkBHRwcaGhrw/ve/H/v376+7/U1NTWhvb3e3swiCIIgZB83X02O+Pv3000uEc/a99vb2CZ8XQRAEMfOg+Xp6zNeVSCQS6OjowMDAgNLvE4QO5DwniCnKgw8+iHPOOQdLly7Ftddei2Qyie9973s45ZRTsG7durrLwLZs2YILLrgAH//4x/FP//RPuOOOO3DRRRfhpJNOwrHHHovTTz8d//Iv/4Lvfve7+PKXv4xjjjkGAPh/ZXj99dcBALNmzYJt2zj33HPx8MMP4+Mf/ziWL1+O++67D5///OexZ88efOc73wEAvPLKK3jve9+L448/Htdddx2i0Si2bNlSM8fsU5/6FPbu3YsHHngAd911V9339corr+C0005Dc3MzvvCFLyAcDuNHP/oRzjjjDDzyyCM4+eSTS17/z//8z2hra8M111yD7du345ZbbsFll12Gu+++W3qfEARBEAcHNF9PZDrN1yMjIxgZGeGOP4IgCGJmQvP1RKb6fD00NIR0Oo3e3l78/Oc/x8svv1xSDCAI37AJgpgS/PSnP7UB2M8++6xt27a9fPlye86cOXZfXx9/zd/+9jc7EAjYF1544YTf27ZtG//eokWLbAD2o48+yr/X09NjR6NR+3Of+xz/3v/8z//YAOyHH37Y1Xu85pprbAD2pk2b7P3799vbtm2zf/SjH9nRaNTu7Oy0R0dH7T/84Q82APv6668v+d0LLrjAtizL3rJli23btv2d73zHBmDv37+/6njbtm2zAdg//elP+ff+3//7f3a1SxcA+5prruH/f/7559uRSMR+/fXX+ff27t1rNzU12aeffjr/HtuHK1eutPP5PP/+5ZdfbgeDQXtgYMDV/rFt+X1KEARBTC9ovp7IdJyvGV/72tdsAPbq1aulf5cgCIKYutB8PZHpNl+vWrXKBmADsCORiP2pT33KTiaTrn6XIExCsS0EMQXZt28f1q9fj4suuqgkDuT444/Hu971Ltxzzz11/8ayZctw2mmn8f/v6OjAUUcdha1bt2q/v6OOOgodHR1YsmQJPvWpT+Hwww/HX/7yFyQSCdxzzz0IBoP4l3/5l5Lf+dznPgfbtvHXv/4VANDa2goA+OMf/4h8Pq/9nsrJ5XK4//77cf7552Pp0qX8+/PmzcPf//3f4/HHH5/QwfyTn/xkyTK10047DblcDjt27DD+/giCIIjpD83X+kzmfP3oo4/iq1/9Kj74wQ/yJfIEQRDEzIPma30mY76+8cYbcf/99+MnP/kJ3vrWtyKdTiObzZrZIIKQgMRzgpiCsMnkqKOOmvCzY445Br29vRgdHa35Nw499NAJ32tra0N/f7/2+/vtb3+LBx54AGvWrMGWLVvw8ssv46STTuLvff78+WhqaprwvtnPAeBDH/oQTjnlFFxyySXo7OzEhz/8YfzmN78xNtHv378fY2NjVfdhPp/Hrl27Sr5fvs/a2toAwMg+IwiCIGYeNF/rM1nz9caNG/H+978fb3jDG/DjH/9Y4Z0TBEEQ0wWar/WZjPl6+fLleNe73oWPfexjeOCBB/DMM8/goosuUtsAgtCAMs8JYoZSrUO4XdZURIXTTz9dOxs0Ho/j0UcfxcMPP4y//OUvuPfee3H33XfjHe94B+6//37fO5wD3u4zgiAIgqgEzdfy6O6zXbt24ayzzkJLSwvuueeeCYIEQRAEQZRD87U8JvdZJBLBueeeixtvvBHJZBLxeFz37RGEa8h5ThBTkEWLFgEANm3aNOFnGzduxOzZs9HQ0KA9jriEyhSLFi3C3r17MTw8XPL9jRs38p8zAoEA3vnOd+Lmm2/Gq6++iq9//et46KGH8PDDD2u/546ODiQSiar7MBAIYOHCha7+FkEQBEFUgubr6Tdf9/X14ayzzkIqlcJ9992HefPmGfvbBEEQxNSE5uvpN19XIplMwrbtCfuCILyGxHOCmILMmzcPy5cvx89+9jMMDAzw77/88su4//778e53v9vIOOwGQRxDl3e/+93I5XL4/ve/X/L973znO7AsC+eccw4A4MCBAxN+d/ny5QCAVCpV9e+7fc/BYBBnnXUW/vjHP2L79u38+93d3fjlL3+JU089Fc3NzS62iCAIgiAqQ/P19JqvR0dH8e53vxt79uzBPffcgyOOOMLI3yUIgiCmNjRfT6/5uqenZ8L3BgYG8Nvf/hYLFy7EnDlzjIxDEG6h2BaCmKL8x3/8B8455xysWLECH//4x5FMJvG9730PLS0tuPbaa42MsXz5cgSDQXzzm9/E4OAgotEo3vGOd2hNRu973/tw5pln4t///d+xfft2nHDCCbj//vvxxz/+EZ/97Gdx2GGHAQCuu+46PProo3jPe96DRYsWoaenBz/
2024-10-04 03:22:46 +08:00
"text/plain": [
"<Figure size 1500x2000 with 12 Axes>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"# Extract the joint position data for the first joint over time\n",
"joint_pos = np.array(controller.hist_data[\"joint_pos\"])[:, 0]\n",
"\n",
"# Number of data points in joint_pos\n",
"n_data_points = len(joint_pos)\n",
"\n",
"# Since you're plotting only one joint, no need for multiple subplots in this case.\n",
"# But to follow the grid requirement, we'll replicate the data across multiple subplots.\n",
"# For example, let's assume you want to visualize this data 9 times in a 3x3 grid.\n",
"\n",
"n_cols = 3\n",
"n_rows = int(np.ceil(torque_nb / n_cols))\n",
"\n",
"# Create the figure and axes for subplots\n",
"fig, axes = plt.subplots(n_rows, n_cols, figsize=(15, 5 * n_rows))\n",
"\n",
"# Flatten the axes array for easy indexing (in case of multiple rows)\n",
"axes = axes.flatten()\n",
"\n",
"# Plot the same joint position data in every subplot (as per grid requirement)\n",
"for i in range(n_rows * n_cols):\n",
" axes[i].plot(joint_pos[:, i])\n",
" axes[i].set_title(f'Joint Position {i+1}')\n",
" axes[i].set_xlabel('Time')\n",
" axes[i].set_ylabel('Position Value')\n",
"\n",
"# Adjust layout\n",
"plt.tight_layout()\n",
"plt.savefig(\"joint_position_profile.png\")\n",
"plt.show()"
]
},
{
"cell_type": "code",
2024-10-10 04:08:54 +08:00
"execution_count": 10,
2024-10-04 03:22:46 +08:00
"metadata": {},
2024-10-10 04:08:54 +08:00
"outputs": [
{
"data": {
"image/png": "iVBORw0KGgoAAAANSUhEUgAABc8AAAPeCAYAAADatOK+AAAAOXRFWHRTb2Z0d2FyZQBNYXRwbG90bGliIHZlcnNpb24zLjkuMiwgaHR0cHM6Ly9tYXRwbG90bGliLm9yZy8hTgPZAAAACXBIWXMAAA9hAAAPYQGoP6dpAAEAAElEQVR4nOydeZgU1b3+3957dhi2YSII7saVaELc8UpkMS7RxJCQRKIRc3ONv8R7NZobUQiGaLxermg0ZHFJJN6baDAxLsEVjYiI4oKoqGwCwzbMPtPTS/3+6D5Vp6q3qumz9Ex/P8/DM9MzxZyq6uo6p97znvfrMwzDAEEQBEEQBEEQBEEQBEEQBEEQJn7dO0AQBEEQBEEQBEEQBEEQBEEQ5QaJ5wRBEARBEARBEARBEARBEAThgMRzgiAIgiAIgiAIgiAIgiAIgnBA4jlBEARBEARBEARBEARBEARBOCDxnCAIgiAIgiAIgiAIgiAIgiAckHhOEARBEARBEARBEARBEARBEA5IPCcIgiAIgiAIgiAIgiAIgiAIBySeEwRBEARBEARBEARBEARBEIQDEs8JgiAIgiAIgiAIgiAIgiAIwgGJ5wRBEIOQ++67Dz6fD5s3b1bS3oQJEzBnzhwlbREEQRDEUIT6boIgCIIYXFDfTQAknhMCYDeTXP+uu+46KW2+++67uOmmm1zfwHbu3InrrrsOZ555Jurq6uDz+fD88897bvf555/HhRdeiKamJoTDYYwePRrnnnsuHnnkEc9/ywuPP/44brrpJqltAMAvf/lL3Hfffa63z/e+NzU1ydtJCUyYMAE+nw9Tp07N+ftf//rX5rG99tprivfOPV7fP4IgKpfB0Hc/88wzuPTSS3HYYYehuroaBx10EL7zne9g586dntqlvtsO9d3lBfXdBEG4ZTD03StXrsR5552HcePGIRqNoqmpCdOnT8c///lPT+1S322H+u7ygvruyiOoeweIocOCBQswceJE28+OPvpoKW29++67mD9/PqZMmYIJEyYU3f7999/HLbfcgkMPPRTHHHMMVq1a5bnNG2+8EQsWLMChhx6KK664AgceeCD27duHxx9/HBdddBEefPBBfP3rXx/A0RTn8ccfx1133SW9I//lL3+JkSNHeprp/MIXvoBvfetbtp9VVVUJ3jP5RKNRPPfcc2hpackahDz44IOIRqPo6+vTtHfZfPOb38SsWbMQiUTMnw3k/SMIorIp5777Rz/6EVpbW/GVr3wFhx56KD7++GPceeedeOyxx7Bu3TpXD4zUd+eG+m49UN9NEIQIyrnv/uCDD+D3+/Hd734XTU1N2L9/P/7whz/g9NNPx9///ndMnz696N+gvjs31HfrgfpuAiDxnBDIjBkzcOKJJ+rejZyccMIJ2LdvHxobG/HnP/8ZX/nKVzz9/z//+c9YsGABvvzlL2PZsmUIhULm76655ho89dRTiMfjond7UHDYYYfhG9/4hvC/m0gkkEqlEA6Hhf/tXJxyyilYs2YN/vd//xf/7//9P/Pnn3zyCV588UV86UtfwsMPP6xkX9wQCAQQCAR07wZBEIOccu67b7/9dpx66qnw+62FktOnT8cZZ5yBO++8EwsXLiz4/6nvzg/13XqgvpsgCBGUc9/9ne98B9/5zndsP/ve976Hgw46CIsXLy4qnlPfnR/qu/VAfTcBUGwLoZBnn30Wp512GmpqajBs2DCcf/752LBhQ9Z2b7zxBmbMmIH6+nrU1tbirLPOwiuvvGL+/r777jPF7zPPPNNc1lMohqWurg6NjY0D3vcbbrgBjY2N+N3vfmfrwBnTpk3DF7/4RfP17t27cdlll2HMmDGIRqM47rjjcP/999v+z+bNm+Hz+XDbbbdh6dKlOPjggxGJRPDZz34Wa9asMbebM2cO7rrrLgD25VqM2267DSeffDJGjBiBqqoqnHDCCfjzn/+c8zj+8Ic/4HOf+xyqq6sxfPhwnH766fjHP/4BIL2Eav369XjhhRfMNqZMmTLgczbQc7F48WLzXLz77rsAgPfeew8XX3wxRo0ahaqqKhx++OH4z//8T9vf2L59Oy699FKMGTMGkUgERx11FH73u9+53s9oNIoLL7wQy5Yts/38j3/8I4YPH45p06Zl/Z+33noLc+bMwUEHHWQuS7z00kuxb9++rG2ff/55nHjiiYhGozj44IPxq1/9CjfddJPtvQTS7/GVV16J5cuX4+ijjzaP5cknn7Rt58xeK/T+5Won198AAMMwsHDhQhxwwAGorq7GmWeeifXr1+c8Z21tbfjBD36AcePGIRKJ4JBDDsEtt9yCVCqVc3uCIAYfOvvu008/3Sacs581Njbm3Acn1HcPHOq701DfTRDEYERn352L6upqjBo1Cm1tbUW3pb574FDfnYb6bkIG5DwnhNHe3o69e/fafjZy5EgAwNNPP40ZM2bgoIMOwk033YTe3l4sWbIEp5xyCl5//XVzCdj69etx2mmnob6+Htdeey1CoRB+9atfYcqUKXjhhRcwefJknH766bjqqqtwxx134Mc//jGOPPJIADC/imbjxo147733cOmll6Kurq7o9r29vZgyZQo+/PBDXHnllZg4cSL+9Kc/Yc6cOWhra7PNrgLAsmXL0NnZiSuuuAI+nw+33norLrzwQnz88ccIhUK44oorsGPHDqxYsQK///3vs9r7n//5H5x33nmYPXs2+vv78dBDD+ErX/kKHnvsMZxzzjnmdvPnz8dNN92Ek08+GQsWLEA4HMbq1avx7LPP4uyzz8bixYvx/e9/H7W1tWYHOWbMmKLH29fXl/W+19XVIRKJeD4X9957L/r6+jB37lxEIhE0NjbirbfewmmnnYZQKIS5c+diwoQJ+Oijj/C3v/0NN998MwBg165d+PznP292gKNGjcITTzyByy67DB0dHfjBD35Q9DgA4Otf/zrOPvtsfPTRRzj44IPN9+fLX/5yzsHbihUr8PHHH+Pb3/42mpqasH79eixduhTr16/HK6+8Ynacb7zxBqZPn46xY8di/vz5SCaTWLBgAUaNGpVzP1566SU88sgj+N73voe6ujrccccduOiii7B161aMGDEi5/8Z6PvnZN68eVi4cCFmzpyJmTNn4vXXX8fZZ5+N/v5+23Y9PT0444wzsH37dlxxxRUYP348Xn75ZVx//fXYuXMnFi9e7LltgiDUM9j67q6uLnR1dZn7mA/quwtDfTf13dR3E8TgZTD03R0dHejv78fevXvxwAMP4J133sGPf/zjgv+H+u7CUN9NfTf13RoxCKJE7r33XgNAzn+M448/3hg9erSxb98+82dvvvmm4ff7jW9961vmzy644AIjHA4bH330kfmzHTt2GHV1dcbpp59u/uxPf/qTAcB47rnnPO+v1//76KOPGgCM//7v/3a1/eLFiw0Axh/+8AfzZ/39/cZJJ51k1NbWGh0dHYZhGMamTZsMAMaIESOM1tbWrPb+9re/mT/7t3/7NyPfx7Wnp8f2ur+/3zj66KONf/mXfzF/tnHjRsPv9xtf+tKXjGQyads+lUqZ3x911FHGGWec4eo4DcPI+77fe++9AzoX9fX1xu7du21tnH766UZdXZ2xZcuWvPt92WWXGWPHjjX27t1r22bWrFlGQ0ND1jlycuCBBxrnnHOOkUgkjKamJuOnP/2pYRiG8e677xoAjBdeeMG8ztesWWP+v1x/949//KMBwFi5cqX5s3PPPdeorq42tm/fbv5s48aNRjAYzHpfARjhcNj48MMPzZ+9+eabBgBjyZIl5s/Y/mzatMn8Wb7378Ybb8x5/Tj/xu7du41wOGycc845tvP74x//2ABgXHLJJebPfvrTnxo1NTXGBx98YPub1113nREIBIytW7dmtUcQRPkw2Ppuxk9/+lMDgPHMM88U3I767vxQ3019N/XdBDE4GUx997Rp08x9C4fDxhVXXGH09vYW/D/Ud+eH+m7qu6nv1gvFthDCuOuuu7BixQrbPwDYuXMn1q1bhzlz5tiiU4499lh84QtfwOOPPw4ASCaT+Mc//oELLrgABx10kLnd2LFj8fWvfx0
"text/plain": [
"<Figure size 1500x1000 with 4 Axes>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"import matplotlib.pyplot as plt\n",
"# Assuming 'controller.hist_data[\"foot_contact_forces_mag\"]' is a dictionary with foot contact force magnitudes\n",
"foot_contact_forces_mag = np.array(controller.hist_data[\"foot_contact_forces_mag\"])\n",
"\n",
"# Number of feet (foot_nb)\n",
"foot_nb = foot_contact_forces_mag.shape[1]\n",
"\n",
"# Number of rows needed for the grid, with 3 columns per row\n",
"n_cols = 3\n",
"n_rows = int(np.ceil(foot_nb / n_cols))\n",
"\n",
"# Create the figure and axes for subplots\n",
"fig, axes = plt.subplots(n_rows, n_cols, figsize=(15, 5 * n_rows))\n",
"\n",
"# Flatten the axes array for easy indexing (in case of multiple rows)\n",
"axes = axes.flatten()\n",
"\n",
"# Plot each foot's contact force magnitude\n",
"for i in range(foot_nb):\n",
" axes[i].plot(foot_contact_forces_mag[:, i])\n",
" axes[i].set_title(f'Foot {i+1} Contact Force Magnitude')\n",
" axes[i].set_xlabel('Time')\n",
" axes[i].set_ylabel('Force Magnitude')\n",
"\n",
"# Remove any empty subplots if foot_nb is not a multiple of 3\n",
"for j in range(foot_nb, len(axes)):\n",
" fig.delaxes(axes[j])\n",
"\n",
"# Adjust layout\n",
"plt.tight_layout()\n",
"plt.savefig(\"foot_contact_profile.png\")\n",
"plt.show()"
]
},
{
"cell_type": "code",
"execution_count": 8,
"metadata": {},
"outputs": [
{
"data": {
"image/png": "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
"text/plain": [
"<Figure size 1500x2000 with 12 Axes>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"# Extract the joint acceleration data for the first joint over time\n",
"joint_acc = np.array(controller.hist_data[\"joint_acc\"])[:, 0]\n",
"\n",
"# Number of data points in joint_acc\n",
"n_data_points = len(joint_acc)\n",
"\n",
"# Number of feet (foot_nb)\n",
"foot_nb = joint_acc.shape[1]\n",
"\n",
"# Number of rows needed for the grid, with 3 columns per row\n",
"n_cols = 3\n",
"n_rows = int(np.ceil(foot_nb / n_cols))\n",
"\n",
"# Create the figure and axes for subplots\n",
"fig, axes = plt.subplots(n_rows, n_cols, figsize=(15, 5 * n_rows))\n",
"\n",
"# Flatten the axes array for easy indexing\n",
"axes = axes.flatten()\n",
"\n",
"# Plot the same joint acceleration data in every subplot (as per grid requirement)\n",
"for i in range(n_rows * n_cols):\n",
" axes[i].plot(joint_acc[:, i])\n",
" axes[i].set_title(f'Joint Acceleration {i+1}')\n",
" axes[i].set_xlabel('Time')\n",
" axes[i].set_ylabel('Acceleration Value')\n",
"\n",
"# Adjust layout to prevent overlap\n",
"plt.tight_layout()\n",
"plt.show()"
]
},
{
"cell_type": "code",
"execution_count": 9,
"metadata": {},
"outputs": [
{
"data": {
"image/png": "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
"text/plain": [
"<Figure size 1500x2000 with 12 Axes>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"# Extract the joint jerk data over time\n",
"joint_jerk = np.array(controller.hist_data[\"joint_jerk\"])[:, 0]\n",
"\n",
"# Number of data points in joint_jerk\n",
"n_data_points = len(joint_jerk)\n",
"\n",
"# Number of joints (assuming the second dimension corresponds to joints)\n",
"num_joints = joint_jerk.shape[1]\n",
"\n",
"# Number of columns per row in the subplot grid\n",
"n_cols = 3\n",
"# Number of rows needed for the grid\n",
"n_rows = int(np.ceil(num_joints / n_cols))\n",
"\n",
"# Create the figure and axes for subplots\n",
"fig, axes = plt.subplots(n_rows, n_cols, figsize=(15, 5 * n_rows))\n",
"\n",
"# Flatten the axes array for easy indexing\n",
"axes = axes.flatten()\n",
"\n",
"# Plot the joint jerk data for each joint\n",
"for i in range(num_joints):\n",
" axes[i].plot(joint_jerk[:, i])\n",
" axes[i].set_title(f'Joint Jerk {i+1}')\n",
" axes[i].set_xlabel('Time')\n",
" axes[i].set_ylabel('Jerk Value')\n",
"\n",
"# Hide any unused subplots\n",
"for i in range(num_joints, len(axes)):\n",
" fig.delaxes(axes[i])\n",
"\n",
"# Adjust layout to prevent overlap\n",
"plt.tight_layout()\n",
"plt.show()\n"
]
},
{
"cell_type": "code",
"execution_count": 10,
"metadata": {},
"outputs": [
{
"data": {
"image/png": "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
"text/plain": [
"<Figure size 1500x1000 with 4 Axes>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"# Extract the foot contact rate data over time\n",
"foot_contact_rate = np.array(controller.hist_data[\"foot_contact_rate\"])[:, 0]\n",
"\n",
"# Number of data points in foot_contact_rate\n",
"n_data_points = foot_contact_rate.shape[0]\n",
"\n",
"# Number of feet (assuming the second dimension corresponds to feet)\n",
"num_feet = foot_contact_rate.shape[1]\n",
"\n",
"# Number of columns per row in the subplot grid\n",
"n_cols = 3\n",
"# Number of rows needed for the grid\n",
"n_rows = int(np.ceil(num_feet / n_cols))\n",
"\n",
"# Create the figure and axes for subplots\n",
"fig, axes = plt.subplots(n_rows, n_cols, figsize=(15, 5 * n_rows))\n",
"\n",
"# Flatten the axes array for easy indexing\n",
"axes = axes.flatten()\n",
"\n",
"# Plot the foot contact rate data for each foot\n",
"for i in range(num_feet):\n",
" axes[i].plot(foot_contact_rate[:, i])\n",
" axes[i].set_title(f'Foot Contact Rate {i+1}')\n",
" axes[i].set_xlabel('Time')\n",
" axes[i].set_ylabel('Contact Rate')\n",
"\n",
"# Hide any unused subplots\n",
"for i in range(num_feet, len(axes)):\n",
" fig.delaxes(axes[i])\n",
"\n",
"# Adjust layout to prevent overlap\n",
"plt.tight_layout()\n",
"plt.show()\n"
]
2024-10-04 03:22:46 +08:00
},
2024-10-02 07:13:03 +08:00
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Test on Real Robot (ToDo)"
]
},
{
"cell_type": "code",
2024-10-10 04:08:54 +08:00
"execution_count": 1,
2024-10-02 07:13:03 +08:00
"metadata": {},
2024-10-10 04:08:54 +08:00
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"pygame 2.6.1 (SDL 2.28.4, Python 3.10.12)\n",
"Hello from the pygame community. https://www.pygame.org/contribute.html\n"
]
}
],
2024-10-02 07:13:03 +08:00
"source": [
"from Go2Py.robot.fsm import FSM\n",
2024-10-10 04:08:54 +08:00
"from Go2Py.robot.remote import XBoxRemote\n",
2024-10-02 07:13:03 +08:00
"from Go2Py.robot.safety import SafetyHypervisor\n",
2024-10-10 04:08:54 +08:00
"from Go2Py.control.cat import *"
2024-10-02 07:13:03 +08:00
]
},
{
"cell_type": "code",
2024-10-10 04:08:54 +08:00
"execution_count": 2,
2024-10-02 07:13:03 +08:00
"metadata": {},
"outputs": [],
"source": [
"from Go2Py.robot.interface import GO2Real\n",
"import numpy as np\n",
"robot = GO2Real(mode='lowlevel')"
]
},
{
"cell_type": "code",
2024-10-10 04:08:54 +08:00
"execution_count": 3,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"{'q': [-0.058897197246551514,\n",
" 1.3202357292175293,\n",
" -2.79561448097229,\n",
" 0.09329245984554291,\n",
" 1.3478487730026245,\n",
" -2.8077938556671143,\n",
" -0.395679235458374,\n",
" 1.2776601314544678,\n",
" -2.820889472961426,\n",
" 0.3574388027191162,\n",
" 1.2831705808639526,\n",
" -2.799926996231079],\n",
" 'dq': [0.003875523805618286,\n",
" -0.01937761902809143,\n",
" 0.03033018670976162,\n",
" 0.007751047611236572,\n",
" 0.03875523805618286,\n",
" -0.012132074683904648,\n",
" -0.03875523805618286,\n",
" 0.01937761902809143,\n",
" 0.016176098957657814,\n",
" -0.03100419044494629,\n",
" 0.09301257133483887,\n",
" -0.044484272599220276],\n",
" 'tau_est': [-0.12369140982627869,\n",
" 0.12369140982627869,\n",
" -0.23707520961761475,\n",
" -0.07421484589576721,\n",
" -0.14842969179153442,\n",
" 0.3319052755832672,\n",
" 0.049476563930511475,\n",
" 0.024738281965255737,\n",
" -0.04741504043340683,\n",
" 0.07421484589576721,\n",
" -0.14842969179153442,\n",
" 0.09483008086681366],\n",
" 'temperature': [31.0,\n",
" 29.0,\n",
" 30.0,\n",
" 29.0,\n",
" 29.0,\n",
" 30.0,\n",
" 31.0,\n",
" 29.0,\n",
" 30.0,\n",
" 31.0,\n",
" 30.0,\n",
" 30.0]}"
]
},
"execution_count": 3,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"robot.getJointStates()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Make sure the robot can take commands from python. The next cell should make the joints free to move (no damping)."
]
},
{
"cell_type": "code",
"execution_count": 4,
2024-10-02 07:13:03 +08:00
"metadata": {},
"outputs": [],
"source": [
2024-10-10 04:08:54 +08:00
"import numpy as np\n",
"import time\n",
"start_time = time.time()\n",
"\n",
"while time.time()-start_time < 10:\n",
" q = np.zeros(12) \n",
" dq = np.zeros(12)\n",
" kp = np.ones(12)*0.0\n",
" kd = np.ones(12)*0.0\n",
" tau = np.zeros(12)\n",
" tau[0] = 0.0\n",
" robot.setCommands(q, dq, kp, kd, tau)\n",
" time.sleep(0.02)"
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Put your stick at reset and do not touch it while calibrating\n"
]
}
],
"source": [
"remote = XBoxRemote() # KeyboardRemote()\n",
2024-10-02 07:13:03 +08:00
"safety_hypervisor = SafetyHypervisor(robot)"
]
},
{
"cell_type": "code",
2024-10-10 04:08:54 +08:00
"execution_count": 5,
2024-10-02 07:13:03 +08:00
"metadata": {},
"outputs": [],
"source": [
2024-10-10 04:08:54 +08:00
"class CaTController:\n",
2024-10-02 07:13:03 +08:00
" def __init__(self, robot, remote, checkpoint):\n",
" self.remote = remote\n",
" self.robot = robot\n",
" self.policy = Policy(checkpoint)\n",
" self.command_profile = CommandInterface()\n",
2024-10-10 04:08:54 +08:00
" self.agent = CaTAgent(self.command_profile, self.robot)\n",
2024-10-02 07:13:03 +08:00
" self.init()\n",
2024-10-10 04:08:54 +08:00
" self.hist_data = {}\n",
2024-10-02 07:13:03 +08:00
"\n",
" def init(self):\n",
" self.obs = self.agent.reset()\n",
" self.policy_info = {}\n",
" self.command_profile.yaw_vel_cmd = 0.0\n",
" self.command_profile.x_vel_cmd = 0.0\n",
" self.command_profile.y_vel_cmd = 0.0\n",
"\n",
" def update(self, robot, remote):\n",
2024-10-10 04:08:54 +08:00
" commands = remote.getCommands()\n",
" self.command_profile.yaw_vel_cmd = -commands[2]\n",
" self.command_profile.x_vel_cmd = max(commands[1] * 0.5, -0.3)\n",
" self.command_profile.y_vel_cmd = -commands[0]\n",
"\n",
2024-10-02 07:13:03 +08:00
" action = self.policy(self.obs, self.policy_info)\n",
" self.obs, self.ret, self.done, self.info = self.agent.step(action)\n",
2024-10-10 04:08:54 +08:00
" for key, value in self.info.items():\n",
" if key in self.hist_data:\n",
" self.hist_data[key].append(value)\n",
" else:\n",
" self.hist_data[key] = [value]"
2024-10-02 07:13:03 +08:00
]
},
{
"cell_type": "code",
2024-10-10 04:08:54 +08:00
"execution_count": 6,
2024-10-02 07:13:03 +08:00
"metadata": {},
2024-10-10 04:08:54 +08:00
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Exported model has been tested with ONNXRuntime, and the result looks good!\n",
"p_gains: [20. 20. 20. 20. 20. 20. 20. 20. 20. 20. 20. 20.]\n"
]
},
{
"name": "stderr",
"output_type": "stream",
"text": [
"/home/Go2py/Go2Py/control/cat.py:100: FutureWarning: You are using `torch.load` with `weights_only=False` (the current default value), which uses the default pickle module implicitly. It is possible to construct malicious pickle data which will execute arbitrary code during unpickling (See https://github.com/pytorch/pytorch/blob/main/SECURITY.md#untrusted-models for more details). In a future release, the default value for `weights_only` will be flipped to `True`. This limits the functions that could be executed during unpickling. Arbitrary objects will no longer be allowed to be loaded via this mode unless they are explicitly allowlisted by the user via `torch.serialization.add_safe_globals`. We recommend you start setting `weights_only=True` for any use case where you don't have full control of the loaded file. Please open an issue on GitHub for any issues related to this experimental feature.\n",
" actor_sd = torch.load(checkpoint_path, map_location=\"cpu\")\n"
]
}
],
2024-10-02 07:13:03 +08:00
"source": [
2024-10-10 04:08:54 +08:00
"from Go2Py import ASSETS_PATH \n",
"import os\n",
"checkpoint_path = os.path.join(ASSETS_PATH, 'checkpoints/SoloParkour/trainparamsconfigmax_epochs1500_taskenvlearnlimitsfoot_contact_force_rate60_soft_07-20-22-43.pt')\n",
"controller = CaTController(robot, remote, checkpoint_path)"
2024-10-02 07:13:03 +08:00
]
},
{
"cell_type": "code",
2024-10-10 04:08:54 +08:00
"execution_count": 10,
2024-10-02 07:13:03 +08:00
"metadata": {},
"outputs": [],
"source": [
2024-10-10 04:08:54 +08:00
"fsm = FSM(robot, remote, safety_hypervisor, control_dT=1./50., user_controller_callback=controller.update)"
2024-10-02 07:13:03 +08:00
]
},
{
"cell_type": "code",
2024-10-10 04:08:54 +08:00
"execution_count": 9,
2024-10-02 07:13:03 +08:00
"metadata": {},
"outputs": [],
"source": [
2024-10-10 04:08:54 +08:00
"fsm.close()"
2024-10-02 07:13:03 +08:00
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
2024-10-10 04:08:54 +08:00
"source": []
2024-10-02 07:13:03 +08:00
}
],
"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.10.12"
}
},
"nbformat": 4,
"nbformat_minor": 2
}
