569 lines
16 KiB
Plaintext
569 lines
16 KiB
Plaintext
{
|
|
"cells": [
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"# Test In Simulation"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"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",
|
|
"execution_count": 2,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"robot = Go2Sim()"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 3,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"map = np.zeros((1200, 1200))\n",
|
|
"map[:200, :200] = 255\n",
|
|
"robot.updateHeightMap(map)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 4,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"remote = KeyboardRemote()\n",
|
|
"robot.sitDownReset()\n",
|
|
"safety_hypervisor = SafetyHypervisor(robot)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 5,
|
|
"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.hist_data = {}\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 = 0.8 # Update these based on your implementation of the remote controller\n",
|
|
" vx = 0.\n",
|
|
" omega = 0.8\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\"\"\"\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]"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"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)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 7,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"controller.command_profile.x_vel_cmd=0.00\n",
|
|
"controller.command_profile.y_vel_cmd=0.6"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 8,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"controller.command_profile.x_vel_cmd=0.00\n",
|
|
"controller.command_profile.y_vel_cmd=-0.6"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 9,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"controller.command_profile.x_vel_cmd=0.6\n",
|
|
"controller.command_profile.y_vel_cmd=0.00"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 10,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"controller.command_profile.x_vel_cmd=-0.6\n",
|
|
"controller.command_profile.y_vel_cmd=0.00"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 11,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"controller.command_profile.yaw_vel_cmd = 1."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 12,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"controller.command_profile.yaw_vel_cmd = -1."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 13,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"controller.command_profile.yaw_vel_cmd = 1.\n",
|
|
"controller.command_profile.x_vel_cmd=-0.00\n",
|
|
"controller.command_profile.y_vel_cmd=0.00"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 14,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"controller.command_profile.yaw_vel_cmd = -1.\n",
|
|
"controller.command_profile.x_vel_cmd=-0.00\n",
|
|
"controller.command_profile.y_vel_cmd=0.00"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 15,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"vy = 0.6 # Update these based on your implementation of the remote controller\n",
|
|
"vx = 0.\n",
|
|
"omega = 0.6\n",
|
|
"controller.command_profile.x_vel_cmd = vx\n",
|
|
"controller.command_profile.y_vel_cmd = vy\n",
|
|
"controller.command_profile.yaw_vel_cmd = omega"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 16,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"vy = -0.6 # Update these based on your implementation of the remote controller\n",
|
|
"vx = 0.\n",
|
|
"omega = -0.6\n",
|
|
"controller.command_profile.x_vel_cmd = vx\n",
|
|
"controller.command_profile.y_vel_cmd = vy\n",
|
|
"controller.command_profile.yaw_vel_cmd = omega"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"max_actions = torch.tensor(controller.hist_data[\"actions\"]).max(dim = 0).values\n",
|
|
"max_actions"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"min_actions = torch.tensor(controller.hist_data[\"actions\"]).min(axis = 0).values\n",
|
|
"min_actions"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 28,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"torch.save(\n",
|
|
" (min_actions, max_actions), \"action_bounds.pt\"\n",
|
|
")"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"(min_actions_test, max_actions_test) = torch.load(\"action_bounds.pt\")\n",
|
|
"(min_actions_test, max_actions_test)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"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": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"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": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"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"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 8,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"controller.command_profile.step_frequency_cmd = 3."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 12,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"controller.command_profile.footswing_height_cmd=0.18"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 7,
|
|
"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\")"
|
|
]
|
|
},
|
|
{
|
|
"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",
|
|
"execution_count": 18,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"fsm.close()\n",
|
|
"robot.close()"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"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": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"# Test on Real Robot"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"from Go2Py.robot.fsm import FSM\n",
|
|
"from Go2Py.robot.remote import UnitreeRemote\n",
|
|
"from Go2Py.robot.safety import SafetyHypervisor\n",
|
|
"from Go2Py.control.walk_these_ways import *"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 2,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"from Go2Py.robot.interface import GO2Real\n",
|
|
"import numpy as np\n",
|
|
"robot = GO2Real(mode='lowlevel')"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 3,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"remote = UnitreeRemote(robot)\n",
|
|
"safety_hypervisor = SafetyHypervisor(robot)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 4,
|
|
"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"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"checkpoint = \"../Go2Py/assets/checkpoints/walk_these_ways/\"\n",
|
|
"controller = walkTheseWaysController(robot, remote, checkpoint)\n",
|
|
"safety_hypervisor = SafetyHypervisor(robot)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 6,
|
|
"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\")"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"fsm = FSM(robot, remote, safety_hypervisor, user_controller_callback=controller.update)"
|
|
]
|
|
},
|
|
{
|
|
"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."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"fsm.close()"
|
|
]
|
|
}
|
|
],
|
|
"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
|
|
}
|