elif control_mode.lower() == 'torque':
#rbdl.InverseDynamics(robot_model.model, qs[ii, :], qdots[ii, :], qddots[ii, :], tau)
#taus_traj[ii, :] = joint_effort_state
#print(joint_traj[ii, joints_to_move] - joint_pos_state[joints_to_move])
#rbdl.NonlinearEffects(robot_model, joint_pos_state, joint_vel_state, g)
#rbdl.NonlinearEffects(robot_model, joint_pos_state, joint_vel_state*0, g)
#a = joint_traj_ddots[ii, :] + \
# default_joint_damping*0 * (joint_traj_dots[ii, :] - joint_vel_state) + \
# default_joint_stiffness*0.0 * (joint_traj[ii, :] - joint_pos_state)
# Computed Torque Control
a = qddots[ii, :] + \
Kd_tau * (qdots[ii, :] - joint_vel_state) + \
Kp_tau * (qs[ii, :] - joint_pos_state)
robot_model.update_torque(tau, joint_pos_state, joint_vel_state, a)
## FeedForward + PD compensation
#robot_model.update_torque(tau, qs[ii, :], qdots[ii, :], qddots[ii, :])
#pd_tau = Kp_tau * (qs[ii, :] - joint_pos_state) + \
# Kd_tau * (qdots[ii, :] - joint_vel_state)
#tau += pd_tau
#pd_tau = default_joint_stiffness * (qs[ii, :] - joint_pos_state) + \
# default_joint_damping * (qdots[ii, :] - joint_vel_state)
#rbdl.InverseDynamics(robot_model, joint_pos_state, joint_vel_state, a, tau)
#rbdl.NonlinearEffects(robot_model, qs[ii, :], joint_vel_state*0, tau)
#tau = np.ones(robot_model.qdot_size)*-0.5
#a = default_joint_damping * (joint_traj_dots[ii, :] - joint_vel_state)
#rbdl.CompositeRigidBodyAlgorithm(robot_model, joint_pos_state, M, update_kinematics=True)
#rbdl.InverseDynamics(robot_model, joint_pos_state, joint_vel_state/freq, qddots[ii, :]/(freq*freq), tau)
#print("Setting impedance to zero...")
#Timp = 0.5
#for ii in range(int(Timp*freq)):
# #des_cmd.position = joint_init_traj[ii, :]
# des_cmd.stiffness = np.zeros_like(qdot)
# des_cmd.damping = np.zeros_like(qdot)
# publisher.publish(des_cmd)
# pub_rate.sleep()
#raw_input("Press key for reproducing trajectory")
print("Reproducing trajectory (ONLY ARMS)...")
joints_to_move = centauro_params['joint_ids']['BA'][5:6]
joints_to_move = [centauro_params['joint_ids']['BA'][6]]
des_cmd.name = [centauro_params['joints_names'][idx] for idx in joints_to_move]
#robot_model.update_torque(tau, joint_pos_state, joint_vel_state, qddot)
robot_model.update_torque(tau, joint_pos_state, qdot, qddot)
print(tau[joints_to_move])
print(joint_effort_state[joints_to_move])
des_cmd.position = []
des_cmd.effort = tau[joints_to_move]
#des_cmd.effort = joint_effort_state[centauro_params['joint_ids']['BA'][:4]]
des_cmd.stiffness = np.zeros_like(default_joint_stiffness[joints_to_move])
des_cmd.damping = np.zeros_like(default_joint_damping[joints_to_move])
qs2 = np.tile(joint_pos_state[:], (T_impedance_zero*freq, 1))
taus2 = np.tile(tau[:], (T_impedance_zero*freq, 1))
sensed_qs2 = np.zeros((T_impedance_zero*freq, robot_model.q_size))
sensed_taus2 = np.zeros((T_impedance_zero*freq, robot_model.qdot_size))
temp_tau = np.zeros((T_impedance_zero*freq, robot_model.q_size))
temp_stiff = np.zeros((T_impedance_zero*freq, robot_model.q_size))
temp_damp = np.zeros((T_impedance_zero*freq, robot_model.q_size))
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,
taus_init_traj,
joint_init_traj,