EmbodiedAI
/
Go2Py
mirror of https://github.com/machines-in-motion/Go2Py.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

walk these ways controller tested on hardware

This commit is contained in:
Rooholla-KhorramBakht 2024-03-21 00:06:35 -04:00
parent 57e401478b
commit 8069f729a2
3 changed files with 64 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
Go2Py/control/walk_these_ways.py
View File

@ -114,7 +114,7 @@ class HistoryWrapper:
class CommandInterface: class CommandInterface:
def __init__(self, limits=None): def __init__(self, limits=None):
self.limits = limits self.limits = limits
gaits = { self.gaits = {
"pronking": [0, 0, 0], "pronking": [0, 0, 0],
"trotting": [0.5, 0, 0], "trotting": [0.5, 0, 0],
"bounding": [0, 0.5, 0], "bounding": [0, 0.5, 0],
@ -123,11 +123,15 @@ class CommandInterface:
self.x_vel_cmd, self.y_vel_cmd, self.yaw_vel_cmd = 0.0, 0.0, 0.0 self.x_vel_cmd, self.y_vel_cmd, self.yaw_vel_cmd = 0.0, 0.0, 0.0
self.body_height_cmd = 0.0 self.body_height_cmd = 0.0
self.step_frequency_cmd = 3.0 self.step_frequency_cmd = 3.0
self.gait = torch.tensor(gaits["trotting"]) self.gait = torch.tensor(self.gaits["trotting"])
self.footswing_height_cmd = 0.03 self.footswing_height_cmd = 0.08
self.pitch_cmd = 0.0 self.pitch_cmd = 0.0
self.roll_cmd = 0.0 self.roll_cmd = 0.0
self.stance_width_cmd = 0.0 self.stance_width_cmd = 0.25
def setGaitType(self, gait_type):
assert gait_type in [key for key in self.gaits.keys()], f'The gain type should be in {[key for key in self.gaits.keys()]}'
self.gait = torch.tensor(self.gaits[gait_type])
def get_command(self): def get_command(self):
command = np.zeros((19,)) command = np.zeros((19,))

30
examples/walk_these_ways_real.ipynb
View File

@ -47,19 +47,19 @@
" self.command_profile.yaw_vel_cmd = 0.0\n", " self.command_profile.yaw_vel_cmd = 0.0\n",
" self.command_profile.x_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.y_vel_cmd = 0.0\n",
" self.command_profile.stance_width_cmd=0.2\n", " self.command_profile.stance_width_cmd=0.25\n",
" self.command_profile.footswing_height_cmd=-0.05\n", " self.command_profile.footswing_height_cmd=0.08\n",
" self.command_profile.step_frequency_cmd = 2.5\n", " self.command_profile.step_frequency_cmd = 3.0\n",
" self.command_profile.bodyHeight = 0.05\n", " self.command_profile.bodyHeight = 0.00\n",
"\n", "\n",
" def update(self, robot, remote):\n", " def update(self, robot, remote):\n",
" action = self.policy(self.obs, self.policy_info)\n", " action = self.policy(self.obs, self.policy_info)\n",
" self.obs, self.ret, self.done, self.info = self.agent.step(action)\n", " self.obs, self.ret, self.done, self.info = self.agent.step(action)\n",
" vy = robot.getRemoteState().rx\n", " vy = -robot.getRemoteState().lx\n",
" vx = robot.getRemoteState().ry\n", " vx = robot.getRemoteState().ly\n",
" omega = robot.getRemoteState().lx*2.2\n", " omega = -robot.getRemoteState().rx*2.2\n",
" self.command_profile.x_vel_cmd = vx\n", " self.command_profile.x_vel_cmd = vx*1.5\n",
" self.command_profile.y_vel_cmd = vy\n", " self.command_profile.y_vel_cmd = vy*1.5\n",
" self.command_profile.yaw_vel_cmd = omega\n" " self.command_profile.yaw_vel_cmd = omega\n"
] ]
}, },
@ -127,10 +127,20 @@
"outputs": [], "outputs": [],
"source": [ "source": [
"checkpoint = \"/home/rstaion/projects/rooholla/locomotion/Go2Py/Go2Py/assets/checkpoints/walk_these_ways/\"\n", "checkpoint = \"/home/rstaion/projects/rooholla/locomotion/Go2Py/Go2Py/assets/checkpoints/walk_these_ways/\"\n",
"# checkpoint = \"/home/rstaion/projects/rooholla/walk-these-ways/runs/gait-conditioned-agility/pretrain-v0/train/025417.456545\"\n",
"\n",
"controller = walkTheseWaysController(robot, remote, checkpoint)\n", "controller = walkTheseWaysController(robot, remote, checkpoint)\n",
"fsm = FSM(robot, remote, safety_hypervisor, user_controller_callback=controller.update)\n", "fsm = FSM(robot, remote, safety_hypervisor, user_controller_callback=controller.update)\n",
"safety_hypervisor = SafetyHypervisor(robot)" "safety_hypervisor = SafetyHypervisor(robot)\n",
"fsm.control_dT = 1./56."
] ]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
} }
], ],
"metadata": { "metadata": {

81
examples/walk_these_ways_sim.ipynb
View File

@ -33,16 +33,10 @@
"metadata": {}, "metadata": {},
"outputs": [], "outputs": [],
"source": [ "source": [
"from Go2Py.control.walk_these_ways import *" "from Go2Py.control.walk_these_ways import *\n",
] "# checkpoint_path = \"/home/rstaion/projects/rooholla/locomotion/Go2Py/Go2Py/assets/checkpoints/walk_these_ways/\"\n",
}, "checkpoint_path = \"/home/rstaion/projects/rooholla/walk-these-ways/runs/gait-conditioned-agility/pretrain-v0/train/025417.456545\"\n",
{ "\n",
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"checkpoint_path = \"/home/rstaion/projects/rooholla/locomotion/Go2Py/Go2Py/assets/checkpoints/walk_these_ways/\"\n",
"\n", "\n",
"cfg = loadParameters(checkpoint_path)\n", "cfg = loadParameters(checkpoint_path)\n",
"policy = Policy(checkpoint_path)\n", "policy = Policy(checkpoint_path)\n",
@ -78,7 +72,7 @@
"source": [ "source": [
"robot.reset()\n", "robot.reset()\n",
"obs = agent.reset()\n", "obs = agent.reset()\n",
"for i in range(5000):\n", "for i in range(50000):\n",
" policy_info = {}\n", " policy_info = {}\n",
" action = policy(obs, policy_info)\n", " action = policy(obs, policy_info)\n",
" if i % (control_dt // simulation_dt) == 0:\n", " if i % (control_dt // simulation_dt) == 0:\n",
@ -87,9 +81,6 @@
" command_profile.yaw_vel_cmd = 0.0\n", " command_profile.yaw_vel_cmd = 0.0\n",
" command_profile.x_vel_cmd = 0.0\n", " command_profile.x_vel_cmd = 0.0\n",
" command_profile.y_vel_cmd = 0.0\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)" " time.sleep(robot.dt/4)"
] ]
}, },
@ -102,7 +93,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 1, "execution_count": null,
"metadata": {}, "metadata": {},
"outputs": [], "outputs": [],
"source": [ "source": [
@ -116,7 +107,16 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 2, "execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"robot = Go2Sim()"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {}, "metadata": {},
"outputs": [], "outputs": [],
"source": [ "source": [
@ -137,10 +137,10 @@
" self.command_profile.yaw_vel_cmd = 0.0\n", " self.command_profile.yaw_vel_cmd = 0.0\n",
" self.command_profile.x_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.y_vel_cmd = 0.0\n",
" self.command_profile.stance_width_cmd=0.2\n", " self.command_profile.stance_width_cmd=0.25\n",
" self.command_profile.footswing_height_cmd=-0.05\n", " self.command_profile.footswing_height_cmd=0.08\n",
" self.command_profile.step_frequency_cmd = 2.5\n", " self.command_profile.step_frequency_cmd = 3.0\n",
" self.command_profile.bodyHeight = 0.05\n", " self.command_profile.body_height_cmd = 0.5\n",
"\n", "\n",
" def update(self, robot, remote):\n", " def update(self, robot, remote):\n",
" action = self.policy(self.obs, self.policy_info)\n", " action = self.policy(self.obs, self.policy_info)\n",
@ -149,31 +149,23 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 3, "execution_count": null,
"metadata": {}, "metadata": {},
"outputs": [], "outputs": [],
"source": [ "source": [
"remote = KeyboardRemote()\n", "remote = KeyboardRemote()\n",
"robot = Go2Sim()\n",
"robot.standUpReset()\n", "robot.standUpReset()\n",
"safety_hypervisor = SafetyHypervisor(robot)" "safety_hypervisor = SafetyHypervisor(robot)"
] ]
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 4, "execution_count": null,
"metadata": {}, "metadata": {},
"outputs": [ "outputs": [],
{
"name": "stdout",
"output_type": "stream",
"text": [
"p_gains: [20. 20. 20. 20. 20. 20. 20. 20. 20. 20. 20. 20.]\n"
]
}
],
"source": [ "source": [
"checkpoint = \"/home/rstaion/projects/rooholla/locomotion/Go2Py/Go2Py/assets/checkpoints/walk_these_ways/\"\n", "checkpoint = \"/home/rstaion/projects/rooholla/locomotion/Go2Py/Go2Py/assets/checkpoints/walk_these_ways/\"\n",
"# checkpoint = \"/home/rstaion/projects/rooholla/walk-these-ways/runs/gait-conditioned-agility/pretrain-v0/train/025417.456545\"\n",
"controller = walkTheseWaysController(robot, remote, checkpoint)\n", "controller = walkTheseWaysController(robot, remote, checkpoint)\n",
"fsm = FSM(robot, remote, safety_hypervisor, user_controller_callback=controller.update)" "fsm = FSM(robot, remote, safety_hypervisor, user_controller_callback=controller.update)"
] ]
@ -182,19 +174,16 @@
"cell_type": "code", "cell_type": "code",
"execution_count": null, "execution_count": null,
"metadata": {}, "metadata": {},
"outputs": [ "outputs": [],
{
"ename": "",
"evalue": "",
"output_type": "error",
"traceback": [
"\u001b[1;31mThe Kernel crashed while executing code in the the current cell or a previous cell. Please review the code in the cell(s) to identify a possible cause of the failure. Click <a href='https://aka.ms/vscodeJupyterKernelCrash'>here</a> for more info. View Jupyter <a href='command:jupyter.viewOutput'>log</a> for further details."
]
}
],
"source": [ "source": [
"robot.standUpReset()\n", "controller.command_profile.pitch_cmd=0.0\n",
"controller.command_profile.roll_cmd=0.3" "controller.command_profile.body_height_cmd=0.1\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(\"pronking\")"
] ]
}, },
{ {
@ -202,7 +191,9 @@
"execution_count": null, "execution_count": null,
"metadata": {}, "metadata": {},
"outputs": [], "outputs": [],
"source": [] "source": [
"fsm.close()"
]
} }
], ],
"metadata": { "metadata": {