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

go2py bridge major update

This commit is contained in:
Rooholla-KhorramBakht 2024-05-05 00:11:18 -04:00
parent 890edeb97f
commit a24bfbb5b2
68 changed files with 433 additions and 1314 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

87
Go2Py/robot/interface/dds.py → Go2Py/robot/interface.py
View File

@ -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

0
Go2Py/robot/interface/__init__.py
View File

313
Go2Py/robot/interface/ros2.py
View File

@ -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()

3
Makefile
View File

@ -1,3 +1,6 @@
messages:
@cd scripts && ./make_msgs.sh
realsense:
@cd deploy/docker && docker build --tag go2py_realsense:latest -f Dockerfile.realsense .

49
dds_messages/idls/BmsState_.idl
View File

@ -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__

33
dds_messages/idls/IMUState_.idl
View File

@ -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__

70
dds_messages/idls/LowState_.idl
View File

@ -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__

37
dds_messages/idls/MotorState_.idl
View File

@ -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__

4
deploy/ros2_nodes/go2py_bridge/CMakeLists.txt
View File

@ -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)

2
deploy/ros2_nodes/go2py_bridge/package.xml
View File

@ -10,7 +10,7 @@
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>unitree_go</depend>
<depend>go2py_common</depend>
<depend>go2py_messages</depend>
<depend>unitree_api</depend>
<depend>rclcpp</depend>
<depend>std_msgs</depend>

250
deploy/ros2_nodes/go2py_bridge/src/bridge.cpp
View File

