1
struct LowCmd_ {
2
    octet head[2];             //帧头，数据校验用（0xFE,0xEF）
3

4
    octet level_flag;          //保留，目前不用
5
    octet frame_reserve;       //保留，目前不用
6
    unsigned long sn[2];       //保留，目前不用
7
    unsigned long version[2];  //保留，目前不用
8
    unsigned short bandwidth;  //保留，目前不用
9

10
    // FR_0 -> 0 , FR_1 -> 1  , FR_2 -> 2   电机控制顺序，目前只用12电机，后面保留
11
    // FL_0 -> 3 , FL_1 -> 4  , FL_2 -> 5
12
    // RR_0 -> 6 , RR_1 -> 7  , RR_2 -> 8
13
    // RL_0 -> 9 , RL_1 -> 10 , RL_2 -> 11
14
    unitree_go::msg::dds_::MotorCmd_ motor_cmd[20];   //电机控制命令数据
15
    unitree_go::msg::dds_::BmsCmd_ bms_cmd;           //电池控制命令数据
16

17
    octet wireless_remote[40];  //保留，目前不用
18
    octet led[12];              //已经改为内部控制，目前不用
19
    octet fan[2];               //已经改为内部控制，目前不用
20

21
    // &0xFE          自动充电打开             ,  |0x01       自动充电关闭
22
    // &0xFD          12个电机的电源开关打开    ,  |0x02       12个电机的电源开关关闭
23
    octet gpio;
24

25
    unsigned long reserve;  //保留位
26
    unsigned long crc;      //数据CRC校验用,为32crc校验用
27
};

1
import rclpy
2
from rclpy.node import Node
3
from rclpy.executors import SingleThreadedExecutor
4
from unitree_go.msg import LowCmd, MotorCmd
5
import numpy as np
6
import time
7

8
class StandGo2Node(Node):
9
    def __init__(self):
10
        super().__init__('stand_go2_ros')
11
        self.publisher_ = self.create_publisher(LowCmd, 'lowcmd', 10)
12
        self.dt = 0.01  # 控制周期
13
        self.running_time = 0.0
14

15
        # 12个关节目标位置
16
        self.stand_up_joint_pos = np.array([0.00571868, 0.608813, -1.21763, -0.00571868, 0.608813, -1.21763,
17
                                            0.00571868, 0.608813, -1.21763, -0.00571868, 0.608813, -1.21763], dtype=float)
18
        self.stand_down_joint_pos = np.array([0.0473455, 1.22187, -2.44375, -0.0473455, 1.22187, -2.44375,
19
                                              0.0473455, 1.22187, -2.44375, -0.0473455, 1.22187, -2.44375], dtype=float)
20

21
        self.cmd = LowCmd()
22
        self.init_cmd()
23

24
    def init_cmd(self):
25
        self.cmd.motor_cmd = [MotorCmd() for _ in range(20)]
26
        for i in range(20):
27
            self.cmd.motor_cmd[i].mode = 0x01
28
            self.cmd.motor_cmd[i].q = 0.0
29
            self.cmd.motor_cmd[i].kp = 0.0
30
            self.cmd.motor_cmd[i].dq = 0.0
31
            self.cmd.motor_cmd[i].kd = 0.0
32
            self.cmd.motor_cmd[i].tau = 0.0
33

34
    def step(self):
35
        """单步控制更新，由外部循环按固定周期调用"""
36
        # 只在秒数变化时打印，避免刷屏
37
        if int(self.running_time) != int(self.running_time - self.dt):
38
            self.get_logger().info(f"Running time: {self.running_time:.1f}s")
39

40
        if self.running_time < 3.0:
41
            # 0~3秒：站起
42
            phase = np.tanh(self.running_time / 1.2)
43
            for i in range(12):
44
                self.cmd.motor_cmd[i].q = phase * self.stand_up_joint_pos[i] + (1 - phase) * self.stand_down_joint_pos[i]
45
                self.cmd.motor_cmd[i].kp = phase * 50.0 + (1 - phase) * 20.0
46
                self.cmd.motor_cmd[i].dq = 0.0
47
                self.cmd.motor_cmd[i].kd = 3.5
48
                self.cmd.motor_cmd[i].tau = 0.0
49
        else:
50
            # 3~7秒：蹲下
51
            phase = np.tanh((self.running_time - 3.0) / 1.2)
52
            for i in range(12):
53
                self.cmd.motor_cmd[i].q = phase * self.stand_down_joint_pos[i] + (1 - phase) * self.stand_up_joint_pos[i]
54
                self.cmd.motor_cmd[i].kp = 50.0
55
                self.cmd.motor_cmd[i].dq = 0.0
