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')
plot_task(time, des['sj_pos'], act['sj_pos'], des['sj_vel'], act['sj_vel'],
phase, 'upper body joint')
plot_rf(time, rf_cmd, phase)
plt.show()
plot_task(time, local_des['task_lfoot_ori_pos'],
local_act['task_lfoot_ori_pos'], local_des['task_lfoot_ori_vel'],
local_act['task_lfoot_ori_vel'], phase, 'left foot ori local')
plot_task(time, local_des['task_rfoot_lin_pos'],
local_act['task_rfoot_lin_pos'], local_des['task_rfoot_lin_vel'],
local_act['task_rfoot_lin_vel'], phase, 'right foot lin local')
plot_task(time, local_des['task_rfoot_ori_pos'],
local_act['task_rfoot_ori_pos'], local_des['task_rfoot_ori_vel'],
local_act['task_rfoot_ori_vel'], phase, 'right foot ori local')
## =============================================================================
## Plot WBC Solutions
## =============================================================================
plot_rf(time, cmd_rf, phase) # assume top six is right foot
plot_joints(joint_label, rfoot_label, time, cmd_joint_positions,
joint_positions, cmd_joint_velocities, joint_velocities,
cmd_joint_torques, phase, "rfoot")
plot_joints(joint_label, lfoot_label, time, cmd_joint_positions,
joint_positions, cmd_joint_velocities, joint_velocities,
cmd_joint_torques, phase, "lfoot")
# l_knee_jd_idx = joint_label.index("l_knee_fe_jd")
# r_knee_jd_idx = joint_label.index("r_knee_fe_jd")
# fig, axes = plt.subplots(2, 1)
# axes[0].set_title('left knee')
# axes[1].set_title('right knee')