265 lines
7.6 KiB
Plaintext
265 lines
7.6 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": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"robot = Go2Sim()"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"remote = KeyboardRemote()\n",
|
|
"robot.standUpReset()\n",
|
|
"safety_hypervisor = SafetyHypervisor(robot)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"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",
|
|
"fsm = FSM(robot, remote, safety_hypervisor, user_controller_callback=controller.update)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"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": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"fsm.close()"
|
|
]
|
|
},
|
|
{
|
|
"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": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"from Go2Py.robot.interface import GO2Real\n",
|
|
"import numpy as np\n",
|
|
"robot = GO2Real(mode='lowlevel')"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"remote = UnitreeRemote(robot)\n",
|
|
"safety_hypervisor = SafetyHypervisor(robot)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"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": null,
|
|
"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": "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.8.18"
|
|
}
|
|
},
|
|
"nbformat": 4,
|
|
"nbformat_minor": 2
|
|
}
|