2024-03-21 05:40:33 +08:00
{
"cells": [
2024-05-21 05:35:10 +08:00
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Test In Simulation"
]
},
{
"cell_type": "code",
2024-10-02 02:45:06 +08:00
"execution_count": 1,
2024-05-21 05:35:10 +08:00
"metadata": {},
2024-10-10 06:23:15 +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-05-21 05:35:10 +08:00
"source": [
"from Go2Py.robot.fsm import FSM\n",
"from Go2Py.robot.remote import KeyboardRemote\n",
"from Go2Py.robot.safety import SafetyHypervisor\n",
"from Go2Py.sim.mujoco import Go2Sim\n",
"from Go2Py.control.walk_these_ways import *"
]
},
{
"cell_type": "code",
2024-11-05 06:27:32 +08:00
"execution_count": 20,
2024-05-21 05:35:10 +08:00
"metadata": {},
"outputs": [],
"source": [
"robot = Go2Sim()"
]
},
2024-10-02 09:07:58 +08:00
{
"cell_type": "code",
2024-11-05 06:27:32 +08:00
"execution_count": 21,
2024-10-02 09:07:58 +08:00
"metadata": {},
"outputs": [],
"source": [
"map = np.zeros((1200, 1200))\n",
"map[:200, :200] = 255\n",
"robot.updateHeightMap(map)"
]
},
2024-05-21 05:35:10 +08:00
{
"cell_type": "code",
2024-11-05 06:27:32 +08:00
"execution_count": 22,
2024-05-21 05:35:10 +08:00
"metadata": {},
"outputs": [],
"source": [
"remote = KeyboardRemote()\n",
2024-10-02 02:45:06 +08:00
"robot.sitDownReset()\n",
2024-05-21 05:35:10 +08:00
"safety_hypervisor = SafetyHypervisor(robot)"
]
},
2024-03-21 05:40:33 +08:00
{
"cell_type": "code",
2024-11-05 06:27:32 +08:00
"execution_count": 23,
2024-03-21 05:40:33 +08:00
"metadata": {},
"outputs": [],
"source": [
"class walkTheseWaysController:\n",
" def __init__(self, robot, remote, checkpoint):\n",
" self.remote = remote\n",
" self.robot = robot\n",
" self.cfg = loadParameters(checkpoint)\n",
" self.policy = Policy(checkpoint)\n",
" self.command_profile = CommandInterface()\n",
" self.agent = WalkTheseWaysAgent(self.cfg, self.command_profile, self.robot)\n",
" self.agent = HistoryWrapper(self.agent)\n",
2024-10-10 06:23:15 +08:00
" self.hist_data = {}\n",
2024-03-21 05:40:33 +08:00
" self.init()\n",
"\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",
2024-03-21 12:06:35 +08:00
" self.command_profile.stance_width_cmd=0.25\n",
" self.command_profile.footswing_height_cmd=0.08\n",
" self.command_profile.step_frequency_cmd = 3.0\n",
" self.command_profile.bodyHeight = 0.00\n",
2024-03-21 05:40:33 +08:00
"\n",
" def update(self, robot, remote):\n",
" action = self.policy(self.obs, self.policy_info)\n",
" self.obs, self.ret, self.done, self.info = self.agent.step(action)\n",
2024-11-05 06:27:32 +08:00
" vy = 0.8 # Update these based on your implementation of the remote controller\n",
2024-10-02 02:45:06 +08:00
" vx = 0.\n",
2024-11-05 06:27:32 +08:00
" omega = 0.8\n",
2024-03-21 12:06:35 +08:00
" self.command_profile.x_vel_cmd = vx*1.5\n",
" self.command_profile.y_vel_cmd = vy*1.5\n",
2024-10-10 06:23:15 +08:00
" self.command_profile.yaw_vel_cmd = omega\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-03-21 05:40:33 +08:00
]
},
{
"cell_type": "code",
2024-11-05 06:27:32 +08:00
"execution_count": 24,
2024-03-21 05:40:33 +08:00
"metadata": {},
2024-10-02 02:45:06 +08:00
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"p_gains: [20. 20. 20. 20. 20. 20. 20. 20. 20. 20. 20. 20.]\n"
]
2024-11-05 06:27:32 +08:00
},
{
"name": "stdout",
"output_type": "stream",
"text": [
"controller timeout\n"
]
2024-10-02 02:45:06 +08:00
}
],
2024-03-21 05:40:33 +08:00
"source": [
2024-05-21 05:35:10 +08:00
"checkpoint = \"../Go2Py/assets/checkpoints/walk_these_ways/\"\n",
"controller = walkTheseWaysController(robot, remote, checkpoint)\n",
"fsm = FSM(robot, remote, safety_hypervisor, user_controller_callback=controller.update)"
2024-05-05 08:09:01 +08:00
]
},
2024-11-05 06:27:32 +08:00
{
"cell_type": "code",
"execution_count": 26,
"metadata": {},
"outputs": [],
"source": [
"import time\n",
"contacts = []\n",
"feet_vels = []\n",
"\n",
"for i in range(2000):\n",
" contact_state = robot.getFootContact()>15\n",
" sites = ['FR_foot', 'FL_foot', 'RR_foot', 'RL_foot']\n",
" feet_vel = [np.linalg.norm(robot.getFootVelInWorld(s)) for s in sites]\n",
" contacts.append(contact_state)\n",
" feet_vels.append(feet_vel)\n",
" time.sleep(0.01)\n",
"\n",
"feet_vels = np.stack(feet_vels)\n",
"contacts = np.stack(contacts)"
]
},
{
"cell_type": "code",
"execution_count": 17,
"metadata": {},
"outputs": [
{
"data": {
"image/png": "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
"text/plain": [
"<Figure size 640x480 with 1 Axes>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"import matplotlib.pyplot as plt\n",
"start = 250\n",
"end = 500\n",
"plt.plot(contacts[start:end,0])\n",
"plt.plot(feet_vels[start:end,0])\n",
"plt.legend(['contact state', 'foot velocity'])\n",
"plt.grid(True)\n",
"plt.tight_layout()\n",
"plt.savefig('walk.png')"
]
},
{
"cell_type": "code",
"execution_count": 27,
"metadata": {},
"outputs": [
{
"data": {
"image/png": "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
"text/plain": [
"<Figure size 640x480 with 1 Axes>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"import matplotlib.pyplot as plt\n",
"start = 250\n",
"end = 500\n",
"plt.plot(contacts[start:end,0])\n",
"plt.plot(feet_vels[start:end,0])\n",
"plt.legend(['contact state', 'foot velocity'])\n",
"plt.grid(True)\n",
"plt.tight_layout()\n",
"plt.savefig('walk_these_ways_random_adaptation.png')"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"controller.policy.adaptation_module"
]
},
2024-05-05 08:09:01 +08:00
{
"cell_type": "code",
2024-10-10 06:23:15 +08:00
"execution_count": 7,
2024-05-05 08:09:01 +08:00
"metadata": {},
"outputs": [],
"source": [
"controller.command_profile.pitch_cmd=0.0\n",
"controller.command_profile.body_height_cmd=0.0\n",
"controller.command_profile.footswing_height_cmd=0.08\n",
"controller.command_profile.roll_cmd=0.0\n",
"controller.command_profile.stance_width_cmd=0.2\n",
"controller.command_profile.x_vel_cmd=-0.2\n",
"controller.command_profile.y_vel_cmd=0.01\n",
"controller.command_profile.setGaitType(\"trotting\")"
]
},
2024-10-02 02:45:06 +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."
]
},
2024-05-05 08:09:01 +08:00
{
"cell_type": "code",
2024-11-05 06:27:32 +08:00
"execution_count": 18,
2024-05-05 08:09:01 +08:00
"metadata": {},
"outputs": [],
"source": [
2024-11-05 06:27:32 +08:00
"fsm.close()\n",
"robot.close()"
2024-05-05 08:09:01 +08:00
]
},
2024-10-10 06:23:15 +08:00
{
"cell_type": "code",
2024-11-05 06:27:32 +08:00
"execution_count": null,
2024-10-10 06:23:15 +08:00
"metadata": {},
2024-11-05 06:27:32 +08:00
"outputs": [],
2024-10-10 06:23:15 +08:00
"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()"
]
},
2024-05-05 08:09:01 +08:00
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Test on Real Robot"
]
},
{
"cell_type": "code",
2024-11-05 06:27:32 +08:00
"execution_count": null,
2024-05-05 08:09:01 +08:00
"metadata": {},
2024-11-05 06:27:32 +08:00
"outputs": [],
2024-05-05 08:09:01 +08:00
"source": [
"from Go2Py.robot.fsm import FSM\n",
2024-05-21 05:35:10 +08:00
"from Go2Py.robot.remote import UnitreeRemote\n",
2024-05-05 08:09:01 +08:00
"from Go2Py.robot.safety import SafetyHypervisor\n",
"from Go2Py.control.walk_these_ways import *"
]
},
{
"cell_type": "code",
2024-10-10 06:23:15 +08:00
"execution_count": 2,
2024-05-05 08:09:01 +08:00
"metadata": {},
"outputs": [],
"source": [
2024-05-21 05:35:10 +08:00
"from Go2Py.robot.interface import GO2Real\n",
2024-05-05 08:09:01 +08:00
"import numpy as np\n",
"robot = GO2Real(mode='lowlevel')"
]
},
2024-03-21 05:40:33 +08:00
{
"cell_type": "code",
2024-10-10 06:23:15 +08:00
"execution_count": 3,
2024-03-21 05:40:33 +08:00
"metadata": {},
"outputs": [],
"source": [
"remote = UnitreeRemote(robot)\n",
"safety_hypervisor = SafetyHypervisor(robot)"
]
},
2024-05-21 05:35:10 +08:00
{
"cell_type": "code",
2024-10-10 06:23:15 +08:00
"execution_count": 4,
2024-05-21 05:35:10 +08:00
"metadata": {},
"outputs": [],
"source": [
"class walkTheseWaysController:\n",
" def __init__(self, robot, remote, checkpoint):\n",
" self.remote = remote\n",
" self.robot = robot\n",
" self.cfg = loadParameters(checkpoint)\n",
" self.policy = Policy(checkpoint)\n",
" self.command_profile = CommandInterface()\n",
" self.agent = WalkTheseWaysAgent(self.cfg, self.command_profile, self.robot)\n",
" self.agent = HistoryWrapper(self.agent)\n",
" self.init()\n",
"\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",
" self.command_profile.stance_width_cmd=0.25\n",
" self.command_profile.footswing_height_cmd=0.08\n",
" self.command_profile.step_frequency_cmd = 3.0\n",
" self.command_profile.bodyHeight = 0.00\n",
"\n",
" def update(self, robot, remote):\n",
" action = self.policy(self.obs, self.policy_info)\n",
" self.obs, self.ret, self.done, self.info = self.agent.step(action)\n",
" vy = -robot.getRemoteState().lx\n",
" vx = robot.getRemoteState().ly\n",
" omega = -robot.getRemoteState().rx*2.2\n",
" self.command_profile.x_vel_cmd = vx*1.5\n",
" self.command_profile.y_vel_cmd = vy*1.5\n",
" self.command_profile.yaw_vel_cmd = omega"
]
},
2024-03-21 05:40:33 +08:00
{
"cell_type": "code",
2024-11-05 06:27:32 +08:00
"execution_count": null,
2024-03-21 05:40:33 +08:00
"metadata": {},
2024-11-05 06:27:32 +08:00
"outputs": [],
2024-03-21 05:40:33 +08:00
"source": [
2024-05-05 08:09:01 +08:00
"checkpoint = \"../Go2Py/assets/checkpoints/walk_these_ways/\"\n",
2024-03-21 05:40:33 +08:00
"controller = walkTheseWaysController(robot, remote, checkpoint)\n",
2024-05-05 08:09:01 +08:00
"safety_hypervisor = SafetyHypervisor(robot)"
2024-03-21 05:40:33 +08:00
]
2024-03-21 12:06:35 +08:00
},
2024-05-05 09:14:41 +08:00
{
"cell_type": "code",
2024-10-10 06:23:15 +08:00
"execution_count": 6,
2024-05-05 09:14:41 +08:00
"metadata": {},
"outputs": [],
"source": [
"controller.command_profile.pitch_cmd=0.0\n",
"controller.command_profile.body_height_cmd=0.0\n",
"controller.command_profile.footswing_height_cmd=0.04\n",
"controller.command_profile.roll_cmd=0.0\n",
"controller.command_profile.stance_width_cmd=0.2\n",
"controller.command_profile.x_vel_cmd=-0.2\n",
"controller.command_profile.y_vel_cmd=0.01\n",
"controller.command_profile.setGaitType(\"trotting\")"
]
},
2024-03-21 12:06:35 +08:00
{
"cell_type": "code",
2024-11-05 06:27:32 +08:00
"execution_count": null,
2024-03-21 12:06:35 +08:00
"metadata": {},
2024-11-05 06:27:32 +08:00
"outputs": [],
2024-05-05 08:09:01 +08:00
"source": [
"fsm = FSM(robot, remote, safety_hypervisor, user_controller_callback=controller.update)"
]
},
2024-10-02 02:45:06 +08:00
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Pressing `L2+A` to command the robot to stand up. After the the robot is on its feet, pressing `start` will hand over the control the RL policy. When you want to stop, pressing `L2+A` again will act similarly to the factory controller and locks the robot in standing mode. Finally, pressing `L2+A` again will command the robot to sit down."
]
},
2024-05-05 08:09:01 +08:00
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"fsm.close()"
]
2024-03-21 05:40:33 +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",
2024-10-02 02:45:06 +08:00
"version": "3.10.12"
2024-03-21 05:40:33 +08:00
}
},
"nbformat": 4,
"nbformat_minor": 2
}
