def calc_next_input(self, predict_delta_state, world_buffer):
predict_size = np.shape(predict_delta_state)
HL_obs_buffer = np.empty((predict_size[0], predict_size[1] - 3))
for i in range(predict_size[0]):
com_ori = np.array([0, 0, world_buffer[i][0]])
delta_ori_com = predict_delta_state[i][0]
delta_pos_com = predict_delta_state[i][1:
3] # delta x,y in com frame
delta_vel_com = predict_delta_state[i][
3:5] # delta vx, vy, in com frame
com2world_ori = daisy_kinematics.CoM2World_ori(com_ori)
delta_pos_world = daisy_kinematics.coordinate_trans(
np.append(delta_pos_com, 0), com2world_ori)
delta_vel_world = daisy_kinematics.coordinate_trans(
np.append(delta_vel_com, 0), com2world_ori)
world_buffer[i][0] = world_buffer[i][0] + delta_ori_com
world_buffer[i][1:3] = world_buffer[i][1:3] + delta_pos_world[0:2]
world_buffer[i][3:5] = world_buffer[i][3:5] + delta_vel_world[0:2]
world2com_ori = daisy_kinematics.World2Com_ori(
np.array([0, 0, world_buffer[i][0]]))
HL_obs_buffer[i] = daisy_kinematics.coordinate_trans(
np.append(world_buffer[i][3:5], 0), world2com_ori)[0:2]
return HL_obs_buffer, world_buffer
def predict2world(self, pre_com_state, predict_delta_state):
delta_ori_com = predict_delta_state[0]
delta_pos_com = predict_delta_state[1:3] # delta x,y in com frame
delta_vel_com = predict_delta_state[3:5]
com2world_ori = daisy_kinematics.CoM2World_ori(
np.array([0, 0, pre_com_state['base_ori_euler'][2]]))
delta_pos_world = daisy_kinematics.coordinate_trans(
np.append(delta_pos_com, 0), com2world_ori)
delta_vel_world = daisy_kinematics.coordinate_trans(
np.append(delta_vel_com, 0), com2world_ori)
state_world = []
state_world.append(delta_ori_com + pre_com_state['base_ori_euler'][2])
state_world.append(delta_pos_world[0] + pre_com_state['base_pos_x'][0])
state_world.append(delta_pos_world[1] + pre_com_state['base_pos_y'][0])
state_world.append(delta_vel_world[0] +
pre_com_state['base_velocity'][0])
state_world.append(delta_vel_world[1] +
pre_com_state['base_velocity'][1])
return state_world
def HL_delta_obs(self, pre_com_state, post_com_state):
'''
Extract useful information from state(dict) and form to np.array()
input:
state: dict
output:
HL_obs: np.array
'''
high_level_delta_obs = []
delta_yaw = post_com_state['base_ori_euler'][2] - pre_com_state[
'base_ori_euler'][2]
if delta_yaw > np.pi:
delta_yaw = 2 * np.pi - delta_yaw
elif delta_yaw < -np.pi:
delta_yaw = -2 * np.pi - delta_yaw
# print(delta_yaw)
world2com_ori = daisy_kinematics.World2Com_ori(
pre_com_state['base_ori_euler'])
delta_pos = []
delta_pos.append(post_com_state['base_pos_x'][0] -
pre_com_state['base_pos_x'][0])
delta_pos.append(post_com_state['base_pos_y'][0] -
pre_com_state['base_pos_y'][0])
delta_pos.append(0)
delta_pos_com = daisy_kinematics.coordinate_trans(
np.array(delta_pos), world2com_ori)
delta_vel_com = daisy_kinematics.coordinate_trans(
post_com_state['base_velocity'] - pre_com_state['base_velocity'],
world2com_ori)
high_level_delta_obs.append(delta_yaw)
for i in range(2):
high_level_delta_obs.append(delta_pos_com[i])
for i in range(2):
high_level_delta_obs.append(delta_vel_com[i])
return np.array(high_level_delta_obs)
def HL_obs(self, state, target):
high_level_obs = []
yaw_term = target[2] - state['base_ori_euler'][2]
world2com_ori = daisy_kinematics.World2Com_ori(state['base_ori_euler'])
pos = np.array([
target[0] - state['base_pos_x'][0],
target[1] - state['base_pos_y'][0], 0
])
pos2com = daisy_kinematics.coordinate_trans(
pos, world2com_ori) # position term
vel2com = daisy_kinematics.coordinate_trans(
state['base_velocity'], world2com_ori) # velocity term
high_level_obs.append(yaw_term)
for i in range(2):
high_level_obs.append(pos2com[i])
for i in range(2):
high_level_obs.append(vel2com[i])
return np.array(high_level_obs)
def HL_obs(self, state):
'''
Extract useful information from state(dict) and form to np.array()
input:
state: dict
output:
HL_obs: np.array
now the obs includes: com velocity in xyz, yaw information
'''
# TODO: form the HL_obs & translate com velocity to com frame
high_level_obs = []
world2com_ori = daisy_kinematics.World2Com_ori(state['base_ori_euler'])
vel2com = daisy_kinematics.coordinate_trans(state['base_velocity'],
world2com_ori)
return vel2com[0:2]