56
                self.cmd.motor_cmd[i].kd = 3.5
57
                self.cmd.motor_cmd[i].tau = 0.0
58

59
        self.publisher_.publish(self.cmd)
60
        self.running_time += self.dt
61

62
        return self.running_time < 7.0  # 返回 True 表示继续运行
63

64
def main(args=None):
65
    rclpy.init(args=args)
66
    node = StandGo2Node()
67
    executor = SingleThreadedExecutor()
68
    executor.add_node(node)
69

70
    dt = node.dt
71
    last_time = time.perf_counter()
72

73
    # 手动控制循环
74
    try:
75
        while rclpy.ok() and node.step():
76
            # 立即检查并处理一个就绪的 ROS 回调，如果没有就绪的回调则等一小会
77
            # 相比于 rclpy.spin_once，executor管理注册的
78
            executor.spin_once(timeout_sec=dt/2)
79

80
            # 控制循环频率
81
            elapsed = time.perf_counter() - last_time
82
            sleep_time = dt - elapsed
83
            if sleep_time > 0:
84
                time.sleep(sleep_time)
85
            last_time = time.perf_counter()
86

87
    finally:
88
        node.destroy_node()
89
        rclpy.shutdown()
90
        print("Node exited successfully.")
91

92
if __name__ == '__main__':
93
    main()

1
import mujoco.viewer
2
import time
3
import ikpy.chain
4
import transforms3d as tf
5
import numpy as np
6

7
# URDF和MJ不一样，需要绕Z轴旋转180度
8
mjrotate = tf.euler.euler2mat(0, 0, 3.14)   # 默认顺序 xyz
9

10
def main():
11
    model = mujoco.MjModel.from_xml_path('mujoco_menagerie/universal_robots_ur5e/scene copy.xml')
12
    data = mujoco.MjData(model)
13
    my_chain = ikpy.chain.Chain.from_urdf_file("./ur5e_orig.urdf")
14

15
    def get_current_joint_angles(data):
16
        return [0] + list(data.qpos[0:6]) + [0]
17

18
    def get_ik_angle(target_pos, target_euler, ref=None):
19
        target_pos1 = mjrotate.dot(target_pos)
20
        target_orientation = mjrotate @ tf.euler.euler2mat(*target_euler)
21
        joint_angles = my_chain.inverse_kinematics(target_pos1, target_orientation, "all", initial_position=get_current_joint_angles(data) if ref is None else ref)
22
        return joint_angles[1:-1]
23

24
    # 压下去多少 产生压力
25
    y_offset = -0.05
26
    # y_offset = 0
27

28
    ee_pos0 = np.array([-0.5, 0.4, 1])
29
    ee_pos1 = np.array([-0.5, 0.5 + y_offset, 1])
30
    ee_pos2 = np.array([0.5, 0.5 + y_offset, 0.2])
31
    ee_euler = [0, 0, 1.57]
32

33
    n_steps = 50
34
    # 预计算正向和反向的关节轨迹
35
    joint_traj_forward = []
36
    joint_traj_backward = []
37
    for i in range(n_steps + 1):
38
        alpha = i / n_steps
39
        ee_pos_fwd = ee_pos1 * (1 - alpha) + ee_pos2 * alpha
40
        # 构造ref为8维，头尾补0
41
        if joint_traj_forward:
42
            ref_full = [0] + list(joint_traj_forward[-1]) + [0]
43
        else:
44
            ref_full = None
45
        joint_traj_forward.append(get_ik_angle(ee_pos_fwd, ee_euler, ref=ref_full))
46
    joint_traj_backward = joint_traj_forward[::-1]
47

48
    # 先到达ee_pos1
49
    data.ctrl[:6] = get_ik_angle(ee_pos0, ee_euler)
50
    with mujoco.viewer.launch_passive(model, data) as viewer:
51
        for _ in range(300):
52
            if not viewer.is_running():
53
                break
54
            mujoco.mj_step(model, data)
55
            viewer.sync()
56
            time.sleep(0.005)
57

58
        # 再开始插值循环
59
        step = 0
60
        r = 16
61
        direction = 1  # 1: pos1->pos2, -1: pos2->pos1
62

63
        while viewer.is_running():
64
            if step % r == 0:
65
                if direction == 1:
66
                    data.ctrl[:6] = joint_traj_forward[step // r]
67
                else:
68
                    data.ctrl[:6] = joint_traj_backward[step // r]
69

70
            mujoco.mj_step(model, data)
71
            viewer.sync()
72
            step += 1
73
            if step > n_steps * r:
74
                step = 0
75
                direction *= -1
76

77
if __name__ == "__main__":
78
    main()