@ -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<nav_msgs::msg::Odometry>("/go2/odom", 1);
pub_imu = this->create_publisher<sensor_msgs::msg::Imu>("/go2/imu", 1);
pub_joint = this->create_publisher<sensor_msgs::msg::JointState>("/go2/joint_states", 1);
sub_twist = this->create_subscription<geometry_msgs::msg::TwistStamped>("/go2/twist_cmd", 1, std::bind(&Custom::twistCmdCallback, this, std::placeholders::_1));
nav2_twist = this->create_subscription<geometry_msgs::msg::Twist>("/go2/cmd_vel", 1, std::bind(&Custom::nav2TwistCmdCallback, this, std::placeholders::_1));
nav2_twist_subr = this->create_subscription<geometry_msgs::msg::Twist>("/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<unitree_go::msg::SportModeState>(
"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<unitree_api::msg::Request>("/api/sport/request", 1);
//Go2 lowlevel interface
//Unitree Topics
init_lowcmd();
lowstate_suber = this->create_subscription<unitree_go::msg::LowState>(
"lowstate", 1, std::bind(&Custom::lowstate_callback, this, std::placeholders::_1));
lowcmd_suber = this->create_subscription<go2py_common::msg::Go2pyLowCmd>(
"/go2/lowcmd", 1, std::bind(&Custom::lowcmd_callback, this, std::placeholders::_1));
unitree_lowstate_suber = this->create_subscription<unitree_go::msg::LowState>(
"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<unitree_api::msg::Request>("/api/sport/request", 1);
lowcmd_puber = this->create_publisher<unitree_go::msg::LowCmd>("/lowcmd", 1);
// Go2py topics
go2py_low_cmd_suber = this->create_subscription<go2py_messages::msg::Go2pyLowCmd>(
"/go2py/low_cmd", 1, std::bind(&Custom::go2py_low_cmd_callback, this, std::placeholders::_1));
go2py_high_cmd_suber = this->create_subscription<go2py_messages::msg::Go2pyHighCmd>(
"/go2py/high_cmd", 1, std::bind(&Custom::go2py_high_cmd_callback, this, std::placeholders::_1));
go2py_state_puber = this->create_publisher<go2py_messages::msg::Go2pyState>("/go2py/state", 1);
status_publisher = this->create_publisher<go2py_messages::msg::Go2pyStatus>("/go2py/status", 1);
api_publisher = this->create_publisher<unitree_api::msg::Request>("/api/robot_state/request", 1);
status_publisher = this->create_publisher<go2py_common::msg::Go2pyStatus>("/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<geometry_msgs::msg::TwistStamped>::SharedPtr sub_twist;
rclcpp::Subscription<geometry_msgs::msg::Twist>::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<geometry_msgs::msg::Twist>::SharedPtr nav2_twist_subr;
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub_imu;
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr pub_joint;
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub_odom;
// Highlevel interface
void highstate_callback(unitree_go::msg::SportModeState::SharedPtr data);
rclcpp::Subscription<unitree_go::msg::SportModeState>::SharedPtr highstate_suber;
rclcpp::Publisher<unitree_api::msg::Request>::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<unitree_go::msg::LowState>::SharedPtr lowstate_suber;
rclcpp::Subscription<go2py_common::msg::Go2pyLowCmd>::SharedPtr lowcmd_suber;
// A struct to store the highlevel states for later use
rclcpp::Publisher<unitree_go::msg::LowCmd>::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<unitree_go::msg::LowState>::SharedPtr unitree_lowstate_suber;
rclcpp::Publisher<unitree_api::msg::Request>::SharedPtr unitree_highreq_puber;
rclcpp::Publisher<unitree_go::msg::LowCmd>::SharedPtr lowcmd_puber;
rclcpp::Publisher<unitree_api::msg::Request>::SharedPtr api_publisher;
rclcpp::Publisher<go2py_common::msg::Go2pyStatus>::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<go2py_messages::msg::Go2pyLowCmd>::SharedPtr go2py_low_cmd_suber;
rclcpp::Subscription<go2py_messages::msg::Go2pyHighCmd>::SharedPtr go2py_high_cmd_suber;
rclcpp::Publisher<go2py_messages::msg::Go2pyState>::SharedPtr go2py_state_puber;
rclcpp::Publisher<go2py_messages::msg::Go2pyStatus>::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<std::chrono::microseconds>(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<std::chrono::microseconds>(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<go2py_common::msg::Go2pyStatus>();
auto msg = std::make_shared<go2py_messages::msg::Go2pyStatus>();
msg->estop = Estop;
msg->sport_mode = sport_mode;
status_publisher->publish(*msg);

2
deploy/ros2_nodes/go2py_common/msg/AudioData.msg
View File

@ -1,2 +0,0 @@
uint64 time_frame
uint8[] data

2
deploy/ros2_nodes/go2py_common/msg/BmsCmd.msg
View File

@ -1,2 +0,0 @@
uint8 off
uint8[3] reserve

9
deploy/ros2_nodes/go2py_common/msg/BmsState.msg
View File

@ -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

2
deploy/ros2_nodes/go2py_common/msg/Error.msg
View File

@ -1,2 +0,0 @@
uint32 source
uint32 state

4
deploy/ros2_nodes/go2py_common/msg/Go2FrontVideoData.msg
View File

@ -1,4 +0,0 @@
uint64 time_frame
uint8[] video720p
uint8[] video360p
uint8[] video180p

15
deploy/ros2_nodes/go2py_common/msg/HeightMap.msg
View File

@ -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

5
deploy/ros2_nodes/go2py_common/msg/IMUState.msg
View File

@ -1,5 +0,0 @@
float32[4] quaternion
float32[3] gyroscope
float32[3] accelerometer
float32[3] rpy
int8 temperature

3
deploy/ros2_nodes/go2py_common/msg/InterfaceConfig.msg
View File

@ -1,3 +0,0 @@
uint8 mode
uint8 value
uint8[2] reserve

17
deploy/ros2_nodes/go2py_common/msg/LidarState.msg
View File

@ -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

15
deploy/ros2_nodes/go2py_common/msg/LowCmd.msg
View File

@ -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

22
deploy/ros2_nodes/go2py_common/msg/LowState.msg
View File

@ -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

7
deploy/ros2_nodes/go2py_common/msg/MotorCmd.msg
View File

@ -1,7 +0,0 @@
uint8 mode
float32 q
float32 dq
float32 tau
float32 kp
float32 kd
uint32[3] reserve

11
deploy/ros2_nodes/go2py_common/msg/MotorState.msg
View File

@ -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

7
deploy/ros2_nodes/go2py_common/msg/PathPoint.msg
View File

@ -1,7 +0,0 @@
float32 t_from_start
float32 x
float32 y
float32 yaw
float32 vx
float32 vy
float32 vyaw

2
deploy/ros2_nodes/go2py_common/msg/Req.msg
View File

@ -1,2 +0,0 @@
string uuid
string body

3
deploy/ros2_nodes/go2py_common/msg/Res.msg
View File

@ -1,3 +0,0 @@
string uuid
uint8[] data
string body

13
deploy/ros2_nodes/go2py_common/msg/SportModeCmd.msg
View File

@ -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

17
deploy/ros2_nodes/go2py_common/msg/SportModeState.msg
View File

@ -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

5
deploy/ros2_nodes/go2py_common/msg/TimeSpec.msg
View File

@ -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

19
deploy/ros2_nodes/go2py_common/msg/UwbState.msg
View File

@ -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

1
deploy/ros2_nodes/go2py_common/msg/UwbSwitch.msg
View File

@ -1 +0,0 @@
uint8 enabled

5
deploy/ros2_nodes/go2py_common/msg/WirelessController.msg
View File

@ -1,5 +0,0 @@
float32 lx
float32 ly
float32 rx
float32 ry
uint16 keys

26
deploy/ros2_nodes/go2py_common/CMakeLists.txt → deploy/ros2_nodes/go2py_messages/CMakeLists.txt
View File

@ -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
)

3
deploy/ros2_nodes/go2py_messages/msg/Go2pyHighCmd.msg Normal file
View File

@ -0,0 +1,3 @@
float32 vx
float32 vy
float32 wz

0
deploy/ros2_nodes/go2py_common/msg/Go2pyLowCmd.msg → deploy/ros2_nodes/go2py_messages/msg/Go2pyLowCmd.msg
View File

13
deploy/ros2_nodes/go2py_messages/msg/Go2pyState.msg Normal file
View File

@ -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

0
deploy/ros2_nodes/go2py_common/msg/Go2pyStatus.msg → deploy/ros2_nodes/go2py_messages/msg/Go2pyStatus.msg
View File

2
deploy/ros2_nodes/go2py_common/package.xml → deploy/ros2_nodes/go2py_messages/package.xml
View File

@ -1,7 +1,7 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>go2py_common</name>
<name>go2py_messages</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="czk@todo.todo">czk</maintainer>

36
examples/00-robot-interface-DDS.ipynb → examples/00-robot-interface copy.ipynb
View File

@ -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": {

156
examples/01-robot-interface-ROS2.ipynb
View File

@ -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
}

111
examples/01-ros2-tools.ipynb Normal file
View File

@ -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
}

3
go2py_common/.idlpy_manifest
View File

@ -1,3 +0,0 @@
Go2pyLowCmd
msg

3
go2py_common/msg/.idlpy_manifest
View File

@ -1,3 +0,0 @@
Go2pyLowCmd
dds_

9
go2py_common/msg/__init__.py
View File

@ -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_", ]

3
go2py_common/msg/dds_/.idlpy_manifest
View File

@ -1,3 +0,0 @@
Go2pyLowCmd
Go2pyLowCmd_

9
go2py_common/msg/dds_/__init__.py
View File

@ -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_", ]

11
go2py_messages/.idlpy_manifest Normal file
View File

@ -0,0 +1,11 @@
Go2pyHighCmd
msg
Go2pyLowCmd
msg
Go2pyState
msg

2
go2py_common/__init__.py → go2py_messages/__init__.py
View File

@ -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
"""

11
go2py_messages/msg/.idlpy_manifest Normal file
View File

@ -0,0 +1,11 @@
Go2pyHighCmd
dds_
Go2pyLowCmd
dds_
Go2pyState
dds_

2
unitree_go/msg/__init__.py → go2py_messages/msg/__init__.py
View File

@ -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
"""

11
go2py_messages/msg/dds_/.idlpy_manifest Normal file
View File

@ -0,0 +1,11 @@
Go2pyHighCmd
Go2pyHighCmd_
Go2pyLowCmd
Go2pyLowCmd_
Go2pyState
Go2pyState_

16
unitree_go/msg/dds_/_IMUState_.py → go2py_messages/msg/dds_/_Go2pyHighCmd.py
View File

@ -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

6
go2py_common/msg/dds_/_Go2pyLowCmd.py → go2py_messages/msg/dds_/_Go2pyLowCmd.py
View File

@ -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]

39
go2py_messages/msg/dds_/_Go2pyState.py Normal file
View File

@ -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

11
go2py_messages/msg/dds_/__init__.py Normal file
View File

@ -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_", ]

11
scripts/dds_messages/Go2pyHighCmd.idl Normal file
View File

@ -0,0 +1,11 @@
module go2py_messages {
module msg {
module dds_{
@final struct Go2pyHighCmd_ {
float vx;
float vy;
float vz;
};
};
};
};

2
dds_messages/idls/Go2pyLowCmd.idl → scripts/dds_messages/Go2pyLowCmd.idl
View File

@ -1,4 +1,4 @@
module go2py_common {
module go2py_messages {
module msg {
module dds_{
@final struct Go2pyLowCmd_ {

21
scripts/dds_messages/Go2pyState.idl Normal file
View File

@ -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;
};
};
};
};

10
dds_messages/make_msgs.sh → scripts/make_msgs.sh
View File

@ -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 ../
# mv unitree_go ../
mv go2py_messages ../

15
unitree_go/.idlpy_manifest
View File

@ -1,15 +0,0 @@
BmsState_
msg
IMUState_
msg
LowState_
msg
MotorState_
msg

9
unitree_go/__init__.py
View File

@ -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", ]

15
unitree_go/msg/.idlpy_manifest
View File

@ -1,15 +0,0 @@
BmsState_
dds_
IMUState_
dds_
LowState_
dds_
MotorState_
dds_

15
unitree_go/msg/dds_/.idlpy_manifest
View File

@ -1,15 +0,0 @@
BmsState_
BmsState_
IMUState_
IMUState_
LowState_
LowState_
MotorState_
MotorState_

35
unitree_go/msg/dds_/_BmsState_.py
View File

@ -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]

48
unitree_go/msg/dds_/_LowState_.py
View File

@ -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

37
unitree_go/msg/dds_/_MotorState_.py
View File

@ -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]

12
unitree_go/msg/dds_/__init__.py
View File

@ -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_", ]