{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "from Go2Py.sim.mujoco import Go2Sim\n",
    "import numpy as np"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "robot = Go2Sim()\n",
    "robot.standUpReset()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "from Go2Py.controllers.walk_these_ways import *"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "checkpoint_path = \"/home/rstaion/projects/rooholla/locomotion/Go2Py/Go2Py/assets/checkpoints/walk_these_ways/\"\n",
    "\n",
    "cfg = loadParameters(checkpoint_path)\n",
    "policy = Policy(checkpoint_path)\n",
    "command_profile = CommandInterface()"
   ]
  },
  {
   "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"
     ]
    }
   ],
   "source": [
    "agent = WalkTheseWaysAgent(cfg, command_profile, robot)\n",
    "agent = HistoryWrapper(agent)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "control_dt = cfg[\"control\"][\"decimation\"] * cfg[\"sim\"][\"dt\"]\n",
    "simulation_dt = robot.dt\n",
    "obs = agent.reset()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "frq: 12.822735624382831 Hz\n",
      "frq: 47.954632763194 Hz\n",
      "frq: 53.309743511528 Hz\n",
      "frq: 54.65674559220214 Hz\n",
      "frq: 45.29730547005778 Hz\n"
     ]
    }
   ],
   "source": [
    "robot.reset()\n",
    "obs = agent.reset()\n",
    "for i in range(5000):\n",
    "    policy_info = {}\n",
    "    action = policy(obs, policy_info)\n",
    "    if i % (control_dt // simulation_dt) == 0:\n",
    "        obs, ret, done, info = agent.step(action)\n",
    "    robot.step()\n",
    "    command_profile.yaw_vel_cmd = 1.2\n",
    "    command_profile.x_vel_cmd = 0.8\n",
    "    command_profile.y_vel_cmd = 0.0\n",
    "    command_profile.stance_width_cmd=0.2\n",
    "    command_profile.footswing_height_cmd=-0.05\n",
    "    command_profile.step_frequency_cmd = 2.5\n",
    "    time.sleep(robot.dt/4)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "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
}