def example_script(name_interface):
########################################################################
# Parameters definition & Initialization #
########################################################################
# Simulation parameters
N_SIMULATION = 30000 # number of time steps simulated
t = 0.0 # time
# Initialize the error for the simulation time
time_error = False
N_SLAVES = 6 # Maximum number of controled drivers
N_SLAVES_CONTROLED = 2 # Current number of controled drivers
iq_sat = 1.0 # Maximum amperage (A)
init_pos = [0.0 for i in range(N_SLAVES * 2)] # List that will store the initial position of motors
state = 0 # State of the system (ready (1) or not (0))
robot_if = mbs.MasterBoardInterface(name_interface) # Robot interface
robot_if.Init() # Initialization of the interface between the computer and the master board
for i in range(N_SLAVES_CONTROLED): # We enable each controler driver and its two associated motors
robot_if.GetDriver(i).motor1.SetCurrentReference(0)
robot_if.GetDriver(i).motor2.SetCurrentReference(0)
robot_if.GetDriver(i).motor1.Enable()
robot_if.GetDriver(i).motor2.Enable()
robot_if.GetDriver(i).EnablePositionRolloverError()
robot_if.GetDriver(i).SetTimeout(5)
robot_if.GetDriver(i).Enable()
qmes8 = np.zeros((8,1))
vmes8 = np.zeros((8,1))
K = 1.0/(9*0.02) # proportionnal gain to convert joint torques to current : tau_mot = K'*I ; tau_joints = Kred*K'*I ; K' = 0.02 ; Kred = 9
def listener_script(name_interface):
cpt = 0 # Iteration counter
dt = 0.001 # Time step
t = 0 # Current time
print("-- Start of listener script --")
os.nice(-20) # Set the process to highest priority (from -20 highest to +20 lowest)
robot_if = mbs.MasterBoardInterface(name_interface, True)
robot_if.Init() # Initialization of the interface between the computer and the master board
last = clock()
while (True):
if ((clock() - last) > dt):
last = clock()
cpt += 1
t += dt
robot_if.ParseSensorData() # Read sensor data sent by the masterboard
if (cpt % 10000 == 0): # Reset stats every 10s
robot_if.ResetPacketLossStats();
if ((cpt % 100) == 0): # Display state of the system once every 100 iterations of the main loop
print(chr(27) + "[2J")
# To read IMU data in Python use robot_if.imu_data_accelerometer(i), robot_if.imu_data_gyroscope(i)
# or robot_if.imu_data_attitude(i) with i = 0, 1 or 2
print("Session ID : {}".format(robot_if.GetSessionId()))
robot_if.PrintIMU()
robot_if.PrintADC()
robot_if.PrintMotors()
robot_if.PrintMotorDrivers()
robot_if.PrintStats()
sys.stdout.flush() # for Python 2, use print( .... , flush=True) for Python 3
print("-- End of example script --")
def __init__(self, plotter):
#Set instance specific variables
self.plotter = plotter
self.robot_if = mbs.MasterBoardInterface("enp4s0")
self.robot_if.Init()
self.motors_spi_connected_indexes = []
self.motor_positions = np.array([0.0 for i in range(Config.N_SLAVES * 2)])
self.motor_velocities = np.array([0.0 for i in range(Config.N_SLAVES * 2)])
#Set initiation specific variables
ready = False
#Initating the master board comunication
os.nice(-20)
for i in range(Config.N_SLAVES_CONTROLLED):
self.robot_if.GetDriver(i).motor1.SetCurrentReference(0)
self.robot_if.GetDriver(i).motor2.SetCurrentReference(0)
self.robot_if.GetDriver(i).motor1.Enable()
self.robot_if.GetDriver(i).motor2.Enable()
self.robot_if.GetDriver(i).EnablePositionRolloverError()
self.robot_if.GetDriver(i).SetTimeout(0)
self.robot_if.GetDriver(i).Enable()
last = perf_counter()
while (not self.robot_if.IsTimeout() and not self.robot_if.IsAckMsgReceived()):
if ((perf_counter() - last) > Config.dt):
last = perf_counter()
self.robot_if.SendInit()
if self.robot_if.IsTimeout():
print("Timeout while waiting for ack.")
else:
for i in range(Config.N_SLAVES_CONTROLLED):
if self.robot_if.GetDriver(i).IsConnected():
self.motors_spi_connected_indexes.append(2 * i)
self.motors_spi_connected_indexes.append(2 * i + 1)
while not ready:
if ((perf_counter() - last) > Config.dt):
last = perf_counter()
self.robot_if.ParseSensorData()
ready = True
for i in self.motors_spi_connected_indexes:
if not (self.robot_if.GetMotor(i).IsEnabled() and self.robot_if.GetMotor(i).IsReady()):
ready = False
self.motor_positions[i] = self.robot_if.GetMotor(i).GetPosition()
self.robot_if.SendCommand()
self.pos_ref = self.motor_positions
self.vel_ref = self.motor_velocities
def example_script(name_interface):
N_SLAVES = 6 # Maximum number of controled drivers
N_SLAVES_CONTROLED = 1 # Current number of controled drivers
cpt = 0 # Iteration counter
dt = 0.001 # Time step
t = 0 # Current time
kp = 5.0 # Proportional gain
kd = 0.1 # Derivative gain
iq_sat = 1.0 # Maximum amperage (A)
constant_velocity = 21 * math.pi # About 200 rpm
init_pos = [0.0 for i in range(N_SLAVES * 2)
] # List that will store the initial position of motors
state = 0 # State of the system (ready (1) or not (0))
p_arr = []
v_arr = []
p_ref_arr = []
v_ref_arr = []
p_err_arr = []
v_err_arr = []
t_arr = []
# contrary to c++, in python it is interesting to build arrays
# with connected motors indexes so we can simply go through them in main loop
motors_spi_connected_indexes = [
] # indexes of the motors on each connected slaves
print("-- Start of example script --")
os.nice(
-20
) # Set the process to highest priority (from -20 highest to +20 lowest)
robot_if = mbs.MasterBoardInterface(name_interface)
robot_if.Init(
) # Initialization of the interface between the computer and the master board
for i in range(
N_SLAVES_CONTROLED
): # We enable each controler driver and its two associated motors
robot_if.GetDriver(i).motor1.SetCurrentReference(0)
robot_if.GetDriver(i).motor2.SetCurrentReference(0)
robot_if.GetDriver(i).motor1.Enable()
robot_if.GetDriver(i).motor2.Enable()
robot_if.GetDriver(i).EnablePositionRolloverError()
robot_if.GetDriver(i).SetTimeout(5)
robot_if.GetDriver(i).Enable()
last = clock()
while (not robot_if.IsTimeout() and not robot_if.IsAckMsgReceived()):
if ((clock() - last) > dt):
last = clock()
robot_if.SendInit()
if robot_if.IsTimeout():
print("Timeout while waiting for ack.")
else:
# fill the connected motors indexes array
for i in range(N_SLAVES_CONTROLED):
if robot_if.GetDriver(i).IsConnected():
# if slave i is connected then motors 2i and 2i+1 are potentially connected
motors_spi_connected_indexes.append(2 * i)
motors_spi_connected_indexes.append(2 * i + 1)
while (
(not robot_if.IsTimeout()) and (clock() < 15)
): # Stop after 15 seconds (around 5 seconds are used at the start for calibration)
if ((clock() - last) > dt):
last = clock()
cpt += 1
t += dt
robot_if.ParseSensorData(
) # Read sensor data sent by the masterboard
if (state == 0): # If the system is not ready
state = 1
# for all motors on a connected slave
for i in motors_spi_connected_indexes: # Check if all motors are enabled and ready
if not (robot_if.GetMotor(i).IsEnabled()
and robot_if.GetMotor(i).IsReady()):
state = 0
init_pos[i] = robot_if.GetMotor(i).GetPosition()
t = 0
else: # If the system is ready
# for all motors on a connected slave
for i in motors_spi_connected_indexes:
if i % 2 == 0 and robot_if.GetDriver(
i // 2).GetErrorCode() == 0xf:
#print("Transaction with SPI{} failed".format(i // 2))
continue #user should decide what to do in that case, here we ignore that motor
if robot_if.GetMotor(
i).IsEnabled() and i == 0: #Just running 1 motor
ref = constant_velocity * t
v_ref = constant_velocity * math.pi
p_err = ref - robot_if.GetMotor(
i).GetPosition() # Position error
v_err = v_ref - robot_if.GetMotor(
i).GetVelocity() # Velocity error
cur = kp * p_err + kd * v_err # Output of the PD controler (amperage)
if (cur > iq_sat): # Check saturation
cur = iq_sat
if (cur < -iq_sat):
cur = -iq_sat
position = robot_if.GetMotor(
i).GetPosition() - init_pos[i]
velocity = robot_if.GetMotor(i).GetVelocity()
p_arr.append(position)
v_arr.append(velocity)
p_ref_arr.append(ref - init_pos[i])
v_ref_arr.append(v_ref)
p_err_arr.append(p_err)
v_err_arr.append(v_err)
t_arr.append(t)
robot_if.GetMotor(i).SetCurrentReference(
cur) # Set reference currents
robot_if.SendCommand(
) # Send the reference currents to the master board
overlay_plot(p_arr, v_arr, p_ref_arr, v_ref_arr, p_err_arr, v_err_arr,
t_arr) #Plot position and velocity
robot_if.Stop(
) # Shut down the interface between the computer and the master board
if robot_if.IsTimeout():
print("Masterboard timeout detected.")
print(
"Either the masterboard has been shut down or there has been a connection issue with the cable/wifi."
)
print("-- End of example script --")
def example_script(name_interface):
N_SLAVES = 6 # Maximum number of controled drivers
N_SLAVES_CONTROLED = 6 # Current number of controled drivers
cpt = 0 # Iteration counter
dt = 0.001 # Time step
t = 0 # Current time
kp = 5.0 # Proportional gain
kd = 0.1 # Derivative gain
iq_sat = 1.0 # Maximum amperage (A)
freq = 0.5 # Frequency of the sine wave
amplitude = math.pi # Amplitude of the sine wave
init_pos = [0.0 for i in range(N_SLAVES * 2)
] # List that will store the initial position of motors
state = 0 # State of the system (ready (1) or not (0))
# contrary to c++, in python it is interesting to build arrays
# with connected motors indexes so we can simply go through them in main loop
motors_spi_connected_indexes = [
] # indexes of the motors on each connected slaves
print("-- Start of example script --")
os.nice(
-20
) # Set the process to highest priority (from -20 highest to +20 lowest)
robot_if = mbs.MasterBoardInterface(name_interface)
robot_if.Init(
) # Initialization of the interface between the computer and the master board
for i in range(
N_SLAVES_CONTROLED
): # We enable each controler driver and its two associated motors
robot_if.GetDriver(i).motor1.SetCurrentReference(0)
robot_if.GetDriver(i).motor2.SetCurrentReference(0)
robot_if.GetDriver(i).motor1.Enable()
robot_if.GetDriver(i).motor2.Enable()
robot_if.GetDriver(i).EnablePositionRolloverError()
robot_if.GetDriver(i).SetTimeout(5)
robot_if.GetDriver(i).Enable()
last = clock()
while (not robot_if.IsTimeout() and not robot_if.IsAckMsgReceived()):
if ((clock() - last) > dt):
last = clock()
robot_if.SendInit()
if robot_if.IsTimeout():
print("Timeout while waiting for ack.")
else:
# fill the connected motors indexes array
for i in range(N_SLAVES_CONTROLED):
if robot_if.GetDriver(i).IsConnected():
# if slave i is connected then motors 2i and 2i+1 are potentially connected
motors_spi_connected_indexes.append(2 * i)
motors_spi_connected_indexes.append(2 * i + 1)
while (
(not robot_if.IsTimeout()) and (clock() < 20)
): # Stop after 15 seconds (around 5 seconds are used at the start for calibration)
if ((clock() - last) > dt):
last = clock()
cpt += 1
t += dt
robot_if.ParseSensorData(
) # Read sensor data sent by the masterboard
if (state == 0): # If the system is not ready
state = 1
# for all motors on a connected slave
for i in motors_spi_connected_indexes: # Check if all motors are enabled and ready
if not (robot_if.GetMotor(i).IsEnabled()
and robot_if.GetMotor(i).IsReady()):
state = 0
init_pos[i] = robot_if.GetMotor(i).GetPosition()
t = 0
else: # If the system is ready
# for all motors on a connected slave
for i in motors_spi_connected_indexes:
if i % 2 == 0 and robot_if.GetDriver(
i // 2).GetErrorCode() == 0xf:
#print("Transaction with SPI{} failed".format(i // 2))
continue #user should decide what to do in that case, here we ignore that motor
if robot_if.GetMotor(i).IsEnabled():
ref = init_pos[i] + amplitude * math.sin(
2.0 * math.pi * freq * t) # Sine wave pattern
v_ref = 2.0 * math.pi * freq * amplitude * math.cos(
2.0 * math.pi * freq * t)
p_err = ref - robot_if.GetMotor(
i).GetPosition() # Position error
v_err = v_ref - robot_if.GetMotor(
i).GetVelocity() # Velocity error
cur = kp * p_err + kd * v_err # Output of the PD controler (amperage)
if (cur > iq_sat): # Check saturation
cur = iq_sat
if (cur < -iq_sat):
cur = -iq_sat
robot_if.GetMotor(i).SetCurrentReference(
cur) # Set reference currents
if (
(cpt % 100) == 0
): # Display state of the system once every 100 iterations of the main loop
print(chr(27) + "[2J")
# To read IMU data in Python use robot_if.imu_data_accelerometer(i), robot_if.imu_data_gyroscope(i)
# or robot_if.imu_data_attitude(i) with i = 0, 1 or 2
robot_if.PrintIMU()
robot_if.PrintADC()
robot_if.PrintMotors()
robot_if.PrintMotorDrivers()
robot_if.PrintStats()
sys.stdout.flush(
) # for Python 2, use print( .... , flush=True) for Python 3
robot_if.SendCommand(
) # Send the reference currents to the master board
robot_if.Stop(
) # Shut down the interface between the computer and the master board
if robot_if.IsTimeout():
print("Masterboard timeout detected.")
print(
"Either the masterboard has been shut down or there has been a connection issue with the cable/wifi."
)
print("-- End of example script --")
def __init__(self):
#self.plot = plot
self.robot_if = mbs.MasterBoardInterface("enp4s0")
self.robot_if.Init()
self.motors_spi_connected_indexes = []
num_joints = config.num_joints
self.num_joints = num_joints
self.motor_positions = np.zeros(num_joints)
self.motor_velocities = np.zeros(num_joints)
#Class wide variables from config file
self.kp = config.kp
self.kd = config.kd
self.current_saturation = config.current_saturation
self.gear_ratio = config.gear_ratio
#Local variables
ready = False
N_SLAVES_CONTROLLED = config.N_SLAVES_CONTROLLED
dt = config.dt
#Make sure master board is ready
for i in range(N_SLAVES_CONTROLLED):
self.robot_if.GetDriver(i).motor1.SetCurrentReference(0)
self.robot_if.GetDriver(i).motor2.SetCurrentReference(0)
self.robot_if.GetDriver(i).motor1.Enable()
self.robot_if.GetDriver(i).motor2.Enable()
self.robot_if.GetDriver(i).EnablePositionRolloverError()
self.robot_if.GetDriver(i).SetTimeout(0)
self.robot_if.GetDriver(i).Enable()
last = time.perf_counter()
while (not self.robot_if.IsTimeout() and not self.robot_if.IsAckMsgReceived()):
if ((time.perf_counter() - last) > dt):
last = time.perf_counter()
self.robot_if.SendInit()
if self.robot_if.IsTimeout():
print("Timout while waiting for ack.")
else:
for i in range(N_SLAVES_CONTROLLED):
if self.robot_if.GetDriver(i).IsConnected():
self.motors_spi_connected_indexes.append(2 * i)
self.motors_spi_connected_indexes.append(2 * i + 1)
ready_wait_time = 0
while not ready:
if ((time.perf_counter() - last) > dt):
last = time.perf_counter()
ready_wait_time = ready_wait_time + dt
self.robot_if.ParseSensorData()
ready = True
for i in self.motors_spi_connected_indexes:
if not (self.robot_if.GetMotor(i).IsEnabled() and self.robot_if.GetMotor(i).IsReady()):
ready = False
self.motor_positions[i] = self.robot_if.GetMotor(i).GetPosition()
self.robot_if.SendCommand()
self.q = np.copy(self.motor_positions)
self.q_prime = np.copy(self.motor_velocities)
self.zero_calibration = np.zeros(8)
self.floor_calibration = np.zeros(8)
self.imu_data = np.zeros(3)
self.imu_correction = np.array([np.pi, 0, 0]) #Our imu is turned upside down
if ready_wait_time < 1:
self.get_calibrations_from_file()
else:
print("Robot needs zero calibration")
print("To calibrate, start calibrate.py")
print("Using previous calibration")
self.get_calibrations_from_file()
print(f"Zero Calibration set to : {np.degrees(self.zero_calibration)} [DEG]")
print(f"Floor Calibration set to : {np.degrees(self.floor_calibration)} [DEG]")
def example_script(name_interface):
N_SLAVES = 6 # Maximum number of controled drivers
N_SLAVES_CONTROLED = 6 # Current number of controled drivers
cpt = 0
dt = 0.001 # Time step
state = 0 # State of the system (ready (1) or not (0))
duration = 10 # Duration in seconds, init included
cmd_lost_list = []
sensor_lost_list = []
cmd_ratio_list = []
sensor_ratio_list = []
time_list = []
histogram_sensor = []
histogram_cmd = []
if (duration == 0):
print("Null duration selected, end of script")
return
sent_list = [0.0 for i in range(int(duration * 1000))]
received_list = [0.0 for i in range(int(duration * 1000))]
loop_duration = []
# contrary to c++, in python it is interesting to build arrays
# with connected motors indexes so we can simply go through them in main loop
motors_spi_connected_indexes = [
] # indexes of the motors on each connected slaves
print("-- Start of example script --")
os.nice(
-20
) # Set the process to highest priority (from -20 highest to +20 lowest)
robot_if = mbs.MasterBoardInterface(name_interface)
robot_if.Init(
) # Initialization of the interface between the computer and the master board
for i in range(
N_SLAVES_CONTROLED
): # We enable each controler driver and its two associated motors
robot_if.GetDriver(i).motor1.SetCurrentReference(0)
robot_if.GetDriver(i).motor2.SetCurrentReference(0)
robot_if.GetDriver(i).motor1.Enable()
robot_if.GetDriver(i).motor2.Enable()
robot_if.GetDriver(i).EnablePositionRolloverError()
robot_if.GetDriver(i).SetTimeout(5)
robot_if.GetDriver(i).Enable()
last = clock()
prev_time = 0
while (not robot_if.IsTimeout() and not robot_if.IsAckMsgReceived()):
if ((clock() - last) > dt):
last = clock()
robot_if.SendInit()
if robot_if.IsTimeout():
print("Timeout while waiting for ack.")
else:
# fill the connected motors indexes array
for i in range(N_SLAVES_CONTROLED):
if robot_if.GetDriver(i).is_connected:
# if slave i is connected then motors 2i and 2i+1 are potentially connected
motors_spi_connected_indexes.append(2 * i)
motors_spi_connected_indexes.append(2 * i + 1)
while (
(not robot_if.IsTimeout()) and (clock() < duration)
): # Stop after 15 seconds (around 5 seconds are used at the start for calibration)
if (received_list[robot_if.GetLastRecvCmdIndex()] == 0):
received_list[robot_if.GetLastRecvCmdIndex()] = clock()
if ((clock() - last) > dt):
last = clock()
cpt += 1
robot_if.ParseSensorData(
) # Read sensor data sent by the masterboard
if (state == 0): # If the system is not ready
state = 1
# for all motors on a connected slave
for i in motors_spi_connected_indexes: # Check if all motors are enabled and ready
if not (robot_if.GetMotor(i).IsEnabled()
and robot_if.GetMotor(i).IsReady()):
state = 0
else: # If the system is ready
# for all motors on a connected slave
for i in motors_spi_connected_indexes:
if i % 2 == 0 and robot_if.GetDriver(
i // 2).error_code == 0xf:
#print("Transaction with SPI{} failed".format(i // 2))
continue #user should decide what to do in that case, here we ignore that motor
if robot_if.GetMotor(i).IsEnabled():
robot_if.GetMotor(i).SetCurrentReference(
0) # Set reference currents
if (
(cpt % 100) == 0
): # Display state of the system once every 100 iterations of the main loop
print(chr(27) + "[2J")
# To read IMU data in Python use robot_if.imu_data_accelerometer(i), robot_if.imu_data_gyroscope(i)
# or robot_if.imu_data_attitude(i) with i = 0, 1 or 2
robot_if.PrintStats()
sys.stdout.flush(
) # for Python 2, use print( .... , flush=True) for Python 3
cmd_lost_list.append(robot_if.GetCmdLost())
sensor_lost_list.append(robot_if.GetSensorsLost())
cmd_ratio_list.append(100. * robot_if.GetCmdLost() /
robot_if.GetCmdSent())
sensor_ratio_list.append(100. * robot_if.GetSensorsLost() /
robot_if.GetSensorsSent())
time_list.append(clock())
current_time = clock()
sent_list[robot_if.GetCmdPacketIndex()] = current_time
if (prev_time != 0):
loop_duration.append(1000 * (current_time - prev_time))
prev_time = current_time
robot_if.SendCommand(
) # Send the reference currents to the master board
robot_if.Stop(
) # Shut down the interface between the computer and the master board
# creation of the folder where the graphs will be stored
if not os.path.isdir("../graphs"):
os.mkdir("../graphs")
dir_name = time.strftime("%m") + time.strftime("%d") + time.strftime(
"%H") + time.strftime("%M") + time.strftime("%S")
if os.path.isdir("../graphs/" + dir_name):
count = 1
while os.path.isdir("../graphs/" + dir_name + "-" + str(count)):
count += 1
dir_name += "-" + str(count)
os.mkdir("../graphs/" + dir_name)
latency = []
for i in range(1, len(sent_list) - 1):
if (received_list[i] != 0 and sent_list[i] != 0):
latency.append(1000 * (received_list[i] - sent_list[i]))
else:
latency.append(0) # 0 means not sent or not received
# computing avg and std for non zero values
nonzero = [latency[i] for i in np.nonzero(latency)[0]]
if len(nonzero) != 0:
average = np.mean(nonzero)
print("average latency: %f ms" % average)
std = np.std(nonzero)
print("standard deviation: %f ms" % std)
plt.figure("wifi-ethernet latency", figsize=(20, 15), dpi=200)
anchored_text = AnchoredText(
"average latency: %f ms\nstandard deviation: %f ms" %
(average, std),
loc=2,
prop=dict(fontsize='xx-large'))
if len(latency) > 5000:
ax1 = plt.subplot(2, 1, 1)
else:
ax1 = plt.subplot(1, 1, 1)
ax1.plot(latency, '.')
ax1.set_xlabel("index", fontsize='xx-large')
ax1.set_ylabel("latency (ms)", fontsize='xx-large')
ax1.add_artist(anchored_text)
# plotting zoomed version to see pattern
if len(latency) > 5000:
ax2 = plt.subplot(2, 1, 2)
ax2.plot(latency, '.')
ax2.set_xlabel("index", fontsize='xx-large')
ax2.set_ylabel("latency (ms)", fontsize='xx-large')
ax2.set_xlim(len(latency) / 2, len(latency) / 2 + 2000)
ax2.set_ylim(-0.1, 2.1)
if (name_interface[0] == 'w'):
current_freq = robot_if.GetWifiChannel()
plt.suptitle("Wifi communication latency", fontsize='xx-large')
else:
plt.suptitle("Ethernet communication latency", fontsize='xx-large')
plt.savefig("../graphs/" + dir_name + '/' + dir_name +
"-wifieth-latency.png")
#Plot histograms and graphs
for i in range(20):
histogram_sensor.append(robot_if.GetSensorHistogram(i))
histogram_cmd.append(robot_if.GetCmdHistogram(i))
plt.figure("wifi-ethernet statistics", figsize=(20, 15), dpi=200)
ax3 = plt.subplot(3, 2, 1)
ax3.bar(range(1, 21), histogram_cmd)
ax3.set_xlabel("Number of consecutive lost command packets",
fontsize='xx-large')
ax3.set_ylabel("Number of occurences", fontsize='xx-large')
ax3.set_xticks(range(1, 21))
ax4 = plt.subplot(3, 2, 2)
ax4.bar(range(1, 21), histogram_sensor)
ax4.set_xlabel("Number of consecutive lost sensor packets",
fontsize='xx-large')
ax4.set_ylabel("Number of occurences", fontsize='xx-large')
ax4.set_xticks(range(1, 21))
ax5 = plt.subplot(3, 2, 3)
ax5.plot(time_list, cmd_lost_list)
ax5.set_xlabel("time (s)", fontsize='xx-large')
ax5.set_ylabel("lost commands", fontsize='xx-large')
ax6 = plt.subplot(3, 2, 4)
ax6.plot(time_list, sensor_lost_list)
ax6.set_xlabel("time (s)", fontsize='xx-large')
ax6.set_ylabel("lost sensors", fontsize='xx-large')
ax7 = plt.subplot(3, 2, 5)
ax7.plot(time_list, cmd_ratio_list)
ax7.set_xlabel("time (s)", fontsize='xx-large')
ax7.set_ylabel("ratio command loss (%)", fontsize='xx-large')
ax8 = plt.subplot(3, 2, 6)
ax8.plot(time_list, sensor_ratio_list)
ax8.set_xlabel("time (s)", fontsize='xx-large')
ax8.set_ylabel("ratio sensor loss (%)", fontsize='xx-large')
if (name_interface[0] == 'w'):
current_freq = robot_if.GetWifiChannel()
plt.suptitle("Wifi statistics: channel " + str(current_freq),
fontsize='xx-large')
else:
plt.suptitle("Ethernet statistics", fontsize='xx-large')
plt.savefig("../graphs/" + dir_name + '/' + dir_name +
"-wifieth-stats.png")
# plotting the evolution of command loop duration
plt.figure("loop duration", figsize=(20, 15), dpi=200)
anchored_text = AnchoredText(
"average duration: %f ms\nstandard deviation: %f ms" %
(np.mean(loop_duration), np.std(loop_duration)),
loc=2,
prop=dict(fontsize='xx-large'))
ax9 = plt.subplot(1, 1, 1)
ax9.plot(loop_duration, '.')
ax9.set_xlabel("iteration", fontsize='xx-large')
ax9.set_ylabel("loop duration (ms)", fontsize='xx-large')
ax9.add_artist(anchored_text)
plt.suptitle("Command loop duration", fontsize='xx-large')
plt.savefig("../graphs/" + dir_name + '/' + dir_name +
"-command-loop-duration.png")
# creation of the text file with system info
text_file = open(
"../graphs/" + dir_name + '/' + dir_name + "-system-info.txt", 'w')
text_file.write("Current date and time: " + time.strftime("%c") + '\n')
text_file.write("Linux distribution: " + platform.linux_distribution()[0] +
' ' + platform.linux_distribution()[1] + ' ' +
platform.linux_distribution()[2] + '\n')
text_file.write("Computer: " + os.uname()[1] + '\n')
text_file.write("Interface: " + name_interface + '\n')
if (name_interface[0] == 'w'):
text_file.write("Wifi channel: " + str(current_freq) + '\n')
text_file.write("Protocol version: " + str(robot_if.GetProtocolVersion()) +
'\n')
text_file.write("Script duration: " + str(duration) + '\n')
text_file.close()
if robot_if.IsTimeout():
print("Masterboard timeout detected.")
print(
"Either the masterboard has been shut down or there has been a connection issue with the cable/wifi."
)
print(
"The graphs have been stored in 'master_board_sdk/graphs' with a text file containing the system info, in a folder named from the current date and time."
)
print("-- End of example script --")
import numpy as np
np.set_printoptions(suppress=True, precision=2)
import time
import libmaster_board_sdk_pywrap as mbs
import libodri_control_interface_pywrap as oci
# Testbench
robot_if = mbs.MasterBoardInterface('ens3')
joints = oci.JointModules(robot_if,
np.array([0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11]),
0.025, 1., 1.,
np.array([
False, False, False, False, False, False,
False, False, False, False, False, False
]),
np.array([-10., -10., -10., -10.,-10., -10., -10., -10.,-10., -10., -10., -10.]),
np.array([ 10., 10., 10., 10., 10., 10., 10., 10., 10., 10., 10., 10.]),
80., 0.5)
imu = oci.IMU(robot_if)
robot = oci.Robot(robot_if, joints, imu)
robot.start()
robot.wait_until_ready()
# As the data is returned by reference, it's enough
# to get hold of the data one. It will update after
# each call to `robot.parse_sensor_data`.
def example_script(name_interface):
N_SLAVES = 6 # Maximum number of controled drivers
N_SLAVES_CONTROLED = 2 # Current number of controled drivers
cpt = 0 # Iteration counter
dt = 0.001 # Time step
t = 0 # Current time
kp = 1.0 # Proportional gain
kd = 0.1 # Derivative gain
iq_sat = 1.0 # Maximum amperage (A)
freq = 0.5 # Frequency of the sine wave
amplitude = math.pi # Amplitude of the sine wave
init_pos = [0.0 for i in range(N_SLAVES * 2)
] # List that will store the initial position of motors
state = 0 # State of the system (ready (1) or not (0))
print("-- Start of example script --")
os.nice(
-20
) # Set the process to highest priority (from -20 highest to +20 lowest)
robot_if = mbs.MasterBoardInterface(name_interface)
robot_if.Init(
) # Initialization of the interface between the computer and the master board
for i in range(
N_SLAVES_CONTROLED
): # We enable each controler driver and its two associated motors
robot_if.GetDriver(i).motor1.SetCurrentReference(0)
robot_if.GetDriver(i).motor2.SetCurrentReference(0)
robot_if.GetDriver(i).motor1.Enable()
robot_if.GetDriver(i).motor2.Enable()
robot_if.GetDriver(i).EnablePositionRolloverError()
robot_if.GetDriver(i).SetTimeout(5)
robot_if.GetDriver(i).Enable()
last = clock()
while (
(not robot_if.IsTimeout()) and (clock() < 20)
): # Stop after 15 seconds (around 5 seconds are used at the start for calibration)
if ((clock() - last) > dt):
last = clock()
cpt += 1
t += dt
robot_if.ParseSensorData(
) # Read sensor data sent by the masterboard
if (state == 0): # If the system is not ready
state = 1
for i in range(N_SLAVES_CONTROLED *
2): # Check if all motors are enabled and ready
if not (robot_if.GetMotor(i).IsEnabled()
and robot_if.GetMotor(i).IsReady()):
state = 0
init_pos[i] = robot_if.GetMotor(i).GetPosition()
t = 0
else: # If the system is ready
for i in range(N_SLAVES_CONTROLED * 2):
if robot_if.GetMotor(i).IsEnabled():
ref = init_pos[i] + amplitude * math.sin(
2.0 * math.pi * freq * t) # Sine wave pattern
v_ref = 2.0 * math.pi * freq * amplitude * math.cos(
2.0 * math.pi * freq * t)
p_err = ref - robot_if.GetMotor(
i).GetPosition() # Position error
v_err = v_ref - robot_if.GetMotor(
i).GetVelocity() # Velocity error
cur = kp * p_err + kd * v_err # Output of the PD controler (amperage)
if (cur > iq_sat): # Check saturation
cur = iq_sat
if (cur < -iq_sat):
cur = -iq_sat
robot_if.GetMotor(i).SetCurrentReference(
cur) # Set reference currents
if (
(cpt % 100) == 0
): # Display state of the system once every 100 iterations of the main loop
print(chr(27) + "[2J")
# To read IMU data in Python use robot_if.imu_data_accelerometer(i), robot_if.imu_data_gyroscope(i)
# or robot_if.imu_data_attitude(i) with i = 0, 1 or 2
robot_if.PrintIMU()
robot_if.PrintADC()
robot_if.PrintMotors()
robot_if.PrintMotorDrivers()
sys.stdout.flush(
) # for Python 2, use print( .... , flush=True) for Python 3
robot_if.SendCommand(
) # Send the reference currents to the master board
robot_if.Stop(
) # Shut down the interface between the computer and the master board
if robot_if.IsTimeout():
print(
"Masterboard timeout detected. Either the masterboard has been shut down or there has been a connection issue with the cable/wifi."
)
print("-- End of example script --")
def example_script(name_interface):
########################################################################
# Parameters definition & Initialization #
########################################################################
t = 0 # time
cpt = 0 # iteration counter
time_error = False # Initialize the error for the simulation time
state = 0 # State of the system (ready (1) or not (0))
os.nice(-20) # Set the highest priority to process the script (~Real-Time)
robot_if = mbs.MasterBoardInterface(name_interface) # Robot interface
robot_if.Init(
) # Initialization of the interface between the computer and the master board
enable_all_motors(robot_if)
qmes = np.zeros((8, 1)) # intialize the measured positions vector
vmes = np.zeros((8, 1)) # intialize the measured velocities vector
# Proportionnal gain to convert joint torques to current : tau_mot = K'*I
# tau_joints = Kred*K'*I ; K' = 0.02 Nm/A ; Kred = 9.0
Kt = 1.0 / (9 * 0.02) # A/Nm
flag_1st_it = True # to intialize the filtered velocities vector
alpha = 0.025 # filter coefficient
print("-- Start of example script --")
########################################################################
# Control loop #
########################################################################
# Initialize the main controller
myController = controller(q0, omega, t)
myLog = log_class.log(N_LOG=14000)
last = clock()
while (
(not robot_if.IsTimeout()) and (clock() < 20)
): # Stop after 15 seconds (around 5 seconds are used at the start for calibration)
if ((clock() - last) > dt):
last = clock()
cpt += 1
# Time incrementation
t += dt
robot_if.ParseSensorData(
) # Read sensor data sent by the masterboard
if (state == 0): # If the system is not ready
t = 0
if (are_all_motor_ready(robot_if)):
state = 1
if (state == 1): # If the system is ready
# Get measured positions and velocities
update_measured_q_v(robot_if, qmes, vmes)
if (flag_1st_it): # initialize the filtered velocities vector
vfilt = vmes
flag_1st_it = False
vfilt = vmes * alpha + vfilt * (1 - alpha)
####################################################################
# Select the appropriate controller #
# & #
# Load the joint torques into the robot #
####################################################################
# If the limit bounds are reached, controller is switched to a pure derivative controller
if (myController.error):
print(
"Safety bounds reached. Switch to a safety controller")
mySafetyController = Safety_controller.controller(
myController.qdes, myController.vdes)
myController = mySafetyController
# If the simulation time is too long, controller is switched to a zero torques controller
time_error = time_error or ((clock() - last) > 0.003)
if (time_error):
print(
"Computation time lasted to long. Switch to a zero torque control"
)
myEmergencyStop = EmergencyStop_controller.controller(
myController.qdes, myController.vdes)
myController = myEmergencyStop
# Retrieve the joint torques from the appropriate controller
jointTorques = myController.control(qmes, vmes, t)
# Set the desired torques to the motors
set_desired_torques(robot_if, jointTorques)
# Logging
myLog.log_method(clock() - last, myController.qdes,
myController.vdes, qmes, vmes, vfilt,
jointTorques)
if (
(cpt % 100) == 0
): # Display state of the system once every 100 iterations of the main loop
print(chr(27) + "[2J")
# To read IMU data in Python use robot_if.imu_data_accelerometer(i), robot_if.imu_data_gyroscope(i)
# or robot_if.imu_data_attitude(i) with i = 0, 1 or 2
robot_if.PrintIMU()
robot_if.PrintADC()
robot_if.PrintMotors()
robot_if.PrintMotorDrivers()
sys.stdout.flush(
) # for Python 2, use print( .... , flush=True) for Python 3
robot_if.SendCommand(
) # Send the reference currents to the master board
print("-- Shutting down --")
np.savez(
'/home/ada/Desktop/Script_Segolene_XP/Quadruped-Experience-scripts/Inverse-Dynamics/logs',
times=myLog.times,
des_positions=myLog.des_positions,
des_velocities=myLog.des_velocities,
meas_positions=myLog.meas_positions,
meas_velocities=myLog.meas_velocities)
myLog.plot_logs_velocities()
robot_if.Stop(
) # Shut down the interface between the computer and the master board
if robot_if.IsTimeout():
print(
"Masterboard timeout detected. Either the masterboard has been shut down or there has been a connection issue with the cable/wifi."
)
print("-- End of example script --")
def example_script(name_interface):
########################################################################
# Parameters definition & Initialization #
########################################################################
t = 0 # time
cpt = 0 # iteration counter
state = 0 # State of the system (ready (1) or not (0))
os.nice(-20) # Set the highest priority to process the script (~Real-Time)
robot_if = mbs.MasterBoardInterface(name_interface) # Robot interface
robot_if.Init(
) # Initialization of the interface between the computer and the master board
enable_all_motors(robot_if)
qmes = np.zeros((8, 1)) # intialize the measured positions vector
vmes = np.zeros((8, 1)) # intialize the measured velocities vector
print("-- Start of example script --")
########################################################################
# Control loop #
########################################################################
# Initialize the main controller
myController = controller(q0, omega, t)
myLog = log_class.log(N_LOG=14000)
last = clock()
while (
(not robot_if.IsTimeout()) and (clock() < 20)
): # Stop after 15 seconds (around 5 seconds are used at the start for calibration)
if ((clock() - last) > dt):
last = clock()
cpt += 1
# Time incrementation
t += dt
robot_if.ParseSensorData(
) # Read sensor data sent by the masterboard
if (state == 0): # The system is not ready
t = 0
if (are_all_motor_ready(robot_if)):
state = 1
if (state == 1): # The system is ready
# Get measured positions and velocities
update_measured_q_v(robot_if, qmes, vmes)
####################################################################
# Select the appropriate controller #
# & #
# Load the joint torques into the robot #
####################################################################
# If the limit bounds are reached, controller is switched to a safety controller
if (myController.error):
print(
"Safety bounds reached. Switch to a safety controller")
myEmergencyStop = EmergencyStop_controller.controller(
myController.qdes, myController.vdes)
myController = myEmergencyStop
# Retrieve the joint torques from the appropriate controller
jointTorques = myController.control(qmes, vmes, t)
# Set the desired torques to the motors
set_desired_torques(robot_if, jointTorques)
# Logging
myLog.log_method(clock() - last, myController.qdes,
myController.vdes, qmes, vmes, vmes * 0.)
if (
(cpt % 100) == 0
): # Display state of the system once every 100 iterations of the main loop
print(chr(27) + "[2J")
# To read IMU data in Python use robot_if.imu_data_accelerometer(i), robot_if.imu_data_gyroscope(i)
# or robot_if.imu_data_attitude(i) with i = 0, 1 or 2
robot_if.PrintIMU()
robot_if.PrintADC()
robot_if.PrintMotors()
robot_if.PrintMotorDrivers()
sys.stdout.flush(
) # for Python 2, use print( .... , flush=True) for Python 3
robot_if.SendCommand(
) # Send the reference currents to the master board
print("-- Shutting down --")
np.savez('logs',
times=myLog.times,
des_positions=myLog.des_positions,
des_velocities=myLog.des_velocities,
meas_positions=myLog.meas_positions,
meas_velocities=myLog.meas_velocities)
myLog.plot_logs_velocities()
robot_if.Stop(
) # Shut down the interface between the computer and the master board
if robot_if.IsTimeout():
print(
"Masterboard timeout detected. Either the masterboard has been shut down or there has been a connection issue with the cable/wifi."
)
print("-- End of example script --")
#! /usr/bin/env python
import numpy as np
np.set_printoptions(suppress=True, precision=2)
import time
import libmaster_board_sdk_pywrap as mbs
import libodri_control_interface_pywrap as oci
# Testbench
robot_if = mbs.MasterBoardInterface("ens3")
joints = oci.JointModules(
robot_if,
np.array([0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11]),
0.025,
1.0,
1.0,
np.array(
[
False,
False,
False,
False,
False,
False,
False,
False,
False,
False,