diff --git a/deploy/deploy_real/act_to_dof.py b/deploy/deploy_real/act_to_dof.py index 3e0a03d..0d3ecf7 100644 --- a/deploy/deploy_real/act_to_dof.py +++ b/deploy/deploy_real/act_to_dof.py @@ -7,12 +7,12 @@ from config import Config from ikctrl import IKCtrl class ActToDof: - def __init__(self, config): + def __init__(self, config, ikctrl): self.config=config - self.ikctrl = IKCtrl( - '../../resources/robots/g1_description/g1_29dof_with_hand_rev_1_0.urdf', - config.arm_joint - ) + self.ikctrl = ikctrl + self.lim_lo_pin = self.ikctrl.robot.model.lowerPositionLimit + self.lim_hi_pin = self.ikctrl.robot.model.upperPositionLimit + self.mot_from_pin = index_map( self.config.motor_joint, self.ikctrl.joint_names) @@ -24,6 +24,10 @@ class ActToDof: self.ikctrl.joint_names, self.config.lab_joint ) + self.pin_from_arm = index_map( + self.ikctrl.joint_names, + self.config.arm_joint + ) self.mot_from_arm = index_map( self.config.motor_joint, self.config.arm_joint @@ -32,12 +36,25 @@ class ActToDof: self.config.motor_joint, self.config.lab_joint ) + self.lab_from_mot = index_map( + self.config.lab_joint, + self.config.motor_joint + ) self.mot_from_nonarm = index_map( self.config.motor_joint, self.config.non_arm_joint ) + + self.lab_from_nonarm = index_map( + self.config.lab_joint, + self.config.non_arm_joint + ) + self.default_nonarm = ( + np.asarray(self.config.lab_joint_offsets)[self.lab_from_nonarm] + ) + def __call__(self, obs, action): - hands_command = obs[..., 119:225] + hands_command = obs[..., 119:125] non_arm_joint_pos = action[..., :22] left_arm_residual = action[..., 22:29] @@ -48,6 +65,9 @@ class ActToDof: q_mot = np.zeros(29) q_mot[self.mot_from_lab] = q_lab + # print('q0', q_mot[self.mot_from_arm]) + # q_mot[i_mot] = q_lab[ lab_from_mot[i_mot] ] + d_quat = quat_from_angle_axis( torch.from_numpy(hands_command[..., 3:]) ).detach().cpu().numpy() @@ -66,40 +86,112 @@ class ActToDof: q_pin, target ) - print('res_q_ik', res_q_ik) + + # q_pin2 = np.copy(q_pin) + # q_pin2[self.pin_from_arm] += res_q_ik + # print('target', target) + # se3=self.ikctrl.fk(q_pin2) + # print('fk(IK(target))', se3.translation, + # xyzw2wxyz(pin.Quaternion(se3.rotation).coeffs())) + + # print('res_q_ik', res_q_ik) + # print('left_arm_residual', + # 0.3 * left_arm_residual, + # np.clip(0.3 * left_arm_residual, -0.2, 0.2)) target_dof_pos = np.zeros(29) - for i_arm in range(len(res_q_ik)): - i_mot = self.mot_from_arm[i_arm] - i_pin = self.pin_from_mot[i_mot] - target_q = ( - q_mot[i_mot] - + res_q_ik[i_arm] - + np.clip(0.3 * left_arm_residual[i_arm], - -0.2, 0.2) - ) - target_q = np.clip(target_q, - self.lim_lo_pin[i_pin], - self.lim_hi_pin[i_pin]) - target_dof_pos[i_mot] = target_q + target_dof_pos += q_mot + target_dof_pos[self.mot_from_arm] += res_q_ik + target_dof_pos[self.mot_from_arm] += np.clip( + 0.3 * left_arm_residual, + -0.2, 0.2) + + # print('default joint pos', self.default_nonarm) + # print('joint order', self.config.non_arm_joint) + # print('mot_from_nonarm', self.mot_from_nonarm) + target_dof_pos[self.mot_from_nonarm] = ( + self.default_nonarm + 0.5 * non_arm_joint_pos + ) + + target_dof_pos[self.mot_from_arm] = np.clip( + target_dof_pos[self.mot_from_arm], + self.lim_lo_pin[self.pin_from_arm], + self.lim_hi_pin[self.pin_from_arm] + ) + return target_dof_pos def main(): + from matplotlib import pyplot as plt import yaml with open('configs/g1_eetrack.yaml', 'r') as fp: d = yaml.safe_load(fp) + config = Config('configs/g1_eetrack.yaml') + ikctrl = IKCtrl( + '../../resources/robots/g1_description/g1_29dof_with_hand_rev_1_0.urdf', + config.arm_joint + ) + act_to_dof = ActToDof(config, ikctrl) - act_to_dof = ActToDof(Config('configs/g1_eetrack.yaml')) - obs = np.load('/tmp/eet4/obs001.npy')[0] - act = np.load('/tmp/eet4/act001.npy')[0] - dof_lab = np.load('/tmp/eet4/dof001.npy')[0] - mot_from_lab = index_map(d['motor_joint'], d['lab_joint']) - target_dof_pos = np.zeros_like(dof_lab) - target_dof_pos[mot_from_lab] = dof_lab + exps = [] + cals = [] + poss = [] + + for i in range(70): + obs = np.load(F'/tmp/eet5/obs{i:03d}.npy')[0] + # print(obs.shape) + act = np.load(F'/tmp/eet5/act{i:03d}.npy')[0] + # print('act', act.shape) + dof_lab = np.load(F'/tmp/eet5/dof{i:03d}.npy')[0] + mot_from_lab = index_map(d['motor_joint'], d['lab_joint']) + target_dof_pos = np.zeros_like(dof_lab) + target_dof_pos[mot_from_lab] = dof_lab - dof = act_to_dof(obs, act) + dof = act_to_dof(obs, act) + + export = target_dof_pos + calc = dof + + mot_from_arm = index_map(d['motor_joint'], d['arm_joint']) + # print('exported', target_dof_pos[mot_from_arm], + # 'calculated', dof[mot_from_arm]) + # print( (export - calc)[mot_from_arm] ) + exps.append( target_dof_pos[mot_from_arm] ) + cals.append( dof[mot_from_arm] ) + + q_lab = obs[..., 32:61] + q_mot = q_lab[act_to_dof.lab_from_mot] + q_arm = q_mot[mot_from_arm] + poss.append(q_arm) + + exps=np.asarray(exps, dtype=np.float32) + cals=np.asarray(cals, dtype=np.float32) + poss=np.asarray(poss, dtype=np.float32) + print(exps.shape) + print(cals.shape) + + fig, ax=plt.subplots(7,1) + + q_lo =act_to_dof.lim_lo_pin[act_to_dof.pin_from_arm] + q_hi =act_to_dof.lim_hi_pin[act_to_dof.pin_from_arm] + RES = True + for i in range(7): + if RES: + ax[i].axhline(0) + else: + ax[i].axhline(q_lo[i], color='k', linestyle='--') + ax[i].axhline(q_hi[i], color='k', linestyle='--') + if RES: + ax[i].plot(exps[:, i]-poss[:,i], label='sim') + ax[i].plot(cals[:, i]-poss[:,i], label='real') + else: + ax[i].plot(poss[:, i], label='pos') + ax[i].plot(exps[:, i], label='sim') + ax[i].plot(cals[:, i], label='real') + ax[i].legend() + plt.show() if __name__ == '__main__': - main() \ No newline at end of file + main() diff --git a/deploy/deploy_real/deploy_real_ros_eetrack.py b/deploy/deploy_real/deploy_real_ros_eetrack.py index 6df7d72..01c18d2 100644 --- a/deploy/deploy_real/deploy_real_ros_eetrack.py +++ b/deploy/deploy_real/deploy_real_ros_eetrack.py @@ -24,6 +24,7 @@ from yourdfpy import URDF from math_utils import * import random as rd +from act_to_dof import ActToDof class Mode(Enum): @@ -499,6 +500,7 @@ class Controller: self.ikctrl = IKCtrl( '../../resources/robots/g1_description/g1_29dof_with_hand_rev_1_0.urdf', config.arm_joint) + self.actmap = ActToDof(config, self.ikctrl) self.lim_lo_pin = self.ikctrl.robot.model.lowerPositionLimit self.lim_hi_pin = self.ikctrl.robot.model.upperPositionLimit @@ -519,6 +521,11 @@ class Controller: self.config.motor_joint, self.config.non_arm_joint ) + self.lab_from_mot = index_map(self.config.lab_joint, + self.config.motor_joint) + self.config.default_angles = np.asarray(self.config.lab_joint_offsets)[ + self.lab_from_mot + ] # Data buffers self.obs = np.zeros(config.num_obs, dtype=np.float32) @@ -627,18 +634,25 @@ class Controller: self._dof_idx = dof_idx # record the current pos - self._init_dof_pos = np.zeros(self._dof_size, - dtype=np.float32) - for i in range(self._dof_size): - self._init_dof_pos[i] = self.low_state.motor_state[dof_idx[i]].q + # self._init_dof_pos = np.zeros(self._dof_size, + # dtype=np.float32) + # for i in range(self._dof_size): + # self._init_dof_pos[i] = self.low_state.motor_state[dof_idx[i]].q + self._init_dof_pos = np.zeros(29) + for i in range(29): + self._init_dof_pos[i] = self.low_state.motor_state[i].q def move_to_default_pos(self): # move to default pos if self.counter < self._num_step: alpha = self.counter / self._num_step - for j in range(self._dof_size): - motor_idx = self._dof_idx[j] - target_pos = self._default_pos[j] + #for j in range(self._dof_size): + for j in range(29): + # motor_idx = self._dof_idx[j] + # target_pos = self._default_pos[j] + motor_idx = j + target_pos = self.config.default_angles[j] + self.low_cmd.motor_cmd[motor_idx].q = ( self._init_dof_pos[j] * (1 - alpha) + target_pos * alpha) self.low_cmd.motor_cmd[motor_idx].dq = 0.0 @@ -682,79 +696,41 @@ class Controller: self.counter += 1 # TODO(ycho): consider using `cmd` for `hands_command` - # self.cmd[0] = self.remote_controller.ly - # self.cmd[1] = self.remote_controller.lx * -1 - # self.cmd[2] = self.remote_controller.rx * -1 + self.cmd[0] = self.remote_controller.ly + self.cmd[1] = self.remote_controller.lx * -1 + self.cmd[2] = self.remote_controller.rx * -1 # FIXME(ycho): implement `_hands_command_` # to use the output of `eetrack`. _hands_command_ = np.zeros(6) + _hands_command_[0] = self.cmd[0] * 0.03 + _hands_command_[2] = self.cmd[1] * 0.03 self.obs[:] = self.obsmap(self.low_state, self.action, _hands_command_) - Path('/tmp/eet/').mkdir(parents=True, - exist_ok=True) - np.save(F'/tmp/eet/obs{self.counter:03d}.npy', - self.obs) + # logpath = Path('/tmp/eet3/') + # logpath.mkdir(parents=True, exist_ok=True) + # np.save(F'{logpath}/obs{self.counter:03d}.npy', + # self.obs) # Get the action from the policy network obs_tensor = torch.from_numpy(self.obs).unsqueeze(0) self.action = self.policy(obs_tensor).detach().numpy().squeeze() - np.save(F'/tmp/eet/act{self.counter:03d}.npy', + # np.save(F'{logpath}/act{self.counter:03d}.npy', + # self.action) + + target_dof_pos = self.actmap(self.obs, self.action) - - non_arm_joint_pos = self.action[..., :22] - left_arm_residual = self.action[..., 22:29] - - q_pin = np.zeros_like(self.ikctrl.cfg.q) - for i_mot in range(len(self.config.motor_joint)): - i_pin = self.pin_from_mot[i_mot] - q_pin[i_pin] = self.low_state.motor_state[i_mot].q - - d_quat = quat_from_angle_axis( - torch.from_numpy(_hands_command_[..., 3:]) - ).detach().cpu().numpy() - - source_pose = self.ikctrl.fk(q_pin) - source_xyz = source_pose.translation - source_quat = xyzw2wxyz(pin.Quaternion(source_pose.rotation).coeffs()) - - target_xyz = source_xyz + _hands_command_[..., :3] - target_quat = quat_mul( - torch.from_numpy(d_quat), - torch.from_numpy(source_quat)).detach().cpu().numpy() - target = np.concatenate([target_xyz, target_quat]) - - res_q_ik = self.ikctrl( - q_pin, - target - ) - print('res_q_ik', res_q_ik) - - target_dof_pos = np.zeros(29) - for i_arm in range(len(res_q_ik)): - i_mot = self.mot_from_arm[i_arm] - i_pin = self.pin_from_mot[i_mot] - target_q = ( - self.low_state.motor_state[i_mot].q - + res_q_ik[i_arm] - + np.clip(0.3 * left_arm_residual[i_arm], - -0.2, 0.2) - ) - target_q = np.clip(target_q, - self.lim_lo_pin[i_pin], - self.lim_hi_pin[i_pin]) - target_dof_pos[i_mot] = target_q - np.save(F'/tmp/eet/dof{self.counter:03d}.npy', - target_dof_pos) + # np.save(F'{logpath}/dof{self.counter:03d}.npy', + # target_dof_pos) # Build low cmd for i in range(len(self.config.motor_joint)): self.low_cmd.motor_cmd[i].q = float(target_dof_pos[i]) self.low_cmd.motor_cmd[i].dq = 0.0 - self.low_cmd.motor_cmd[i].kp = 0.0 * float(self.config.kps[i]) - self.low_cmd.motor_cmd[i].kd = 0.0 * float(self.config.kds[i]) + self.low_cmd.motor_cmd[i].kp = 0.05 * float(self.config.kps[i]) + self.low_cmd.motor_cmd[i].kd = 0.05 * float(self.config.kds[i]) self.low_cmd.motor_cmd[i].tau = 0.0 # send the command