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/compliance_test.ipynb

461 lines
128 KiB
Plaintext
Raw Normal View History

go2py bridge bug fix
2024-05-01 10:32:27 +08:00
{
"cells": [
{
"cell_type": "code",
"execution_count": 1,
"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.ros2 import GO2Real, ros2_init, ROS2ExecutorManager\n",
"import time\n",
"ros2_init()\n",
"robot = GO2Real(mode='lowlevel')\n",
"ros2_exec_manager = ROS2ExecutorManager()\n",
"ros2_exec_manager.add_node(robot)\n",
"ros2_exec_manager.start()"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {},
"outputs": [],
"source": [
"from Go2Py.robot.model import Go2Model\n",
"model = Go2Model()"
]
},
{
"cell_type": "code",
"execution_count": 48,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"{'q': array([ 0.19625151, 0.62121832, -1.45901692, -0.8040427 , 0.15673077,\n",
" -1.30231106, 0.76652366, 0.2306025 , -1.40467536, -0.80055571,\n",
" 0.02943856, -1.08567142]),\n",
" 'dq': array([ 0.10076362, 0. , -0.00202201, 0.03875524, -0.01162657,\n",
" -0.0161761 , 0.01937762, 0.00775105, 0.00808805, 0.05425733,\n",
" 0. , -0.03639622]),\n",
" 'ddq': array([0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.]),\n",
" 'tau_est': array([-0.14842969, 0.04947656, -0.23707521, -0.14842969, -0.12369141,\n",
" 0.28449023, 0. , 0. , -0.04741504, 0.02473828,\n",
" -0.02473828, 0.04741504]),\n",
" 'temperature': array([35., 31., 30., 32., 30., 29., 35., 30., 29., 35., 30., 29.])}"
]
},
"execution_count": 48,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"robot.getJointStates()"
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {},
"outputs": [],
"source": [
"\n",
"import socket\n",
"import struct\n",
"import threading\n",
"import time\n",
"class AtiFTSensor:\n",
" def __init__(self):\n",
" self.initialized_ = False\n",
" self.going_ = False\n",
" self.streaming_ = False\n",
" self.F_bias_ = [0.0] * 3\n",
" self.T_bias_ = [0.0] * 3\n",
" self.F_ = [0.0] * 3\n",
" self.T_ = [0.0] * 3\n",
" self.rdt_sequence_ = 0\n",
" self.ft_sequence_ = 0\n",
" self.status_ = 0\n",
" self.count_per_force_ = 1000000.0\n",
" self.count_per_torque_ = 1000000.0\n",
" self.force_torque_ = [0.0] * 6\n",
" self.mutex_ = threading.Lock()\n",
" self.socket_ = None\n",
" self.local_address_ = ('', 49152)\n",
" self.remote_address_ = ('192.168.4.1', 49152)\n",
"\n",
" def initialize(self):\n",
" if self.initialized_:\n",
" print(\"warning already initialized\")\n",
" return True\n",
" print(\"initializing\")\n",
" \n",
" try:\n",
" self.socket_ = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)\n",
" self.socket_.bind(self.local_address_)\n",
" self.socket_.connect(self.remote_address_)\n",
" except socket.error as e:\n",
" print(f\"Error: {e}\")\n",
" return False\n",
"\n",
" print(\"created sockets\")\n",
" self.initialized_ = True\n",
" return self.initialized_\n",
"\n",
" def read_ft(self):\n",
" msg = struct.pack('>HHH', 0x1234, 0x0002, 0)\n",
" self.socket_.send(msg)\n",
"\n",
" internal_going = True\n",
"\n",
" while internal_going:\n",
" data = self.socket_.recv(36) # Assuming message size is 48 bytes\n",
" if len(data) != 36:\n",
" print(f\"Received message of unexpected length {len(data)}\")\n",
"\n",
" self.mutex_.acquire()\n",
" # print(data)\n",
" # breakpoint()\n",
" (self.rdt_sequence_, self.ft_sequence_, self.status_, \n",
" self.Fx, self.Fy, self.Fz, \n",
" self.Tx, self.Ty, self.Tz) = struct.unpack('>3I6i', data)\n",
" \n",
" \n",
" self.F_[0] = self.Fx / self.count_per_force_ - self.F_bias_[0]\n",
" self.F_[1] = self.Fy / self.count_per_force_ - self.F_bias_[1]\n",
" self.F_[2] = self.Fz / self.count_per_force_ - self.F_bias_[2]\n",
" self.T_[0] = self.Tx / self.count_per_torque_ - self.T_bias_[0]\n",
" self.T_[1] = self.Ty / self.count_per_torque_ - self.T_bias_[1]\n",
" self.T_[2] = self.Tz / self.count_per_torque_ - self.T_bias_[2]\n",
"\n",
" if self.streaming_:\n",
" # Implement streaming logic here\n",
" pass\n",
"\n",
" internal_going = self.going_\n",
" self.mutex_.release()\n",
"\n",
" def set_bias(self, force=None, torque=None):\n",
" self.mutex_.acquire()\n",
" for i in range(3):\n",
" if force is None:\n",
" self.F_bias_[i] = self.F_[i]\n",
" else:\n",
" self.F_bias_[i] = force[i]\n",
"\n",
" if torque is None:\n",
" self.T_bias_[i] = self.T_[i]\n",
" else:\n",
" self.T_bias_[i] = torque[i]\n",
" self.mutex_.release()\n",
"\n",
" def reset_bias(self):\n",
" self.mutex_.acquire()\n",
" self.F_bias_ = [0.0] * 3\n",
" self.T_bias_ = [0.0] * 3\n",
" self.mutex_.release()\n",
"\n",
" def get_status(self):\n",
" self.mutex_.acquire()\n",
" status = (self.rdt_sequence_, self.ft_sequence_, self.status_)\n",
" self.mutex_.release()\n",
" return status\n",
"\n",
" def get_ft(self):\n",
" self.mutex_.acquire()\n",
" ft = (self.F_[:], self.T_[:])\n",
" self.mutex_.release()\n",
" return ft\n",
"\n",
" def get_ft_vector(self):\n",
" self.mutex_.acquire()\n",
" ft_vector = self.F_ + self.T_\n",
" self.mutex_.release()\n",
" return ft_vector\n",
"\n",
" def stop(self):\n",
" if self.initialized_:\n",
" self.mutex_.acquire()\n",
" self.going_ = False\n",
" self.mutex_.release()\n",
" self.socket_.close()\n",
" self.initialized_ = False\n",
"\n",
" def stream(self, stream):\n",
" self.mutex_.acquire()\n",
" self.streaming_ = stream\n",
" self.mutex_.release()\n",
"\n",
" def __del__(self):\n",
" self.stop()\n"
]
},
{
"cell_type": "code",
"execution_count": 5,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"initializing\n",
"created sockets\n"
]
},
{
"data": {
"text/plain": [
"True"
]
},
"execution_count": 5,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"sensor = AtiFTSensor()\n",
"sensor.initialize()"
]
},
{
"cell_type": "code",
"execution_count": 6,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"5.372439"
]
},
"execution_count": 6,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"sensor.read_ft() \n",
"f, t = sensor.get_ft() # R\n",
"f[2]"
]
},
{
"cell_type": "code",
"execution_count": 7,
"metadata": {},
"outputs": [],
"source": [
"fz_bias = 0.0\n",
"for i in range(300):\n",
" time.sleep(0.01)\n",
" sensor.read_ft() \n",
" f, t = sensor.get_ft()\n",
" fz_bias += f[2]\n",
"\n",
"fz_bias /=1000"
]
},
{
"cell_type": "code",
"execution_count": 72,
"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[72], line 21\u001b[0m\n\u001b[1;32m 19\u001b[0m T \u001b[38;5;241m=\u001b[39m np\u001b[38;5;241m.\u001b[39meye(\u001b[38;5;241m4\u001b[39m)\n\u001b[1;32m 20\u001b[0m T[\u001b[38;5;241m0\u001b[39m:\u001b[38;5;241m3\u001b[39m,\u001b[38;5;241m0\u001b[39m:\u001b[38;5;241m3\u001b[39m]\u001b[38;5;241m=\u001b[39mR\n\u001b[0;32m---> 21\u001b[0m \u001b[43mmodel\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mupdateAllPose\u001b[49m\u001b[43m(\u001b[49m\u001b[43mstate\u001b[49m\u001b[43m[\u001b[49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[38;5;124;43mq\u001b[39;49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[43m]\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mstate\u001b[49m\u001b[43m[\u001b[49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[38;5;124;43mdq\u001b[39;49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[43m]\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mT\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mnp\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mzeros\u001b[49m\u001b[43m(\u001b[49m\u001b[38;5;241;43m6\u001b[39;49m\u001b[43m)\u001b[49m\u001b[43m)\u001b[49m\n\u001b[1;32m 22\u001b[0m info \u001b[38;5;241m=\u001b[39m model\u001b[38;5;241m.\u001b[39mgetInfo()\n\u001b[1;32m 23\u001b[0m FR_nle \u001b[38;5;241m=\u001b[39m info[\u001b[38;5;124m'\u001b[39m\u001b[38;5;124mnle\u001b[39m\u001b[38;5;124m'\u001b[39m][\u001b[38;5;241m6\u001b[39m:][\u001b[38;5;241m0\u001b[39m:\u001b[38;5;241m3\u001b[39m]\n",
"File \u001b[0;32m~/projects/rooholla/locomotion/Go2Py/Go2Py/robot/model.py:213\u001b[0m, in \u001b[0;36mGo2Model.updateAllPose\u001b[0;34m(self, q, dq, T, v)\u001b[0m\n\u001b[1;32m 211\u001b[0m q_ \u001b[38;5;241m=\u001b[39m np\u001b[38;5;241m.\u001b[39mhstack([pin\u001b[38;5;241m.\u001b[39mSE3ToXYZQUATtuple(pin\u001b[38;5;241m.\u001b[39mSE3(T)), q[\u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mq_reordering_idx]])\n\u001b[1;32m 212\u001b[0m dq_ \u001b[38;5;241m=\u001b[39m np\u001b[38;5;241m.\u001b[39mhstack([v, dq[\u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mq_reordering_idx]])\n\u001b[0;32m--> 213\u001b[0m \u001b[38;5;28;43mself\u001b[39;49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mupdateKinematics\u001b[49m\u001b[43m(\u001b[49m\u001b[43mq_\u001b[49m\u001b[43m)\u001b[49m\n\u001b[1;32m 214\u001b[0m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mupdateDynamics(q_, dq_)\n",
"File \u001b[0;32m~/projects/rooholla/locomotion/Go2Py/Go2Py/robot/model.py:153\u001b[0m, in \u001b[0;36mGo2Model.updateKinematics\u001b[0;34m(self, q)\u001b[0m\n\u001b[1;32m 151\u001b[0m Jb \u001b[38;5;241m=\u001b[39m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mrobot\u001b[38;5;241m.\u001b[39mgetFrameJacobian(\u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mrobot\u001b[38;5;241m.\u001b[39mmodel\u001b[38;5;241m.\u001b[39mgetFrameId(ef_frame), pin\u001b[38;5;241m.\u001b[39mReferenceFrame\u001b[38;5;241m.\u001b[39mLOCAL)\n\u001b[1;32m 152\u001b[0m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mef_Jw_[ef_frame]\u001b[38;5;241m=\u001b[39mJw[:, \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mdq_reordering_idx]\n\u001b[0;32m--> 153\u001b[0m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mef_Jb_[ef_frame]\u001b[38;5;241m=\u001b[39m\u001b[43mJb\u001b[49m\u001b[43m[\u001b[49m\u001b[43m:\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;28;43mself\u001b[39;49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mdq_reordering_idx\u001b[49m\u001b[43m]\u001b[49m\n",
"\u001b[0;31mKeyboardInterrupt\u001b[0m: "
]
}
],
"source": [
"import numpy as np\n",
"import pinocchio as pin\n",
"import time\n",
"vel = []\n",
"data_force = []\n",
"data_error = []\n",
"x0 = 0.23\n",
"z0 = -0.33\n",
"q = 12*[0.0] \n",
"dq = 12*[0.0] \n",
"kp = 12*[0.0] \n",
"kd = 12*[0.0] \n",
"tau = 12*[0.0] \n",
"for i in range(1000000):\n",
" state = robot.getJointStates()\n",
" Quat = robot.getIMU()['quat']\n",
" R = pin.Quaternion(np.hstack([Quat[1:],Quat[0]])).matrix()\n",
" R=np.eye(3)\n",
" T = np.eye(4)\n",
" T[0:3,0:3]=R\n",
" model.updateAllPose(state['q'], state['dq'], T, np.zeros(6))\n",
" info = model.getInfo()\n",
" FR_nle = info['nle'][6:][0:3]\n",
" FR_position = model.forwardKinematics(T, state['q'])['FR_foot'][0:3,-1]\n",
" FR_J = info['J_w']['FR_foot'][0:3, 6:][:,0:3]\n",
" FR_vel = FR_J@state['dq'][0:3].reshape(3,1)\n",
" \n",
" z_des = z0 \n",
" x_des = x0\n",
" task_kp = 650.0\n",
" fz = task_kp*(z_des - FR_position[2])\n",
" fx = 200*(x_des - FR_position[0]) + 4.5*(0-FR_vel[0])\n",
" Fx = np.array([fx[0],\n",
" 0,\n",
" fz]).reshape(3,1)\n",
" \n",
" tau_cmd = FR_J.T@Fx\n",
" data_error.append(z_des - FR_position[2])\n",
" sensor.read_ft() \n",
" f, t = sensor.get_ft() # R\n",
" data_force.append(f[2]-fz_bias)\n",
" \n",
"\n",
" kp[0] = 50.0\n",
" kd[0] = 2.0\n",
" # print(tau_cmd[1], x_des - FR_position[0], FR_vel[0])\n",
" # vel.append(FR_vel[0])\n",
" tau[1] = FR_nle[1]+tau_cmd[1].squeeze()\n",
" tau[2] = FR_nle[2]+tau_cmd[2].squeeze()\n",
" # tau[1] = 15.0* (0.7856252789497375-state['q'][1]) + 0.5* (0.-state['dq'][1])\n",
" robot.setCommands(q, dq, kp, kd, tau)\n",
" time.sleep(0.0007) "
]
},
{
"cell_type": "code",
"execution_count": 73,
"metadata": {},
"outputs": [],
"source": [
"import matplotlib.pyplot as plt\n",
"import numpy as np\n",
"\n",
"data_error = np.array(data_error)\n",
"data_force = np.array(data_force)"
]
},
{
"cell_type": "code",
"execution_count": 74,
"metadata": {},
"outputs": [],
"source": [
"import pickle\n",
"with open(f'go2_impact_test2_{task_kp}.pkl', 'wb') as f:\n",
" pickle.dump({\n",
" 'P':kp,\n",
" 'z_error':data_error,\n",
" 'ATI_Fz':data_force\n",
" }, f)"
]
},
{
"cell_type": "code",
"execution_count": 75,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"[<matplotlib.lines.Line2D at 0x7f8317a1ddf0>]"
]
},
"execution_count": 75,
"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": [
"plt.plot(data_force)"
]
},
{
"cell_type": "code",
"execution_count": 77,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"Text(0.5, 0, 'Error(meters)')"
]
},
"execution_count": 77,
"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": [
"start = 0\n",
"end = -1\n",
"plt.plot(data_error[start:end], data_force[start:end])\n",
"plt.grid(True)\n",
"plt.ylabel('Fz (N)')\n",
"plt.xlabel('Error(meters)')"
]
}
],
"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
}