real_left_task_space_traj_dots[ii, :] = J_left.dot(joint_vel_state)
real_right_task_space_traj_dots[ii, :] = J_right.dot(joint_vel_state)
# Calculate pose and velocities errors
task_left_pose_error = compute_cartesian_error(left_task_space_traj[ii, :], real_left_task_space_traj[ii, :])
task_right_pose_error = compute_cartesian_error(right_task_space_traj[ii, :], real_right_task_space_traj[ii, :])
task_left_vel_error = left_task_space_traj_dots[ii, :] - real_left_task_space_traj_dots[ii, :]
task_right_vel_error = right_task_space_traj_dots[ii, :] - real_right_task_space_traj_dots[ii, :]
# Reference task-space acceleration(s)
x_left_ddot_r = left_task_space_traj_ddots[ii, :] + Kp_task.dot(task_left_pose_error) + Kd_task.dot(task_left_vel_error)
x_right_ddot_r = right_task_space_traj_ddots[ii, :] + Kp_task.dot(task_right_pose_error) + Kd_task.dot(task_right_vel_error)
# Update Mass matrix
# robot_model.update_inertia_matrix(M, joint_traj[ii, :])
robot_model.update_inertia_matrix(M_left, current_joint_pos)
robot_model.update_inertia_matrix(M_right, current_joint_pos)
#M_left[bigman_params['joint_ids']['LB'], bigman_params['joint_ids']['LB']] = 0
#M_left[bigman_params['joint_ids']['TO'], bigman_params['joint_ids']['TO']] = 0
#M_left[bigman_params['joint_ids']['RA'], bigman_params['joint_ids']['RA']] = 0
#M_right[bigman_params['joint_ids']['LB'], bigman_params['joint_ids']['LB']] = 0
#M_right[bigman_params['joint_ids']['TO'], bigman_params['joint_ids']['TO']] = 0
#M_right[bigman_params['joint_ids']['LA'], bigman_params['joint_ids']['LA']] = 0
# #F = M_left_bar.dot(x_left_ddot_r)
# rbdl.CompositeRigidBodyAlgorithm(robot_rbdl_model, current_joint_pos, M, update_kinematics=True)
# M_left_bar[:, :] = np.linalg.inv(J_left.dot(np.linalg.inv(M)).dot(J_left.T))
# rbdl.NonlinearEffects(robot_rbdl_model, current_joint_pos, current_joint_vel, c_plus_g)
# tau_left = J_left.T.dot(M_left_bar).dot(x_left_ddot_r) + c_plus_g
# # Null space
# #u_null = Kp_null.dot(joint_traj[ii, :] - current_joint_pos)
des_cmd.damping = np.zeros_like(tau[joints_to_move])
else:
#des_cmd.position = joint_init_traj[ii, joints_to_move]
#des_cmd.stiffness = default_joint_stiffness[joints_to_move]
#des_cmd.damping = default_joint_damping[joints_to_move]
#taus_init_traj[ii, :] = joint_effort_state
#print(joint_init_traj[ii, joints_to_move] - joint_pos_state[joints_to_move])
#robot_model.update_torque(tau, joint_init_traj[ii, :], joint_init_traj_dots[ii, :]*freq,
# joint_init_traj_ddots[ii, :]*freq*freq)
#robot_model.update_coriolis_forces(tau, joint_pos_state, joint_vel_state)
#robot_model.update_coriolis_forces(tau, joint_pos_state, joint_vel_state*0)
#a = joint_init_traj_ddots[ii, :] + \
a = default_joint_damping * (joint_init_traj_dots[ii, :] - joint_vel_state) + \
default_joint_stiffness * (joint_init_traj[ii, :] - joint_pos_state)
robot_model.update_inertia_matrix(M, joint_pos_state)
robot_model.update_torque(tau, joint_pos_state, joint_vel_state,
joint_init_traj_ddots[ii, :])
tau += M.dot(a)
des_cmd.effort = tau[joints_to_move]
des_cmd.stiffness = np.zeros_like(tau[joints_to_move])
des_cmd.damping = np.zeros_like(tau[joints_to_move])
publisher.publish(des_cmd)
taus_init_traj[ii, :] = joint_effort_state
taus_cmd_init_traj[ii, :] = tau
pub_rate.sleep()
joints_to_plot = bigman_params['joint_ids']['LA']
cols = 3
joint_names = [bigman_params['joints_names'][idx] for idx in joints_to_plot]
plot_desired_sensed_torque_position(joints_to_plot,
taus_cmd_init_traj,