diff --git a/deploy/deploy_real/config.py b/deploy/deploy_real/config.py index 9032687..0c89a8d 100644 --- a/deploy/deploy_real/config.py +++ b/deploy/deploy_real/config.py @@ -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"] diff --git a/deploy/deploy_real/configs/ik.yaml b/deploy/deploy_real/configs/ik.yaml index 337d9a1..bedc969 100644 --- a/deploy/deploy_real/configs/ik.yaml +++ b/deploy/deploy_real/configs/ik.yaml @@ -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, diff --git a/deploy/deploy_real/deploy_real_ros_ikctrl.py b/deploy/deploy_real/deploy_real_ros_ikctrl.py index 54626a4..a1cb882 100644 --- a/deploy/deploy_real/deploy_real_ros_ikctrl.py +++ b/deploy/deploy_real/deploy_real_ros_ikctrl.py @@ -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] diff --git a/deploy/deploy_real/ikctrl.py b/deploy/deploy_real/ikctrl.py index 6bcc6c4..69c2c7f 100644 --- a/deploy/deploy_real/ikctrl.py +++ b/deploy/deploy_real/ikctrl.py @@ -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()