def callback_multi_torque_control(multi_servo_command): global num, id, initial_process_flag, target_torque, pre_target_torque, voltage # execute only single time if initial_process_flag == 1: num = set_the_num_of_servo() id = set_servo_id() for i in range(num): Kondo_B3M.resetServo(id[i]) Kondo_B3M.enFreeServo(id[i]) Kondo_B3M.reset_encoder_total_count(id[i]) # mode : 00>positionCTRL, 04>velocityCTRL, 08>current(torque)CTRL, 12>feedforwardCTRL Kondo_B3M.change_servocontrol_mode(id[i], 8) pre_target_torque.append(0) print("") rospy.logwarn( "you are controlling [" + str(num) + "] servos whose IDs is : " + str(id) + " with TORQUE CONTROL MODE. If you want to change the number of servos or their IDs, abort this code and try again after execute <$ rosparam set /num_of_servo THE_NUMBER_OF_SERVOS> and <$ rosparam set /multi_servo_id [YOUR_ID#1, YOUR_ID#2 etc]> or change them via launch file" ) initial_process_flag = 0 target_torque = multi_servo_command.target_torque target_torque = list(target_torque) for i in range(num): # damp target torque since drastic difference of target torque may cause lock of servo target_torque[i] = damp_target_torque(target_torque[i], pre_target_torque[i]) # print(str(target_torque)) Kondo_B3M.control_servo_by_Torque(id[i], target_torque[i]) pre_target_torque[i] = target_torque[i] publish_servo_info()
def torque_control(servo_command): global pre_target_torque, id, initial_process_flag target_torque = servo_command.target_torque # execute only single time if initial_process_flag == 1: id = set_servo_id() Kondo_B3M.resetServo(id) Kondo_B3M.enFreeServo(id) Kondo_B3M.reset_encoder_total_count(id) # mode : 00>positionCTRL, 04>velocityCTRL, 08>current(torque)CTRL, 12>feedforwardCTRL Kondo_B3M.change_servocontrol_mode(id, 8) print("") rospy.logwarn( "you are controlling servo ID : " + str(id) + ". If you want to change the ID, abort this code and try again after execute <$ rosparam set /servo_id YOUR_ID>, or change it via launch file." ) initial_process_flag = 0 # ramp target torque since drastic difference of target torque may cause lock of servo target_torque = ramp_target_torque(target_torque, pre_target_torque) Kondo_B3M.control_servo_by_Torque(id, target_torque) publish_servo_info() pre_target_torque = target_torque
def callback_servo_command(multi_servo_command): global num, target_torque, pre_target_torque, ramped_target_torque target_torque = multi_servo_command.target_torque target_torque = list(target_torque) # print(pre_target_torque) # print(target_torque) for i in range(num): # ramp target torque since drastic difference of target torque may cause lock of servo ramped_target_torque[i] = ramp_target_torque( target_torque[i], pre_target_torque[i]) Kondo_B3M.control_servo_by_Torque(id[i], ramped_target_torque[i]) # print(ramped_target_torque) # print("") pre_target_torque = ramped_target_torque publish_servo_info()