rf_cmd.append(d['rf_cmd'])
except EOFError:
break
for k, v in des.items():
des[k] = np.stack(v, axis=0)
for k, v in act.items():
act[k] = np.stack(v, axis=0)
rf_cmd = np.stack(rf_cmd, axis=0)
phase = np.stack(phase, axis=0)
## =============================================================================
## Plot Task
## =============================================================================
plot_task(time, des['com_pos'], act['com_pos'], des['com_vel'], act['com_vel'],
phase, 'com lin')
plot_task(time, des['torso_com_link_quat'], act['torso_com_link_quat'],
des['torso_com_link_ang_vel'], act['torso_com_link_ang_vel'], phase,
'torso ori')
plot_task(time, des['l_foot_contact_pos'], act['l_foot_contact_pos'],
des['l_foot_contact_vel'], act['l_foot_contact_vel'], phase,
'left foot lin')
plot_task(time, des['l_foot_contact_quat'], act['l_foot_contact_quat'],
des['l_foot_contact_ang_vel'], act['l_foot_contact_ang_vel'], phase,
'left foot ori')
plot_task(time, des['r_foot_contact_pos'], act['r_foot_contact_pos'],
des['r_foot_contact_vel'], act['r_foot_contact_vel'], phase,
phase = np.genfromtxt('experiment_data/phase.txt', delimiter='\n', dtype=int)
r_rf_cmd = np.genfromtxt('experiment_data/cmd_r_rf.txt',
delimiter=None,
dtype=float)
l_rf_cmd = np.genfromtxt('experiment_data/cmd_l_rf.txt',
delimiter=None,
dtype=float)
rf_cmd = np.concatenate((r_rf_cmd, l_rf_cmd), axis=1)
## Plot
plot_task(time, des['com_pos'], act['com_pos'], des['com_vel'], act['com_vel'],
phase, 'com lin')
plot_task(time, des['base_ori'], act['base_ori'], des['base_angvel'],
act['base_angvel'], phase, 'pelvis ori')
plot_task(time, des['rfoot_pos'], act['rfoot_pos'], des['rfoot_vel'],
act['rfoot_vel'], phase, 'right foot lin')
plot_task(time, des['lfoot_pos'], act['lfoot_pos'], des['lfoot_vel'],
act['lfoot_vel'], phase, 'left foot lin')
plot_task(time, des['rfoot_ori'], act['rfoot_ori'], des['rfoot_angvel'],
act['rfoot_angvel'], phase, 'right foot ori')
plot_task(time, des['lfoot_ori'], act['lfoot_ori'], des['lfoot_angvel'],
act['lfoot_angvel'], phase, 'left foot ori')
rf_cmd.append(d['rf_cmd'])
except EOFError:
break
for k, v in des.items():
des[k] = np.stack(v, axis=0)
for k, v in act.items():
act[k] = np.stack(v, axis=0)
rf_cmd = np.stack(rf_cmd, axis=0)
phase = np.stack(phase, axis=0)
## =============================================================================
## Plot Task
## =============================================================================
plot_task(time, des['com_pos'], act['com_pos'], des['com_vel'], act['com_vel'],
phase, 'com lin')
plot_task(time, des['pelvis_com_quat'], act['pelvis_com_quat'],
des['pelvis_com_ang_vel'], act['pelvis_com_ang_vel'], phase,
'pelvis ori')
plot_task(time, des['selected_joint_pos'], act['selected_joint_pos'],
des['selected_joint_vel'], act['selected_joint_vel'], phase,
'upperbody joint')
plot_task(time, des['l_sole_pos'], act['l_sole_pos'], des['l_sole_vel'],
act['l_sole_vel'], phase, 'left foot lin')
plot_task(time, des['l_sole_quat'], act['l_sole_quat'], des['l_sole_ang_vel'],
act['l_sole_ang_vel'], phase, 'left foot ori')
rf_cmd.append(d['rf_cmd'])
except EOFError:
break
for k, v in des.items():
des[k] = np.stack(v, axis=0)
for k, v in act.items():
act[k] = np.stack(v, axis=0)
rf_cmd = np.stack(rf_cmd, axis=0)
phase = np.stack(phase, axis=0)
## =============================================================================
## Plot Task
## =============================================================================
plot_task(time, des['com_pos'], act['com_pos'], des['com_vel'], act['com_vel'],
phase, 'com lin')
plot_task(time, des['chassis_quat'], act['chassis_quat'],
des['chassis_ang_vel'], act['chassis_ang_vel'], phase, 'pelvis ori')
plot_task(time, des['toeFL_pos'], act['toeFL_pos'], des['toeFL_vel'],
act['toeFL_vel'], phase, 'left foot lin')
plot_task(time, des['toeFR_pos'], act['toeFR_pos'], des['toeFR_vel'],
act['toeFR_vel'], phase, 'left foot ori')
plot_task(time, des['toeRR_pos'], act['toeRR_pos'], des['toeRR_vel'],
act['toeRR_vel'], phase, 'right foot lin')
plot_task(time, des['toeRL_pos'], act['toeRL_pos'], des['toeRL_vel'],
act['toeRL_vel'], phase, 'right foot ori')
local_act[k] = np.stack(v, axis=0)
# right foot first
cmd_rf = np.concatenate((cmd_rfoot_rf, cmd_lfoot_rf), axis=1)
phase = np.stack(phase, axis=0)
cmd_joint_positions = np.stack(cmd_joint_positions, axis=0)
cmd_joint_velocities = np.stack(cmd_joint_velocities, axis=0)
cmd_joint_torques = np.stack(cmd_joint_torques, axis=0)
joint_positions = np.stack(joint_positions, axis=0)
joint_velocities = np.stack(joint_velocities, axis=0)
## =============================================================================
## Plot Task
## =============================================================================
plot_task(time, des['task_com_pos'], act['task_com_pos'], des['task_com_vel'],
act['task_com_vel'], phase, 'com lin')
plot_task(time, local_des['task_com_pos'], local_act['task_com_pos'],
local_des['task_com_vel'], local_act['task_com_vel'], phase,
'com lin local')
plot_task(time, des['icp'], act['icp'], des['icp_dot'], act['icp_dot'], phase,
'icp')
# plot_momentum_task(time, des['task_cam_vel'], act['task_cam_vel'], phase,
# 'cam')
plot_task(time, des['task_torso_ori_pos'], act['task_torso_ori_pos'],
des['task_torso_ori_vel'], act['task_torso_ori_vel'], phase,
'torso ori')
break
for k, v in des.items():
des[k] = np.stack(v, axis=0)
for k, v in act.items():
act[k] = np.stack(v, axis=0)
for k, v in quat_err_list.items():
quat_err_list[k] = np.stack(v, axis=0)
rf_cmd = np.stack(rf_cmd, axis=0)
phase = np.stack(phase, axis=0)
## =============================================================================
## Plot Task
## =============================================================================
plot_task(time, des['com_pos'], act['com_pos'], des['com_vel'], act['com_vel'],
phase, 'com lin')
plot_task(time, des['torso_com_link_quat'], act['torso_com_link_quat'],
des['torso_com_link_ang_vel'], act['torso_com_link_ang_vel'], phase,
'torso ori')
# plot_task(time, des['selected_joint_pos'], act['selected_joint_pos'],
# des['selected_joint_vel'], act['selected_joint_vel'], phase,
# 'neck joint')
"""
plot_task(time, des['l_foot_contact_pos'], act['l_foot_contact_pos'],
des['l_foot_contact_vel'], act['l_foot_contact_vel'], phase,
'left foot lin')
plot_task(time, des['l_foot_contact_quat'], act['l_foot_contact_quat'],
des['l_foot_contact_ang_vel'], act['l_foot_contact_ang_vel'], phase,
ee_vel_act.append(d['ee_vel_act'])
jpos_des.append(d['jpos_des'])
jpos_act.append(d['jpos_act'])
jvel_des.append(d['jvel_des'])
jvel_act.append(d['jvel_act'])
qddot_des.append(d['qddot_des'])
ee_acc_des.append(d['ee_acc_des'])
except EOFError:
break
time = np.stack(time, axis=0)
ee_pos_des = np.stack(ee_pos_des, axis=0)
ee_pos_act = np.stack(ee_pos_act, axis=0)
ee_vel_des = np.stack(ee_vel_des, axis=0)
ee_vel_act = np.stack(ee_vel_act, axis=0)
jpos_des = np.stack(jpos_des, axis=0)
jpos_act = np.stack(jpos_act, axis=0)
jvel_des = np.stack(jvel_des, axis=0)
jvel_act = np.stack(jvel_act, axis=0)
qddot_des = np.stack(qddot_des, axis=0)
ee_acc_des = np.stack(ee_acc_des, axis=0)
plot_task(time, ee_pos_des, ee_pos_act, ee_vel_des, ee_vel_act,
[1] * ee_pos_des.shape[0], 'ee')
plot_task(time, jpos_des, jpos_act, jvel_des, jvel_act,
[1] * ee_pos_des.shape[0], 'joint')
plot_vector_traj(time, ee_acc_des, "ee ff acc")
plot_vector_traj(time, qddot_des, "qddot des")
plt.show()