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()