working...integration...??
This commit is contained in:
yjinzero
parent
016fd440f1
commit
a7bdde6ad2
|
|
@ -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()
|
||||
main()
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in New Issue