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

1078 lines
402 KiB
Plaintext
Raw Normal View History

Go2 DDS bridge is added.
2025-01-25 12:01:51 +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": null,
"metadata": {},
"outputs": [],
"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.neuro_diff_sim 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": [
"remote = KeyboardRemote() # XBoxRemote() # KeyboardRemote()\n",
"robot.sitDownReset()\n",
"safety_hypervisor = SafetyHypervisor(robot)"
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {},
"outputs": [],
"source": [
"def getRemote(remote):\n",
" commands = remote.getCommands()\n",
" commands[0] *= 0.6\n",
" commands[1] *= 0.6\n",
" zero_commands_xy = np.linalg.norm(commands[:2]) <= 0.2\n",
" zero_commands_yaw = np.abs(commands[2]) <= 0.2\n",
" if zero_commands_xy:\n",
" commands[:2] = np.zeros_like(commands[:2])\n",
" if zero_commands_yaw:\n",
" commands[2] = 0\n",
" return commands"
]
},
{
"cell_type": "code",
"execution_count": 5,
"metadata": {},
"outputs": [],
"source": [
"class NeuroDiffSimController:\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 = NeuroDiffSimAgent(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": null,
"metadata": {},
"outputs": [],
"source": [
"robot.getJointStates()"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"from Go2Py import ASSETS_PATH\n",
"import os\n",
"checkpoint_path = os.path.join(ASSETS_PATH, 'checkpoints/neuro_diff_sim/neuro_diff_sim.pt')\n",
"\n",
"controller = NeuroDiffSimController(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": "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": null,
"metadata": {},
"outputs": [],
"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": null,
"metadata": {},
"outputs": [],
"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.neuro_diff_sim 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": null,
"metadata": {},
"outputs": [],
"source": [
"remote = XBoxRemote() # KeyboardRemote()\n",
"safety_hypervisor = SafetyHypervisor(robot)"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"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": null,
"metadata": {},
"outputs": [],
"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] *= 0.6\n",
" zero_commands = np.logical_and(\n",
" np.linalg.norm(commands[:2]) <= 0.2,\n",
" np.abs(commands[2]) <= 0.2\n",
" )\n",
" if zero_commands:\n",
" commands = np.zeros_like(commands)\n",
" return commands"
]
},
{
"cell_type": "code",
"execution_count": 7,
"metadata": {},
"outputs": [],
"source": [
"class NeuroDiffSimController:\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 = NeuroDiffSimAgent(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": null,
"metadata": {},
"outputs": [],
"source": [
"from Go2Py import ASSETS_PATH \n",
"import os\n",
"checkpoint_path = os.path.join(ASSETS_PATH, 'checkpoints/neuro_diff_sim/neuro_diff_sim.pt')\n",
"\n",
"controller = NeuroDiffSimController(robot, remote, checkpoint_path)"
]
},
{
"cell_type": "code",
"execution_count": 10,
"metadata": {},
"outputs": [],
"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": []
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {},
"outputs": [
{
"ename": "ModuleNotFoundError",
"evalue": "No module named 'rclpy'",
"output_type": "error",
"traceback": [
"\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
"\u001b[0;31mModuleNotFoundError\u001b[0m Traceback (most recent call last)",
"Cell \u001b[0;32mIn[1], line 1\u001b[0m\n\u001b[0;32m----> 1\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mGo2Py\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mutils\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mros2\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m PoindCloud2Bridge, ros2_init, ros2_close, ROS2ExecutorManager, ROS2CameraReader\n\u001b[1;32m 2\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mGo2Py\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mutils\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mros2\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m ROS2OdometryReader\n\u001b[1;32m 3\u001b[0m ros2_init()\n",
"File \u001b[0;32m/home/Go2py/Go2Py/utils/ros2.py:5\u001b[0m\n\u001b[1;32m 3\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mtime\u001b[39;00m\n\u001b[1;32m 4\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnumpy\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mas\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnp\u001b[39;00m\n\u001b[0;32m----> 5\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mrclpy\u001b[39;00m\n\u001b[1;32m 6\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mtf2_ros\u001b[39;00m\n\u001b[1;32m 7\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mrclpy\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mnode\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m Node\n",
"\u001b[0;31mModuleNotFoundError\u001b[0m: No module named 'rclpy'"
]
}
],
"source": [
"from Go2Py.utils.ros2 import PoindCloud2Bridge, ros2_init, ros2_close, ROS2ExecutorManager, ROS2CameraReader\n",
"from Go2Py.utils.ros2 import ROS2OdometryReader\n",
"ros2_init()\n",
"map_bridge = PoindCloud2Bridge('/Laser_map')\n",
"fast_lio_odom = ROS2OdometryReader('/Odometry', 'odometry_subscriber')\n",
"rgb_camera_bridge = ROS2CameraReader('/camera/color/image_raw', 'rgb_reader')\n",
"depth_camera_bridge = ROS2CameraReader('/camera/depth/image_rect_raw', 'depth_reader')\n",
"\n",
"ros2_exec_manager = ROS2ExecutorManager()\n",
"ros2_exec_manager.add_node(map_bridge)\n",
"ros2_exec_manager.add_node(fast_lio_odom)\n",
"ros2_exec_manager.add_node(rgb_camera_bridge)\n",
"ros2_exec_manager.add_node(depth_camera_bridge)\n",
"ros2_exec_manager.start()"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {},
"outputs": [],
"source": [
"import numpy as np\n",
"body_T_lidar = np.array([\n",
" [ 0.9743701, 0.0, 0.2249511, 0.1870 ],\n",
" [ 0.0, 1.0, 0.0, 0.0 ],\n",
" [-0.2249511, 0.0, 0.9743701, 0.0803 ],\n",
" [ 0.0, 0.0, 0.0, 1.0 ]\n",
"])"
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"array([[ 0.97319254, 0.00978982, -0.22978327, -0.29256741],\n",
" [-0.01415969, 0.99974875, -0.01737615, -0.05825406],\n",
" [ 0.22955543, 0.02016401, 0.97308665, -0.03344418],\n",
" [ 0. , 0. , 0. , 1. ]])"
]
},
"execution_count": 4,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"lio_T_lidar = fast_lio_odom.get_pose()\n",
"lio_T_body = lio_T_lidar @ np.linalg.inv(body_T_lidar)\n",
"lio_T_body"
]
},
{
"cell_type": "code",
"execution_count": 5,
"metadata": {},
"outputs": [],
"source": [
"import matplotlib.pyplot as plt\n",
"import cv2\n",
"depth = depth_camera_bridge.get_image()\n",
"depth = cv2.resize(depth, (320, 240))\n"
]
},
{
"cell_type": "code",
"execution_count": 6,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"15124"
]
},
"execution_count": 6,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"depth.max()"
]
},
{
"cell_type": "code",
"execution_count": 7,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"<matplotlib.image.AxesImage at 0xffff46306070>"
]
},
"execution_count": 7,
"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.imshow(rgb_camera_bridge.get_image())"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {},
"outputs": [],
"source": [
"import socket\n",
"import struct\n",
"import numpy as np\n",
"import time\n",
"import threading\n",
"from cyclonedds.domain import DomainParticipant\n",
"from cyclonedds.topic import Topic\n",
"from cyclonedds.sub import DataReader\n",
"from cyclonedds.pub import Publisher, DataWriter\n",
"from dataclasses import dataclass\n",
"from Go2DDSMsgs import PointCloud, RGBImage, DepthImage, Pose, HighCommand\n",
"from scipy.spatial.transform import Rotation as R\n",
"\n",
"\n",
"def get_last_msg(reader, topic_type):\n",
" \"\"\" \"\"\"\n",
" last_msg = reader.take()\n",
"\n",
" if last_msg:\n",
" while True:\n",
" a = reader.take()\n",
" if not a:\n",
" break\n",
" else:\n",
" last_msg = a\n",
" if last_msg:\n",
" msg = last_msg[0]\n",
" if type(msg) == topic_type:\n",
" return msg\n",
" else:\n",
" return None\n",
"\n",
" else:\n",
" return None\n",
" \n",
"class Go2DDSServer:\n",
" def __init__(self, robot_name):\n",
" self.robot_name = robot_name\n",
" self.domain = DomainParticipant()\n",
" self.rgb_topic = Topic(self.domain, f'{self.robot_name}_rgb', RGBImage)\n",
" self.depth_topic = Topic(self.domain, f'{self.robot_name}_depth', DepthImage) \n",
" self.map_topic = Topic(self.domain, f'{self.robot_name}_lio_map', PointCloud)\n",
" self.lidar_odom_topic = Topic(self.domain, f'{self.robot_name}_lio_odom', Pose)\n",
" self.high_cmd_topic = Topic(self.domain, f'{self.robot_name}_high_cmd', HighCommand)\n",
" self.rgb_publisher = Publisher(self.domain)\n",
" self.depth_publisher = Publisher(self.domain)\n",
" self.map_publisher = Publisher(self.domain)\n",
" self.lidar_odom_publisher = Publisher(self.domain)\n",
" self.high_cmd_reader = DataReader(self.domain, self.high_cmd_topic)\n",
" self.rgb_writer = DataWriter(self.rgb_publisher, self.rgb_topic)\n",
" self.depth_writer = DataWriter(self.depth_publisher, self.depth_topic)\n",
" self.map_writer = DataWriter(self.map_publisher, self.map_topic)\n",
" self.lidar_odom_writer = DataWriter(self.lidar_odom_publisher, self.lidar_odom_topic)\n",
" \n",
" \n",
" \n",
"\n",
" def sendRGB(self, rgb):\n",
" self.rgb_writer.write(RGBImage(data=rgb.reshape(-1).tolist(), width=rgb.shape[1], height=rgb.shape[0], timestamp=''))\n",
" \n",
" def sendDepth(self, depth):\n",
" self.depth_writer.write(DepthImage(data=depth.reshape(-1).tolist(), width=depth.shape[1], height=depth.shape[0], timestamp=''))\n",
"\n",
" def sendMap(self, pcd):\n",
" map_pcd_mgs = PointCloud(x = pcd[:,0].tolist(), \n",
" y = pcd[:,1].tolist(), \n",
" z = pcd[:,2].tolist(), \n",
" timestamp=''\n",
" )\n",
" self.map_writer.write(map_pcd_mgs)\n",
" \n",
" def sendLidarOdom(self, lio_T_body):\n",
" q = R.from_matrix(lio_T_body[:3,:3]).as_quat()\n",
" t = lio_T_body[:3,3]\n",
" pose_msg = Pose(quat=q.tolist(), trans=t.tolist(), timestamp='')\n",
" self.lidar_odom_writer.write(pose_msg)\n",
"\n",
" def getHighCmd(self):\n",
" return get_last_msg(self.high_cmd_reader, HighCommand) \n",
" \n",
"\n",
"# class Go2DDSClient:\n",
"# def __init__(self, robot_name):\n",
"# self.robot_name = robot_name\n",
"# self.domain = DomainParticipant()\n",
"# self.rgb_topic = Topic(self.domain, f'{self.robot_name}_rgb', RGBImage)\n",
"# self.depth_topic = Topic(self.domain, f'{self.robot_name}_depth', DepthImage) \n",
"# self.map_topic = Topic(self.domain, f'{self.robot_name}_lio_map', PointCloud)\n",
"# self.lidar_odom_topic = Topic(self.domain, f'{self.robot_name}_lio_odom', Pose)\n",
"# self.high_cmd_topic = Topic(self.domain, f'{self.robot_name}_high_cmd', HighCommand)\n",
"# self.rgb_reader = DataReader(self.domain, self.rgb_topic)\n",
"# self.depth_reader = DataReader(self.domain, self.depth_topic)\n",
"# self.map_reader = DataReader(self.domain, self.map_topic)\n",
"# self.lidar_odom_reader = DataReader(self.domain, self.lidar_odom_topic)\n",
"# self.high_cmd_writer = DataWriter(self.domain, self.high_cmd_topic)\n",
"\n",
"# def getRGB(self):\n",
"# return get_last_msg(self.rgb_reader, RGBImage)\n",
" \n",
"# def getDepth(self):\n",
"# return get_last_msg(self.depth_reader, DepthImage)\n",
" \n",
"# def getMap(self):\n",
"# return get_last_msg(self.map_reader, PointCloud)\n",
" \n",
"# def getLidarOdom(self):\n",
"# return get_last_msg(self.lidar_odom_reader, Pose)\n",
" \n",
"# def sendHighCmd(self, vx, vy, omega):\n",
"# self.high_cmd_writer.write(HighCommand(vx=vx, vy=vy, omega=omega, timestamp=''))\n",
" "
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {},
"outputs": [],
"source": [
"from dds_telemetry import Go2DDSServer, Go2DDSClient"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {},
"outputs": [],
"source": [
"server = Go2DDSServer(robot_name='go2')"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {},
"outputs": [],
"source": [
"client = Go2DDSClient(robot_name='go2')"
]
},
{
"cell_type": "code",
"execution_count": 6,
"metadata": {},
"outputs": [],
"source": [
"img = client.getRGB()"
]
},
{
"cell_type": "code",
"execution_count": 19,
"metadata": {},
"outputs": [],
"source": [
"client.sendHighCmd(0,0,0)"
]
},
{
"cell_type": "code",
"execution_count": 23,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"HighCommand(vx=0.0, vy=0.0, omega=0.0, timestamp='')"
]
},
"execution_count": 23,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"server.getHighCmd()"
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {},
"outputs": [],
"source": [
"import numpy as np\n",
"rgb = np.random.randint(0, 255, (10, 10, 3)).astype(np.uint8)\n",
"server.sendRGB(rgb)\n"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "usr",
"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.10"
}
},
"nbformat": 4,
"nbformat_minor": 2
}