EmbodiedAI
/
unitree_rl_gym
mirror of https://github.com/unitreerobotics/unitree_rl_gym.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

abs-ik, test ik

This commit is contained in:
Yoonyoung Cho 2025-02-13 21:14:56 +09:00
parent d6a314c037
commit 701ca6b366
4 changed files with 151 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
deploy/deploy_real/config.py
View File

@ -46,6 +46,11 @@ class Config:
self.motor_joint = config['motor_joint']
else:
self.motor_joint=[]
if 'act_joint' in config:
self.act_joint = config['act_joint']
else:
self.act_joint=[]
self.ang_vel_scale = config["ang_vel_scale"]
self.dof_pos_scale = config["dof_pos_scale"]

23
deploy/deploy_real/configs/ik.yaml
View File

@ -9,6 +9,15 @@ lowstate_topic: "rt/lowstate"
policy_path: "{LEGGED_GYM_ROOT_DIR}/deploy/pre_train/g1/policy.pt"
act_joint: [
"left_shoulder_pitch_joint",
"left_shoulder_roll_joint",
"left_shoulder_yaw_joint",
"left_elbow_joint",
"left_wrist_roll_joint",
"left_wrist_pitch_joint",
"left_wrist_yaw_joint"
]
motor_joint: [
'left_hip_pitch_joint',
'left_hip_roll_joint',
@ -59,18 +68,24 @@ kps: [
100, 100, 50, 50, 20, 20, 20
]
kds: [
2, 2, 2, 4, 2, 2, 2, 2, 2, 4, 2, 2,
# left leg
2, 2, 2, 4, 2, 2,
# right leg
2, 2, 2, 4, 2, 2,
# waist
3, 3, 3,
# left arm
2, 2, 2, 2, 1, 1, 1,
# right arm
2, 2, 2, 2, 1, 1, 1
]
default_angles: [
-0.2, 0.0, 0.0, 0.42, -0.23, 0.0,
-0.2, 0.0, 0.0, 0.42, -0.23, 0.0,
0., 0., 0.,
0.35, 0.16, 0., 0.87, 0., 0., 0.,
0.35, -0.16, 0., 0.87, 0., 0., 0.,
]
0.0, 0.0, 0., 0.0, 0., 0., 0.,
0.0, 0.0, 0., 0.0, 0., 0., 0.,
]
# arm_waist_joint2motor_idx: [12, 13, 14,
# 15, 16, 17, 18, 19, 20, 21,

45
deploy/deploy_real/deploy_real_ros_ikctrl.py
View File

