277 lines
115 KiB
Plaintext
277 lines
115 KiB
Plaintext
|
{
|
||
|
|
"cells": [
|
||
|
|
{
|
||
|
|
"cell_type": "markdown",
|
||
|
|
"metadata": {},
|
||
|
|
"source": [
|
||
|
|
"# Lowlevel Control"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 2,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [
|
||
|
|
{
|
||
|
|
"name": "stdout",
|
||
|
|
"output_type": "stream",
|
||
|
|
"text": [
|
||
|
|
"pygame 2.5.2 (SDL 2.28.2, Python 3.8.18)\n",
|
||
|
|
"Hello from the pygame community. https://www.pygame.org/contribute.html\n"
|
||
|
|
]
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"source": [
|
||
|
|
"from Go2Py.robot.interface import GO2Real\n",
|
||
|
|
"import time\n",
|
||
|
|
"robot = GO2Real(mode='lowlevel')"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 3,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [
|
||
|
|
{
|
||
|
|
"data": {
|
||
|
|
"text/plain": [
|
||
|
|
"{'q': [-0.053992271423339844,\n",
|
||
|
|
" 1.2556536197662354,\n",
|
||
|
|
" -2.810431957244873,\n",
|
||
|
|
" 0.05044972896575928,\n",
|
||
|
|
" 1.2568042278289795,\n",
|
||
|
|
" -2.7854413986206055,\n",
|
||
|
|
" -0.37021589279174805,\n",
|
||
|
|
" 1.2639139890670776,\n",
|
||
|
|
" -2.7981419563293457,\n",
|
||
|
|
" 0.3528062701225281,\n",
|
||
|
|
" 1.2717256546020508,\n",
|
||
|
|
" -2.8037500381469727],\n",
|
||
|
|
" 'dq': [-0.011626571416854858,\n",
|
||
|
|
" -0.03100419044494629,\n",
|
||
|
|
" 0.028308173641562462,\n",
|
||
|
|
" 0.046506285667419434,\n",
|
||
|
|
" 0.027128666639328003,\n",
|
||
|
|
" -0.04246226325631142,\n",
|
||
|
|
" -0.03100419044494629,\n",
|
||
|
|
" 0.05038180947303772,\n",
|
||
|
|
" 0.02022012509405613,\n",
|
||
|
|
" -0.03100419044494629,\n",
|
||
|
|
" -0.01937761902809143,\n",
|
||
|
|
" -0.02022012509405613],\n",
|
||
|
|
" 'tau_est': [-0.024738281965255737,\n",
|
||
|
|
" 0.07421484589576721,\n",
|
||
|
|
" -0.04741504043340683,\n",
|
||
|
|
" -0.07421484589576721,\n",
|
||
|
|
" -0.14842969179153442,\n",
|
||
|
|
" 0.04741504043340683,\n",
|
||
|
|
" -0.024738281965255737,\n",
|
||
|
|
" 0.024738281965255737,\n",
|
||
|
|
" 0.0,\n",
|
||
|
|
" 0.049476563930511475,\n",
|
||
|
|
" -0.049476563930511475,\n",
|
||
|
|
" 0.0],\n",
|
||
|
|
" 'temperature': [32.0,\n",
|
||
|
|
" 29.0,\n",
|
||
|
|
" 28.0,\n",
|
||
|
|
" 31.0,\n",
|
||
|
|
" 28.0,\n",
|
||
|
|
" 28.0,\n",
|
||
|
|
" 32.0,\n",
|
||
|
|
" 29.0,\n",
|
||
|
|
" 27.0,\n",
|
||
|
|
" 33.0,\n",
|
||
|
|
" 30.0,\n",
|
||
|
|
" 28.0]}"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
"execution_count": 3,
|
||
|
|
"metadata": {},
|
||
|
|
"output_type": "execute_result"
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"source": [
|
||
|
|
"robot.getJointStates()"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 4,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [],
|
||
|
|
"source": [
|
||
|
|
"from Go2Py.utils.ros2 import ROS2ExecutorManager, ROS2TFInterface\n",
|
||
|
|
"import rclpy\n",
|
||
|
|
"rclpy.init() \n",
|
||
|
|
"executor_manager = ROS2ExecutorManager()\n",
|
||
|
|
"vicon_sensor = ROS2TFInterface('vicon/World', 'vicon/GO2/GO2', node_name='vicon_sensor_node')\n",
|
||
|
|
"executor_manager.add_node(vicon_sensor)\n",
|
||
|
|
"executor_manager.start()"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 32,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [
|
||
|
|
{
|
||
|
|
"data": {
|
||
|
|
"text/plain": [
|
||
|
|
"array([[-0.80060801, 0.58043764, 0.14872442, 0.26726318],\n",
|
||
|
|
" [-0.58674233, -0.80977164, 0.00182437, -0.42307191],\n",
|
||
|
|
" [ 0.12149175, -0.08580231, 0.988877 , 0.3776654 ],\n",
|
||
|
|
" [ 0. , 0. , 0. , 1. ]])"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
"execution_count": 32,
|
||
|
|
"metadata": {},
|
||
|
|
"output_type": "execute_result"
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"source": [
|
||
|
|
"vicon_sensor.get_pose()"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 48,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [],
|
||
|
|
"source": [
|
||
|
|
"dT = 0.001\n",
|
||
|
|
"duration = 60\n",
|
||
|
|
"\n",
|
||
|
|
"qs = []\n",
|
||
|
|
"dqs = []\n",
|
||
|
|
"taus =[]\n",
|
||
|
|
"Ts = []\n",
|
||
|
|
"stamps = []\n",
|
||
|
|
"for i in range(int(duration/dT)):\n",
|
||
|
|
" stamps.append(time.time())\n",
|
||
|
|
" state = robot.getJointStates()\n",
|
||
|
|
" q = state['q']\n",
|
||
|
|
" dq = state['dq']\n",
|
||
|
|
" tau = state['tau_est']\n",
|
||
|
|
" world_T_vicon = vicon_sensor.get_pose()\n",
|
||
|
|
" qs.append(q)\n",
|
||
|
|
" dqs.append(dq)\n",
|
||
|
|
" taus.append(tau)\n",
|
||
|
|
" Ts.append(world_T_vicon.reshape(1,4,4))\n",
|
||
|
|
" time.sleep(dT)\n",
|
||
|
|
"qs = np.vstack(qs)\n",
|
||
|
|
"dqs = np.vstack(dqs)\n",
|
||
|
|
"taus = np.vstack(taus)\n",
|
||
|
|
"Ts = np.vstack(Ts)"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 49,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [],
|
||
|
|
"source": [
|
||
|
|
"import pickle\n",
|
||
|
|
"dataset = {\n",
|
||
|
|
" 'q':qs,\n",
|
||
|
|
" 'dq':dqs,\n",
|
||
|
|
" 'tau':taus, \n",
|
||
|
|
" 'world_T_marker':Ts\n",
|
||
|
|
"}\n",
|
||
|
|
"with open('datasets/walking.pkl', 'wb') as f:\n",
|
||
|
|
" pickle.dump(dataset, f)\n",
|
||
|
|
" "
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 50,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [
|
||
|
|
{
|
||
|
|
"data": {
|
||
|
|
"text/plain": [
|
||
|
|
"[<matplotlib.lines.Line2D at 0x7fad6029f640>,\n",
|
||
|
|
" <matplotlib.lines.Line2D at 0x7fadb07e6eb0>,\n",
|
||
|
|
" <matplotlib.lines.Line2D at 0x7fad6034e460>,\n",
|
||
|
|
" <matplotlib.lines.Line2D at 0x7fad60db2b80>,\n",
|
||
|
|
" <matplotlib.lines.Line2D at 0x7fad609d19d0>,\n",
|
||
|
|
" <matplotlib.lines.Line2D at 0x7fad609d1190>,\n",
|
||
|
|
" <matplotlib.lines.Line2D at 0x7fadb04f6160>,\n",
|
||
|
|
" <matplotlib.lines.Line2D at 0x7fad610919a0>,\n",
|
||
|
|
" <matplotlib.lines.Line2D at 0x7fad6029f580>,\n",
|
||
|
|
" <matplotlib.lines.Line2D at 0x7fad6028b820>,\n",
|
||
|
|
" <matplotlib.lines.Line2D at 0x7fad59ffd040>,\n",
|
||
|
|
" <matplotlib.lines.Line2D at 0x7fad6029fcd0>]"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
"execution_count": 50,
|
||
|
|
"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(qs[:58000,:])"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 40,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [
|
||
|
|
{
|
||
|
|
"data": {
|
||
|
|
"text/plain": [
|
||
|
|
"(60000, 12)"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
"execution_count": 40,
|
||
|
|
"metadata": {},
|
||
|
|
"output_type": "execute_result"
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"source": [
|
||
|
|
"qs.shape"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": null,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [],
|
||
|
|
"source": []
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"metadata": {
|
||
|
|
"kernelspec": {
|
||
|
|
"display_name": "Python 3",
|
||
|
|
"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": 4
|
||
|
|
}
|