EmbodiedAI
/
Go2Py_SIM
mirror of https://github.com/machines-in-motion/Go2Py.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
Go2Py_SIM/examples/07-CaT-Parkour-RL-controlle...

2517 lines
222 KiB
Plaintext
Raw Normal View History

Many modifs.
2024-11-05 06:27:32 +08:00
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"RL policy based on the [SoloParkour: Constrained Reinforcement Learning for Visual Locomotion from Privileged Experience](https://arxiv.org/abs/2409.13678). "
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Flat Ground"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Test In Simulation"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"pygame 2.6.1 (SDL 2.28.4, Python 3.10.12)\n",
"Hello from the pygame community. https://www.pygame.org/contribute.html\n"
]
}
],
"source": [
"from Go2Py.robot.fsm import FSM\n",
"from Go2Py.robot.remote import KeyboardRemote, XBoxRemote\n",
"from Go2Py.robot.safety import SafetyHypervisor\n",
"from Go2Py.sim.mujoco import Go2Sim\n",
"from Go2Py.control.cat_parkour import *\n",
"import torch"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {},
"outputs": [],
"source": [
"from Go2Py.robot.model import FrictionModel\n",
"friction_model = None\n",
"Fs = np.zeros(12)\n",
"mu_v = np.zeros(12)\n",
"#mu_v[[2,5,8,11]] = np.array([0.2167, -0.0647, -0.0420, -0.0834])\n",
"#Fs[[2,5,8,11]] = np.array([1.5259, 1.2380, 0.8917, 2.2461])\n",
"\n",
"#mu_v[[0,3,6,9]] = np.array([0., 0., 0., 0.])\n",
"#Fs[[0,3,6,9]] = np.array([1.5, 1.5, 1.5, 1.5])\n",
"#mu_v[[2,5,8,11]] = np.array([0., 0., 0., 0.])\n",
"#Fs[[2,5,8,11]] = np.array([1.5, 1.5, 1.5, 1.5])\n",
"\n",
"friction_model = FrictionModel(Fs=1.5, mu_v=0.3)\n",
"##friction_model = FrictionModel(Fs=0., mu_v=0.)\n",
"#friction_model = FrictionModel(Fs=Fs, mu_v=mu_v)\n",
"robot = Go2Sim(dt = 0.001, friction_model=friction_model)"
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {},
"outputs": [],
"source": [
"map = np.zeros((1200, 1200))\n",
"map[:200, :200] = 50\n",
"robot.updateHeightMap(map)"
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Put your stick at reset and do not touch it while calibrating\n"
]
}
],
"source": [
"remote = XBoxRemote() # KeyboardRemote()\n",
"robot.sitDownReset()\n",
"safety_hypervisor = SafetyHypervisor(robot)"
]
},
{
"cell_type": "code",
"execution_count": 5,
"metadata": {},
"outputs": [],
"source": [
"def getRemote(remote):\n",
" commands = remote.getCommands()\n",
" commands[0] *= 0.6\n",
" commands[1] = max(commands[1], 0) * 0.6\n",
" zero_commands_xy = np.logical_or(\n",
" np.linalg.norm(commands[:2]) <= 0.2,\n",
" commands[1] <= 0.2\n",
" )\n",
" if zero_commands_xy:\n",
" commands[:2] = np.zeros_like(commands[:2])\n",
" commands[2] = 0\n",
" print(commands)\n",
" return commands"
]
},
{
"cell_type": "code",
"execution_count": 6,
"metadata": {},
"outputs": [],
"source": [
"class CaTController:\n",
" def __init__(self, robot, remote, checkpoint):\n",
" self.remote = remote\n",
" self.robot = robot\n",
" self.policy = Policy(checkpoint)\n",
" self.command_profile = CommandInterface()\n",
" self.agent = CaTAgent(self.command_profile, self.robot)\n",
" self.hist_data = {}\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",
"\n",
" def update(self, robot, remote):\n",
" if not hasattr(self, \"obs\"):\n",
" self.init()\n",
" commands = getRemote(remote)\n",
" self.command_profile.yaw_vel_cmd = -commands[2]\n",
" self.command_profile.x_vel_cmd = commands[1]\n",
" self.command_profile.y_vel_cmd = -commands[0]\n",
"\n",
" self.obs = self.agent.get_obs()\n",
" action = self.policy(self.obs, self.policy_info)\n",
" _, self.ret, self.done, self.info = self.agent.step(action)\n",
" for key, value in self.info.items():\n",
" if key in self.hist_data:\n",
" self.hist_data[key].append(value)\n",
" else:\n",
" self.hist_data[key] = [value]"
]
},
{
"cell_type": "code",
"execution_count": 7,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"{'q': array([-0.02489972, 1.26249508, -2.82800513, 0.04556739, 1.25053519,\n",
" -2.79318037, -0.3062963 , 1.28285276, -2.82290189, 0.26406768,\n",
" 1.29357252, -2.84247318]),\n",
" 'dq': array([ 0.05639392, -0.00138966, 0.26148655, -0.06824655, -0.00160641,\n",
" 0.1753318 , -0.05681151, 0.01524675, 0.2468449 , 0.06539485,\n",
" 0.01677717, 0.29524517]),\n",
" 'tau_est': array([0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.])}"
]
},
"execution_count": 7,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"robot.getJointStates()"
]
},
{
"cell_type": "code",
"execution_count": 8,
"metadata": {},
"outputs": [
{
"name": "stderr",
"output_type": "stream",
"text": [
"/home/Go2py/Go2Py/control/cat_parkour.py:100: FutureWarning: You are using `torch.load` with `weights_only=False` (the current default value), which uses the default pickle module implicitly. It is possible to construct malicious pickle data which will execute arbitrary code during unpickling (See https://github.com/pytorch/pytorch/blob/main/SECURITY.md#untrusted-models for more details). In a future release, the default value for `weights_only` will be flipped to `True`. This limits the functions that could be executed during unpickling. Arbitrary objects will no longer be allowed to be loaded via this mode unless they are explicitly allowlisted by the user via `torch.serialization.add_safe_globals`. We recommend you start setting `weights_only=True` for any use case where you don't have full control of the loaded file. Please open an issue on GitHub for any issues related to this experimental feature.\n",
" actor_sd = torch.load(checkpoint_path, map_location=\"cpu\")\n"
]
},
{
"name": "stdout",
"output_type": "stream",
"text": [
"Exported model has been tested with ONNXRuntime, and the result looks good!\n",
"p_gains: [20. 20. 20. 20. 20. 20. 20. 20. 20. 20. 20. 20.]\n"
]
},
{
"name": "stdout",
"output_type": "stream",
"text": [
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0.23611101 0. ]\n",
"[0. 0.26843012 0. ]\n",
"[0. 0.29316842 0. ]\n",
"[0. 0.32559739 0. ]\n",
"[0. 0.35136111 0. ]\n",
"[0. 0.36655933 0. ]\n",
"[0. 0.37256537 0. ]\n",
"[0. 0.3831675 0. ]\n",
"[0. 0.39495986 0. ]\n",
"[0. 0.40979185 0. ]\n",
"[0. 0.4265282 0. ]\n",
"[0. 0.44436322 0. ]\n",
"[0. 0.45511184 0. ]\n",
"[0. 0.45994597 0. ]\n",
"[0. 0.4644505 0. ]\n",
"[0. 0.46650135 0. ]\n",
"[0. 0.47443005 0. ]\n",
"[0. 0.48893245 0. ]\n",
"[0. 0.49803307 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.50471662 0. ]\n",
"[0. 0.50654773 0. ]\n",
"[0. 0.50654773 0. ]\n",
"[0. 0.50654773 0. ]\n",
"[0. 0.50654773 0. ]\n",
"[0. 0.50654773 0. ]\n",
"[0. 0.50530258 0. ]\n",
"[0. 0.50530258 0. ]\n",
"[0. 0.50530258 0. ]\n",
"[0. 0.50530258 0. ]\n",
"[0. 0.50407573 0. ]\n",
"[0. 0.50407573 0. ]\n",
"[0. 0.50407573 0. ]\n",
"[0. 0.50407573 0. ]\n",
"[0. 0.50407573 0. ]\n",
"[0. 0.50407573 0. ]\n",
"[0. 0.50407573 0. ]\n",
"[0. 0.50407573 0. ]\n",
"[0. 0.50407573 0. ]\n",
"[0. 0.50407573 0. ]\n",
"[0. 0.50407573 0. ]\n",
"[0. 0.50407573 0. ]\n",
"[0. 0.50407573 0. ]\n",
"[0. 0.50407573 0. ]\n",
"[0. 0.50407573 0. ]\n",
"[0. 0.50407573 0. ]\n",
"[0. 0.50407573 0. ]\n",
"[0. 0.50407573 0. ]\n",
"[0. 0.50407573 0. ]\n",
"[0. 0.50407573 0. ]\n",
"[0. 0.50407573 0. ]\n",
"[0. 0.50407573 0. ]\n",
"[0. 0.50407573 0. ]\n",
"[0. 0.50407573 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.5028672 0. ]\n",
"[0. 0.50478987 0. ]\n",
"[0. 0.50478987 0. ]\n",
"[0. 0.50643787 0. ]\n",
"[0. 0.50643787 0. ]\n",
"[0. 0.50643787 0. ]\n",
"[0. 0.50643787 0. ]\n",
"[0. 0.50815911 0. ]\n",
"[0. 0.50815911 0. ]\n",
"[0. 0.50815911 0. ]\n",
"[0. 0.50815911 0. ]\n",
"[0. 0.50815911 0. ]\n",
"[0. 0.50815911 0. ]\n",
"[0. 0.50815911 0. ]\n",
"[0. 0.50815911 0. ]\n",
"[0. 0.50815911 0. ]\n",
"[0. 0.50815911 0. ]\n",
"[0. 0.50815911 0. ]\n",
"[0. 0.50815911 0. ]\n",
"[0. 0.50815911 0. ]\n",
"[0. 0.50815911 0. ]\n",
"[0. 0.50815911 0. ]\n",
"[0. 0.50815911 0. ]\n",
"[0. 0.50815911 0. ]\n",
"[0. 0.50815911 0. ]\n",
"[0. 0.50815911 0. ]\n",
"[0. 0.50266578 0. ]\n",
"[0. 0.49682454 0. ]\n",
"[0. 0.49230169 0. ]\n",
"[0. 0.49045227 0. ]\n",
"[0. 0.48904232 0. ]\n",
"[0. 0.48904232 0. ]\n",
"[0. 0.48904232 0. ]\n",
"[0. 0.48512374 0. ]\n",
"[0. 0.47823876 0. ]\n",
"[0. 0.47250739 0. ]\n",
"[0. 0.46992552 0. ]\n",
"[0. 0.46822259 0. ]\n",
"[0.08252003 0.46673939 0. ]\n",
"[0.14424677 0.46673939 0. ]\n",
"[0.21324301 0.46556748 0. ]\n",
"[0.2704286 0.46556748 0. ]\n",
"[0.31820227 0.46556748 0. ]\n",
"[0.3724947 0.46556748 0. ]\n",
"[0.41821753 0.46556748 0. ]\n",
"[0.4361441 0.46556748 0. ]\n",
"[0.45167192 0.46556748 0. ]\n",
"[0.46038801 0.46556748 0. ]\n",
"[0.46663209 0.46556748 0. ]\n",
"[0.46881112 0.46556748 0. ]\n",
"[0.46881112 0.46556748 0. ]\n",
"[0.46881112 0.46556748 0. ]\n",
"[0.46881112 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.47075209 0.46556748 0. ]\n",
"[0.46095565 0.47289192 0. ]\n",
"[0.29752903 0.49819787 0. ]\n",
"[0.25352745 0.51861475 0. ]\n",
"[0.1874976 0.52949155 0. ]\n",
"[0. 0.53381297 0. ]\n",
"[0. 0.53659626 0. ]\n",
"[0. 0.53835412 0. ]\n",
"[-0.09718516 0.54020355 0. ]\n",
"[-0.14715617 0.53443555 0. ]\n",
"[-0.19533269 0.52013457 0. ]\n",
"[-0.23894974 0.50585191 0. ]\n",
"[-0.27066458 0.49570756 0. ]\n",
"[-0.28679666 0.49034241 0. ]\n",
"[-0.29307737 0.48838312 0. ]\n",
"[-0.29836928 0.48704641 0. ]\n",
"[-0.30040181 0.48567307 0. ]\n",
"[-0.30179346 0.48567307 0. ]\n",
"[-0.30311186 0.48849298 0. ]\n",
"[-0.30311186 0.49460889 0. ]\n",
"[-0.30311186 0.50310525 0. ]\n",
"[-0.30311186 0.509624 0. ]\n",
"[-0.30311186 0.51220587 0. ]\n",
"[-0.30311186 0.51385386 0. ]\n",
"[-0.30311186 0.51550186 0. ]\n",
"[-0.30032857 0.51550186 0. ]\n",
"[-0.28890244 0.52026275 0. ]\n",
"[-0.26844893 0.5310663 0. ]\n",
"[-0.23847365 0.53941617 0. ]\n",
"[-0.21968646 0.55391856 0. ]\n",
"[-0.20943224 0.5772286 0. ]\n",
"[-0.20390229 0.5772286 0. ]\n",
"[-0.20219935 0.5772286 0. ]\n",
"[-0.18262478 0.5772286 0. ]\n",
"[-0.16918443 0.5772286 0. ]\n",
"[-0.1589119 0.5772286 0. ]\n",
"[-0.14741252 0.5772286 0. ]\n",
"[-0.14290799 0.5772286 0. ]\n",
"[-0.13787244 0.5772286 0. ]\n",
"[-0.13787244 0.5772286 0. ]\n",
"[-0.13596808 0.5772286 0. ]\n",
"[-0.12941271 0.5772286 0. ]\n",
"[-0.12066 0.5772286 0. ]\n",
"[-0.09729503 0.5772286 0. ]\n",
"[-0.07200739 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0.06657106 0.5772286 0. ]\n",
"[0.2133895 0.5772286 0. ]\n",
"[0.25067091 0.5772286 0. ]\n",
"[0.2869269 0.57448194 0. ]\n",
"[0.30228992 0.57103945 0. ]\n",
"[0.3171036 0.56757865 0. ]\n",
"[0.32874947 0.56373332 0. ]\n",
"[0.34517453 0.55327768 0. ]\n",
"[0.35903604 0.54668568 0. ]\n",
"[0.36667177 0.54271217 0. ]\n",
"[0.3695283 0.54000212 0. ]\n",
"[0.37135941 0.53619341 0. ]\n",
"[0.37511319 0.53460035 0. ]\n",
"[0.38011212 0.53192693 0. ]\n",
"[0.38252919 0.52641528 0. ]\n",
"[0.3924538 0.52057404 0. ]\n",
"[0.40528989 0.51476942 0. ]\n",
"[0.41695406 0.50989867 0. ]\n",
"[0.42167833 0.5086352 0. ]\n",
"[0.42460811 0.50565049 0. ]\n",
"[0.42667726 0.50438702 0. ]\n",
"[0.42867317 0.50876338 0. ]\n",
"[0.40902536 0.52260657 0. ]\n",
"[0.39642731 0.52912533 0. ]\n",
"[0.38545896 0.53557084 0. ]\n",
"[0.37782323 0.54047821 0. ]\n",
"[0.36520688 0.55111696 0. ]\n",
"[0.34750004 0.5772286 0. ]\n",
"[0.32417169 0.5772286 0. ]\n",
"[0.30809454 0.5772286 0. ]\n",
"[0.29249348 0.5772286 0. ]\n",
"[0.25521207 0.5772286 0. ]\n",
"[0.21628266 0.5772286 0. ]\n",
"[0.18396356 0.5772286 0. ]\n",
"[0.13163042 0.5772286 0. ]\n",
"[0.11438135 0.5772286 0. ]\n",
"[0.09878029 0.5772286 0. ]\n",
"[0.09077834 0.5772286 0. ]\n",
"[0.08631043 0.5772286 0. ]\n",
"[0.08480892 0.5772286 0. ]\n",
"[0.08480892 0.5772286 0. ]\n",
"[0.08338065 0.5772286 0. ]\n",
"[0.07541532 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[-0.06662393 0.5772286 0. ]\n",
"[-0.0858689 0.5772286 0. ]\n",
"[-0.13226924 0.5772286 0. ]\n",
"[-0.18180078 0.5772286 0. ]\n",
"[-0.22408112 0.5772286 0. ]\n",
"[-0.25759045 0.5772286 0. ]\n",
"[-0.28066244 0.5772286 0. ]\n",
"[-0.29441408 0.5772286 0. ]\n",
"[-0.2997243 0.5772286 0. ]\n",
"[-0.30329497 0.5772286 0. ]\n",
"[-0.3064811 0.5772286 0. ]\n",
"[-0.3064811 0.5772286 0. ]\n",
"[-0.30840377 0.5772286 0. ]\n",
"[-0.30990528 0.5772286 0. ]\n",
"[-0.30704874 0.5772286 0. ]\n",
"[-0.29047719 0.5772286 0. ]\n",
"[-0.27161675 0.5772286 0. ]\n",
"[-0.25775525 0.5772286 0. ]\n",
"[-0.23519597 0.5772286 0. ]\n",
"[-0.21904557 0.5772286 0. ]\n",
"[-0.20728984 0.5772286 0. ]\n",
"[-0.17876114 0.5772286 0. ]\n",
"[-0.1564399 0.5772286 0. ]\n",
"[-0.13997822 0.5772286 0. ]\n",
"[-0.12776471 0.5772286 0. ]\n",
"[-0.1219784 0.5772286 0. ]\n",
"[-0.11097342 0.5772286 0. ]\n",
"[-0.10919725 0.5772286 0. ]\n",
"[-0.10679849 0.5772286 0. ]\n",
"[-0.10273343 0.5772286 0. ]\n",
"[-0.10273343 0.5772286 0. ]\n",
"[-0.10273343 0.5772286 0. ]\n",
"[-0.10273343 0.5772286 0. ]\n",
"[-0.10273343 0.5772286 0. ]\n",
"[-0.10273343 0.5772286 0. ]\n",
"[-0.10273343 0.5772286 0. ]\n",
"[-0.10273343 0.5772286 0. ]\n",
"[-0.10273343 0.5772286 0. ]\n",
"[-0.10273343 0.5772286 0. ]\n",
"[-0.10273343 0.5772286 0. ]\n",
"[-0.10273343 0.5772286 0. ]\n",
"[-0.10273343 0.5772286 0. ]\n",
"[-0.10273343 0.5772286 0. ]\n",
"[-0.10273343 0.5772286 0. ]\n",
"[-0.10394196 0.5772286 0. ]\n",
"[-0.10394196 0.5772286 0. ]\n",
"[-0.10394196 0.5772286 0. ]\n",
"[-0.10394196 0.5772286 0. ]\n",
"[-0.10394196 0.5772286 0. ]\n",
"[-0.10394196 0.5772286 0. ]\n",
"[-0.10394196 0.5772286 0. ]\n",
"[-0.10394196 0.5772286 0. ]\n",
"[-0.10394196 0.5772286 0. ]\n",
"[-0.10394196 0.5772286 0. ]\n",
"[-0.10394196 0.56160923 0. ]\n",
"[-0.10394196 0.53388621 0. ]\n",
"[-0.09883316 0.5036912 0. ]\n",
"[-0.06979175 0.44537033 0. ]\n",
"[0. 0.28681447 0. ]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0.28978086 0. ]\n",
"[0. 0.3254509 0. ]\n",
"[0. 0.3669988 0. ]\n",
"[0. 0.41363718 0. ]\n",
"[0. 0.4756569 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[-0.06464633 0.5772286 0. ]\n",
"[-0.10341094 0.5772286 0. ]\n",
"[-0.12933946 0.5772286 0. ]\n",
"[-0.16969714 0.5772286 0. ]\n",
"[-0.20007526 0.5772286 0. ]\n",
"[-0.21847793 0.5772286 0. ]\n",
"[-0.24922227 0.5772286 0. ]\n",
"[-0.2711956 0.5772286 0. ]\n",
"[-0.31462954 0.56109652 0. ]\n",
"[-0.34861495 0.55056763 0. ]\n",
"[-0.37033193 0.54448834 0. ]\n",
"[-0.40019734 0.52670826 0. ]\n",
"[-0.43522648 0.49649494 0. ]\n",
"[-0.45600959 0.48146152 0. ]\n",
"[-0.46697794 0.47278205 0. ]\n",
"[-0.47148247 0.46935788 0. ]\n",
"[-0.47926469 0.45692464 0. ]\n",
"[-0.49759411 0.43672749 0. ]\n",
"[-0.5214901 0.41105532 0. ]\n",
"[-0.53762219 0.39479506 0. ]\n",
"[-0.54351836 0.38783684 0. ]\n",
"[-0.54866378 0.38172093 0. ]\n",
"[-0.55163018 0.37866297 0. ]\n",
"[-0.55455996 0.37725302 0. ]\n",
"[-0.55455996 0.37725302 0. ]\n",
"[-0.55580511 0.37606279 0. ]\n",
"[-0.55580511 0.37606279 0. ]\n",
"[-0.55580511 0.37606279 0. ]\n",
"[-0.55580511 0.37606279 0. ]\n",
"[-0.55580511 0.37606279 0. ]\n",
"[-0.55580511 0.37606279 0. ]\n",
"[-0.55580511 0.37606279 0. ]\n",
"[-0.55580511 0.37606279 0. ]\n",
"[-0.55580511 0.37606279 0. ]\n",
"[-0.55580511 0.37606279 0. ]\n",
"[-0.55580511 0.37606279 0. ]\n",
"[-0.55580511 0.37606279 0. ]\n",
"[-0.55580511 0.37805871 0. ]\n",
"[-0.55580511 0.38509017 0. ]\n",
"[-0.55580511 0.39550919 0. ]\n",
"[-0.54262112 0.41010314 0. ]\n",
"[-0.51883499 0.43590349 0. ]\n",
"[-0.49962664 0.45467237 0. ]\n",
"[-0.48486789 0.46743521 0. ]\n",
"[-0.46448763 0.48349405 0. ]\n",
"[-0.44559057 0.50603502 0. ]\n",
"[-0.43134453 0.51848657 0. ]\n",
"[-0.37551397 0.5772286 0. ]\n",
"[-0.32444429 0.5772286 0. ]\n",
"[-0.26894333 0.5772286 0. ]\n",
"[-0.23816236 0.5772286 0. ]\n",
"[-0.19276913 0.5772286 0. ]\n",
"[-0.17962176 0.5772286 0. ]\n",
"[-0.16101768 0.5772286 0. ]\n",
"[-0.14558141 0.5772286 0. ]\n",
"[-0.13164666 0.5772286 0. ]\n",
"[-0.12386444 0.5772286 0. ]\n",
"[-0.1195064 0.5772286 0. ]\n",
"[-0.11732738 0.5772286 0. ]\n",
"[-0.11732738 0.5772286 0. ]\n",
"[-0.11732738 0.5772286 0. ]\n",
"[-0.11562444 0.5772286 0. ]\n",
"[-0.11562444 0.5772286 0. ]\n",
"[-0.11562444 0.5772286 0. ]\n",
"[-0.11562444 0.5772286 0. ]\n",
"[-0.11562444 0.5772286 0. ]\n",
"[-0.11562444 0.5772286 0. ]\n",
"[-0.11562444 0.5772286 0. ]\n",
"[-0.11562444 0.5772286 0. ]\n",
"[-0.11562444 0.5772286 0. ]\n",
"[-0.11562444 0.5772286 0. ]\n",
"[-0.11386658 0.5772286 0. ]\n",
"[-0.11386658 0.5772286 0. ]\n",
"[-0.11386658 0.5772286 0. ]\n",
"[-0.11386658 0.5772286 0. ]\n",
"[-0.11386658 0.5772286 0. ]\n",
"[-0.11386658 0.5772286 0. ]\n",
"[-0.11386658 0.5772286 0. ]\n",
"[-0.11386658 0.5772286 0. ]\n",
"[-0.11386658 0.5772286 0. ]\n",
"[-0.11386658 0.5772286 0. ]\n",
"[-0.11386658 0.5772286 0. ]\n",
"[-0.11386658 0.5772286 0. ]\n",
"[-0.11386658 0.5772286 0. ]\n",
"[-0.11386658 0.5772286 0. ]\n",
"[-0.11386658 0.5772286 0. ]\n",
"[-0.11386658 0.5772286 0. ]\n",
"[-0.11386658 0.5772286 0. ]\n",
"[-0.11386658 0.5772286 0. ]\n",
"[-0.11386658 0.5772286 0. ]\n",
"[-0.11386658 0.5772286 0. ]\n",
"[-0.10932542 0.5772286 0. ]\n",
"[-0.1018728 0.5772286 0. ]\n",
"[-0.09473147 0.5772286 0. ]\n",
"[-0.08647316 0.5772286 0. ]\n",
"[-0.07587103 0.5772286 0. ]\n",
"[-0.06308988 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0.0743899 0.5772286 0. ]\n",
"[0.09202349 0.5772286 0. ]\n",
"[0.11027967 0.5772286 0. ]\n",
"[0.13591521 0.5772286 0. ]\n",
"[0.1660736 0.5772286 0. ]\n",
"[0.18806524 0.5772286 0. ]\n",
"[0.19681795 0.5772286 0. ]\n",
"[0.20485653 0.5772286 0. ]\n",
"[0.21181475 0.5772286 0. ]\n",
"[0.22184923 0.5772286 0. ]\n",
"[0.24675234 0.5772286 0. ]\n",
"[0.27652619 0.5772286 0. ]\n",
"[0.3040661 0.56624194 0. ]\n",
"[0.33396813 0.55547501 0. ]\n",
"[0.35520902 0.54620959 0. ]\n",
"[0.38241932 0.53234808 0. ]\n",
"[0.40541807 0.520336 0. ]\n",
"[0.41724704 0.51405529 0. ]\n",
"[0.42175157 0.5102832 0. ]\n",
"[0.4336721 0.50074311 0. ]\n",
"[0.44824774 0.48625903 0. ]\n",
"[0.46064436 0.47148197 0. ]\n",
"[0.47853431 0.45137637 0. ]\n",
"[0.48882515 0.4400418 0. ]\n",
"[0.49917093 0.42672962 0. ]\n",
"[0.50310782 0.4214194 0. ]\n",
"[0.51295919 0.40924252 0. ]\n",
"[0.54178087 0.37410351 0. ]\n",
"[0.55745518 0.34202245 0. ]\n",
"[0.56792913 0.32352823 0. ]\n",
"[0.57259846 0.31660663 0. ]\n",
"[0.58153428 0.31069215 0. ]\n",
"[0.58153428 0.30757926 0. ]\n",
"[0.58153428 0.30640735 0. ]\n",
"[0.58153428 0.30514388 0. ]\n",
"[0.58210193 0.30514388 0. ]\n",
"[0.58268788 0.30514388 0. ]\n",
"[0.58153428 0.30514388 0. ]\n",
"[0.58210193 0.30514388 0. ]\n",
"[0.58268788 0.30514388 0. ]\n",
"[0.58268788 0.30400859 0. ]\n",
"[0.58268788 0.30400859 0. ]\n",
"[0.58268788 0.30400859 0. ]\n",
"[0.58210193 0.30400859 0. ]\n",
"[0.58210193 0.30400859 0. ]\n",
"[0.58268788 0.30400859 0. ]\n",
"[0.58210193 0.30400859 0. ]\n",
"[0.58268788 0.30400859 0. ]\n",
"[0.58268788 0.30400859 0. ]\n",
"[0.58268788 0.30400859 0. ]\n",
"[0.58268788 0.30400859 0. ]\n",
"[0.58268788 0.30400859 0. ]\n",
"[0.58268788 0.30400859 0. ]\n",
"[0.58268788 0.30400859 0. ]\n",
"[0.57239704 0.32486494 0. ]\n",
"[0.55068007 0.37044129 0. ]\n",
"[0.35042982 0.5772286 0. ]\n",
"[0.19621368 0.5772286 0. ]\n",
"[0.17510098 0.5772286 0. ]\n",
"[0.16292409 0.5772286 0. ]\n",
"[0.15552641 0.5772286 0. ]\n",
"[0.1480921 0.5772286 0. ]\n",
"[0.12824286 0.5772286 0. ]\n",
"[0.11110367 0.5772286 0. ]\n",
"[0.09238972 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0. 0.5772286 0. ]\n",
"[0.06105941 0.5772286 0. ]\n",
"[0.06864021 0.5772286 0. ]\n",
"[0.07169817 0.5772286 0. ]\n",
"[0.07673372 0.5772286 0. ]\n",
"[0.09325034 0.5772286 0. ]\n",
"[0.31525418 0.54154026 0. ]\n",
"[0.34246449 0.53185368 0. ]\n",
"[0.40322073 0.50081636 0. ]\n",
"[0.44064863 0.47113406 0. ]\n",
"[0.47285787 0.44656055 0. ]\n",
"[0.49724826 0.42275611 0. ]\n",
"[0.51475368 0.40460981 0. ]\n",
"[0.52346977 0.39593034 0. ]\n",
"[0.54185412 0.38100679 0. ]\n",
"[0.5535366 0.36075471 0. ]\n",
"[0.58268788 0.33825036 0. ]\n",
"[0.58268788 0.29285713 0. ]\n",
"[0.58268788 0.23631244 0. ]\n",
"[0.58268788 0.2186056 0. ]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0.15953654 0.20578782 0. ]\n",
"[0.12967113 0.20831476 0. ]\n",
"[0.10777104 0.21007262 0. ]\n",
"[0.09321372 0.2118488 0. ]\n",
"[0.08314261 0.20633716 0. ]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n"
]
}
],
"source": [
"from Go2Py import ASSETS_PATH\n",
"import os\n",
"#checkpoint_path = os.path.join(ASSETS_PATH, 'checkpoints/SoloParkour/parkour_flat_test_4_24-21-28-37.pt')\n",
"#checkpoint_path = os.path.join(ASSETS_PATH, 'checkpoints/SoloParkour/parkour_flat_test_no_ang_rew_25-03-14-28.pt')\n",
"#checkpoint_path = os.path.join(ASSETS_PATH, 'checkpoints/SoloParkour/parkour_flat_test_base_ori_hard_25-02-53-00.pt')\n",
"checkpoint_path = os.path.join(ASSETS_PATH, 'checkpoints/SoloParkour/parkour_flat_test_solo_heading_yaw_deadzone_25-22-45-23.pt')\n",
"\n",
"controller = CaTController(robot, remote, checkpoint_path)\n",
"decimation = 20\n",
"fsm = FSM(robot, remote, safety_hypervisor, control_dT=decimation * robot.dt, user_controller_callback=controller.update)"
]
},
{
"cell_type": "code",
"execution_count": 8,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"[<matplotlib.lines.Line2D at 0x717aa3239990>]"
]
},
"execution_count": 8,
"metadata": {},
"output_type": "execute_result"
},
{
"data": {
"image/png": "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
"text/plain": [
"<Figure size 640x480 with 1 Axes>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"import matplotlib.pyplot as plt\n",
"plt.plot(np.array(controller.hist_data[\"body_angular_vel\"])[:, 0, 2])"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Slippage Analysis"
]
},
{
"cell_type": "code",
"execution_count": 8,
"metadata": {},
"outputs": [],
"source": [
"contacts = []\n",
"feet_vels = []\n",
"\n",
"while True:\n",
" if remote.xbox_controller.digital_cmd[1]:\n",
" break\n",
" contact_state = robot.getFootContact()>15\n",
" sites = ['FR_foot', 'FL_foot', 'RR_foot', 'RL_foot']\n",
" feet_vel = [np.linalg.norm(robot.getFootVelInWorld(s)) for s in sites]\n",
" contacts.append(contact_state)\n",
" feet_vels.append(feet_vel)\n",
" time.sleep(0.01)\n",
"\n",
"feet_vels = np.stack(feet_vels)\n",
"contacts = np.stack(contacts)"
]
},
{
"cell_type": "code",
"execution_count": 9,
"metadata": {},
"outputs": [
{
"data": {
"image/png": "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
"text/plain": [
"<Figure size 640x480 with 1 Axes>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"import matplotlib.pyplot as plt\n",
"start = 300\n",
"end = 1200\n",
"plt.plot(contacts[start:end,0])\n",
"plt.plot(feet_vels[start:end,0])\n",
"plt.legend(['contact state', 'foot velocity'])\n",
"plt.grid(True)\n",
"plt.tight_layout()\n",
"plt.savefig('foot_slipping_fric0.2.png')"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"**To Do**\n",
"- Train a policy without any actuator friction and check the plots for friction 0.2 and 0.6 \n",
"- Do the same experiment for the walk-these-ways policy\n",
"- While testing the walk these ways, check the output of the adaptation module for various friction numbers, any correlation?"
]
},
{
"cell_type": "code",
"execution_count": 8,
"metadata": {},
"outputs": [],
"source": [
"fsm.close()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Foot Contanct Analysis"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"np.array(controller.hist_data[\"body_pos\"])[0, 0, -1]"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"plt.plot(np.array(controller.hist_data[\"body_pos\"])[:, 0, -1])"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"import matplotlib.pyplot as plt\n",
"# Assuming 'controller.hist_data[\"torques\"]' is a dictionary with torque profiles\n",
"torques = np.array(controller.hist_data[\"body_linear_vel\"])[:, 0, :, 0]\n",
"\n",
"# Number of torque profiles\n",
"torque_nb = torques.shape[1]\n",
"\n",
"# Number of rows needed for the grid, with 3 columns per row\n",
"n_cols = 3\n",
"n_rows = int(np.ceil(torque_nb / n_cols))\n",
"\n",
"# Create the figure and axes for subplots\n",
"fig, axes = plt.subplots(n_rows, n_cols, figsize=(15, 5 * n_rows))\n",
"\n",
"# Flatten the axes array for easy indexing (in case of multiple rows)\n",
"axes = axes.flatten()\n",
"\n",
"# Plot each torque profile\n",
"for i in range(torque_nb):\n",
" axes[i].plot(np.arange(torques.shape[0]) * robot.dt * decimation, torques[:, i])\n",
" axes[i].set_title(f'Torque {i+1}')\n",
" axes[i].set_xlabel('Time')\n",
" axes[i].set_ylabel('Torque Value')\n",
" axes[i].grid(True)\n",
"\n",
"# Remove any empty subplots if torque_nb is not a multiple of 3\n",
"for j in range(torque_nb, len(axes)):\n",
" fig.delaxes(axes[j])\n",
"\n",
"# Adjust layout\n",
"plt.tight_layout()\n",
"plt.savefig(\"torque_profile.png\")\n",
"plt.show()"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"import matplotlib.pyplot as plt\n",
"# Assuming 'controller.hist_data[\"torques\"]' is a dictionary with torque profiles\n",
"torques = np.array(controller.hist_data[\"torques\"])\n",
"\n",
"# Number of torque profiles\n",
"torque_nb = torques.shape[1]\n",
"\n",
"# Number of rows needed for the grid, with 3 columns per row\n",
"n_cols = 3\n",
"n_rows = int(np.ceil(torque_nb / n_cols))\n",
"\n",
"# Create the figure and axes for subplots\n",
"fig, axes = plt.subplots(n_rows, n_cols, figsize=(15, 5 * n_rows))\n",
"\n",
"# Flatten the axes array for easy indexing (in case of multiple rows)\n",
"axes = axes.flatten()\n",
"\n",
"# Plot each torque profile\n",
"for i in range(torque_nb):\n",
" axes[i].plot(np.arange(torques.shape[0]) * robot.dt * decimation, torques[:, i])\n",
" axes[i].set_title(f'Torque {i+1}')\n",
" axes[i].set_xlabel('Time')\n",
" axes[i].set_ylabel('Torque Value')\n",
" axes[i].grid(True)\n",
"\n",
"# Remove any empty subplots if torque_nb is not a multiple of 3\n",
"for j in range(torque_nb, len(axes)):\n",
" fig.delaxes(axes[j])\n",
"\n",
"# Adjust layout\n",
"plt.tight_layout()\n",
"plt.savefig(\"torque_profile.png\")\n",
"plt.show()"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# Extract the joint position data for the first joint over time\n",
"joint_pos = np.array(controller.hist_data[\"joint_vel\"])[:, 0]\n",
"\n",
"# Number of data points in joint_pos\n",
"n_data_points = len(joint_pos)\n",
"\n",
"# Since you're plotting only one joint, no need for multiple subplots in this case.\n",
"# But to follow the grid requirement, we'll replicate the data across multiple subplots.\n",
"# For example, let's assume you want to visualize this data 9 times in a 3x3 grid.\n",
"\n",
"n_cols = 3\n",
"n_rows = int(np.ceil(torque_nb / n_cols))\n",
"\n",
"# Create the figure and axes for subplots\n",
"fig, axes = plt.subplots(n_rows, n_cols, figsize=(15, 5 * n_rows))\n",
"\n",
"# Flatten the axes array for easy indexing (in case of multiple rows)\n",
"axes = axes.flatten()\n",
"\n",
"# Plot the same joint position data in every subplot (as per grid requirement)\n",
"for i in range(n_rows * n_cols):\n",
" axes[i].plot(joint_pos[:, i])\n",
" axes[i].set_title(f'Joint Position {i+1}')\n",
" axes[i].set_xlabel('Time')\n",
" axes[i].set_ylabel('Position Value')\n",
"\n",
"# Adjust layout\n",
"plt.tight_layout()\n",
"plt.savefig(\"joint_position_profile.png\")\n",
"plt.show()"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"import matplotlib.pyplot as plt\n",
"# Assuming 'controller.hist_data[\"foot_contact_forces_mag\"]' is a dictionary with foot contact force magnitudes\n",
"foot_contact_forces_mag = np.array(controller.hist_data[\"foot_contact_forces_mag\"])\n",
"\n",
"# Number of feet (foot_nb)\n",
"foot_nb = foot_contact_forces_mag.shape[1]\n",
"\n",
"# Number of rows needed for the grid, with 3 columns per row\n",
"n_cols = 3\n",
"n_rows = int(np.ceil(foot_nb / n_cols))\n",
"\n",
"# Create the figure and axes for subplots\n",
"fig, axes = plt.subplots(n_rows, n_cols, figsize=(15, 5 * n_rows))\n",
"\n",
"# Flatten the axes array for easy indexing (in case of multiple rows)\n",
"axes = axes.flatten()\n",
"\n",
"# Plot each foot's contact force magnitude\n",
"for i in range(foot_nb):\n",
" axes[i].plot(foot_contact_forces_mag[:, i])\n",
" axes[i].set_title(f'Foot {i+1} Contact Force Magnitude')\n",
" axes[i].set_xlabel('Time')\n",
" axes[i].set_ylabel('Force Magnitude')\n",
"\n",
"# Remove any empty subplots if foot_nb is not a multiple of 3\n",
"for j in range(foot_nb, len(axes)):\n",
" fig.delaxes(axes[j])\n",
"\n",
"# Adjust layout\n",
"plt.tight_layout()\n",
"plt.savefig(\"foot_contact_profile.png\")\n",
"plt.show()"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# Extract the joint acceleration data for the first joint over time\n",
"joint_acc = np.array(controller.hist_data[\"joint_acc\"])[:, 0]\n",
"\n",
"# Number of data points in joint_acc\n",
"n_data_points = len(joint_acc)\n",
"\n",
"# Number of feet (foot_nb)\n",
"foot_nb = joint_acc.shape[1]\n",
"\n",
"# Number of rows needed for the grid, with 3 columns per row\n",
"n_cols = 3\n",
"n_rows = int(np.ceil(foot_nb / n_cols))\n",
"\n",
"# Create the figure and axes for subplots\n",
"fig, axes = plt.subplots(n_rows, n_cols, figsize=(15, 5 * n_rows))\n",
"\n",
"# Flatten the axes array for easy indexing\n",
"axes = axes.flatten()\n",
"\n",
"# Plot the same joint acceleration data in every subplot (as per grid requirement)\n",
"for i in range(n_rows * n_cols):\n",
" axes[i].plot(joint_acc[:, i])\n",
" axes[i].set_title(f'Joint Acceleration {i+1}')\n",
" axes[i].set_xlabel('Time')\n",
" axes[i].set_ylabel('Acceleration Value')\n",
"\n",
"# Adjust layout to prevent overlap\n",
"plt.tight_layout()\n",
"plt.show()"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# Extract the joint jerk data over time\n",
"joint_jerk = np.array(controller.hist_data[\"joint_jerk\"])[:, 0]\n",
"\n",
"# Number of data points in joint_jerk\n",
"n_data_points = len(joint_jerk)\n",
"\n",
"# Number of joints (assuming the second dimension corresponds to joints)\n",
"num_joints = joint_jerk.shape[1]\n",
"\n",
"# Number of columns per row in the subplot grid\n",
"n_cols = 3\n",
"# Number of rows needed for the grid\n",
"n_rows = int(np.ceil(num_joints / n_cols))\n",
"\n",
"# Create the figure and axes for subplots\n",
"fig, axes = plt.subplots(n_rows, n_cols, figsize=(15, 5 * n_rows))\n",
"\n",
"# Flatten the axes array for easy indexing\n",
"axes = axes.flatten()\n",
"\n",
"# Plot the joint jerk data for each joint\n",
"for i in range(num_joints):\n",
" axes[i].plot(joint_jerk[:, i])\n",
" axes[i].set_title(f'Joint Jerk {i+1}')\n",
" axes[i].set_xlabel('Time')\n",
" axes[i].set_ylabel('Jerk Value')\n",
"\n",
"# Hide any unused subplots\n",
"for i in range(num_joints, len(axes)):\n",
" fig.delaxes(axes[i])\n",
"\n",
"# Adjust layout to prevent overlap\n",
"plt.tight_layout()\n",
"plt.show()\n"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# Extract the foot contact rate data over time\n",
"foot_contact_rate = np.array(controller.hist_data[\"foot_contact_rate\"])[:, 0]\n",
"\n",
"# Number of data points in foot_contact_rate\n",
"n_data_points = foot_contact_rate.shape[0]\n",
"\n",
"# Number of feet (assuming the second dimension corresponds to feet)\n",
"num_feet = foot_contact_rate.shape[1]\n",
"\n",
"# Number of columns per row in the subplot grid\n",
"n_cols = 3\n",
"# Number of rows needed for the grid\n",
"n_rows = int(np.ceil(num_feet / n_cols))\n",
"\n",
"# Create the figure and axes for subplots\n",
"fig, axes = plt.subplots(n_rows, n_cols, figsize=(15, 5 * n_rows))\n",
"\n",
"# Flatten the axes array for easy indexing\n",
"axes = axes.flatten()\n",
"\n",
"# Plot the foot contact rate data for each foot\n",
"for i in range(num_feet):\n",
" axes[i].plot(foot_contact_rate[:, i])\n",
" axes[i].set_title(f'Foot Contact Rate {i+1}')\n",
" axes[i].set_xlabel('Time')\n",
" axes[i].set_ylabel('Contact Rate')\n",
"\n",
"# Hide any unused subplots\n",
"for i in range(num_feet, len(axes)):\n",
" fig.delaxes(axes[i])\n",
"\n",
"# Adjust layout to prevent overlap\n",
"plt.tight_layout()\n",
"plt.show()\n"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Test on Real Robot (ToDo)"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"pygame 2.6.1 (SDL 2.28.4, Python 3.10.12)\n",
"Hello from the pygame community. https://www.pygame.org/contribute.html\n"
]
}
],
"source": [
"from Go2Py.robot.fsm import FSM\n",
"from Go2Py.robot.remote import XBoxRemote\n",
"from Go2Py.robot.safety import SafetyHypervisor\n",
"from Go2Py.control.cat_parkour import *"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {},
"outputs": [],
"source": [
"from Go2Py.robot.interface import GO2Real\n",
"import numpy as np\n",
"robot = GO2Real(mode='lowlevel')"
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Put your stick at reset and do not touch it while calibrating\n"
]
}
],
"source": [
"remote = XBoxRemote() # KeyboardRemote()\n",
"safety_hypervisor = SafetyHypervisor(robot)"
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"{'q': [0.06103205680847168,\n",
" 1.305066704750061,\n",
" -2.814870834350586,\n",
" 0.03419072926044464,\n",
" 1.2731844186782837,\n",
" -2.831963300704956,\n",
" -0.35298794507980347,\n",
" 1.2857441902160645,\n",
" -2.8204946517944336,\n",
" 0.33624452352523804,\n",
" 1.2935254573822021,\n",
" -2.8247439861297607],\n",
" 'dq': [0.10076361894607544,\n",
" -0.05038180947303772,\n",
" 0.04246226325631142,\n",
" 0.027128666639328003,\n",
" 0.05813286080956459,\n",
" -0.010110062547028065,\n",
" -0.015502095222473145,\n",
" -0.03875523805618286,\n",
" 0.0,\n",
" -0.023253142833709717,\n",
" 0.0,\n",
" -0.02022012509405613],\n",
" 'tau_est': [-0.12369140982627869,\n",
" 0.12369140982627869,\n",
" -0.2844902276992798,\n",
" -0.17316797375679016,\n",
" -0.07421484589576721,\n",
" 0.3319052755832672,\n",
" 0.049476563930511475,\n",
" 0.049476563930511475,\n",
" 0.0,\n",
" 0.049476563930511475,\n",
" -0.024738281965255737,\n",
" 0.09483008086681366],\n",
" 'temperature': [25.0,\n",
" 23.0,\n",
" 25.0,\n",
" 24.0,\n",
" 23.0,\n",
" 25.0,\n",
" 25.0,\n",
" 25.0,\n",
" 26.0,\n",
" 25.0,\n",
" 25.0,\n",
" 25.0]}"
]
},
"execution_count": 4,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"robot.getJointStates()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Make sure the robot can take commands from python. The next cell should make the joints free to move (no damping)."
]
},
{
"cell_type": "code",
"execution_count": 5,
"metadata": {},
"outputs": [
{
"ename": "KeyboardInterrupt",
"evalue": "",
"output_type": "error",
"traceback": [
"\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
"\u001b[0;31mKeyboardInterrupt\u001b[0m Traceback (most recent call last)",
"Cell \u001b[0;32mIn[5], line 13\u001b[0m\n\u001b[1;32m 11\u001b[0m tau[\u001b[38;5;241m0\u001b[39m] \u001b[38;5;241m=\u001b[39m \u001b[38;5;241m0.0\u001b[39m\n\u001b[1;32m 12\u001b[0m robot\u001b[38;5;241m.\u001b[39msetCommands(q, dq, kp, kd, tau)\n\u001b[0;32m---> 13\u001b[0m \u001b[43mtime\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43msleep\u001b[49m\u001b[43m(\u001b[49m\u001b[38;5;241;43m0.02\u001b[39;49m\u001b[43m)\u001b[49m\n",
"\u001b[0;31mKeyboardInterrupt\u001b[0m: "
]
}
],
"source": [
"import numpy as np\n",
"import time\n",
"start_time = time.time()\n",
"\n",
"while time.time()-start_time < 30:\n",
" q = np.zeros(12) \n",
" dq = np.zeros(12)\n",
" kp = np.ones(12)*0.0\n",
" kd = np.ones(12)*0.0\n",
" tau = np.zeros(12)\n",
" tau[0] = 0.0\n",
" robot.setCommands(q, dq, kp, kd, tau)\n",
" time.sleep(0.02)"
]
},
{
"cell_type": "code",
"execution_count": 6,
"metadata": {},
"outputs": [],
"source": [
"def getRemote(remote):\n",
" commands = remote.getCommands()\n",
" commands[0] *= 0.6\n",
" commands[1] = max(commands[1], 0) * 0.6\n",
" zero_commands_xy = np.linalg.norm(commands[:2]) <= 0.2\n",
" if zero_commands_xy:\n",
" commands[:2] = np.zeros_like(commands[:2])\n",
" commands[2] = 0\n",
" print(commands)\n",
" return commands"
]
},
{
"cell_type": "code",
"execution_count": 7,
"metadata": {},
"outputs": [],
"source": [
"class CaTController:\n",
" def __init__(self, robot, remote, checkpoint):\n",
" self.remote = remote\n",
" self.robot = robot\n",
" self.policy = Policy(checkpoint)\n",
" self.command_profile = CommandInterface()\n",
" self.agent = CaTAgent(self.command_profile, self.robot)\n",
" self.hist_data = {}\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",
"\n",
" def update(self, robot, remote):\n",
" if not hasattr(self, \"obs\"):\n",
" self.init()\n",
" commands = getRemote(remote)\n",
" self.command_profile.yaw_vel_cmd = -commands[2]\n",
" self.command_profile.x_vel_cmd = commands[1]\n",
" self.command_profile.y_vel_cmd = -commands[0]\n",
"\n",
" self.obs = self.agent.get_obs()\n",
" action = self.policy(self.obs, self.policy_info)\n",
" _, self.ret, self.done, self.info = self.agent.step(action)\n",
" for key, value in self.info.items():\n",
" if key in self.hist_data:\n",
" self.hist_data[key].append(value)\n",
" else:\n",
" self.hist_data[key] = [value]\n",
"\n",
" "
]
},
{
"cell_type": "code",
"execution_count": 8,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Exported model has been tested with ONNXRuntime, and the result looks good!\n",
"p_gains: [20. 20. 20. 20. 20. 20. 20. 20. 20. 20. 20. 20.]\n"
]
},
{
"name": "stderr",
"output_type": "stream",
"text": [
"/home/Go2py/Go2Py/control/cat_parkour.py:100: FutureWarning: You are using `torch.load` with `weights_only=False` (the current default value), which uses the default pickle module implicitly. It is possible to construct malicious pickle data which will execute arbitrary code during unpickling (See https://github.com/pytorch/pytorch/blob/main/SECURITY.md#untrusted-models for more details). In a future release, the default value for `weights_only` will be flipped to `True`. This limits the functions that could be executed during unpickling. Arbitrary objects will no longer be allowed to be loaded via this mode unless they are explicitly allowlisted by the user via `torch.serialization.add_safe_globals`. We recommend you start setting `weights_only=True` for any use case where you don't have full control of the loaded file. Please open an issue on GitHub for any issues related to this experimental feature.\n",
" actor_sd = torch.load(checkpoint_path, map_location=\"cpu\")\n"
]
}
],
"source": [
"from Go2Py import ASSETS_PATH \n",
"import os\n",
"checkpoint_path = os.path.join(ASSETS_PATH, 'checkpoints/SoloParkour/parkour_flat_test_no_ang_rew_25-03-14-28.pt')\n",
"\n",
"\n",
"controller = CaTController(robot, remote, checkpoint_path)"
]
},
{
"cell_type": "code",
"execution_count": 9,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[ 0.20090864 -0. 0. ]\n",
"[ 0.20272144 -0. 0. ]\n",
"[ 0.20272144 -0. 0. ]\n",
"[ 0.20451593 -0. 0. ]\n",
"[ 0.20621887 -0. 0. ]\n",
"[ 0.20621887 -0. 0. ]\n",
"[ 0.20828802 -0. 0. ]\n",
"[ 0.21000926 -0. 0. ]\n",
"[ 0.21000926 -0. 0. ]\n",
"[ 0.21000926 -0. 0. ]\n",
"[ 0.21000926 -0. 0. ]\n",
"[ 0.21000926 -0. 0. ]\n",
"[ 0.21182206 -0. 0. ]\n",
"[ 0.21182206 -0. 0. ]\n",
"[ 0.21182206 -0. 0. ]\n",
"[ 0.21182206 -0. 0. ]\n",
"[ 0.21182206 -0. 0. ]\n",
"[ 0.21182206 -0. 0. ]\n",
"[ 0.21182206 -0. 0. ]\n",
"[ 0.21182206 -0. 0. ]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n",
"[0. 0. 0.]\n"
]
}
],
"source": [
"fsm = FSM(robot, remote, safety_hypervisor, control_dT=1./50., user_controller_callback=controller.update)"
]
},
{
"cell_type": "code",
"execution_count": 10,
"metadata": {},
"outputs": [],
"source": [
"fsm.close()"
]
},
{
"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.10.12"
}
},
"nbformat": 4,
"nbformat_minor": 2
}