def get_batch_tensor(batch_num):
x = []
target = []
# tmp1_list = []
# tmp2_list = []
for i in range(batch_num):
act = get_random_joint_angle()
next_state, reward, done, _ = env.step(act)
# env.interface.GAZEBO_Step(1)
# time.sleep(0.1)
# input('stop')
# continue
tcp_pq = pq_dict_to_list(
env.interface.GAZEBO_GetLinkPQByName("vs060", "J6"))
q = [tcp_pq[3], tcp_pq[4], tcp_pq[5], tcp_pq[6]]
euler = quat2euler(q) #W,X,Y,Z
tcp_pq_euler = [
tcp_pq[0] / 1.8, tcp_pq[1] / 1.8, tcp_pq[2] / 1.8, tcp_pq[3],
tcp_pq[4], tcp_pq[5], tcp_pq[6], euler[0] / np.pi,
euler[1] / np.pi, euler[2] / np.pi
]
x.append(tcp_pq_euler)
# act = [act[0]/1.57,act[1]/2.1,act[2]/2.1,act[3]/1.57,act[4]/1.57,act[5]/1.57] ##Scalar
target.append(act)
# print(np.max(x),np.min(x))
# input("stop")
x_tensor = torch.from_numpy(np.array(x)).double().to(device)
target_tensor = torch.from_numpy(np.array(target)).double().to(device)
# print("time:",time.clock()-start_t)
return x_tensor, target_tensor
def get_batch_tensor(batch_num):
x = []
target = []
# tmp1_list = []
# tmp2_list = []
for i in range(batch_num):
act = get_random_joint_angle()
next_state, reward, done, _ = env.step(act)
tcp_pq = pq_dict_to_list(
env.interface.GAZEBO_GetLinkPQByName("vs060", "J6"))
# print(tcp_pq)
q = [tcp_pq[3], tcp_pq[4], tcp_pq[5], tcp_pq[6]]
euler = quat2euler(q) #W,X,Y,Z
tcp_euler = [
tcp_pq[0] / 1.8, tcp_pq[1] / 1.8, tcp_pq[2] / 1.8,
euler[0] / np.pi, euler[1] / np.pi, euler[2] / np.pi
]
# tmp1 = [tcp_pq[0],tcp_pq[1],tcp_pq[2]]
# tmp2 = [euler[0],euler[1],euler[2]]
# print("euler:",tmp_euler) #yaw(z), pitch(y), roll(x)
# tmp_qu = euler2quat(tmp_euler[0],tmp_euler[1],tmp_euler[2]) #yaw(z), pitch(y), roll(x)
# print("qu:",tmp_qu) #W,X,Y,Z
# print(tcp_euler)
# input("stop")
x.append(tcp_euler)
# tmp1_list.append(tmp1)
# tmp2_list.append(tmp2)
act = [
act[0] / 1.57, act[1] / 2.1, act[2] / 2.1, act[3] / 1.57,
act[4] / 1.57, act[5] / 1.57
]
target.append(act)
# env.interface.GAZEBO_Step(1)
# time.sleep(.01)
# print(np.shape(x),np.shape(target))
# print(np.max(x),np.min(x))
# print(np.max(tmp1_list),np.min(tmp1_list))
# print(np.max(tmp2_list),np.min(tmp2_list))
# print(np.max(target),np.min(target))
# input('stop')
x_tensor = torch.from_numpy(np.array(x)).float().to(device)
target_tensor = torch.from_numpy(np.array(target)).float().to(device)
# print("time:",time.clock()-start_t)
return x_tensor, target_tensor
def test():
resume_model = "IK_model_4layers0.009619119577109814"
model = load_model(resume_model)
model.cuda()
model.eval()
# env = GAZEBO("16004")
criterion = torch.nn.MSELoss()
first_act = get_random_joint_angle()
next_state, reward, done, _ = env.step(first_act)
env.interface.GAZEBO_Step(1)
tcp_pq_list = []
act_list = []
act = first_act.copy()
with torch.no_grad():
print("collect...")
for i in range(100):
# act = get_random_joint_angle()
next_state, reward, done, _ = env.step(act)
env.interface.GAZEBO_Step(1)
tcp_pq = pq_dict_to_list(
env.interface.GAZEBO_GetLinkPQByName("vs060", "J6"))
tcp_pq_list.append(tcp_pq)
act_list.append(act)
act[0] -= 0.02
for j in range(len(pos_limit)):
act[j] = np.clip(act[j], pos_limit[j][0], pos_limit[j][1])
print(i)
input()
next_state, reward, done, _ = env.step(first_act)
env.interface.GAZEBO_Step(1)
print(np.shape(tcp_pq_list), np.shape(act_list))
print("predict...")
for i in range(len(tcp_pq_list)):
tcp_pq = tcp_pq_list[i]
q = [tcp_pq[3], tcp_pq[4], tcp_pq[5], tcp_pq[6]]
euler = quat2euler(q) #W,X,Y,Z
tcp_pq_euler = [
tcp_pq[0] / 1.8, tcp_pq[1] / 1.8, tcp_pq[2] / 1.8, tcp_pq[3],
tcp_pq[4], tcp_pq[5], tcp_pq[6], euler[0] / np.pi,
euler[1] / np.pi, euler[2] / np.pi
]
act = act_list[i]
act = [
act[0] / 1.57, act[1] / 2.1, act[2] / 2.1, act[3] / 1.57,
act[4] / 1.57, act[5] / 1.57
]
for i in range(len(pos_limit)):
act = np.clip(act, pos_limit[i][0], pos_limit[i][1])
input_tensor = torch.from_numpy(
np.array(tcp_pq_euler)).float().to(device).unsqueeze(0)
target_tensor = torch.from_numpy(
np.array(act)).float().to(device).unsqueeze(0)
# print("input shape:",input_tensor.size())
# print("target shape:",target_tensor.size())
predict_act = model(input_tensor).detach()
loss = criterion(predict_act, target_tensor)
print("loss:", loss.item())
# print("before multiply:",predict_act)
scalar = torch.tensor([1.57, 2.1, 2.1, 1.57, 1.57,
1.57]).to(device)
predict_act *= scalar
# print("after multiply:",predict_act)
predict_act = predict_act.cpu().numpy().reshape(-1)
print("tcp:", tcp_pq)
print("target_joint:", act_list[i])
print("predict_joint:", predict_act)
next_state, reward, done, _ = env.step(predict_act)
env.interface.GAZEBO_Step(1)
input("stop")