@ -14,7 +14,8 @@ from config import Config
from common.crc import CRC
from enum import Enum
from ikctrl import IKCtrl
import pinocchio as pin
from ikctrl import IKCtrl, xyzw2wxyz
class Mode(Enum):
wait = 0
@ -28,29 +29,35 @@ class Controller:
def __init__(self, config: Config) -> None:
self.config = config
self.remote_controller = RemoteController()
act_joint = ["left_shoulder_pitch_joint",
"left_shoulder_roll_joint",
"left_shoulder_yaw_joint",
"left_elbow_joint",
"left_wrist_roll_joint",
"left_wrist_pitch_joint",
"left_wrist_yaw_joint"]
act_joint = config.act_joint
self.ikctrl = IKCtrl('../../resources/robots/g1_description/g1_29dof_with_hand_rev_1_0.urdf',
act_joint)
self.lim_lo_pin = self.ikctrl.robot.model.lowerPositionLimit
self.lim_hi_pin = self.ikctrl.robot.model.upperPositionLimit
# == build index map ==
self.pin_from_mot = np.zeros(29, dtype=np.int32) # FIXME(ycho): hardcoded
self.mot_from_pin = np.zeros(43, dtype=np.int32) # FIXME(ycho): hardcoded
self.mot_from_pin_act = np.zeros(7, dtype=np.int32) # FIXME(ycho): hardcoded
self.mot_from_act = np.zeros(7, dtype=np.int32) # FIXME(ycho): hardcoded
for i_mot, j in enumerate( self.config.motor_joint ):
i_pin = (self.ikctrl.robot.index(j) - 1)
self.pin_from_mot[i_mot] = i_pin
self.mot_from_pin[i_pin] = i_mot
if j in act_joint:
i_act = act_joint.index(j)
self.mot_from_pin_act[i_act] = i_mot
self.mot_from_act[i_act] = i_mot
q_mot = np.array(config.default_angles)
q_pin = np.zeros_like(self.ikctrl.cfg.q)
q_pin[self.pin_from_mot] = q_mot
if True:
default_pose = self.ikctrl.fk(q_pin)
xyz = default_pose.translation
quat_wxyz = xyzw2wxyz(pin.Quaternion(default_pose.rotation).coeffs())
self.default_pose = np.concatenate([xyz, quat_wxyz])
self.target_pose = np.copy(self.default_pose)
# Initialize the policy network
self.policy = torch.jit.load(config.policy_path)
@ -224,14 +231,24 @@ class Controller:
for i_mot in range(len(self.config.motor_joint)):
i_pin = self.pin_from_mot[i_mot]
self.qj[i_pin] = self.low_state.motor_state[i_mot].q
self.cmd[0] = self.remote_controller.ly
self.cmd[1] = self.remote_controller.lx * -1
self.cmd[2] = self.remote_controller.rx * -1
delta = np.concatenate([self.cmd,
[1,0,0,0]])
res_q = self.ikctrl(self.qj, delta, rel=True)
if False:
delta = np.concatenate([self.cmd,
[1,0,0,0]])
res_q = self.ikctrl(self.qj, delta, rel=True)
else:
# FIXME(ycho): 0.01 --> cmd_scale ?
self.target_pose[..., :3] += 0.01 * self.cmd
res_q = self.ikctrl(self.qj,
self.target_pose,
rel=False)
for i_act in range(len(res_q)):
i_mot = self.mot_from_pin_act[i_act]
i_mot = self.mot_from_act[i_act]
i_pin = self.pin_from_mot[i_mot]
target_q = (
self.low_state.motor_state[i_mot].q + res_q[i_act]

109
deploy/deploy_real/ikctrl.py
View File

@ -4,8 +4,10 @@ from contextlib import contextmanager
from pathlib import Path
import time
import yaml
import numpy as np
import torch as th
from yourdfpy import URDF
import pinocchio as pin
import pink
@ -61,31 +63,42 @@ def dls_ik(
class IKCtrl:
def __init__(self,
urdf_path: str,
joint_names: Tuple[str, ...],
act_joints: Tuple[str, ...],
frame: str = 'left_hand_palm_link',
sqlmda: float = 0.05**2):
path = Path(urdf_path)
print('urdf path', path.parent.resolve())
with with_dir(path.parent):
robot = pin.RobotWrapper.BuildFromURDF(filename=path.name,
package_dirs=["."],
root_joint=None)
self.robot = robot
# NOTE(ycho): build index map between pin.q and other set(s) of ordered joints.
larm_from_pin = []
for j in joint_names:
larm_from_pin.append(robot.index(j) - 1)
self.larm_from_pin = np.asarray(larm_from_pin, dtype=np.int32)
self.task = FrameTask("left_hand_palm_link",
position_cost=1.0,
orientation_cost=0.0)
# NOTE(ycho): we skip joint#0(="universe")
joint_names = list(self.robot.model.names)
assert (joint_names[0] == 'universe')
self.joint_names = joint_names[1:]
# NOTE(ycho): build index map between pin.q and other set(s) of ordered
# joints.
act_from_pin = []
for j in act_joints:
act_from_pin.append(robot.index(j) - 1)
self.frame = frame
self.act_from_pin = np.asarray(act_from_pin, dtype=np.int32)
self.task = FrameTask(frame, position_cost=1.0, orientation_cost=0.0)
self.sqlmda = sqlmda
self.cfg = pink.Configuration(robot.model, robot.data,
np.zeros_like(robot.q0))
def fk(self, q: np.ndarray):
robot = self.robot
return pink.Configuration(
robot.model, robot.data, q).get_transform_frame_to_world(
self.frame)
def __call__(self,
q0: np.ndarray,
target_pose: np.ndarray,
rel:bool=False
rel: bool = False
):
"""
Arg:
@ -100,7 +113,7 @@ class IKCtrl:
# source pose
self.cfg.update(q0)
T0 = self.cfg.get_transform_frame_to_world("left_hand_palm_link")
T0 = self.cfg.get_transform_frame_to_world(self.frame)
# target pose
dst_xyz = target_pose[..., 0:3]
@ -116,7 +129,7 @@ class IKCtrl:
# jacobian
self.task.set_target(T0)
jac = self.task.compute_jacobian(self.cfg)
jac = jac[:, self.larm_from_pin]
jac = jac[:, self.act_from_pin]
# error&ik
dT = T1.actInv(T0)
@ -124,3 +137,73 @@ class IKCtrl:
dq = dls_ik(dpose, jac, self.sqlmda)
return dq
def main():
urdf_path = '../../resources/robots/g1_description/g1_29dof_with_hand_rev_1_0.urdf'
# == init yourdfpy robot ==
path = Path(urdf_path)
with with_dir(path.parent):
viz = URDF.load(path.name,
build_collision_scene_graph=True,
load_meshes=False,
load_collision_meshes=True)
# == populate with defaults ==
with open('./configs/ik.yaml', 'r') as fp:
data = yaml.safe_load(fp)
q_mot = np.asarray(data['default_angles'])
q_viz = np.zeros((len(viz.actuated_joint_names)))
viz_from_mot = np.zeros(len(data['motor_joint']),
dtype=np.int32)
for i_mot, j in enumerate(data['motor_joint']):
i_viz = viz.actuated_joint_names.index(j)
viz_from_mot[i_mot] = i_viz
q_viz[viz_from_mot] = q_mot
ctrl = IKCtrl(urdf_path, data['act_joint'])
q_pin = np.zeros_like(ctrl.cfg.q)
pin_from_mot = np.zeros(len(data['motor_joint']),
dtype=np.int32)
for i_mot, j in enumerate(data['motor_joint']):
i_pin = ctrl.joint_names.index(j)
pin_from_mot[i_mot] = i_pin
mot_from_act = np.zeros(len(data['act_joint']),
dtype=np.int32)
for i_act, j in enumerate(data['act_joint']):
i_mot = data['motor_joint'].index(j)
mot_from_act[i_act] = i_mot
viz.update_cfg(q_viz)
# viz.show(collision_geometry=True)
if True:
current_pose = viz.get_transform(ctrl.frame)
print('curpose (viz)', current_pose)
print('curpose (pin)', ctrl.fk(q_pin).homogeneous)
def callback(scene):
if True:
current_pose = viz.get_transform(ctrl.frame)
T = pin.SE3()
T.translation = (current_pose[..., :3, 3]
+ [0.01, 0.0, 0.0])
T.rotation = current_pose[..., :3, :3]
target_pose = pin.SE3ToXYZQUAT(T)
target_pose[..., 3:7] = xyzw2wxyz(target_pose[..., 3:7])
q_pin[pin_from_mot] = q_mot
dq = ctrl(q_pin, target_pose, rel=False)
q_mot[mot_from_act] += dq
q_viz[viz_from_mot] = q_mot
viz.update_cfg(q_viz)
viz.show(collision_geometry=True,
callback=callback)
if __name__ == '__main__':
main()