walk these ways controller tested on hardware
This commit is contained in:
Rooholla-KhorramBakht
parent
57e401478b
commit
8069f729a2
|
|
@ -114,7 +114,7 @@ class HistoryWrapper:
|
|||
class CommandInterface:
|
||||
def __init__(self, limits=None):
|
||||
self.limits = limits
|
||||
gaits = {
|
||||
self.gaits = {
|
||||
"pronking": [0, 0, 0],
|
||||
"trotting": [0.5, 0, 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.body_height_cmd = 0.0
|
||||
self.step_frequency_cmd = 3.0
|
||||
self.gait = torch.tensor(gaits["trotting"])
|
||||
self.footswing_height_cmd = 0.03
|
||||
self.gait = torch.tensor(self.gaits["trotting"])
|
||||
self.footswing_height_cmd = 0.08
|
||||
self.pitch_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):
|
||||
command = np.zeros((19,))
|
||||
|
|
|
|||
|
|
@ -47,19 +47,19 @@
|
|||
" 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",
|
||||
" 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().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",
|
||||
" 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\n"
|
||||
]
|
||||
},
|
||||
|
|
@ -127,10 +127,20 @@
|
|||
"outputs": [],
|
||||
"source": [
|
||||
"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",
|
||||
"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": {
|
||||
|
|
|
|||
|
|
@ -33,16 +33,10 @@
|
|||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"from Go2Py.control.walk_these_ways import *"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"checkpoint_path = \"/home/rstaion/projects/rooholla/locomotion/Go2Py/Go2Py/assets/checkpoints/walk_these_ways/\"\n",
|
||||
"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",
|
||||
"\n",
|
||||
"cfg = loadParameters(checkpoint_path)\n",
|
||||
"policy = Policy(checkpoint_path)\n",
|
||||
|
|
@ -78,7 +72,7 @@
|
|||
"source": [
|
||||
"robot.reset()\n",
|
||||
"obs = agent.reset()\n",
|
||||
"for i in range(5000):\n",
|
||||
"for i in range(50000):\n",
|
||||
" policy_info = {}\n",
|
||||
" action = policy(obs, policy_info)\n",
|
||||
" if i % (control_dt // simulation_dt) == 0:\n",
|
||||
|
|
@ -87,9 +81,6 @@
|
|||
" command_profile.yaw_vel_cmd = 0.0\n",
|
||||
" command_profile.x_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)"
|
||||
]
|
||||
},
|
||||
|
|
@ -102,7 +93,7 @@
|
|||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 1,
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
|
|
@ -116,7 +107,16 @@
|
|||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2,
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"robot = Go2Sim()"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
|
|
@ -137,10 +137,10 @@
|
|||
" 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",
|
||||
" 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.body_height_cmd = 0.5\n",
|
||||
"\n",
|
||||
" def update(self, robot, remote):\n",
|
||||
" action = self.policy(self.obs, self.policy_info)\n",
|
||||
|
|
@ -149,31 +149,23 @@
|
|||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 3,
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"remote = KeyboardRemote()\n",
|
||||
"robot = Go2Sim()\n",
|
||||
"robot.standUpReset()\n",
|
||||
"safety_hypervisor = SafetyHypervisor(robot)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 4,
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"p_gains: [20. 20. 20. 20. 20. 20. 20. 20. 20. 20. 20. 20.]\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"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",
|
||||
"fsm = FSM(robot, remote, safety_hypervisor, user_controller_callback=controller.update)"
|
||||
]
|
||||
|
|
@ -182,19 +174,16 @@
|
|||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"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."
|
||||
]
|
||||
}
|
||||
],
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"robot.standUpReset()\n",
|
||||
"controller.command_profile.roll_cmd=0.3"
|
||||
"controller.command_profile.pitch_cmd=0.0\n",
|
||||
"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,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": []
|
||||
"source": [
|
||||
"fsm.close()"
|
||||
]
|
||||
}
|
||||
],
|
||||
"metadata": {
|
||||
|
|
|
|||
Loading…
Reference in New Issue