From a24bfbb5b29a150964e6fae4beb4f795a83a4f5b Mon Sep 17 00:00:00 2001 From: Rooholla-KhorramBakht Date: Sun, 5 May 2024 00:11:18 -0400 Subject: [PATCH] go2py bridge major update --- .../robot/{interface/dds.py => interface.py} | 87 +++-- Go2Py/robot/interface/__init__.py | 0 Go2Py/robot/interface/ros2.py | 313 ------------------ Makefile | 3 + dds_messages/idls/BmsState_.idl | 49 --- dds_messages/idls/IMUState_.idl | 33 -- dds_messages/idls/LowState_.idl | 70 ---- dds_messages/idls/MotorState_.idl | 37 --- deploy/ros2_nodes/go2py_bridge/CMakeLists.txt | 4 +- deploy/ros2_nodes/go2py_bridge/package.xml | 2 +- deploy/ros2_nodes/go2py_bridge/src/bridge.cpp | 250 ++++++-------- .../ros2_nodes/go2py_common/msg/AudioData.msg | 2 - deploy/ros2_nodes/go2py_common/msg/BmsCmd.msg | 2 - .../ros2_nodes/go2py_common/msg/BmsState.msg | 9 - deploy/ros2_nodes/go2py_common/msg/Error.msg | 2 - .../go2py_common/msg/Go2FrontVideoData.msg | 4 - .../ros2_nodes/go2py_common/msg/HeightMap.msg | 15 - .../ros2_nodes/go2py_common/msg/IMUState.msg | 5 - .../go2py_common/msg/InterfaceConfig.msg | 3 - .../go2py_common/msg/LidarState.msg | 17 - deploy/ros2_nodes/go2py_common/msg/LowCmd.msg | 15 - .../ros2_nodes/go2py_common/msg/LowState.msg | 22 -- .../ros2_nodes/go2py_common/msg/MotorCmd.msg | 7 - .../go2py_common/msg/MotorState.msg | 11 - .../ros2_nodes/go2py_common/msg/PathPoint.msg | 7 - deploy/ros2_nodes/go2py_common/msg/Req.msg | 2 - deploy/ros2_nodes/go2py_common/msg/Res.msg | 3 - .../go2py_common/msg/SportModeCmd.msg | 13 - .../go2py_common/msg/SportModeState.msg | 17 - .../ros2_nodes/go2py_common/msg/TimeSpec.msg | 5 - .../ros2_nodes/go2py_common/msg/UwbState.msg | 19 -- .../ros2_nodes/go2py_common/msg/UwbSwitch.msg | 1 - .../go2py_common/msg/WirelessController.msg | 5 - .../CMakeLists.txt | 26 +- .../go2py_messages/msg/Go2pyHighCmd.msg | 3 + .../msg/Go2pyLowCmd.msg | 0 .../go2py_messages/msg/Go2pyState.msg | 13 + .../msg/Go2pyStatus.msg | 0 .../package.xml | 2 +- ...DS.ipynb => 00-robot-interface copy.ipynb} | 36 +- examples/01-robot-interface-ROS2.ipynb | 156 --------- examples/01-ros2-tools.ipynb | 111 +++++++ go2py_common/.idlpy_manifest | 3 - go2py_common/msg/.idlpy_manifest | 3 - go2py_common/msg/__init__.py | 9 - go2py_common/msg/dds_/.idlpy_manifest | 3 - go2py_common/msg/dds_/__init__.py | 9 - go2py_messages/.idlpy_manifest | 11 + {go2py_common => go2py_messages}/__init__.py | 2 +- go2py_messages/msg/.idlpy_manifest | 11 + .../msg/__init__.py | 2 +- go2py_messages/msg/dds_/.idlpy_manifest | 11 + .../msg/dds_/_Go2pyHighCmd.py | 16 +- .../msg/dds_/_Go2pyLowCmd.py | 6 +- go2py_messages/msg/dds_/_Go2pyState.py | 39 +++ go2py_messages/msg/dds_/__init__.py | 11 + scripts/dds_messages/Go2pyHighCmd.idl | 11 + .../dds_messages}/Go2pyLowCmd.idl | 2 +- scripts/dds_messages/Go2pyState.idl | 21 ++ {dds_messages => scripts}/make_msgs.sh | 10 +- unitree_go/.idlpy_manifest | 15 - unitree_go/__init__.py | 9 - unitree_go/msg/.idlpy_manifest | 15 - unitree_go/msg/dds_/.idlpy_manifest | 15 - unitree_go/msg/dds_/_BmsState_.py | 35 -- unitree_go/msg/dds_/_LowState_.py | 48 --- unitree_go/msg/dds_/_MotorState_.py | 37 --- unitree_go/msg/dds_/__init__.py | 12 - 68 files changed, 433 insertions(+), 1314 deletions(-) rename Go2Py/robot/{interface/dds.py => interface.py} (67%) delete mode 100644 Go2Py/robot/interface/__init__.py delete mode 100644 Go2Py/robot/interface/ros2.py delete mode 100644 dds_messages/idls/BmsState_.idl delete mode 100644 dds_messages/idls/IMUState_.idl delete mode 100644 dds_messages/idls/LowState_.idl delete mode 100644 dds_messages/idls/MotorState_.idl delete mode 100755 deploy/ros2_nodes/go2py_common/msg/AudioData.msg delete mode 100755 deploy/ros2_nodes/go2py_common/msg/BmsCmd.msg delete mode 100755 deploy/ros2_nodes/go2py_common/msg/BmsState.msg delete mode 100755 deploy/ros2_nodes/go2py_common/msg/Error.msg delete mode 100755 deploy/ros2_nodes/go2py_common/msg/Go2FrontVideoData.msg delete mode 100755 deploy/ros2_nodes/go2py_common/msg/HeightMap.msg delete mode 100755 deploy/ros2_nodes/go2py_common/msg/IMUState.msg delete mode 100755 deploy/ros2_nodes/go2py_common/msg/InterfaceConfig.msg delete mode 100755 deploy/ros2_nodes/go2py_common/msg/LidarState.msg delete mode 100755 deploy/ros2_nodes/go2py_common/msg/LowCmd.msg delete mode 100755 deploy/ros2_nodes/go2py_common/msg/LowState.msg delete mode 100755 deploy/ros2_nodes/go2py_common/msg/MotorCmd.msg delete mode 100755 deploy/ros2_nodes/go2py_common/msg/MotorState.msg delete mode 100755 deploy/ros2_nodes/go2py_common/msg/PathPoint.msg delete mode 100755 deploy/ros2_nodes/go2py_common/msg/Req.msg delete mode 100755 deploy/ros2_nodes/go2py_common/msg/Res.msg delete mode 100755 deploy/ros2_nodes/go2py_common/msg/SportModeCmd.msg delete mode 100755 deploy/ros2_nodes/go2py_common/msg/SportModeState.msg delete mode 100755 deploy/ros2_nodes/go2py_common/msg/TimeSpec.msg delete mode 100755 deploy/ros2_nodes/go2py_common/msg/UwbState.msg delete mode 100755 deploy/ros2_nodes/go2py_common/msg/UwbSwitch.msg delete mode 100755 deploy/ros2_nodes/go2py_common/msg/WirelessController.msg rename deploy/ros2_nodes/{go2py_common => go2py_messages}/CMakeLists.txt (70%) create mode 100644 deploy/ros2_nodes/go2py_messages/msg/Go2pyHighCmd.msg rename deploy/ros2_nodes/{go2py_common => go2py_messages}/msg/Go2pyLowCmd.msg (100%) create mode 100644 deploy/ros2_nodes/go2py_messages/msg/Go2pyState.msg rename deploy/ros2_nodes/{go2py_common => go2py_messages}/msg/Go2pyStatus.msg (100%) rename deploy/ros2_nodes/{go2py_common => go2py_messages}/package.xml (96%) rename examples/{00-robot-interface-DDS.ipynb => 00-robot-interface copy.ipynb} (79%) delete mode 100644 examples/01-robot-interface-ROS2.ipynb create mode 100644 examples/01-ros2-tools.ipynb delete mode 100644 go2py_common/.idlpy_manifest delete mode 100644 go2py_common/msg/.idlpy_manifest delete mode 100644 go2py_common/msg/__init__.py delete mode 100644 go2py_common/msg/dds_/.idlpy_manifest delete mode 100644 go2py_common/msg/dds_/__init__.py create mode 100644 go2py_messages/.idlpy_manifest rename {go2py_common => go2py_messages}/__init__.py (84%) create mode 100644 go2py_messages/msg/.idlpy_manifest rename {unitree_go => go2py_messages}/msg/__init__.py (82%) create mode 100644 go2py_messages/msg/dds_/.idlpy_manifest rename unitree_go/msg/dds_/_IMUState_.py => go2py_messages/msg/dds_/_Go2pyHighCmd.py (53%) rename {go2py_common => go2py_messages}/msg/dds_/_Go2pyLowCmd.py (82%) create mode 100644 go2py_messages/msg/dds_/_Go2pyState.py create mode 100644 go2py_messages/msg/dds_/__init__.py create mode 100644 scripts/dds_messages/Go2pyHighCmd.idl rename {dds_messages/idls => scripts/dds_messages}/Go2pyLowCmd.idl (88%) create mode 100644 scripts/dds_messages/Go2pyState.idl rename {dds_messages => scripts}/make_msgs.sh (62%) delete mode 100644 unitree_go/.idlpy_manifest delete mode 100644 unitree_go/__init__.py delete mode 100644 unitree_go/msg/.idlpy_manifest delete mode 100644 unitree_go/msg/dds_/.idlpy_manifest delete mode 100644 unitree_go/msg/dds_/_BmsState_.py delete mode 100644 unitree_go/msg/dds_/_LowState_.py delete mode 100644 unitree_go/msg/dds_/_MotorState_.py delete mode 100644 unitree_go/msg/dds_/__init__.py diff --git a/Go2Py/robot/interface/dds.py b/Go2Py/robot/interface.py similarity index 67% rename from Go2Py/robot/interface/dds.py rename to Go2Py/robot/interface.py index ea1fc1b..1cd7934 100644 --- a/Go2Py/robot/interface/dds.py +++ b/Go2Py/robot/interface.py @@ -6,8 +6,9 @@ import numpy.linalg as LA from scipy.spatial.transform import Rotation as R from cyclonedds.domain import DomainParticipant -from go2py_common.msg.dds_ import Go2pyLowCmd_ -from unitree_go.msg.dds_ import LowState_ +from go2py_messages.msg.dds_ import Go2pyLowCmd_ +from go2py_messages.msg.dds_ import Go2pyHighCmd_ +from go2py_messages.msg.dds_ import Go2pyState_ from cyclonedds.topic import Topic from cyclonedds.pub import DataWriter @@ -31,8 +32,6 @@ class GO2Real(): ): assert mode in ['highlevel', 'lowlevel'], "mode should be either 'highlevel' or 'lowlevel'" self.mode = mode - if self.mode == 'highlevel': - raise NotImplementedError('DDS interface for the highlevel commands is not implemented yet. Please use our ROS2 interface.') self.simulated = False self.prestanding_q = np.array([ 0.0, 1.26186061, -2.5, 0.0, 1.25883281, -2.5, @@ -50,56 +49,58 @@ class GO2Real(): 0.0, 0.75422204, -1.53229916]) self.latest_command_stamp = time.time() self.highcmd_topic_name = "rt/go2/twist_cmd" - self.lowcmd_topic_name = "rt/go2/lowcmd" - self.lowstate_topic_name = "rt/lowstate" + self.lowcmd_topic_name = "rt/go2py/low_cmd" + self.highcmd_topic_name = "rt/go2py/high_cmd" + self.lowstate_topic_name = "rt/go2py/state" self.participant = DomainParticipant() - self.lowstate_topic = Topic(self.participant, self.lowstate_topic_name, LowState_) - self.lowstate_reader = DataReader(self.participant, self.lowstate_topic) + self.lowstate_topic = Topic(self.participant, self.lowstate_topic_name, Go2pyState_) + self.state_reader = DataReader(self.participant, self.lowstate_topic) self.lowcmd_topic = Topic(self.participant, self.lowcmd_topic_name, Go2pyLowCmd_) self.lowcmd_writer = DataWriter(self.participant, self.lowcmd_topic) + self.highcmd_topic = Topic(self.participant, self.highcmd_topic_name, Go2pyHighCmd_) + self.highcmd_writer = DataWriter(self.participant, self.highcmd_topic) + self.vx_max = vx_max + self.vy_max = vy_max + self.P_v_max = np.diag([1 / self.vx_max**2, 1 / self.vy_max**2]) + self.ωz_max = ωz_max + self.ωz_min = -ωz_max + self.state = None - self.setCommands = {'lowlevel':self.setCommandsLow}[self.mode] - self.lowstate_thread = Thread(target = self.lowstate_update) + self.setCommands = {'lowlevel':self.setCommandsLow, + 'highlevel':self.setCommandsHigh}[self.mode] + self.state_thread = Thread(target = self.state_update) self.running = True - self.lowstate_thread.start() + self.state_thread.start() - - def lowstate_update(self): + def state_update(self): """ Retrieve the state of the robot """ while self.running: - for msg in self.lowstate_reader.take_iter(timeout=duration(milliseconds=1.)): + for msg in self.state_reader.take_iter(timeout=duration(milliseconds=1.)): self.state = msg def getIMU(self): - accel = self.state.imu_state.accelerometer - gyro = self.state.imu_state.gyroscope - quat = self.state.imu_state.quaternion - rpy = self.state.imu_state.rpy - temp = self.state.imu_state.temperature - return {'accel':accel, 'gyro':gyro, 'quat':quat, "rpy":rpy, 'temp':temp} + accel = self.state.accel + gyro = self.state.gyro + quat = self.state.quat + temp = self.state.imu_temp + return {'accel':accel, 'gyro':gyro, 'quat':quat, 'temp':temp} def getFootContacts(self): """Returns the raw foot contact forces""" - footContacts = self.state.foot_force - return np.array(footContacts) + footContacts = self.state.contact + return footContacts def getJointStates(self): """Returns the joint angles (q) and velocities (dq) of the robot""" - motor_state = np.array([[self.state.motor_state[i].q, - self.state.motor_state[i].dq, - self.state.motor_state[i].ddq, - self.state.motor_state[i].tau_est, - self.state.motor_state[i].temperature] for i in range(12)]) - return {'q':motor_state[:,0], - 'dq':motor_state[:,1], - 'ddq':motor_state[:,2], - 'tau_est':motor_state[:,3], - 'temperature':motor_state[:,4]} + return {'q':self.state.q, + 'dq':self.state.dq, + 'tau_est':self.state.tau, + 'temperature':self.state.motor_temp} def getRemoteState(self): """A method to get the state of the wireless remote control. @@ -149,18 +150,17 @@ class GO2Real(): def getBatteryState(self): """Returns the battery percentage of the robot""" - batteryState = self.state.bms_state - return batteryState.soc + return self.state.soc def setCommandsHigh(self, v_x, v_y, ω_z, bodyHeight=0.0, footRaiseHeight=0.0, mode=2): self.cmd_watchdog_timer = time.time() _v_x, _v_y, _ω_z = self.clip_velocity(v_x, v_y, ω_z) - self.highcmd.header.stamp = self.get_clock().now().to_msg() - self.highcmd.header.frame_id = "base_link" - self.highcmd.twist.linear.x = _v_x - self.highcmd.twist.linear.y = _v_y - self.highcmd.twist.angular.z = _ω_z - self.highcmd_publisher.publish(self.highcmd) + highcmd = Go2pyHighCmd_( + _v_x, + _v_y, + _ω_z + ) + self.highcmd_writer.write(highcmd) def setCommandsLow(self, q_des, dq_des, kp, kd, tau_ff): assert q_des.size == dq_des.size == kp.size == kd.size == tau_ff.size == 12, "q, dq, kp, kd, tau_ff should have size 12" @@ -177,11 +177,6 @@ class GO2Real(): def close(self): self.running = False - def check_calf_collision(self, q): - self.pin_robot.update(q) - in_collision = self.pin_robot.check_calf_collision(q) - return in_collision - def clip_velocity(self, v_x, v_y, ω_z): _v = np.array([[v_x], [v_y]]) _scale = np.sqrt(_v.T @ self.P_v_max @ _v)[0, 0] @@ -198,6 +193,6 @@ class GO2Real(): def getGravityInBody(self): q = self.getIMU()['quat'] - R = Rotation.from_quat([q[1], q[2], q[3], q[0]]).as_matrix() + R = Rotation.from_quat(q).as_matrix() g_in_body = R.T@np.array([0.0, 0.0, -1.0]).reshape(3, 1) return g_in_body \ No newline at end of file diff --git a/Go2Py/robot/interface/__init__.py b/Go2Py/robot/interface/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/Go2Py/robot/interface/ros2.py b/Go2Py/robot/interface/ros2.py deleted file mode 100644 index b0ecec1..0000000 --- a/Go2Py/robot/interface/ros2.py +++ /dev/null @@ -1,313 +0,0 @@ -import struct -import threading -import time -import numpy as np -import numpy.linalg as LA -from scipy.spatial.transform import Rotation as R -import rclpy -import tf2_ros -from rclpy.node import Node -from rclpy.qos import QoSProfile -from rclpy.executors import MultiThreadedExecutor -from geometry_msgs.msg import TransformStamped -from Go2Py.joy import xKeySwitch, xRockerBtn -from geometry_msgs.msg import TwistStamped -from unitree_go.msg import LowState, Go2pyLowCmd -from nav_msgs.msg import Odometry -from scipy.spatial.transform import Rotation -import tf2_ros -import numpy as np -from scipy.spatial.transform import Rotation as R - - - -def ros2_init(args=None): - rclpy.init(args=args) - - -def ros2_close(): - rclpy.shutdown() - -class ROS2ExecutorManager: - """A class to manage the ROS2 executor. It allows to add nodes and start the executor in a separate thread.""" - def __init__(self): - self.executor = MultiThreadedExecutor() - self.nodes = [] - self.executor_thread = None - - def add_node(self, node: Node): - """Add a new node to the executor.""" - self.nodes.append(node) - self.executor.add_node(node) - - def _run_executor(self): - try: - self.executor.spin() - except KeyboardInterrupt: - pass - finally: - self.terminate() - - def start(self): - """Start spinning the nodes in a separate thread.""" - self.executor_thread = threading.Thread(target=self._run_executor) - self.executor_thread.start() - - def terminate(self): - """Terminate all nodes and shutdown rclpy.""" - for node in self.nodes: - node.destroy_node() - rclpy.shutdown() - if self.executor_thread: - self.executor_thread.join() - -class GO2Real(Node): - def __init__( - self, - mode = 'highlevel', # 'highlevel' or 'lowlevel' - vx_max=0.5, - vy_max=0.4, - ωz_max=0.5, - ): - assert mode in ['highlevel', 'lowlevel'], "mode should be either 'highlevel' or 'lowlevel'" - self.simulated = False - self.prestanding_q = np.array([ 0.0, 1.26186061, -2.5, - 0.0, 1.25883281, -2.5, - 0.0, 1.27193761, -2.6, - 0.0, 1.27148342, -2.6]) - - self.sitting_q = np.array([-0.02495611, 1.26249647, -2.82826662, - 0.04563564, 1.2505368 , -2.7933557 , - -0.30623949, 1.28283751, -2.82314873, - 0.26400229, 1.29355574, -2.84276843]) - - self.standing_q = np.array([ 0.0, 0.77832842, -1.56065452, - 0.0, 0.76754963, -1.56634164, - 0.0, 0.76681757, -1.53601146, - 0.0, 0.75422204, -1.53229916]) - self.latest_command_stamp = time.time() - self.mode = mode - self.node_name = "go2py_highlevel_subscriber" - self.highcmd_topic = "/go2/twist_cmd" - self.lowcmd_topic = "/go2/lowcmd" - self.joint_state_topic = "/go2/joint_states" - self.lowstate_topic = "/lowstate" - super().__init__(self.node_name) - - self.lowstate_subscriber = self.create_subscription( - LowState, self.lowstate_topic, self.lowstate_callback, 1 - ) - self.lowcmd_publisher = self.create_publisher(Go2pyLowCmd, self.lowcmd_topic, 1) - - self.odometry_subscriber = self.create_subscription( - Odometry, "/utlidar/robot_odom", self.odom_callback, 1 - ) - - self.highcmd_publisher = self.create_publisher(TwistStamped, self.highcmd_topic, 1) - self.highcmd = TwistStamped() - # create pinocchio robot - # self.pin_robot = PinRobot() - - # for velocity clipping - self.vx_max = vx_max - self.vy_max = vy_max - self.P_v_max = np.diag([1 / self.vx_max**2, 1 / self.vy_max**2]) - self.ωz_max = ωz_max - self.ωz_min = -ωz_max - self.running = True - self.setCommands = {'lowlevel':self.setCommandsLow, - 'highlevel':self.setCommandsHigh}[self.mode] - self.state = None - - def lowstate_callback(self, msg): - """ - Retrieve the state of the robot - """ - self.state = msg - - def odom_callback(self, msg): - """ - Retrieve the odometry of the robot - """ - self.odom = msg - - def getOdometry(self): - """Returns the odometry of the robot""" - stamp = self.odom.header.stamp - position = np.array([self.odom.pose.pose.position.x, self.odom.pose.pose.position.y, self.odom.pose.pose.position.z]) - orientation = np.array([self.odom.pose.pose.orientation.x, self.odom.pose.pose.orientation.y, self.odom.pose.pose.orientation.z, self.odom.pose.pose.orientation.w]) - stamp_nanosec = stamp.sec + stamp.nanosec * 1e-9 - return {'stamp_nanosec':stamp_nanosec, 'position':position, 'orientation':orientation} - - def getIMU(self): - accel = self.state.imu_state.accelerometer - gyro = self.state.imu_state.gyroscope - quat = self.state.imu_state.quaternion - rpy = self.state.imu_state.rpy - temp = self.state.imu_state.temperature - # return accel, gyro, quat, temp - return {'accel':accel, 'gyro':gyro, 'quat':quat, "rpy":rpy, 'temp':temp} - - def getFootContacts(self): - """Returns the raw foot contact forces""" - footContacts = self.state.foot_force - return np.array(footContacts) - - def getJointStates(self): - """Returns the joint angles (q) and velocities (dq) of the robot""" - if self.state is None: - return None - motor_state = np.array([[self.state.motor_state[i].q, - self.state.motor_state[i].dq, - self.state.motor_state[i].ddq, - self.state.motor_state[i].tau_est, - self.state.motor_state[i].temperature] for i in range(12)]) - return {'q':motor_state[:,0], - 'dq':motor_state[:,1], - 'ddq':motor_state[:,2], - 'tau_est':motor_state[:,3], - 'temperature':motor_state[:,4]} - - def getRemoteState(self): - """A method to get the state of the wireless remote control. - Returns a xRockerBtn object: - - head: [head1, head2] - - keySwitch: xKeySwitch object - - lx: float - - rx: float - - ry: float - - L2: float - - ly: float - """ - wirelessRemote = self.state.wireless_remote[:24] - - binary_data = bytes(wirelessRemote) - - format_str = "<2BH5f" - data = struct.unpack(format_str, binary_data) - - head = list(data[:2]) - lx = data[3] - rx = data[4] - ry = data[5] - L2 = data[6] - ly = data[7] - - _btn = bin(data[2])[2:].zfill(16) - btn = [int(char) for char in _btn] - btn.reverse() - - keySwitch = xKeySwitch(*btn) - rockerBtn = xRockerBtn(head, keySwitch, lx, rx, ry, L2, ly) - return rockerBtn - - def getCommandFromRemote(self): - """Do not use directly for control!!!""" - rockerBtn = self.getRemoteState() - - lx = rockerBtn.lx - ly = rockerBtn.ly - rx = rockerBtn.rx - - v_x = ly * self.vx_max - v_y = lx * self.vy_max - ω = rx * self.ωz_max - - return v_x, v_y, ω - - def getBatteryState(self): - """Returns the battery percentage of the robot""" - batteryState = self.state.bms - return batteryState.SOC - - def setCommandsHigh(self, v_x, v_y, ω_z, bodyHeight=0.0, footRaiseHeight=0.0, mode=2): - self.cmd_watchdog_timer = time.time() - _v_x, _v_y, _ω_z = self.clip_velocity(v_x, v_y, ω_z) - self.highcmd.header.stamp = self.get_clock().now().to_msg() - self.highcmd.header.frame_id = "base_link" - self.highcmd.twist.linear.x = _v_x - self.highcmd.twist.linear.y = _v_y - self.highcmd.twist.angular.z = _ω_z - self.highcmd_publisher.publish(self.highcmd) - - def setCommandsLow(self, q_des, dq_des, kp, kd, tau_ff): - # assert q_des.size == dq_des.size == kp.size == kd.size == tau_ff.size == 12, "q, dq, kp, kd, tau_ff should have size 12" - lowcmd = Go2pyLowCmd() - lowcmd.q = q_des - lowcmd.dq = dq_des - lowcmd.kp = kp - lowcmd.kd = kd - lowcmd.tau = tau_ff - self.lowcmd_publisher.publish(lowcmd) - self.latest_command_stamp = time.time() - - - def close(self): - self.running = False - self.thread.join() - self.destroy_node() - - def check_calf_collision(self, q): - self.pin_robot.update(q) - in_collision = self.pin_robot.check_calf_collision(q) - return in_collision - - def clip_velocity(self, v_x, v_y, ω_z): - _v = np.array([[v_x], [v_y]]) - _scale = np.sqrt(_v.T @ self.P_v_max @ _v)[0, 0] - - if _scale > 1.0: - scale = 1.0 / _scale - else: - scale = 1.0 - - return scale * v_x, scale * v_y, np.clip(ω_z, self.ωz_min, self.ωz_max) - - def overheat(self): - return False - - def getGravityInBody(self): - q = self.getIMU()['quat'] - R = Rotation.from_quat([q[1], q[2], q[3], q[0]]).as_matrix() - g_in_body = R.T@np.array([0.0, 0.0, -1.0]).reshape(3, 1) - return g_in_body - -class ROS2TFInterface(Node): - - def __init__(self, parent_name, child_name, node_name): - super().__init__(f'{node_name}_tf2_listener') - self.parent_name = parent_name - self.child_name = child_name - self.tfBuffer = tf2_ros.Buffer() - self.listener = tf2_ros.TransformListener(self.tfBuffer, self) - self.T = None - self.stamp = None - self.running = True - self.thread = threading.Thread(target=self.update_loop) - self.thread.start() - self.trans = None - - def update_loop(self): - while self.running: - try: - self.trans = self.tfBuffer.lookup_transform(self.parent_name, self.child_name, rclpy.time.Time(), rclpy.time.Duration(seconds=0.1)) - except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: - pass - time.sleep(0.01) - - def get_pose(self): - if self.trans is None: - return None - else: - translation = [self.trans.transform.translation.x, self.trans.transform.translation.y, self.trans.transform.translation.z] - rotation = [self.trans.transform.rotation.x, self.trans.transform.rotation.y, self.trans.transform.rotation.z, self.trans.transform.rotation.w] - self.T = np.eye(4) - self.T[0:3, 0:3] = R.from_quat(rotation).as_matrix() - self.T[:3, 3] = translation - self.stamp = self.trans.header.stamp.nanosec * 1e-9 + self.trans.header.stamp.sec - return self.T - - def close(self): - self.running = False - self.thread.join() - self.destroy_node() \ No newline at end of file diff --git a/Makefile b/Makefile index 7d4b055..fa2b244 100644 --- a/Makefile +++ b/Makefile @@ -1,3 +1,6 @@ +messages: + @cd scripts && ./make_msgs.sh + realsense: @cd deploy/docker && docker build --tag go2py_realsense:latest -f Dockerfile.realsense . diff --git a/dds_messages/idls/BmsState_.idl b/dds_messages/idls/BmsState_.idl deleted file mode 100644 index 9efe4c4..0000000 --- a/dds_messages/idls/BmsState_.idl +++ /dev/null @@ -1,49 +0,0 @@ -// generated from rosidl_generator_dds_idl/resource/idl.idl.em -// with input from unitree_go:msg/BmsState.idl -// generated code does not contain a copyright notice - -#ifndef __unitree_go__msg__bms_state__idl__ -#define __unitree_go__msg__bms_state__idl__ -module unitree_go { - -module msg { - -module dds_ { - - -struct BmsState_ { -octet version_high; //电池版本 -octet version_low; //电池版本 - -// 0：SAFE,（未开启电池） -// 1：WAKE_UP,（唤醒事件） - -// 6：PRECHG, （电池预冲电中） -// 7：CHG, （电池正常充电中） -// 8：DCHG, （电池正常放电中） -// 9：SELF_DCHG, （电池自放电中） - -// 11：ALARM, （电池存在警告） -// 12：RESET_ALARM, （等待按键复位警告中） -// 13：AUTO_RECOVERY （复位中） -octet status; //电池状态信息。 - -octet soc; //电池电量信息：（类型：uint8_t）(范围1% - 100%) -long current; //充放电信息：（正：代表充电，负代表放电）可按照实际数值显示 -unsigned short cycle; //充电循环次数 -octet bq_ntc[2]; //电池内部两个NTC的温度（int8_t）（范围：-100 - 150）。 0- BAT1; 1- BAT2 - -octet mcu_ntc[2]; //电池NTC数组：0 - RES，1 - MOS （int8_t）（范围：-100 - 150）。 - -unsigned short cell_vol[15]; //电池内部15节电池的电压。 - - -}; - - -}; // module dds_ - -}; // module msg - -}; // module unitree_go -#endif // __unitree_go__msg__bms_state__idl__ \ No newline at end of file diff --git a/dds_messages/idls/IMUState_.idl b/dds_messages/idls/IMUState_.idl deleted file mode 100644 index e1c2afe..0000000 --- a/dds_messages/idls/IMUState_.idl +++ /dev/null @@ -1,33 +0,0 @@ -// generated from rosidl_generator_dds_idl/resource/idl.idl.em -// with input from unitree_go:msg/IMUState.idl -// generated code does not contain a copyright notice - -#ifndef __unitree_go__msg__imu_state__idl__ -#define __unitree_go__msg__imu_state__idl__ -module unitree_go { - -module msg { - -module dds_ { - - -struct IMUState_ { -float quaternion[4]; //四元数数据 - -float gyroscope[3]; //角速度信息（0 -> x ,0 -> y ,0 -> z） - -float accelerometer[3]; //加速度信息（0 -> x ,0 -> y ,0 -> z） - -float rpy[3]; //欧拉角信息：默认为弧度值（可按照实际情况改为角度值），可按照实际数值显示（弧度值范围：-7 - +7，显示3位小数）。（数组：0-roll（翻滚角），1-pitch（俯仰角），2-yaw（偏航角））。 - -octet temperature; //IMU 温度信息（摄氏度）。 - -}; - - -}; // module dds_ - -}; // module msg - -}; // module unitree_go -#endif // __unitree_go__msg__imu_state__idl__ \ No newline at end of file diff --git a/dds_messages/idls/LowState_.idl b/dds_messages/idls/LowState_.idl deleted file mode 100644 index 1b26cff..0000000 --- a/dds_messages/idls/LowState_.idl +++ /dev/null @@ -1,70 +0,0 @@ -// generated from rosidl_generator_dds_idl/resource/idl.idl.em -// with input from unitree_go:msg/LowState.idl -// generated code does not contain a copyright notice -#include "BmsState_.idl" -#include "IMUState_.idl" -#include "MotorState_.idl" - -#ifndef __unitree_go__msg__low_state__idl__ -#define __unitree_go__msg__low_state__idl__ -module unitree_go { - -module msg { - -module dds_ { - - -struct LowState_ { -octet head[2]; //帧头，数据校验用（0xFE,0xEF）。 - -octet level_flag; //沿用的，但是目前不用。 -octet frame_reserve; //沿用的，但是目前不用。 -unsigned long sn[2]; //已经改为文件存储形式，目前没用。 -unsigned long version[2]; //沿用的，但是目前不用。 -unsigned short bandwidth; //沿用的，但是目前不用。。 - -unitree_go::msg::dds_::IMUState_ imu_state; //IMU数据信息。 - -// FR_0 -> 0 , FR_1 -> 1 , FR_2 -> 2 电机顺序，目前只用12电机，后面保留。 -// FL_0 -> 3 , FL_1 -> 4 , FL_2 -> 5 -// RR_0 -> 6 , RR_1 -> 7 , RR_2 -> 8 -// RL_0 -> 9 , RL_1 -> 10 , RL_2 -> 11 -unitree_go::msg::dds_::MotorState_ motor_state[20]; //电机总数据。 -unitree_go::msg::dds_::BmsState_ bms_state; //电池总数据。 - -short foot_force[4]; //足端力（范围0-4095），可按照实际数值显示。（数组：0-FR，1-FL，2-RR, 3-RL） -short foot_force_est[4]; //沿用的，但是目前不用。 - -unsigned long tick; //1ms计时用，按照1ms递增。 -octet wireless_remote[40]; //遥控器原始数据。 - -//&0x80 - 电机 超时标志 1-超时 0-正常 -//&0x40 - 小Mcu 超时标志 1-超时 0-正常 -//&0x20 - 遥控器 超时标志 1-超时 0-正常 -//&0x10 - 电池 超时标志 1-超时 0-正常 - -//&0x04 - 自动充电 自动充电状态标志 1-不充电 0-充电 -//&0x02 - 板载电流错误标志 错误标志 1-板载电流异常 0-正常 -//&0x01 - 运控命令超时 超时标志 1-超时 0-正常 -octet bit_flag; //各个组件状态显示 - -float adc_reel; //卷线器电流（范围：0 - 3A）。 -octet temperature_ntc1; //主板中心温度值（范围：-20 - 100℃）。 -octet temperature_ntc2; //自动充电温度（范围：-20 - 100℃）。 -float power_v; //此电压值为主板电压 -> 电池电压 。 -float power_a; //此电流值为主板电流值 -> 电机电流。 - -unsigned short fan_frequency[4]; //风扇转速（目前可按照实际数值显示0-10000）。（0-左后转速 , 1-右后转速，2-前转速，单位转/分钟）（堵转检测：3-&0x01：左后堵转 , &0x02：右后堵转，&0x04：前堵转） - -unsigned long reserve; //保留位。 -unsigned long crc; //数据CRC校验用。 - -}; - - -}; // module dds_ - -}; // module msg - -}; // module unitree_go -#endif // __unitree_go__msg__low_state__idl__ \ No newline at end of file diff --git a/dds_messages/idls/MotorState_.idl b/dds_messages/idls/MotorState_.idl deleted file mode 100644 index a9bbeea..0000000 --- a/dds_messages/idls/MotorState_.idl +++ /dev/null @@ -1,37 +0,0 @@ -// generated from rosidl_generator_dds_idl/resource/idl.idl.em -// with input from unitree_go:msg/MotorState.idl -// generated code does not contain a copyright notice - -#ifndef __unitree_go__msg__motor_state__idl__ -#define __unitree_go__msg__motor_state__idl__ -module unitree_go { - -module msg { - -module dds_ { - - -struct MotorState_ { -octet mode; //电机控制模式（Foc模式（工作模式）-> 0x01 ，stop模式（待机模式）-> 0x00。） -float q; //关机反馈位置信息：默认为弧度值（可按照实际情况改为角度值），可按照实际数值显示（弧度值范围：-7 - +7，显示3位小数）。 -float dq; //关节反馈速度 -float ddq; //关节反馈加速度 -float tau_est; //关节反馈力矩 - -float q_raw; //沿用的，但是目前不用。 -float dq_raw; //沿用的，但是目前不用。 -float ddq_raw; //沿用的，但是目前不用。 -octet temperature; //电机温度信息：类型：int8_t ，可按照实际数值显示（范围：-100 - 150）。 -unsigned long lost; //电机丢包信息：可按照实际数值显示（范围：0-9999999999）。 -unsigned long reserve[2]; //当前电机通信频率+电机错误标志位：（数组：0-电机错误标志位（范围：0-255，可按照实际数值显示），1-当前电机通信频率（范围：0-800，可按照实际数值显示）） - - -}; - - -}; // module dds_ - -}; // module msg - -}; // module unitree_go -#endif // __unitree_go__msg__motor_state__idl__ \ No newline at end of file diff --git a/deploy/ros2_nodes/go2py_bridge/CMakeLists.txt b/deploy/ros2_nodes/go2py_bridge/CMakeLists.txt index bddc9d8..a8456db 100644 --- a/deploy/ros2_nodes/go2py_bridge/CMakeLists.txt +++ b/deploy/ros2_nodes/go2py_bridge/CMakeLists.txt @@ -21,7 +21,7 @@ link_directories(src) set ( DEPENDENCY_LIST - go2py_common + go2py_messages unitree_go unitree_api rclcpp @@ -39,7 +39,7 @@ set ( # find dependencies find_package(ament_cmake REQUIRED) find_package(unitree_go REQUIRED) -find_package(go2py_common REQUIRED) +find_package(go2py_messages REQUIRED) find_package(unitree_api REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) diff --git a/deploy/ros2_nodes/go2py_bridge/package.xml b/deploy/ros2_nodes/go2py_bridge/package.xml index 892289f..fd9038b 100644 --- a/deploy/ros2_nodes/go2py_bridge/package.xml +++ b/deploy/ros2_nodes/go2py_bridge/package.xml @@ -10,7 +10,7 @@ ament_cmake unitree_go - go2py_common + go2py_messages unitree_api rclcpp std_msgs diff --git a/deploy/ros2_nodes/go2py_bridge/src/bridge.cpp b/deploy/ros2_nodes/go2py_bridge/src/bridge.cpp index 881fafc..943cd3d 100644 --- a/deploy/ros2_nodes/go2py_bridge/src/bridge.cpp +++ b/deploy/ros2_nodes/go2py_bridge/src/bridge.cpp @@ -1,8 +1,10 @@ #include "unitree_go/msg/low_state.hpp" #include "unitree_go/msg/imu_state.hpp" #include "unitree_go/msg/motor_state.hpp" -#include "go2py_common/msg/go2py_status.hpp" -#include "go2py_common/msg/go2py_low_cmd.hpp" +#include "go2py_messages/msg/go2py_status.hpp" +#include "go2py_messages/msg/go2py_low_cmd.hpp" +#include "go2py_messages/msg/go2py_high_cmd.hpp" +#include "go2py_messages/msg/go2py_state.hpp" #include "unitree_go/msg/low_cmd.hpp" #include "unitree_go/msg/motor_cmd.hpp" @@ -31,112 +33,87 @@ class Custom: public rclcpp::Node public: Custom() : Node("go2py_bridge_node") { + // Standard ROS2 Topics watchdog_timer = this->create_wall_timer(std::chrono::milliseconds(10), std::bind(&Custom::watchdog, this)); pub_odom = this->create_publisher("/go2/odom", 1); pub_imu = this->create_publisher("/go2/imu", 1); pub_joint = this->create_publisher("/go2/joint_states", 1); - sub_twist = this->create_subscription("/go2/twist_cmd", 1, std::bind(&Custom::twistCmdCallback, this, std::placeholders::_1)); - nav2_twist = this->create_subscription("/go2/cmd_vel", 1, std::bind(&Custom::nav2TwistCmdCallback, this, std::placeholders::_1)); + nav2_twist_subr = this->create_subscription("/go2/cmd_vel", 1, std::bind(&Custom::nav2TwistCmdCallback, this, std::placeholders::_1)); - // Go2 highlevel subscriber and publishers - // the state_suber is set to subscribe "sportmodestate" topic - highstate_suber = this->create_subscription( - "sportmodestate", 10, std::bind(&Custom::highstate_callback, this, std::placeholders::_1)); - // the req_puber is set to subscribe "/api/sport/request" topic with dt - highreq_puber = this->create_publisher("/api/sport/request", 1); - - //Go2 lowlevel interface + //Unitree Topics init_lowcmd(); - lowstate_suber = this->create_subscription( - "lowstate", 1, std::bind(&Custom::lowstate_callback, this, std::placeholders::_1)); - - lowcmd_suber = this->create_subscription( - "/go2/lowcmd", 1, std::bind(&Custom::lowcmd_callback, this, std::placeholders::_1)); + unitree_lowstate_suber = this->create_subscription( + "lowstate", 1, std::bind(&Custom::unitree_lowstate_callback, this, std::placeholders::_1)); + // the req_puber is set to subscribe "/api/sport/request" topic with dt + unitree_highreq_puber = this->create_publisher("/api/sport/request", 1); lowcmd_puber = this->create_publisher("/lowcmd", 1); + + // Go2py topics + go2py_low_cmd_suber = this->create_subscription( + "/go2py/low_cmd", 1, std::bind(&Custom::go2py_low_cmd_callback, this, std::placeholders::_1)); + go2py_high_cmd_suber = this->create_subscription( + "/go2py/high_cmd", 1, std::bind(&Custom::go2py_high_cmd_callback, this, std::placeholders::_1)); + go2py_state_puber = this->create_publisher("/go2py/state", 1); + status_publisher = this->create_publisher("/go2py/status", 1); api_publisher = this->create_publisher("/api/robot_state/request", 1); - status_publisher = this->create_publisher("/go2py/status", 1); - } private: rclcpp::TimerBase::SharedPtr watchdog_timer; void watchdog(); - // Highlevel twist command through standard ROS2 message type - void twistCmdCallback(const geometry_msgs::msg::TwistStamped::SharedPtr msg); - void nav2TwistCmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg); + void init_lowcmd(); + int Estop = 0; + int sport_mode = 1; + xRockerBtnDataStruct _keyData; + + // Standard ROS2 geometry_msgs::msg::TwistStamped twist_cmd; - geometry_msgs::msg::Twist twist; - rclcpp::Subscription::SharedPtr sub_twist; - rclcpp::Subscription::SharedPtr nav2_twist; - uint64_t last_highcmd_stamp = 0; - - // ROS2 standard joint state, IMU, and odometry publishers + void nav2TwistCmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg); + rclcpp::Subscription::SharedPtr nav2_twist_subr; rclcpp::Publisher::SharedPtr pub_imu; rclcpp::Publisher::SharedPtr pub_joint; rclcpp::Publisher::SharedPtr pub_odom; - // Highlevel interface - void highstate_callback(unitree_go::msg::SportModeState::SharedPtr data); - rclcpp::Subscription::SharedPtr highstate_suber; - rclcpp::Publisher::SharedPtr highreq_puber; - unitree_api::msg::Request highreq; // Unitree Go2 ROS2 request message - SportClient sport_req; - - // Lowlevel interface - void lowstate_callback(unitree_go::msg::LowState::SharedPtr data); - void lowcmd_callback(go2py_common::msg::Go2pyLowCmd::SharedPtr data); - rclcpp::Subscription::SharedPtr lowstate_suber; - rclcpp::Subscription::SharedPtr lowcmd_suber; - // A struct to store the highlevel states for later use - rclcpp::Publisher::SharedPtr lowcmd_puber; + // Unitree Interface + unitree_api::msg::Request highreq; unitree_go::msg::LowCmd lowcmd_msg; - uint64_t last_lowcmd_stamp = 0; - xRockerBtnDataStruct _keyData; + SportClient sport_req; + void unitree_lowstate_callback(unitree_go::msg::LowState::SharedPtr data); + rclcpp::Subscription::SharedPtr unitree_lowstate_suber; + rclcpp::Publisher::SharedPtr unitree_highreq_puber; + rclcpp::Publisher::SharedPtr lowcmd_puber; rclcpp::Publisher::SharedPtr api_publisher; - rclcpp::Publisher::SharedPtr status_publisher; - void init_lowcmd() - { + // Lowlevel interface + void go2py_low_cmd_callback(go2py_messages::msg::Go2pyLowCmd::SharedPtr data); + void go2py_high_cmd_callback(go2py_messages::msg::Go2pyHighCmd::SharedPtr data); + rclcpp::Subscription::SharedPtr go2py_low_cmd_suber; + rclcpp::Subscription::SharedPtr go2py_high_cmd_suber; + rclcpp::Publisher::SharedPtr go2py_state_puber; + rclcpp::Publisher::SharedPtr status_publisher; - for (int i = 0; i < 20; i++) - { - lowcmd_msg.motor_cmd[i].mode = 0x01; //Set toque mode, 0x00 is passive mode - lowcmd_msg.motor_cmd[i].q = PosStopF; - lowcmd_msg.motor_cmd[i].kp = 0; - lowcmd_msg.motor_cmd[i].dq = VelStopF; - lowcmd_msg.motor_cmd[i].kd = 0; - lowcmd_msg.motor_cmd[i].tau = 0; - } - } - - struct { - float px; - float py; - float height; - float roll; - float pitch; - float yaw; - float imu_temp; - float wx; - float wy; - float wz; - float ax; - float qy; - float az; - float vx; - float vy; - float foot_force[4]; - }highstate; - int Estop = 0; - int sport_mode = 1; }; -void Custom::lowstate_callback(unitree_go::msg::LowState::SharedPtr data) +void Custom::init_lowcmd() +{ + + for (int i = 0; i < 20; i++) + { + lowcmd_msg.motor_cmd[i].mode = 0x01; //Set toque mode, 0x00 is passive mode + lowcmd_msg.motor_cmd[i].q = PosStopF; + lowcmd_msg.motor_cmd[i].kp = 0; + lowcmd_msg.motor_cmd[i].dq = VelStopF; + lowcmd_msg.motor_cmd[i].kd = 0; + lowcmd_msg.motor_cmd[i].tau = 0; + } +} + +void Custom::unitree_lowstate_callback(unitree_go::msg::LowState::SharedPtr data) { - // std::cout << data->tick << std::endl; sensor_msgs::msg::Imu imu; sensor_msgs::msg::JointState joint_state; nav_msgs::msg::Odometry odom_state; + go2py_messages::msg::Go2pyState go2py_state; // Load the IMU message imu.header.stamp=rclcpp::Clock().now(); @@ -151,6 +128,20 @@ void Custom::lowstate_callback(unitree_go::msg::LowState::SharedPtr data) imu.angular_velocity.x = data->imu_state.gyroscope[0]; imu.angular_velocity.y = data->imu_state.gyroscope[1]; imu.angular_velocity.z = data->imu_state.gyroscope[2]; + + go2py_state.time_sec = imu.header.stamp.sec; + go2py_state.time_nsec = imu.header.stamp.nanosec; + go2py_state.quat[0] = imu.orientation.x; + go2py_state.quat[1] = imu.orientation.y; + go2py_state.quat[2] = imu.orientation.z; + go2py_state.quat[3] = imu.orientation.w; + for(int i=0; i<3; i++) + { + go2py_state.accel[i] = data->imu_state.accelerometer[i]; + go2py_state.gyro[i] = data->imu_state.gyroscope[i]; + } + go2py_state.imu_temp = data->imu_state.temperature; + // Load the joint state messages joint_state.header.stamp = imu.header.stamp; joint_state.header.frame_id = "trunk"; @@ -175,10 +166,20 @@ void Custom::lowstate_callback(unitree_go::msg::LowState::SharedPtr data) joint_state.position.push_back(data->motor_state[i].q); joint_state.velocity.push_back(data->motor_state[i].dq); joint_state.effort.push_back(data->motor_state[i].tau_est); + go2py_state.q[i]=data->motor_state[i].q; + go2py_state.dq[i]=data->motor_state[i].dq; + go2py_state.tau[i]=data->motor_state[i].tau_est; + go2py_state.motor_temp[i] = data->motor_state[i].temperature; } - + for(int i=0; i<4; i++) + go2py_state.contact[i]=data->foot_force[i]; + + for(int i=0; i<40; i++) + go2py_state.wireless_remote[i]=data->wireless_remote[i]; + go2py_state.soc = data->bms_state.soc; pub_joint->publish(joint_state); pub_imu->publish(imu); + go2py_state_puber->publish(go2py_state); // Check for emergency stop memcpy(&_keyData, &data->wireless_remote[0], 40); if (_keyData.btn.components.R2 == 1 && _keyData.btn.components.L2 == 1) @@ -212,83 +213,29 @@ void Custom::lowstate_callback(unitree_go::msg::LowState::SharedPtr data) // std::cout << "Estop: " << Estop << std::endl; } -void Custom::highstate_callback(unitree_go::msg::SportModeState::SharedPtr data) -{ - highstate.px = data->position[0]; - highstate.py = data->position[1]; - highstate.roll = data->imu_state.rpy[0]; - highstate.pitch = data->imu_state.rpy[1]; - highstate.yaw = data->imu_state.rpy[2]; - for(int i=0; i<4; i++) - { - highstate.foot_force[i]=data->foot_force[i]; - } - highstate.height = data->body_height; - - nav_msgs::msg::Odometry odom_state; - // odometry states published by the onboard high-level controller - odom_state.header.stamp = rclcpp::Clock().now(); - odom_state.header.frame_id = "odom"; - odom_state.child_frame_id = "base_link"; - odom_state.pose.pose.position.x = data->position[0]; - odom_state.pose.pose.position.y = data->position[1]; - odom_state.pose.pose.position.z = data->body_height; - - - odom_state.twist.twist.linear.x = data->velocity[0]; - odom_state.twist.twist.linear.y = data->velocity[1]; - odom_state.twist.twist.linear.z = data->velocity[2]; - odom_state.twist.twist.angular.z= data->yaw_speed; - double position_R = 0.05; - double orientation_R = 0.005; - odom_state.pose.covariance = { - position_R, 0, 0, 0, 0, 0, // Covariance for position x - 0, position_R, 0, 0, 0, 0, // Covariance for position y - 0, 0, position_R, 0, 0, 0, // Covariance for position z - 0, 0, 0, orientation_R, 0, 0, // Covariance for orientation roll - 0, 0, 0, 0, orientation_R, 0, // Covariance for orientation pitch - 0, 0, 0, 0, 0, orientation_R // Covariance for orientation yaw - }; - - odom_state.twist.covariance = { - position_R, 0, 0, 0, 0, 0, // Covariance for position x - 0, position_R, 0, 0, 0, 0, // Covariance for position y - 0, 0, position_R, 0, 0, 0, // Covariance for position z - 0, 0, 0, orientation_R, 0, 0, // Covariance for orientation roll - 0, 0, 0, 0, orientation_R, 0, // Covariance for orientation pitch - 0, 0, 0, 0, 0, orientation_R // Covariance for orientation yaw - }; - - pub_odom->publish(odom_state); -} - -void Custom::twistCmdCallback(const geometry_msgs::msg::TwistStamped::SharedPtr msg) -{ - auto stamp_now = std::chrono::high_resolution_clock::now(); - last_highcmd_stamp = std::chrono::duration_cast(stamp_now.time_since_epoch()).count(); - if(Estop == 0) - sport_req.Move(highreq, msg->twist.linear.x, msg->twist.linear.y, msg->twist.angular.z); - else - sport_req.Damp(highreq); - - // Publish request messages with desired body velocity - highreq_puber->publish(highreq); -} - void Custom::nav2TwistCmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg) { - auto stamp_now = std::chrono::high_resolution_clock::now(); - last_highcmd_stamp = std::chrono::duration_cast(stamp_now.time_since_epoch()).count(); if(Estop == 0) sport_req.Move(highreq, msg->linear.x, msg->linear.y, msg->angular.z); else sport_req.Damp(highreq); // Publish request messages with desired body velocity - highreq_puber->publish(highreq); + unitree_highreq_puber->publish(highreq); } -void Custom::lowcmd_callback(go2py_common::msg::Go2pyLowCmd::SharedPtr data) +void Custom::go2py_high_cmd_callback(go2py_messages::msg::Go2pyHighCmd::SharedPtr data) +{ + if(Estop == 0) + sport_req.Move(highreq, data->vx, data->vy, data->wz); + else + sport_req.Damp(highreq); + + // Publish request messages with desired body velocity + unitree_highreq_puber->publish(highreq); +} + +void Custom::go2py_low_cmd_callback(go2py_messages::msg::Go2pyLowCmd::SharedPtr data) { if(Estop == 0) { @@ -321,7 +268,7 @@ void Custom::watchdog() if(Estop == 1) { sport_req.Damp(highreq); - highreq_puber->publish(highreq); + unitree_highreq_puber->publish(highreq); for(int i=0; i<12; i++) { lowcmd_msg.motor_cmd[i].q = 0.; // Taregt angular(rad) @@ -329,12 +276,11 @@ void Custom::watchdog() lowcmd_msg.motor_cmd[i].dq = 0.; // Taregt angular velocity(rad/ss) lowcmd_msg.motor_cmd[i].kd = 3; // Poinstion(rad) control kd gain lowcmd_msg.motor_cmd[i].tau = 0.; // Feedforward toque 1N.m - get_crc(lowcmd_msg); //Compute the CRC and load it into the message - lowcmd_puber->publish(lowcmd_msg); //Publish lowcmd message } + get_crc(lowcmd_msg); //Compute the CRC and load it into the message lowcmd_puber->publish(lowcmd_msg); } - auto msg = std::make_shared(); + auto msg = std::make_shared(); msg->estop = Estop; msg->sport_mode = sport_mode; status_publisher->publish(*msg); diff --git a/deploy/ros2_nodes/go2py_common/msg/AudioData.msg b/deploy/ros2_nodes/go2py_common/msg/AudioData.msg deleted file mode 100755 index 6e889bc..0000000 --- a/deploy/ros2_nodes/go2py_common/msg/AudioData.msg +++ /dev/null @@ -1,2 +0,0 @@ -uint64 time_frame -uint8[] data \ No newline at end of file diff --git a/deploy/ros2_nodes/go2py_common/msg/BmsCmd.msg b/deploy/ros2_nodes/go2py_common/msg/BmsCmd.msg deleted file mode 100755 index bdbf9c3..0000000 --- a/deploy/ros2_nodes/go2py_common/msg/BmsCmd.msg +++ /dev/null @@ -1,2 +0,0 @@ -uint8 off -uint8[3] reserve \ No newline at end of file diff --git a/deploy/ros2_nodes/go2py_common/msg/BmsState.msg b/deploy/ros2_nodes/go2py_common/msg/BmsState.msg deleted file mode 100755 index 052e4d5..0000000 --- a/deploy/ros2_nodes/go2py_common/msg/BmsState.msg +++ /dev/null @@ -1,9 +0,0 @@ -uint8 version_high -uint8 version_low -uint8 status -uint8 soc -int32 current -uint16 cycle -int8[2] bq_ntc -int8[2] mcu_ntc -uint16[15] cell_vol \ No newline at end of file diff --git a/deploy/ros2_nodes/go2py_common/msg/Error.msg b/deploy/ros2_nodes/go2py_common/msg/Error.msg deleted file mode 100755 index 4a5a631..0000000 --- a/deploy/ros2_nodes/go2py_common/msg/Error.msg +++ /dev/null @@ -1,2 +0,0 @@ -uint32 source -uint32 state \ No newline at end of file diff --git a/deploy/ros2_nodes/go2py_common/msg/Go2FrontVideoData.msg b/deploy/ros2_nodes/go2py_common/msg/Go2FrontVideoData.msg deleted file mode 100755 index 3c294f3..0000000 --- a/deploy/ros2_nodes/go2py_common/msg/Go2FrontVideoData.msg +++ /dev/null @@ -1,4 +0,0 @@ -uint64 time_frame -uint8[] video720p -uint8[] video360p -uint8[] video180p \ No newline at end of file diff --git a/deploy/ros2_nodes/go2py_common/msg/HeightMap.msg b/deploy/ros2_nodes/go2py_common/msg/HeightMap.msg deleted file mode 100755 index 4e6f3b2..0000000 --- a/deploy/ros2_nodes/go2py_common/msg/HeightMap.msg +++ /dev/null @@ -1,15 +0,0 @@ -# Header -float64 stamp # timestamp -string frame_id # world frame id - -# Map info -float32 resolution # The map resolution [m/cell] -uint32 width # Map width along x-axis [cells] -uint32 height # Map height alonge y-axis [cells] -float32[2] origin # Map frame origin xy-position [m], the xyz-axis direction of map frame is aligned with the world frame - -# Map data, in x-major order, starting with [0,0], ending with [width, height] -# For a cell whose 2d-array-index is [ix, iy]， -# its position in world frame is: [ix * resolution + origin[0], iy * resolution + origin[1]] -# its cell value is: data[width * iy + ix] -float32[] data \ No newline at end of file diff --git a/deploy/ros2_nodes/go2py_common/msg/IMUState.msg b/deploy/ros2_nodes/go2py_common/msg/IMUState.msg deleted file mode 100755 index e065d04..0000000 --- a/deploy/ros2_nodes/go2py_common/msg/IMUState.msg +++ /dev/null @@ -1,5 +0,0 @@ -float32[4] quaternion -float32[3] gyroscope -float32[3] accelerometer -float32[3] rpy -int8 temperature diff --git a/deploy/ros2_nodes/go2py_common/msg/InterfaceConfig.msg b/deploy/ros2_nodes/go2py_common/msg/InterfaceConfig.msg deleted file mode 100755 index c43bd20..0000000 --- a/deploy/ros2_nodes/go2py_common/msg/InterfaceConfig.msg +++ /dev/null @@ -1,3 +0,0 @@ -uint8 mode -uint8 value -uint8[2] reserve \ No newline at end of file diff --git a/deploy/ros2_nodes/go2py_common/msg/LidarState.msg b/deploy/ros2_nodes/go2py_common/msg/LidarState.msg deleted file mode 100755 index d8b4eb2..0000000 --- a/deploy/ros2_nodes/go2py_common/msg/LidarState.msg +++ /dev/null @@ -1,17 +0,0 @@ -float64 stamp -string firmware_version -string software_version -string sdk_version -float32 sys_rotation_speed -float32 com_rotation_speed -uint8 error_state -float32 cloud_frequency -float32 cloud_packet_loss_rate -uint32 cloud_size -uint32 cloud_scan_num -float32 imu_frequency -float32 imu_packet_loss_rate -float32[3] imu_rpy -float64 serial_recv_stamp -uint32 serial_buffer_size -uint32 serial_buffer_read diff --git a/deploy/ros2_nodes/go2py_common/msg/LowCmd.msg b/deploy/ros2_nodes/go2py_common/msg/LowCmd.msg deleted file mode 100755 index 464a31d..0000000 --- a/deploy/ros2_nodes/go2py_common/msg/LowCmd.msg +++ /dev/null @@ -1,15 +0,0 @@ -uint8[2] head -uint8 level_flag -uint8 frame_reserve -uint32[2] sn -uint32[2] version -uint16 bandwidth -MotorCmd[20] motor_cmd -BmsCmd bms_cmd -uint8[40] wireless_remote -uint8[12] led -uint8[2] fan -uint8 gpio -uint32 reserve -uint32 crc - diff --git a/deploy/ros2_nodes/go2py_common/msg/LowState.msg b/deploy/ros2_nodes/go2py_common/msg/LowState.msg deleted file mode 100755 index ceaed32..0000000 --- a/deploy/ros2_nodes/go2py_common/msg/LowState.msg +++ /dev/null @@ -1,22 +0,0 @@ -uint8[2] head -uint8 level_flag -uint8 frame_reserve -uint32[2] sn -uint32[2] version -uint16 bandwidth -IMUState imu_state -MotorState[20] motor_state -BmsState bms_state -int16[4] foot_force -int16[4] foot_force_est -uint32 tick -uint8[40] wireless_remote -uint8 bit_flag -float32 adc_reel -int8 temperature_ntc1 -int8 temperature_ntc2 -float32 power_v -float32 power_a -uint16[4] fan_frequency -uint32 reserve -uint32 crc \ No newline at end of file diff --git a/deploy/ros2_nodes/go2py_common/msg/MotorCmd.msg b/deploy/ros2_nodes/go2py_common/msg/MotorCmd.msg deleted file mode 100755 index 67cd1bf..0000000 --- a/deploy/ros2_nodes/go2py_common/msg/MotorCmd.msg +++ /dev/null @@ -1,7 +0,0 @@ -uint8 mode -float32 q -float32 dq -float32 tau -float32 kp -float32 kd -uint32[3] reserve \ No newline at end of file diff --git a/deploy/ros2_nodes/go2py_common/msg/MotorState.msg b/deploy/ros2_nodes/go2py_common/msg/MotorState.msg deleted file mode 100755 index 2e1a739..0000000 --- a/deploy/ros2_nodes/go2py_common/msg/MotorState.msg +++ /dev/null @@ -1,11 +0,0 @@ -uint8 mode -float32 q -float32 dq -float32 ddq -float32 tau_est -float32 q_raw -float32 dq_raw -float32 ddq_raw -int8 temperature -uint32 lost -uint32[2] reserve \ No newline at end of file diff --git a/deploy/ros2_nodes/go2py_common/msg/PathPoint.msg b/deploy/ros2_nodes/go2py_common/msg/PathPoint.msg deleted file mode 100755 index 08b4945..0000000 --- a/deploy/ros2_nodes/go2py_common/msg/PathPoint.msg +++ /dev/null @@ -1,7 +0,0 @@ -float32 t_from_start -float32 x -float32 y -float32 yaw -float32 vx -float32 vy -float32 vyaw \ No newline at end of file diff --git a/deploy/ros2_nodes/go2py_common/msg/Req.msg b/deploy/ros2_nodes/go2py_common/msg/Req.msg deleted file mode 100755 index af14a7d..0000000 --- a/deploy/ros2_nodes/go2py_common/msg/Req.msg +++ /dev/null @@ -1,2 +0,0 @@ -string uuid -string body \ No newline at end of file diff --git a/deploy/ros2_nodes/go2py_common/msg/Res.msg b/deploy/ros2_nodes/go2py_common/msg/Res.msg deleted file mode 100755 index 8070258..0000000 --- a/deploy/ros2_nodes/go2py_common/msg/Res.msg +++ /dev/null @@ -1,3 +0,0 @@ -string uuid -uint8[] data -string body \ No newline at end of file diff --git a/deploy/ros2_nodes/go2py_common/msg/SportModeCmd.msg b/deploy/ros2_nodes/go2py_common/msg/SportModeCmd.msg deleted file mode 100755 index e5ccb46..0000000 --- a/deploy/ros2_nodes/go2py_common/msg/SportModeCmd.msg +++ /dev/null @@ -1,13 +0,0 @@ -uint8 mode -uint8 gait_type -uint8 speed_level -float32 foot_raise_height -float32 body_height -float32[2] position -float32[3] euler -float32[2] velocity -float32 yaw_speed -BmsCmd bms_cmd -PathPoint[30] path_point - - diff --git a/deploy/ros2_nodes/go2py_common/msg/SportModeState.msg b/deploy/ros2_nodes/go2py_common/msg/SportModeState.msg deleted file mode 100755 index 7a6c1e6..0000000 --- a/deploy/ros2_nodes/go2py_common/msg/SportModeState.msg +++ /dev/null @@ -1,17 +0,0 @@ -TimeSpec stamp -uint32 error_code -IMUState imu_state -uint8 mode -float32 progress -uint8 gait_type -float32 foot_raise_height -float32[3] position -float32 body_height -float32[3] velocity -float32 yaw_speed -float32[4] range_obstacle -int16[4] foot_force -float32[12] foot_position_body -float32[12] foot_speed_body - - diff --git a/deploy/ros2_nodes/go2py_common/msg/TimeSpec.msg b/deploy/ros2_nodes/go2py_common/msg/TimeSpec.msg deleted file mode 100755 index 188b3c1..0000000 --- a/deploy/ros2_nodes/go2py_common/msg/TimeSpec.msg +++ /dev/null @@ -1,5 +0,0 @@ -# Time indicates a specific point in time, relative to a clock's 0 point. -# The seconds component, valid over all int32 values. -int32 sec -# The nanoseconds component, valid in the range [0, 10e9). -uint32 nanosec diff --git a/deploy/ros2_nodes/go2py_common/msg/UwbState.msg b/deploy/ros2_nodes/go2py_common/msg/UwbState.msg deleted file mode 100755 index ed1da3a..0000000 --- a/deploy/ros2_nodes/go2py_common/msg/UwbState.msg +++ /dev/null @@ -1,19 +0,0 @@ -uint8[2] version -uint8 channel -uint8 joy_mode -float32 orientation_est -float32 pitch_est -float32 distance_est -float32 yaw_est -float32 tag_roll -float32 tag_pitch -float32 tag_yaw -float32 base_roll -float32 base_pitch -float32 base_yaw -float32[2] joystick -uint8 error_state -uint8 buttons -uint8 enabled_from_app - - diff --git a/deploy/ros2_nodes/go2py_common/msg/UwbSwitch.msg b/deploy/ros2_nodes/go2py_common/msg/UwbSwitch.msg deleted file mode 100755 index db8ec91..0000000 --- a/deploy/ros2_nodes/go2py_common/msg/UwbSwitch.msg +++ /dev/null @@ -1 +0,0 @@ -uint8 enabled \ No newline at end of file diff --git a/deploy/ros2_nodes/go2py_common/msg/WirelessController.msg b/deploy/ros2_nodes/go2py_common/msg/WirelessController.msg deleted file mode 100755 index 12965ff..0000000 --- a/deploy/ros2_nodes/go2py_common/msg/WirelessController.msg +++ /dev/null @@ -1,5 +0,0 @@ -float32 lx -float32 ly -float32 rx -float32 ry -uint16 keys \ No newline at end of file diff --git a/deploy/ros2_nodes/go2py_common/CMakeLists.txt b/deploy/ros2_nodes/go2py_messages/CMakeLists.txt similarity index 70% rename from deploy/ros2_nodes/go2py_common/CMakeLists.txt rename to deploy/ros2_nodes/go2py_messages/CMakeLists.txt index dd3ef68..5e2cb40 100755 --- a/deploy/ros2_nodes/go2py_common/CMakeLists.txt +++ b/deploy/ros2_nodes/go2py_messages/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(go2py_common) +project(go2py_messages) # Default to C99 if(NOT CMAKE_C_STANDARD) @@ -26,29 +26,9 @@ find_package(rosidl_default_generators REQUIRED) find_package(rosidl_generator_dds_idl REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} - "msg/AudioData.msg" - "msg/BmsCmd.msg" - "msg/BmsState.msg" - "msg/Error.msg" - "msg/Go2FrontVideoData.msg" - "msg/HeightMap.msg" - "msg/IMUState.msg" - "msg/InterfaceConfig.msg" - "msg/LidarState.msg" - "msg/LowCmd.msg" - "msg/LowState.msg" - "msg/MotorCmd.msg" - "msg/MotorState.msg" - "msg/PathPoint.msg" - "msg/Req.msg" - "msg/Res.msg" - "msg/SportModeCmd.msg" - "msg/SportModeState.msg" - "msg/TimeSpec.msg" - "msg/UwbState.msg" - "msg/UwbSwitch.msg" - "msg/WirelessController.msg" "msg/Go2pyLowCmd.msg" + "msg/Go2pyHighCmd.msg" + "msg/Go2pyState.msg" "msg/Go2pyStatus.msg" DEPENDENCIES geometry_msgs ) diff --git a/deploy/ros2_nodes/go2py_messages/msg/Go2pyHighCmd.msg b/deploy/ros2_nodes/go2py_messages/msg/Go2pyHighCmd.msg new file mode 100644 index 0000000..361dcc0 --- /dev/null +++ b/deploy/ros2_nodes/go2py_messages/msg/Go2pyHighCmd.msg @@ -0,0 +1,3 @@ +float32 vx +float32 vy +float32 wz \ No newline at end of file diff --git a/deploy/ros2_nodes/go2py_common/msg/Go2pyLowCmd.msg b/deploy/ros2_nodes/go2py_messages/msg/Go2pyLowCmd.msg similarity index 100% rename from deploy/ros2_nodes/go2py_common/msg/Go2pyLowCmd.msg rename to deploy/ros2_nodes/go2py_messages/msg/Go2pyLowCmd.msg diff --git a/deploy/ros2_nodes/go2py_messages/msg/Go2pyState.msg b/deploy/ros2_nodes/go2py_messages/msg/Go2pyState.msg new file mode 100644 index 0000000..45070fa --- /dev/null +++ b/deploy/ros2_nodes/go2py_messages/msg/Go2pyState.msg @@ -0,0 +1,13 @@ +uint64 time_sec +uint64 time_nsec +float32[12] q +float32[12] dq +float32[12] tau +float32[4] contact +float32[3] accel +float32[3] gyro +float32[4] quat +float32 imu_temp +float32[12] motor_temp +uint8[40] wireless_remote +uint8 soc \ No newline at end of file diff --git a/deploy/ros2_nodes/go2py_common/msg/Go2pyStatus.msg b/deploy/ros2_nodes/go2py_messages/msg/Go2pyStatus.msg similarity index 100% rename from deploy/ros2_nodes/go2py_common/msg/Go2pyStatus.msg rename to deploy/ros2_nodes/go2py_messages/msg/Go2pyStatus.msg diff --git a/deploy/ros2_nodes/go2py_common/package.xml b/deploy/ros2_nodes/go2py_messages/package.xml similarity index 96% rename from deploy/ros2_nodes/go2py_common/package.xml rename to deploy/ros2_nodes/go2py_messages/package.xml index 5053c64..fe77d57 100755 --- a/deploy/ros2_nodes/go2py_common/package.xml +++ b/deploy/ros2_nodes/go2py_messages/package.xml @@ -1,7 +1,7 @@ - go2py_common + go2py_messages 0.0.0 TODO: Package description czk diff --git a/examples/00-robot-interface-DDS.ipynb b/examples/00-robot-interface copy.ipynb similarity index 79% rename from examples/00-robot-interface-DDS.ipynb rename to examples/00-robot-interface copy.ipynb index d773b42..253b6ac 100644 --- a/examples/00-robot-interface-DDS.ipynb +++ b/examples/00-robot-interface copy.ipynb @@ -7,16 +7,6 @@ "# Lowlevel Control" ] }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "import unitree_go\n", - "unitree_go.__file__" - ] - }, { "cell_type": "code", "execution_count": null, @@ -65,14 +55,36 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 2, "metadata": {}, - "outputs": [], + "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.dds import GO2Real\n", "import time\n", "robot = GO2Real(mode='highlevel')" ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [], + "source": [ + "import numpy as np\n", + "import time\n", + "for i in range(200):\n", + " robot.setCommandsHigh(0,0,0.2)\n", + " time.sleep(0.01) " + ] } ], "metadata": { diff --git a/examples/01-robot-interface-ROS2.ipynb b/examples/01-robot-interface-ROS2.ipynb deleted file mode 100644 index 423e064..0000000 --- a/examples/01-robot-interface-ROS2.ipynb +++ /dev/null @@ -1,156 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "# Lowlevel Control" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "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": null, - "metadata": {}, - "outputs": [], - "source": [ - "import time\n", - "import numpy as np\n", - "qs = []\n", - "dqs = []\n", - "taus = []\n", - "stamps = []\n", - "for i in range(5000):\n", - " time.sleep(0.01)\n", - " state = robot.getJointStates()\n", - " q = state['q']\n", - " dq = state['dq']\n", - " tau = state['tau_est']\n", - " stamp = time.time()\n", - " qs.append(q)\n", - " dqs.append(dq)\n", - " taus.append(tau)\n", - " stamps.append(stamp)\n", - "\n", - "q=np.array(qs)\n", - "dq=np.array(dqs)\n", - "tau=np.array(taus)\n", - "stamp=np.array(stamps)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "import pickle\n", - "with open('go2_rotation.pkl', 'wb') as f:\n", - " pickle.dump(\n", - " {\n", - " 'q':q,\n", - " 'dq':dq, \n", - " 'tau':tau, \n", - " 'stamp':stamp\n", - " }\n", - " ,f\n", - " )" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "import numpy as np\n", - "import time\n", - "for i in range(10000):\n", - " q = np.zeros(12) \n", - " dq = np.zeros(12)\n", - " kp = np.zeros(12)\n", - " kd = np.zeros(12)\n", - " tau = np.zeros(12)\n", - " tau[0] = -0.8\n", - " robot.setCommands(q, dq, kp, kd, tau)\n", - " time.sleep(0.01) " - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "import matplotlib.pyplot as plt\n", - "plt.plot(taus)" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "# Highlevel Control" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "from Go2Py.robot.interface.ros2 import GO2Real, ros2_init, ROS2ExecutorManager\n", - "import time\n", - "ros2_init()\n", - "robot = GO2Real(mode='highlevel')\n", - "ros2_exec_manager = ROS2ExecutorManager()\n", - "ros2_exec_manager.add_node(robot)\n", - "ros2_exec_manager.start()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "robot.getJointStates()" - ] - } - ], - "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 -} diff --git a/examples/01-ros2-tools.ipynb b/examples/01-ros2-tools.ipynb new file mode 100644 index 0000000..253b6ac --- /dev/null +++ b/examples/01-ros2-tools.ipynb @@ -0,0 +1,111 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Lowlevel Control" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "from Go2Py.robot.interface.dds import GO2Real\n", + "import time\n", + "robot = GO2Real(mode='lowlevel')" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "robot.getJointStates()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "import numpy as np\n", + "import time\n", + "for i in range(1000000):\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.01) " + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Highlevel 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.dds import GO2Real\n", + "import time\n", + "robot = GO2Real(mode='highlevel')" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [], + "source": [ + "import numpy as np\n", + "import time\n", + "for i in range(200):\n", + " robot.setCommandsHigh(0,0,0.2)\n", + " time.sleep(0.01) " + ] + } + ], + "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 +} diff --git a/go2py_common/.idlpy_manifest b/go2py_common/.idlpy_manifest deleted file mode 100644 index 4ff480a..0000000 --- a/go2py_common/.idlpy_manifest +++ /dev/null @@ -1,3 +0,0 @@ -Go2pyLowCmd -msg - diff --git a/go2py_common/msg/.idlpy_manifest b/go2py_common/msg/.idlpy_manifest deleted file mode 100644 index 4ab2cc0..0000000 --- a/go2py_common/msg/.idlpy_manifest +++ /dev/null @@ -1,3 +0,0 @@ -Go2pyLowCmd -dds_ - diff --git a/go2py_common/msg/__init__.py b/go2py_common/msg/__init__.py deleted file mode 100644 index 9eb7ef5..0000000 --- a/go2py_common/msg/__init__.py +++ /dev/null @@ -1,9 +0,0 @@ -""" - Generated by Eclipse Cyclone DDS idlc Python Backend - Cyclone DDS IDL version: v0.11.0 - Module: go2py_common.msg - -""" - -from . import dds_ -__all__ = ["dds_", ] diff --git a/go2py_common/msg/dds_/.idlpy_manifest b/go2py_common/msg/dds_/.idlpy_manifest deleted file mode 100644 index bae2f4a..0000000 --- a/go2py_common/msg/dds_/.idlpy_manifest +++ /dev/null @@ -1,3 +0,0 @@ -Go2pyLowCmd - -Go2pyLowCmd_ diff --git a/go2py_common/msg/dds_/__init__.py b/go2py_common/msg/dds_/__init__.py deleted file mode 100644 index f4ecbc5..0000000 --- a/go2py_common/msg/dds_/__init__.py +++ /dev/null @@ -1,9 +0,0 @@ -""" - Generated by Eclipse Cyclone DDS idlc Python Backend - Cyclone DDS IDL version: v0.11.0 - Module: go2py_common.msg.dds_ - -""" - -from ._Go2pyLowCmd import Go2pyLowCmd_ -__all__ = ["Go2pyLowCmd_", ] diff --git a/go2py_messages/.idlpy_manifest b/go2py_messages/.idlpy_manifest new file mode 100644 index 0000000..d21fd04 --- /dev/null +++ b/go2py_messages/.idlpy_manifest @@ -0,0 +1,11 @@ +Go2pyHighCmd +msg + + +Go2pyLowCmd +msg + + +Go2pyState +msg + diff --git a/go2py_common/__init__.py b/go2py_messages/__init__.py similarity index 84% rename from go2py_common/__init__.py rename to go2py_messages/__init__.py index 29bab3d..b1e8e5a 100644 --- a/go2py_common/__init__.py +++ b/go2py_messages/__init__.py @@ -1,7 +1,7 @@ """ Generated by Eclipse Cyclone DDS idlc Python Backend Cyclone DDS IDL version: v0.11.0 - Module: go2py_common + Module: go2py_messages """ diff --git a/go2py_messages/msg/.idlpy_manifest b/go2py_messages/msg/.idlpy_manifest new file mode 100644 index 0000000..3c5861d --- /dev/null +++ b/go2py_messages/msg/.idlpy_manifest @@ -0,0 +1,11 @@ +Go2pyHighCmd +dds_ + + +Go2pyLowCmd +dds_ + + +Go2pyState +dds_ + diff --git a/unitree_go/msg/__init__.py b/go2py_messages/msg/__init__.py similarity index 82% rename from unitree_go/msg/__init__.py rename to go2py_messages/msg/__init__.py index f8cae9b..fe3a99b 100644 --- a/unitree_go/msg/__init__.py +++ b/go2py_messages/msg/__init__.py @@ -1,7 +1,7 @@ """ Generated by Eclipse Cyclone DDS idlc Python Backend Cyclone DDS IDL version: v0.11.0 - Module: unitree_go.msg + Module: go2py_messages.msg """ diff --git a/go2py_messages/msg/dds_/.idlpy_manifest b/go2py_messages/msg/dds_/.idlpy_manifest new file mode 100644 index 0000000..362eb92 --- /dev/null +++ b/go2py_messages/msg/dds_/.idlpy_manifest @@ -0,0 +1,11 @@ +Go2pyHighCmd + +Go2pyHighCmd_ + +Go2pyLowCmd + +Go2pyLowCmd_ + +Go2pyState + +Go2pyState_ diff --git a/unitree_go/msg/dds_/_IMUState_.py b/go2py_messages/msg/dds_/_Go2pyHighCmd.py similarity index 53% rename from unitree_go/msg/dds_/_IMUState_.py rename to go2py_messages/msg/dds_/_Go2pyHighCmd.py index 473d9cb..ddab137 100644 --- a/unitree_go/msg/dds_/_IMUState_.py +++ b/go2py_messages/msg/dds_/_Go2pyHighCmd.py @@ -1,8 +1,8 @@ """ Generated by Eclipse Cyclone DDS idlc Python Backend Cyclone DDS IDL version: v0.11.0 - Module: unitree_go.msg.dds_ - IDL file: IMUState_.idl + Module: go2py_messages.msg.dds_ + IDL file: Go2pyHighCmd.idl """ @@ -15,17 +15,15 @@ import cyclonedds.idl.annotations as annotate import cyclonedds.idl.types as types # root module import for resolving types -import unitree_go +import go2py_messages @dataclass @annotate.final @annotate.autoid("sequential") -class IMUState_(idl.IdlStruct, typename="unitree_go.msg.dds_.IMUState_"): - quaternion: types.array[types.float32, 4] - gyroscope: types.array[types.float32, 3] - accelerometer: types.array[types.float32, 3] - rpy: types.array[types.float32, 3] - temperature: types.uint8 +class Go2pyHighCmd_(idl.IdlStruct, typename="go2py_messages.msg.dds_.Go2pyHighCmd_"): + vx: types.float32 + vy: types.float32 + vz: types.float32 diff --git a/go2py_common/msg/dds_/_Go2pyLowCmd.py b/go2py_messages/msg/dds_/_Go2pyLowCmd.py similarity index 82% rename from go2py_common/msg/dds_/_Go2pyLowCmd.py rename to go2py_messages/msg/dds_/_Go2pyLowCmd.py index 0f37ccd..9e19dbc 100644 --- a/go2py_common/msg/dds_/_Go2pyLowCmd.py +++ b/go2py_messages/msg/dds_/_Go2pyLowCmd.py @@ -1,7 +1,7 @@ """ Generated by Eclipse Cyclone DDS idlc Python Backend Cyclone DDS IDL version: v0.11.0 - Module: go2py_common.msg.dds_ + Module: go2py_messages.msg.dds_ IDL file: Go2pyLowCmd.idl """ @@ -15,13 +15,13 @@ import cyclonedds.idl.annotations as annotate import cyclonedds.idl.types as types # root module import for resolving types -import go2py_common +import go2py_messages @dataclass @annotate.final @annotate.autoid("sequential") -class Go2pyLowCmd_(idl.IdlStruct, typename="go2py_common.msg.dds_.Go2pyLowCmd_"): +class Go2pyLowCmd_(idl.IdlStruct, typename="go2py_messages.msg.dds_.Go2pyLowCmd_"): q: types.array[types.float32, 12] dq: types.array[types.float32, 12] kp: types.array[types.float32, 12] diff --git a/go2py_messages/msg/dds_/_Go2pyState.py b/go2py_messages/msg/dds_/_Go2pyState.py new file mode 100644 index 0000000..d562faa --- /dev/null +++ b/go2py_messages/msg/dds_/_Go2pyState.py @@ -0,0 +1,39 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: go2py_messages.msg.dds_ + IDL file: Go2pyState.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +import go2py_messages + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class Go2pyState_(idl.IdlStruct, typename="go2py_messages.msg.dds_.Go2pyState_"): + time_sec: types.uint64 + time_nsec: types.uint64 + q: types.array[types.float32, 12] + dq: types.array[types.float32, 12] + tau: types.array[types.float32, 12] + contact: types.array[types.float32, 4] + accel: types.array[types.float32, 3] + gyro: types.array[types.float32, 3] + quat: types.array[types.float32, 4] + imu_temp: types.float32 + motor_temp: types.array[types.float32, 12] + wireless_remote: types.array[types.uint8, 40] + soc: types.uint8 + + diff --git a/go2py_messages/msg/dds_/__init__.py b/go2py_messages/msg/dds_/__init__.py new file mode 100644 index 0000000..b26fdeb --- /dev/null +++ b/go2py_messages/msg/dds_/__init__.py @@ -0,0 +1,11 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: go2py_messages.msg.dds_ + +""" + +from ._Go2pyHighCmd import Go2pyHighCmd_ +from ._Go2pyLowCmd import Go2pyLowCmd_ +from ._Go2pyState import Go2pyState_ +__all__ = ["Go2pyHighCmd_", "Go2pyLowCmd_", "Go2pyState_", ] diff --git a/scripts/dds_messages/Go2pyHighCmd.idl b/scripts/dds_messages/Go2pyHighCmd.idl new file mode 100644 index 0000000..7da6f03 --- /dev/null +++ b/scripts/dds_messages/Go2pyHighCmd.idl @@ -0,0 +1,11 @@ +module go2py_messages { + module msg { + module dds_{ + @final struct Go2pyHighCmd_ { + float vx; + float vy; + float vz; + }; + }; + }; +}; \ No newline at end of file diff --git a/dds_messages/idls/Go2pyLowCmd.idl b/scripts/dds_messages/Go2pyLowCmd.idl similarity index 88% rename from dds_messages/idls/Go2pyLowCmd.idl rename to scripts/dds_messages/Go2pyLowCmd.idl index 6c944fe..22707df 100644 --- a/dds_messages/idls/Go2pyLowCmd.idl +++ b/scripts/dds_messages/Go2pyLowCmd.idl @@ -1,4 +1,4 @@ -module go2py_common { +module go2py_messages { module msg { module dds_{ @final struct Go2pyLowCmd_ { diff --git a/scripts/dds_messages/Go2pyState.idl b/scripts/dds_messages/Go2pyState.idl new file mode 100644 index 0000000..fb94c7c --- /dev/null +++ b/scripts/dds_messages/Go2pyState.idl @@ -0,0 +1,21 @@ +module go2py_messages { + module msg { + module dds_{ + @final struct Go2pyState_ { + Unsigned long long time_sec; + Unsigned long long time_nsec; + float q[12]; + float dq[12]; + float tau[12]; + float contact[4]; + float accel[3]; + float gyro[3]; + float quat[4]; + float imu_temp; + float motor_temp[12]; + octet wireless_remote[40]; + octet soc; + }; + }; + }; +}; \ No newline at end of file diff --git a/dds_messages/make_msgs.sh b/scripts/make_msgs.sh similarity index 62% rename from dds_messages/make_msgs.sh rename to scripts/make_msgs.sh index 070decf..f5607a8 100755 --- a/dds_messages/make_msgs.sh +++ b/scripts/make_msgs.sh @@ -5,14 +5,14 @@ NC='\033[0m' # No Color echo -e "${GREEN} Starting DDS type generation...${NC}" # Clean -rm -r ../unitree_go -rm -r ../go2py_common +# rm -r ../unitree_go +rm -r ../go2py_messages # Make -for file in idls/*.idl +for file in dds_messages/*.idl do echo "Processing $file file..." idlc -l py $file done -mv unitree_go ../ -mv go2py_common ../ \ No newline at end of file +# mv unitree_go ../ +mv go2py_messages ../ \ No newline at end of file diff --git a/unitree_go/.idlpy_manifest b/unitree_go/.idlpy_manifest deleted file mode 100644 index e5dd99d..0000000 --- a/unitree_go/.idlpy_manifest +++ /dev/null @@ -1,15 +0,0 @@ -BmsState_ -msg - - -IMUState_ -msg - - -LowState_ -msg - - -MotorState_ -msg - diff --git a/unitree_go/__init__.py b/unitree_go/__init__.py deleted file mode 100644 index b553006..0000000 --- a/unitree_go/__init__.py +++ /dev/null @@ -1,9 +0,0 @@ -""" - Generated by Eclipse Cyclone DDS idlc Python Backend - Cyclone DDS IDL version: v0.11.0 - Module: unitree_go - -""" - -from . import msg -__all__ = ["msg", ] diff --git a/unitree_go/msg/.idlpy_manifest b/unitree_go/msg/.idlpy_manifest deleted file mode 100644 index 5ca29a3..0000000 --- a/unitree_go/msg/.idlpy_manifest +++ /dev/null @@ -1,15 +0,0 @@ -BmsState_ -dds_ - - -IMUState_ -dds_ - - -LowState_ -dds_ - - -MotorState_ -dds_ - diff --git a/unitree_go/msg/dds_/.idlpy_manifest b/unitree_go/msg/dds_/.idlpy_manifest deleted file mode 100644 index bc9d94d..0000000 --- a/unitree_go/msg/dds_/.idlpy_manifest +++ /dev/null @@ -1,15 +0,0 @@ -BmsState_ - -BmsState_ - -IMUState_ - -IMUState_ - -LowState_ - -LowState_ - -MotorState_ - -MotorState_ diff --git a/unitree_go/msg/dds_/_BmsState_.py b/unitree_go/msg/dds_/_BmsState_.py deleted file mode 100644 index baba8cc..0000000 --- a/unitree_go/msg/dds_/_BmsState_.py +++ /dev/null @@ -1,35 +0,0 @@ -""" - Generated by Eclipse Cyclone DDS idlc Python Backend - Cyclone DDS IDL version: v0.11.0 - Module: unitree_go.msg.dds_ - IDL file: BmsState_.idl - -""" - -from enum import auto -from typing import TYPE_CHECKING, Optional -from dataclasses import dataclass - -import cyclonedds.idl as idl -import cyclonedds.idl.annotations as annotate -import cyclonedds.idl.types as types - -# root module import for resolving types -import unitree_go - - -@dataclass -@annotate.final -@annotate.autoid("sequential") -class BmsState_(idl.IdlStruct, typename="unitree_go.msg.dds_.BmsState_"): - version_high: types.uint8 - version_low: types.uint8 - status: types.uint8 - soc: types.uint8 - current: types.int32 - cycle: types.uint16 - bq_ntc: types.array[types.uint8, 2] - mcu_ntc: types.array[types.uint8, 2] - cell_vol: types.array[types.uint16, 15] - - diff --git a/unitree_go/msg/dds_/_LowState_.py b/unitree_go/msg/dds_/_LowState_.py deleted file mode 100644 index 27b1175..0000000 --- a/unitree_go/msg/dds_/_LowState_.py +++ /dev/null @@ -1,48 +0,0 @@ -""" - Generated by Eclipse Cyclone DDS idlc Python Backend - Cyclone DDS IDL version: v0.11.0 - Module: unitree_go.msg.dds_ - IDL file: LowState_.idl - -""" - -from enum import auto -from typing import TYPE_CHECKING, Optional -from dataclasses import dataclass - -import cyclonedds.idl as idl -import cyclonedds.idl.annotations as annotate -import cyclonedds.idl.types as types - -# root module import for resolving types -import unitree_go - - -@dataclass -@annotate.final -@annotate.autoid("sequential") -class LowState_(idl.IdlStruct, typename="unitree_go.msg.dds_.LowState_"): - head: types.array[types.uint8, 2] - level_flag: types.uint8 - frame_reserve: types.uint8 - sn: types.array[types.uint32, 2] - version: types.array[types.uint32, 2] - bandwidth: types.uint16 - imu_state: 'unitree_go.msg.dds_.IMUState_' - motor_state: types.array['unitree_go.msg.dds_.MotorState_', 20] - bms_state: 'unitree_go.msg.dds_.BmsState_' - foot_force: types.array[types.int16, 4] - foot_force_est: types.array[types.int16, 4] - tick: types.uint32 - wireless_remote: types.array[types.uint8, 40] - bit_flag: types.uint8 - adc_reel: types.float32 - temperature_ntc1: types.uint8 - temperature_ntc2: types.uint8 - power_v: types.float32 - power_a: types.float32 - fan_frequency: types.array[types.uint16, 4] - reserve: types.uint32 - crc: types.uint32 - - diff --git a/unitree_go/msg/dds_/_MotorState_.py b/unitree_go/msg/dds_/_MotorState_.py deleted file mode 100644 index 2d28c7b..0000000 --- a/unitree_go/msg/dds_/_MotorState_.py +++ /dev/null @@ -1,37 +0,0 @@ -""" - Generated by Eclipse Cyclone DDS idlc Python Backend - Cyclone DDS IDL version: v0.11.0 - Module: unitree_go.msg.dds_ - IDL file: MotorState_.idl - -""" - -from enum import auto -from typing import TYPE_CHECKING, Optional -from dataclasses import dataclass - -import cyclonedds.idl as idl -import cyclonedds.idl.annotations as annotate -import cyclonedds.idl.types as types - -# root module import for resolving types -import unitree_go - - -@dataclass -@annotate.final -@annotate.autoid("sequential") -class MotorState_(idl.IdlStruct, typename="unitree_go.msg.dds_.MotorState_"): - mode: types.uint8 - q: types.float32 - dq: types.float32 - ddq: types.float32 - tau_est: types.float32 - q_raw: types.float32 - dq_raw: types.float32 - ddq_raw: types.float32 - temperature: types.uint8 - lost: types.uint32 - reserve: types.array[types.uint32, 2] - - diff --git a/unitree_go/msg/dds_/__init__.py b/unitree_go/msg/dds_/__init__.py deleted file mode 100644 index a360471..0000000 --- a/unitree_go/msg/dds_/__init__.py +++ /dev/null @@ -1,12 +0,0 @@ -""" - Generated by Eclipse Cyclone DDS idlc Python Backend - Cyclone DDS IDL version: v0.11.0 - Module: unitree_go.msg.dds_ - -""" - -from ._BmsState_ import BmsState_ -from ._IMUState_ import IMUState_ -from ._LowState_ import LowState_ -from ._MotorState_ import MotorState_ -__all__ = ["BmsState_", "IMUState_", "LowState_", "MotorState_", ]