walk these ways real deployment without actuatornet
This commit is contained in:
Rooholla-KhorramBakht
parent
00c6a89eb2
commit
57e401478b
|
|
@ -17,6 +17,8 @@ from cyclonedds.util import duration
|
|||
from Go2Py.unitree_go.msg.dds_ import LowState_
|
||||
from threading import Thread
|
||||
from scipy.spatial.transform import Rotation
|
||||
from Go2Py.joy import xKeySwitch, xRockerBtn
|
||||
|
||||
|
||||
|
||||
class GO2Real():
|
||||
|
|
|
|||
|
|
@ -0,0 +1,157 @@
|
|||
{
|
||||
"cells": [
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"from Go2Py.robot.interface.dds import GO2Real\n",
|
||||
"import numpy as np\n",
|
||||
"robot = GO2Real(mode='lowlevel')"
|
||||
]
|
||||
},
|
||||
{
|
||||
"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.control.walk_these_ways import *\n",
|
||||
"import numpy as np"
|
||||
]
|
||||
},
|
||||
{
|
||||
"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.2\n",
|
||||
" self.command_profile.footswing_height_cmd=-0.05\n",
|
||||
" self.command_profile.step_frequency_cmd = 2.5\n",
|
||||
" self.command_profile.bodyHeight = 0.05\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().rx\n",
|
||||
" vx = robot.getRemoteState().ry\n",
|
||||
" omega = robot.getRemoteState().lx*2.2\n",
|
||||
" self.command_profile.x_vel_cmd = vx\n",
|
||||
" self.command_profile.y_vel_cmd = vy\n",
|
||||
" self.command_profile.yaw_vel_cmd = omega\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"class BaseRemote:\n",
|
||||
" def __init__(self):\n",
|
||||
" pass\n",
|
||||
" def startSeq(self):\n",
|
||||
" return False\n",
|
||||
" def standUpDownSeq(self):\n",
|
||||
" return False\n",
|
||||
"\n",
|
||||
" def flushStates(self):\n",
|
||||
" pass\n",
|
||||
"\n",
|
||||
" def getEstop(self):\n",
|
||||
" return False\n",
|
||||
"\n",
|
||||
"class UnitreeRemote(BaseRemote):\n",
|
||||
" def __init__(self, robot):\n",
|
||||
" self.robot = robot\n",
|
||||
"\n",
|
||||
" def startSeq(self):\n",
|
||||
" remote = self.robot.getRemoteState()\n",
|
||||
" if remote.btn.start:\n",
|
||||
" return True\n",
|
||||
" else:\n",
|
||||
" return False\n",
|
||||
"\n",
|
||||
" def standUpDownSeq(self):\n",
|
||||
" remote = self.robot.getRemoteState()\n",
|
||||
" if remote.btn.L2 and remote.btn.A:\n",
|
||||
" return True\n",
|
||||
" else:\n",
|
||||
" return False\n",
|
||||
"\n",
|
||||
" def getEstop(self):\n",
|
||||
" remote = self.robot.getRemoteState()\n",
|
||||
" if remote.btn.L2 and remote.btn.R2:\n",
|
||||
" return True\n",
|
||||
" else:\n",
|
||||
" return False\n",
|
||||
" "
|
||||
]
|
||||
},
|
||||
{
|
||||
"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": [
|
||||
"checkpoint = \"/home/rstaion/projects/rooholla/locomotion/Go2Py/Go2Py/assets/checkpoints/walk_these_ways/\"\n",
|
||||
"controller = walkTheseWaysController(robot, remote, checkpoint)\n",
|
||||
"fsm = FSM(robot, remote, safety_hypervisor, user_controller_callback=controller.update)\n",
|
||||
"safety_hypervisor = SafetyHypervisor(robot)"
|
||||
]
|
||||
}
|
||||
],
|
||||
"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
|
||||
}
|
||||
Loading…
Reference in New Issue