Unitree 3. Low level控制

参考资料:

 官方代码:

 消息:

1. 准备工作

网线连接go1,手柄控制go1蹲下,并使用l1+l2+start切换到lowlevel模式.

2. 代码

# -*- coding: utf-8 -*-
'''------------------------------------------------------------------@File Name:        read_state@Created:          2024 2024/10/08 14:33@Software:         PyCharm@Author:           Jiayu ZENG@Email:            jiayuzeng@asagi.waseda.jp@Description: 运行前，需要将机器人damp，然后同时按l1 l2 start改为lowlevel模式。如果不damp机器人会突然倒下sdk.LowState()uint8[2] head
uint8 levelFlag
uint8 frameReserveuint32[2] SN
uint32[2] version
uint16 bandWidth
IMU imufloat32[4] quaternionfloat32[3] gyroscopefloat32[3] accelerometerfloat32[3] rpyint8 temperature
MotorState[20] motorStateuint8 mode           # motor current modefloat32 q            # motor current position（rad）float32 dq           # motor current speed（rad/s）float32 ddq          # motor current speed（rad/s）float32 tauEst       # current estimated output torque（N*m）float32 q_raw        # motor current position（rad）float32 dq_raw       # motor current speed（rad/s）float32 ddq_raw      # motor current speed（rad/s）int8 temperature     # motor temperature（slow conduction of temperature leads to lag）uint32[2] reserve
BmsState bmsuint8 version_huint8 version_luint8 bms_statusuint8 SOC                  # SOC 0-100%int32 current              # mAuint16 cycleint8[2] BQ_NTC             # x1 degrees centigradeint8[2] MCU_NTC            # x1 degrees centigradeuint16[10] cell_vol        # cell voltage mV
int16[4] footForce
int16[4] footForceEst
uint32 tick
uint8[40] wirelessRemote
uint32 reserve
uint32 crc# Old version Aliengo does not have:
Cartesian[4] eeForceRaw
Cartesian[4] eeForce          #it's a 1-DOF force infact, but we use 3-DOF here just for visualization
Cartesian position            # will delete
Cartesian velocity            # will delete
Cartesian velocity_w            # will delete------------------------------------------------------------------
'''import sys
import time
import math
import numpy as npsys.path.append('/home/blamlight/ROS1_WS/unitree_ws/src/unitree_legged_sdk/lib/python/amd64')
import robot_interface as sdkdef jointLinearInterpolation(initPos, targetPos, rate):rate = np.fmin(np.fmax(rate, 0.0), 1.0)p = initPos * (1 - rate) + targetPos * ratereturn pif __name__ == '__main__':d = {'FR_0': 0, 'FR_1': 1, 'FR_2': 2,'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}PosStopF = math.pow(10, 9)VelStopF = 16000.0HIGHLEVEL = 0xeeLOWLEVEL = 0xffsin_mid_q = [0.0, 1.2, -2.0]dt = 0.002qInit = [0, 0, 0]qDes = [0, 0, 0]sin_count = 0rate_count = 0Kp = [0, 0, 0]Kd = [0, 0, 0]udp = sdk.UDP(LOWLEVEL, 8080, "192.168.123.10", 8007)safe = sdk.Safety(sdk.LeggedType.Go1)cmd = sdk.LowCmd()state = sdk.LowState()udp.InitCmdData(cmd)Tpi = 0motiontime = 0while True:time.sleep(0.002)motiontime += 1# print(motiontime)# print("footForce: ", state.footForce)# print("imu rpy: ", state.imu.quaternion)# print("position: ", state.position.x)udp.Recv()udp.GetRecv(state)if (motiontime > 10):safe.PowerProtect(cmd, state, 1)# 必须发送否则没有recudp.SetSend(cmd)udp.Send()

3. RVIZ同步数据

#!/usr/bin/env python
# -*- coding: utf-8 -*-
'''------------------------------------------------------------------@File Name:        real_to_rviz@Created:          2024 2024/10/08 21:53@Software:         PyCharm@Author:           Jiayu ZENG@Email:            jiayuzeng@asagi.waseda.jp@Description:      ------------------------------------------------------------------
'''import randomimport numpy as np
import rospy
from unitree_legged_msgs.msg import LowCmd, LowState, MotorCmd, MotorState
from geometry_msgs.msg import WrenchStamped, TransformStamped
from sensor_msgs.msg import Imu, JointState
from std_msgs.msg import Header, Float64MultiArray, Float64
from visualization_msgs.msg import Marker, MarkerArray
import tfimport time
import redis
import msgpack
import msgpack_numpy
msgpack_numpy.patch()
import mathimport sys
sys.path.append('/home/blamlight/ROS1_WS/unitree_ws/src/unitree_legged_sdk//lib/python/amd64')
import robot_interface as sdk# Global variables
servo_pub = [None] * 12
lowCmd = LowCmd()
lowState = LowState()d = {'FR_0': 0, 'FR_1': 1, 'FR_2': 2,'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}
PosStopF = math.pow(10, 9)
VelStopF = 16000.0
HIGHLEVEL = 0xee
LOWLEVEL = 0xff
sin_mid_q = [0.0, 1.2, -2.0]
dt = 0.002
qInit = [0, 0, 0]
qDes = [0, 0, 0]
sin_count = 0
rate_count = 0
Kp = [0, 0, 0]
Kd = [0, 0, 0]class rviz_show:def __init__(self, rname):self.robot_name = rnameself.lowState = LowState()self.foot_names = ["FR_foot", "FL_foot", "RR_foot", "RL_foot"]self.footForce_sub = []self.joint_names = ["FR_hip", "FR_thigh", "FR_calf", "FL_hip", "FL_thigh", "FL_calf","RR_hip", "RR_thigh", "RR_calf", "RL_hip", "RL_thigh", "RL_calf"]self.servo_sub = []self.lowState_pub = rospy.Publisher(f"/{self.robot_name}_gazebo/lowState/state", LowState, queue_size=1)self.joint_state = JointState()self.joint_state.header = Header()self.joint_state.name = [f"{name}_joint" for name in self.joint_names]self.joint_state.velocity = [0]self.joint_state.effort = []self.joint_state.position = np.zeros(12, dtype=np.float64)self.rviz_joint_pub = rospy.Publisher(f"/joint_states", JointState, queue_size=1) # /{self.robot_name}_rvizself.terrain_marker_pub = rospy.Publisher('/marker/terrain_marker', MarkerArray, queue_size=1)self.body_marker_pub = rospy.Publisher('/marker/body_marker', MarkerArray, queue_size=1)self.foot_maker_pb = rospy.Publisher('/marker/foot_maker', MarkerArray, queue_size=1)self.br = tf.TransformBroadcaster()# ground height detect pointsself.terrain_array = MarkerArray()measured_points_x = [-0.8, -0.7, -0.6, -0.5, -0.4, -0.3, -0.2, -0.1, 0., 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7,0.8]  # 1mx1.6m rectangle (without center line)measured_points_y = [-0.5, -0.4, -0.3, -0.2, -0.1, 0., 0.1, 0.2, 0.3, 0.4, 0.5]marker_id = 0scale = 0.02  # Set the scale (size) of the spherescolor = [0.0, 1.0, 0.0, 1.0]  # Set the color (r, g, b, a) of the spheresself.x = []self.y = []for i in measured_points_x:for j in measured_points_y:marker = self.create_marker(marker_id, i, j, 0., scale, color)self.terrain_array.markers.append(marker)self.x.append(i)self.y.append(j)marker_id += 1def update(self, state):# self.update_terrain_marker()self.update_joint(state)self.update_pose(state)def update_terrain_marker(self):height = self.isaac_data.rviz_cmd.terrain_measured_points_z.flatten()stamp = rospy.Time.now()for marker, x, y, z in zip(self.terrain_array.markers, self.x, self.y, height):marker.header.stamp = stamp# marker.pose.position.z = random.uniform(-0.01, 0.01)marker.pose.position.x = x + self.isaac_data.rviz_cmd.body_pos[0]marker.pose.position.y = y + self.isaac_data.rviz_cmd.body_pos[1]marker.pose.position.z = z + self.isaac_data.rviz_cmd.body_pos[2]self.terrain_marker_pub.publish(self.terrain_array)def update_joint(self, state):self.joint_state.header.stamp = rospy.Time.now()for i, _ in enumerate(self.joint_names):self.joint_state.position[i] = state.motorState[i].qself.rviz_joint_pub.publish(self.joint_state)def update_pose(self, state):t = TransformStamped()t.header.stamp = rospy.Time.now()t.header.frame_id = 'base't.child_frame_id = 'trunk'# go1不回传position# t.transform.translation.x = state.position[0]# t.transform.translation.y = state.position[1]# t.transform.translation.z = state.position[2] # + np.mean(self.isaac_data.rviz_cmd.body_height)t.transform.rotation.x = state.imu.quaternion[1]  # 注意quat对应顺序t.transform.rotation.y = state.imu.quaternion[2]t.transform.rotation.z = state.imu.quaternion[3]t.transform.rotation.w = state.imu.quaternion[0]# Broadcast the transformself.br.sendTransformMessage(t)def create_marker(self, marker_id, x, y, z, scale, color):marker = Marker()marker.header.frame_id = "base"marker.header.stamp = rospy.Time.now()marker.ns = "ground_marker"marker.id = marker_idmarker.type = Marker.SPHEREmarker.action = Marker.ADDmarker.pose.position.x = xmarker.pose.position.y = ymarker.pose.position.z = zmarker.pose.orientation.x = 0.0marker.pose.orientation.y = 0.0marker.pose.orientation.z = 0.0marker.pose.orientation.w = 1.0# Set the scale of the marker (size of the sphere)marker.scale.x = scalemarker.scale.y = scalemarker.scale.z = scale# Set the color of the marker (r, g, b, a)marker.color.a = color[3]marker.color.r = color[0]marker.color.g = color[1]marker.color.b = color[2]return markerdef main():udp = sdk.UDP(LOWLEVEL, 8080, "192.168.123.10", 8007)safe = sdk.Safety(sdk.LeggedType.Go1)cmd = sdk.LowCmd()state = sdk.LowState()udp.InitCmdData(cmd)motiontime = 0rospy.init_node('real_to_rviz')robot_name = rospy.get_param("/robot_name")print(f"robot_name: {robot_name}")rate = rospy.Rate(500)rshow = rviz_show(robot_name)while not rospy.is_shutdown():motiontime += 1udp.Recv()udp.GetRecv(state)# print(motiontime)# print("footForce: ", state.footForce)# print("imu rpy: ", state.imu.quaternion)# print("position: ", state.velocity)rshow.update(state)if motiontime > 10:safe.PowerProtect(cmd, state, 1)# 必须发送否则没有recudp.SetSend(cmd)udp.Send()rate.sleep()if __name__ == "__main__":try:main()except rospy.ROSInterruptException:pass