{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Test In Simulation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "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": [
    "remote = KeyboardRemote()\n",
    "robot.sitDownReset()\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 = 0. # Update these based on your implementation of the remote controller\n",
    "        vx = 0.\n",
    "        omega = 0.\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": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "p_gains: [20. 20. 20. 20. 20. 20. 20. 20. 20. 20. 20. 20.]\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "frq: 0.06860146952866389 Hz\n",
      "frq: 43.78233593252539 Hz\n",
      "frq: 44.41613012538123 Hz\n",
      "frq: 46.552687074074896 Hz\n",
      "frq: 46.25288370349132 Hz\n",
      "frq: 45.19091075603633 Hz\n",
      "frq: 46.16786095609198 Hz\n",
      "frq: 45.455376979181345 Hz\n",
      "frq: 45.34774899450763 Hz\n",
      "frq: 46.05987129647932 Hz\n",
      "frq: 45.999758721663504 Hz\n",
      "frq: 46.80150414532633 Hz\n",
      "frq: 45.81385238828631 Hz\n",
      "frq: 45.29241401652179 Hz\n",
      "frq: 45.742902947880424 Hz\n"
     ]
    }
   ],
   "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": 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.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": 7,
   "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": "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
}