아래와 같이 초기 설정 변환 코드를 공유드립니다.
self.ema_angles = np.array([
# 왼팔 7개
math.radians(16.440354124679903), # left_shoulder_pitch
math.radians(13.097076417475272), # left_shoulder_roll
math.radians(-0.7024383721112378), # left_shoulder_yaw
math.radians(55.689698095794256), # left_elbow
math.radians(4.657516667202273), # left_wrist_roll
math.radians(-0.20478517295434204), # left_wrist_pitch
math.radians(0.15952149598445434), # left_wrist_yaw
# 오른팔 7개
math.radians(16.838608794805005), # right_shoulder_pitch
math.radians(-12.567672723775814), # right_shoulder_roll
math.radians(0.42366027911447995), # right_shoulder_yaw
math.radians(56.177902942193846), # right_elbow
math.radians(-6.834869557141502), # right_wrist_roll
math.radians(-0.5462402700860972), # right_wrist_pitch
math.radians(-0.08833008074976102), # right_wrist_yaw
# 허리
0.0 # waist_yaw
], dtype=float)
해당 부분을 적용하지 않으면, moveit에서 관절 각도를 전달할때 저 각도를 zero position으로 주다보니까 zero position으로 급격히 이동하는 문제가 발생했었습니다. 향후에 참고하시기 바랍니다!!!
"""
g1_29dof_arm7_ys_waist_release.py:
- /hmr2/angles 받음
- EMA 스무딩 적용
- G1 관절 인덱스로 매핑
- PD 제어로 모터 명령 생성
- Unitree SDK로 로봇에 전송
- Stage 1: Release Arm 포지션으로 초기 자세 설정
원본: g1_29dof_arm7_ys_waist.py
변경사항: Stage 1에서 zero posture 대신 Release Arm 포지션으로 이동
"""
import time
import sys
import math
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float32MultiArray
sys.path.insert(0, '/home/megagen/unitree_g1_lee/src/unitree_sdk2_python')
from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize
from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_
from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowState_
from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_
from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_
from unitree_sdk2py.utils.crc import CRC
from unitree_sdk2py.utils.thread import RecurrentThread
import numpy as np
class G1JointIndex:
# Left leg
LeftHipPitch = 0
LeftHipRoll = 1
LeftHipYaw = 2
LeftKnee = 3
LeftAnklePitch = 4
LeftAnkleB = 4
LeftAnkleRoll = 5
LeftAnkleA = 5
# Right leg
RightHipPitch = 6
RightHipRoll = 7
RightHipYaw = 8
RightKnee = 9
RightAnklePitch = 10
RightAnkleB = 10
RightAnkleRoll = 11
RightAnkleA = 11
WaistYaw = 12
WaistRoll = 13 # NOTE: INVALID for g1 23dof/29dof with waist locked
WaistA = 13 # NOTE: INVALID for g1 23dof/29dof with waist locked
WaistPitch = 14 # NOTE: INVALID for g1 23dof/29dof with waist locked
WaistB = 14 # NOTE: INVALID for g1 23dof/29dof with waist locked
# Left arm
LeftShoulderPitch = 15
LeftShoulderRoll = 16
LeftShoulderYaw = 17
LeftElbow = 18
LeftWristRoll = 19
LeftWristPitch = 20 # NOTE: INVALID for g1 23dof
LeftWristYaw = 21 # NOTE: INVALID for g1 23dof
# Right arm
RightShoulderPitch = 22
RightShoulderRoll = 23
RightShoulderYaw = 24
RightElbow = 25
RightWristRoll = 26
RightWristPitch = 27 # NOTE: INVALID for g1 23dof
RightWristYaw = 28 # NOTE: INVALID for g1 23dof
kNotUsedJoint = 29 # NOTE: Weight
class Custom(Node):
def __init__(self):
super().__init__('unitree_g1_sdk_node')
self.time_ = 0.0
self.control_dt_ = 0.02
self.duration_ = 3.0
self.counter_ = 0
self.weight = 0.
self.weight_rate = 0.2
self.kp = 180.
self.kd = 6.0
self.dq = 0.
self.tau_ff = 0.
self.mode_machine_ = 0
self.low_cmd = unitree_hg_msg_dds__LowCmd_()
self.low_state = None
self.first_update_low_state = False
self.crc = CRC()
self.done = False
self.stand_up_joint_pos = np.zeros(29, dtype=float)
# Release Arm 포지션으로 초기값 설정 (Stage 2 진입 시 zero로 튀는 것 방지)
# 왼팔
self.stand_up_joint_pos[15] = math.radians(16.440354124679903)
self.stand_up_joint_pos[16] = math.radians(13.097076417475272)
self.stand_up_joint_pos[17] = math.radians(-0.7024383721112378)
self.stand_up_joint_pos[18] = math.radians(55.689698095794256)
self.stand_up_joint_pos[19] = math.radians(4.657516667202273)
self.stand_up_joint_pos[20] = math.radians(-0.20478517295434204)
self.stand_up_joint_pos[21] = math.radians(0.15952149598445434)
# 오른팔
self.stand_up_joint_pos[22] = math.radians(16.838608794805005)
self.stand_up_joint_pos[23] = math.radians(-12.567672723775814)
self.stand_up_joint_pos[24] = math.radians(0.42366027911447995)
self.stand_up_joint_pos[25] = math.radians(56.177902942193846)
self.stand_up_joint_pos[26] = math.radians(-6.834869557141502)
self.stand_up_joint_pos[27] = math.radians(-0.5462402700860972)
self.stand_up_joint_pos[28] = math.radians(-0.08833008074976102)
# EMA parameters (Release Arm 포지션으로 초기값 설정하여 zero로 튀는 것 방지)
self.ema_angles = np.array([
# 왼팔 7개
math.radians(16.440354124679903), # left_shoulder_pitch
math.radians(13.097076417475272), # left_shoulder_roll
math.radians(-0.7024383721112378), # left_shoulder_yaw
math.radians(55.689698095794256), # left_elbow
math.radians(4.657516667202273), # left_wrist_roll
math.radians(-0.20478517295434204), # left_wrist_pitch
math.radians(0.15952149598445434), # left_wrist_yaw
# 오른팔 7개
math.radians(16.838608794805005), # right_shoulder_pitch
math.radians(-12.567672723775814), # right_shoulder_roll
math.radians(0.42366027911447995), # right_shoulder_yaw
math.radians(56.177902942193846), # right_elbow
math.radians(-6.834869557141502), # right_wrist_roll
math.radians(-0.5462402700860972), # right_wrist_pitch
math.radians(-0.08833008074976102), # right_wrist_yaw
# 허리
0.0 # waist_yaw
], dtype=float)
self.alpha = 0.15 # Smoothing factor for EMA
self.arm_joints = [
G1JointIndex.LeftShoulderPitch, G1JointIndex.LeftShoulderRoll,
G1JointIndex.LeftShoulderYaw, G1JointIndex.LeftElbow,
G1JointIndex.LeftWristRoll, G1JointIndex.LeftWristPitch,
G1JointIndex.LeftWristYaw,
G1JointIndex.RightShoulderPitch, G1JointIndex.RightShoulderRoll,
G1JointIndex.RightShoulderYaw, G1JointIndex.RightElbow,
G1JointIndex.RightWristRoll, G1JointIndex.RightWristPitch,
G1JointIndex.RightWristYaw,
G1JointIndex.WaistYaw,
G1JointIndex.WaistRoll,
G1JointIndex.WaistPitch
]
# Release Arm 초기 포지션 (arm_joints 순서와 동일)
# 왼팔 7개 + 오른팔 7개 + 허리 3개
self.initial_target_pos = [
# 왼팔: [16.44, 13.10, -0.70, 55.69, 4.66, -0.20, 0.16] (degrees)
math.radians(16.440354124679903),
math.radians(13.097076417475272),
math.radians(-0.7024383721112378),
math.radians(55.689698095794256),
math.radians(4.657516667202273),
math.radians(-0.20478517295434204),
math.radians(0.15952149598445434),
# 오른팔: [16.84, -12.57, 0.42, 56.18, -6.83, -0.55, -0.09] (degrees)
math.radians(16.838608794805005),
math.radians(-12.567672723775814),
math.radians(0.42366027911447995),
math.radians(56.177902942193846),
math.radians(-6.834869557141502),
math.radians(-0.5462402700860972),
math.radians(-0.08833008074976102),
# 허리 3개: [0, 0, 0]
0.0, 0.0, 0.0
]
# Unitree SDK DDS 통신 (로봇 하드웨어와 직접 통신)
# create publisher #
self.arm_sdk_publisher = ChannelPublisher("rt/arm_sdk", LowCmd_)
self.arm_sdk_publisher.Init()
# Unitree SDK DDS 통신 (로봇 하드웨어와 직접 통신)
# create subscriber #
self.lowstate_subscriber = ChannelSubscriber("rt/lowstate", LowState_)
self.lowstate_subscriber.Init(self.LowStateHandler, 10)
self.create_subscription(
Float32MultiArray,
'/hmr2/angles',
self.angles_callback,
10
)
def Start(self):
self.lowCmdWriteThreadPtr = RecurrentThread(
interval=self.control_dt_, target=self.LowCmdWrite, name="control"
)
while self.first_update_low_state == False:
time.sleep(1)
if self.first_update_low_state == True:
self.lowCmdWriteThreadPtr.Start()
def angles_callback(self, data):
angles = np.array(data.data, dtype=np.float32)
# Update EMA values (급격한 변화 방지)
self.ema_angles = self.alpha * angles + (1 - self.alpha) * self.ema_angles
# moveit에서 들어오는 joint 값
self.stand_up_joint_pos[22] = self.ema_angles[7] # right_shoulder_pitch
self.stand_up_joint_pos[23] = self.ema_angles[8] # right_shoulder_roll
self.stand_up_joint_pos[24] = self.ema_angles[9] # right_shoulder_yaw
self.stand_up_joint_pos[25] = self.ema_angles[10] # right_elbow
self.stand_up_joint_pos[26] = self.ema_angles[11] # right_wrist_roll
self.stand_up_joint_pos[27] = self.ema_angles[12] # right_wrist_pitch
self.stand_up_joint_pos[28] = self.ema_angles[13] # right_wrist_yaw
self.stand_up_joint_pos[15] = self.ema_angles[0] # left_shoulder_pitch
self.stand_up_joint_pos[16] = self.ema_angles[1] # left_shoulder_roll
self.stand_up_joint_pos[17] = self.ema_angles[2] # left_shoulder_yaw
self.stand_up_joint_pos[18] = self.ema_angles[3] # left_elbow
self.stand_up_joint_pos[19] = self.ema_angles[4] # left_wrist_roll
self.stand_up_joint_pos[20] = self.ema_angles[5] # left_wrist_pitch
self.stand_up_joint_pos[21] = self.ema_angles[6] # left_wrist_yaw
self.stand_up_joint_pos[12] = self.ema_angles[-1] # wrist_yaw
print(self.stand_up_joint_pos)
def LowStateHandler(self, msg: LowState_):
self.low_state = msg
if self.first_update_low_state == False:
self.first_update_low_state = True
def LowCmdWrite(self):
self.time_ += self.control_dt_
if self.time_ < self.duration_ :
# [Stage 1]: Release Arm 포지션으로 이동
self.low_cmd.motor_cmd[G1JointIndex.kNotUsedJoint].q = 1 # 1:Enable arm_sdk, 0:Disable arm_sdk
for i,joint in enumerate(self.arm_joints):
ratio = np.clip(self.time_ / self.duration_, 0.0, 1.0)
self.low_cmd.motor_cmd[joint].tau = 0.
self.low_cmd.motor_cmd[joint].q = ratio * self.initial_target_pos[i] + (1.0 - ratio) * self.low_state.motor_state[joint].q
self.low_cmd.motor_cmd[joint].dq = 0.
self.low_cmd.motor_cmd[joint].kp = self.kp
self.low_cmd.motor_cmd[joint].kd = self.kd
self.get_logger().info("Arm and wrist motors smoothly transitioning to Release Arm position.")
else:
# angles_callback에서 moveit에서 목표 각도가 들어오면 self.stand_up_joint_pos[joint] 그 값으로 제어
for i,joint in enumerate(self.arm_joints):
self.low_cmd.motor_cmd[joint].tau = 0.
self.low_cmd.motor_cmd[joint].q = self.stand_up_joint_pos[joint]
self.low_cmd.motor_cmd[joint].dq = 0.
self.low_cmd.motor_cmd[joint].kp = self.kp
self.low_cmd.motor_cmd[joint].kd = self.kd
self.low_cmd.crc = self.crc.Crc(self.low_cmd)
self.arm_sdk_publisher.Write(self.low_cmd)
def main(args=None):
print("WARNING: Please ensure there are no obstacles around the robot while running this example.")
# input("Press Enter to continue...")
rclpy.init(args=args) # Initialize ROS2 node
if len(sys.argv) > 1:
ChannelFactoryInitialize(0, sys.argv[1])
else:
ChannelFactoryInitialize(0)
custom_node = Custom()
custom_node.Start()
rclpy.spin(custom_node) # Keep the node alive and allow callbacks to be executed
custom_node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
