def set_high_priority(logger):
""" Change process scheduler and priority. """
# use "real time" scheduler
done = False
sched = os.SCHED_RR
if os.sched_getscheduler(0) == sched:
# already running with RR scheduler, likely set from init system, don't touch priority
done = True
else:
prio = (os.sched_get_priority_max(sched) - os.sched_get_priority_min(sched)) // 2
param = os.sched_param(prio)
try:
os.sched_setscheduler(0, sched, param)
except OSError:
logger.warning("Failed to set real time process scheduler to %u, priority %u" % (sched, prio))
else:
done = True
logger.info("Process real time scheduler set to %u, priority %u" % (sched, prio))
if not done:
# renice to highest priority
target_niceness = -19
previous_niceness = os.nice(0)
delta_niceness = target_niceness - previous_niceness
try:
new_niceness = os.nice(delta_niceness)
except OSError:
new_niceness = previous_niceness
if new_niceness != target_niceness:
logger.warning("Unable to renice process to %d, current niceness is %d" % (target_niceness, new_niceness))
else:
logger.info("Process reniced from %d to %d" % (previous_niceness, new_niceness))
def set_high_priority(logger):
""" Change process scheduler and priority. """
# use "real time" scheduler
done = False
sched = os.SCHED_RR
if os.sched_getscheduler(0) == sched:
# already running with RR scheduler, likely set from init system, don't touch priority
done = True
else:
prio = (os.sched_get_priority_max(sched) -
os.sched_get_priority_min(sched)) // 2
param = os.sched_param(prio)
try:
os.sched_setscheduler(0, sched, param)
except OSError:
logger.warning(
"Failed to set real time process scheduler to %u, priority %u"
% (sched, prio))
else:
done = True
logger.info("Process real time scheduler set to %u, priority %u" %
(sched, prio))
if not done:
# renice to highest priority
target_niceness = -19
previous_niceness = os.nice(0)
delta_niceness = target_niceness - previous_niceness
try:
new_niceness = os.nice(delta_niceness)
except OSError:
new_niceness = previous_niceness
if new_niceness != target_niceness:
logger.warning(
"Unable to renice process to %d, current niceness is %d" %
(target_niceness, new_niceness))
else:
logger.info("Process reniced from %d to %d" %
(previous_niceness, new_niceness))
def get_priority_min(self, sched):
return os.sched_get_priority_min(sched)
def test_priority_in_boundaries(self):
self.assertGreaterEqual(config.PROCESS_PRIORITY_REAL,
os.sched_get_priority_min(os.SCHED_RR))
self.assertLessEqual(config.PROCESS_PRIORITY_REAL,
os.sched_get_priority_max(os.SCHED_RR))
if os.name != "nt":
print("Making highest priority, os.SCHED_RR")
try:
pid = os.getpid()
niceValue = os.nice(-20)
sys.setswitchinterval(0.5)
print("sys.getswitchinterval", sys.getswitchinterval())
os.sched_setaffinity(pid, [(os.cpu_count() or 1) - 1])
os.sched_setscheduler(pid, os.SCHED_RR, os.sched_param(1))
print("sched_getscheduler", os.sched_getscheduler(pid))
print("sched_getparam", os.sched_getparam(pid))
print("sched_getaffinity", os.sched_getaffinity(pid))
print("sched_getprioritymax", os.sched_get_priority_max(0))
print("sched_getprioritymin", os.sched_get_priority_min(0))
print("sched_rr_getinterval", os.sched_rr_get_interval(pid))
print("nice", os.nice(0))
except PermissionError:
print("run as root to make top OS priority for more accurate results.")
else:
print("lol windows good luck")
for i in range(5):
print("pass", i + 1)
for j in range(1_000_000):
if sum(j for j in range(10)) < 0:
raise RuntimeError
py_intflag_call_time_member = timeit.timeit(setup="build_enums()",
def run(self):
# Priority settings for "MasterBoardTask"
print(
f"Master board task started with pid: {os.getpid()} and priority: {os.sched_getparam(os.getpid())}"
)
pid = os.getpid() # Identify process ID
sched = os.SCHED_FIFO # FIFO real time scheduler
param = os.sched_param(90) # Process priority
os.sched_setscheduler(pid, sched, param) # Update priority settings
print(
f"Master board task with pid: {os.getpid()} changed to priority: {os.sched_getparam(os.getpid())}\n"
)
# Local variables
dt = config.dt
timeout = False
current_time = 0.0
cpt = 0
# Troubleshooting arrays (Normally not used)
plot_duration = 1000
plot_array_size = int(plot_duration / dt)
time_per_timestep_array = np.zeros(plot_array_size + 1)
cycle_time_array = np.zeros(plot_array_size + 1)
ic_time_array = np.zeros(plot_array_size + 1)
# Trajectory and Queue initiation
#self.trajectory_queue_init()
q, q_prime = self.master_board.get_state()
trajectory = None
# Timer initiation
cycle_time = 0
program_start = time.perf_counter()
timestep_start = time.perf_counter()
timestep_end = time.perf_counter()
cycle_start = time.perf_counter()
# Communication with MasterBoard
while (self._running and not timeout):
if trajectory == None:
# No trajectory, try to get new from queue.
if not self.trajectory_queue.empty():
trajectory = self.trajectory_queue.get(
block=False) # Check queue
initial_trajectory_time = time.perf_counter()
else:
# Follow current trajectory
time_before_initial_condition = time.perf_counter()
current_trajectory_time = time.perf_counter(
) - initial_trajectory_time
q, q_prime = trajectory.step(current_trajectory_time)
ic_time_array[cpt] = time.perf_counter(
) - time_before_initial_condition
timeout = self.master_board.set_reference(q, q_prime)
if trajectory.is_done():
trajectory = None
# Update current reference to each joint.
timeout = self.master_board.track_reference(None)
for i in range(config.N_SLAVES_CONTROLLED * 2):
self.plotter.add_data(f"Joint {i}, Position Set Point", q)
self.plotter.add_time(current_time)
attitude = self.master_board.get_attitude()
if trajectory == None:
self.initial_position = self.balance(attitude,
self.initial_position)
# Cycle time is time spent calculating trajectory
cycle_time = time.perf_counter() - cycle_start
cycle_time_array[cpt] = cycle_time
if cycle_time >= dt:
cycle_time = dt
# Sleep MasterBoard for 1ms.
time.sleep(dt - cycle_time) # ideally: dt - cycle_time = 1ms
cycle_start = time.perf_counter() # Start time for next cycle
timestep_end = cycle_start
time_per_timestep_array[cpt] = timestep_end - timestep_start
timestep_start = cycle_start
current_time = current_time + dt
if cpt < plot_array_size:
cpt = cpt + 1
self.master_board.terminate()
#Reset process priority
sched = os.SCHED_OTHER
param = os.sched_param(os.sched_get_priority_min(sched))
os.sched_setscheduler(pid, sched, param)
print(
f"Master board task with pid: {os.getpid()} changed to priority: {os.sched_getparam(os.getpid())}"
)
plt.plot(time_per_timestep_array[0:cpt - 1])
plt.show()
plt.plot(cycle_time_array[0:cpt - 1])
plt.show()
plt.plot(ic_time_array[0:cpt - 1])
plt.show()
for i in range(config.N_SLAVES_CONTROLLED):
self.plotter.set_num_cols(2)
self.plotter.create_axis([f"Joint {2*i}, Position Set Point"])
self.plotter.plot(grid=True)
def run(self):
# Priority settings for "MasterBoardTask"
print(
f"Master board task started with pid: {os.getpid()} and priority: {os.sched_getparam(os.getpid())}"
)
pid = os.getpid() # Identify process ID
sched = os.SCHED_FIFO # FIFO real time scheduler
param = os.sched_param(90) # Process priority
os.sched_setscheduler(pid, sched, param) # Update priority settings
print(
f"Master board task with pid: {os.getpid()} changed to priority: {os.sched_getparam(os.getpid())}\n"
)
# Local variables
dt = config.dt
timeout = False
current_time = 0.0
cpt = 0
# Troubleshooting arrays (Normally not used)
plot_duration = 1000
plot_array_size = int(plot_duration / dt)
time_per_timestep_array = np.zeros(plot_array_size + 1)
cycle_time_array = np.zeros(plot_array_size + 1)
# Trajectory and Queue initiation
#self.trajectory_queue_init()
q, _ = self.master_board.get_state()
# Timer initiation
cycle_time = 0
program_start = time.perf_counter()
timestep_start = time.perf_counter()
timestep_end = time.perf_counter()
cycle_start = time.perf_counter()
self.prev_t = current_time
self.acc_error = np.zeros(2)
# Communication with MasterBoard
while (self._running and not timeout):
attitude = self.master_board.get_attitude()
current_position = self.master_board.get_state()[0]
q = self.balance(attitude, current_position, current_time)
#print(np.degrees(q))
timeout = self.master_board.set_reference(q, np.zeros(8))
timeout = self.master_board.track_reference(None)
self.plotter.add_data("Attitude Reading", attitude)
self.plotter.add_time(current_time)
# Cycle time is time spent calculating trajectory
cycle_time = time.perf_counter() - cycle_start
cycle_time_array[cpt] = cycle_time
if cycle_time >= dt:
cycle_time = dt
# Sleep MasterBoard for 1ms.
time.sleep(dt - cycle_time) # ideally: dt - cycle_time = 1ms
cycle_start = time.perf_counter() # Start time for next cycle
timestep_end = cycle_start
time_per_timestep_array[cpt] = timestep_end - timestep_start
timestep_start = cycle_start
current_time = current_time + dt
if cpt < plot_array_size:
cpt = cpt + 1
self.master_board.terminate()
#Reset process priority
sched = os.SCHED_OTHER
param = os.sched_param(os.sched_get_priority_min(sched))
os.sched_setscheduler(pid, sched, param)
print(
f"Master board task with pid: {os.getpid()} changed to priority: {os.sched_getparam(os.getpid())}"
)
plt.plot(time_per_timestep_array[0:cpt - 1])
plt.show()
plt.plot(cycle_time_array[0:cpt - 1])
plt.show()
def run(self):
# Priority settings for "MasterBoardTask"
print(
f"Master board task started with pid: {os.getpid()} and priority: {os.sched_getparam(os.getpid())}"
)
pid = os.getpid() # Identify process ID
sched = os.SCHED_FIFO # FIFO real time scheduler
param = os.sched_param(90) # Process priority
os.sched_setscheduler(pid, sched, param) # Update priority settings
print(
f"Master board task with pid: {os.getpid()} changed to priority: {os.sched_getparam(os.getpid())}\n"
)
# Local variables
dt = config.dt
timeout = False
current_time = 0.0
cpt = 0
# Troubleshooting arrays (Normally not used)
plot_duration = 1000
plot_array_size = int(plot_duration / dt)
time_per_timestep_array = np.zeros(plot_array_size + 1)
cycle_time_array = np.zeros(plot_array_size + 1)
ic_time_array = np.zeros(plot_array_size + 1)
# Trajectory and Queue initiation
#self.trajectory_queue_init()
q, q_prime = self.master_board.get_state()
with self.shared_position.get_lock():
self.shared_position[:] = q
self.masterboard_started_event.set()
trajectory = None
# Timer initiation
cycle_time = 0
program_start = time.perf_counter()
timestep_start = time.perf_counter()
timestep_end = time.perf_counter()
cycle_start = time.perf_counter()
self.prev_t = 0
self.prev_att_err = 0
self.acc_error = np.zeros(2)
self.kp = 200 #200
self.kd = 3.5 #2
self.ki = 6000 #3000
# Communication with MasterBoard
while (self._running and not timeout):
balance_mode = self.balance_mode
#Shared position. This allows InputTask to read the current position from a shared array
current_position = self.master_board.get_state()[0]
with self.shared_position.get_lock():
self.shared_position[:] = current_position
if balance_mode:
if not self.message_queue.empty():
input_message = self.message_queue.get(block=False)
self.handle_input(input_message, q)
attitude = self.master_board.get_attitude()
if self.prev_t == 0:
self.prev_t = current_time
q, q_prime = self.balance(attitude, current_position,
current_time)
timeout = self.master_board.set_reference(q, q_prime)
elif trajectory == None:
if not self.message_queue.empty():
input_message = self.message_queue.get(block=False)
self.handle_input(input_message, q)
# No trajectory, try to get new from queue.
elif not self.trajectory_queue.empty():
trajectory = self.trajectory_queue.get(
block=False) # Check queue
initial_trajectory_time = time.perf_counter()
else:
if self._terminating:
self._running = False
else:
# Follow current trajectory
time_before_initial_condition = time.perf_counter()
current_trajectory_time = time.perf_counter(
) - initial_trajectory_time
q, q_prime = trajectory.step(current_trajectory_time)
ic_time_array[cpt] = time.perf_counter(
) - time_before_initial_condition
timeout = self.master_board.set_reference(q, q_prime)
'''
for i in range(len(q)):
if i < 2:
self.plotter.add_data(f"Joint {i}, Position Set Point", q[i])
self.plotter.add_data(f"Joint {i}, Velocity Set Point", q_prime[i])
#pos_reading, vel_reading = self.master_board.get_state()
#self.plotter.add_data(f"Joint {i}, Position Reading", pos_reading[i])
#self.plotter.add_data(f"Joint {i}, Velocity Reading", vel_reading[i])
#self.plotter.add_data(f"Joint {i}, Position Error", pos_reading[i] - q[i])
#self.plotter.add_data(f"Joint {i}, Velocity Error", vel_reading[i] - q_prime[i])
'''
if trajectory.is_done():
trajectory = None
# Update current reference to each joint.
if self.debug_plot:
timeout = self.master_board.track_reference(self.plotter)
self.plotter.add_time(current_time)
else:
timeout = self.master_board.track_reference(None)
#self.plot.add_time(current_time)
# Cycle time is time spent calculating trajectory
cycle_time = time.perf_counter() - cycle_start
cycle_time_array[cpt] = cycle_time
if cycle_time >= dt:
cycle_time = dt
# Sleep MasterBoard for 1ms.
time.sleep(dt - cycle_time) # ideally: dt - cycle_time = 1ms
cycle_start = time.perf_counter() # Start time for next cycle
timestep_end = cycle_start
time_per_timestep_array[cpt] = timestep_end - timestep_start
timestep_start = cycle_start
current_time = current_time + dt
if cpt < plot_array_size:
cpt = cpt + 1
self.master_board.terminate()
#Reset process priority
sched = os.SCHED_OTHER
param = os.sched_param(os.sched_get_priority_min(sched))
os.sched_setscheduler(pid, sched, param)
print(
f"Master board task with pid: {os.getpid()} changed to priority: {os.sched_getparam(os.getpid())}"
)
plt.plot(time_per_timestep_array[0:cpt - 1])
plt.show()
plt.plot(cycle_time_array[0:cpt - 1])
plt.show()
plt.plot(ic_time_array[0:cpt - 1])
plt.show()
if self.debug_plot:
#for i in range(config.N_SLAVES_CONTROLLED * 2):
# self.plotter.set_num_cols(3)
# self.plotter.create_axis([f"Joint {2*i}, Position Reference", f"Joint {2*i}, Position Reading"])
# self.plotter.create_axis([f"Joint {2*i}, Velocity Reference"])
# self.plotter.create_axis([f"Joint {2*i}, Velocity Reading"])
# self.plotter.create_axis([f"Joint {2*i+1}, Position Reference", f"Joint {2*i+1}, Position Reading"])
# self.plotter.create_axis([f"Joint {2*i+1}, Velocity Reference"])
# self.plotter.create_axis([f"Joint {2*i+1}, Velocity Reading"])
#self.plotter.create_axis([
# f"Joint {2*i}, Velocity Set Point",
# f"Joint {2*i+1}, Velocity Set Point"
#])
self.plotter.create_axis([
f"Joint {0}, Position Reference",
f"Joint {0}, Position Reading"
])
self.plotter.create_axis([
f"Joint {2}, Position Reference",
f"Joint {2}, Position Reading"
])
self.plotter.create_axis([
f"Joint {4}, Position Reference",
f"Joint {4}, Position Reading"
])
self.plotter.create_axis([
f"Joint {6}, Position Reference",
f"Joint {6}, Position Reading"
])
self.plotter.create_axis([
f"Joint {1}, Position Reference",
f"Joint {1}, Position Reading"
])
self.plotter.create_axis([
f"Joint {3}, Position Reference",
f"Joint {3}, Position Reading"
])
self.plotter.create_axis([
f"Joint {5}, Position Reference",
f"Joint {5}, Position Reading"
])
self.plotter.create_axis([
f"Joint {7}, Position Reference",
f"Joint {7}, Position Reading"
])
self.plotter.set_num_cols(4)
self.plotter.plot(grid=True)
#! /usr/bin/python3
import os
print ("FIFO : {} <= prio <= {}".format(
os.sched_get_priority_min(os.SCHED_FIFO),
os.sched_get_priority_max(os.SCHED_FIFO)))
print ("RR : {} <= prio <= {}".format(
os.sched_get_priority_min(os.SCHED_RR),
os.sched_get_priority_max(os.SCHED_RR)))
print ("OTHER: {} <= prio <= {}".format(
os.sched_get_priority_min(os.SCHED_OTHER),
os.sched_get_priority_max(os.SCHED_OTHER)))
#! /usr/bin/python3
# ------------------------------------------------------------------
# exemple-get-priority-min-max.py
# Fichier d'exemple du livre "Developpement Systeme sous Linux"
# (C) 2015-2019 - Christophe BLAESS <*****@*****.**>
# https://www.blaess.fr/christophe/
# ------------------------------------------------------------------
import os
print("FIFO : {} <= prio <= {}".format(
os.sched_get_priority_min(os.SCHED_FIFO),
os.sched_get_priority_max(os.SCHED_FIFO)))
print("RR : {} <= prio <= {}".format(os.sched_get_priority_min(os.SCHED_RR),
os.sched_get_priority_max(os.SCHED_RR)))
print("OTHER: {} <= prio <= {}".format(
os.sched_get_priority_min(os.SCHED_OTHER),
os.sched_get_priority_max(os.SCHED_OTHER)))
def run(self):
# Priority settings for "MasterBoardTask"
print(
f"Master board task started with pid: {os.getpid()} and priority: {os.sched_getparam(os.getpid())}"
)
pid = os.getpid() # Identify process ID
sched = os.SCHED_FIFO # FIFO real time scheduler
param = os.sched_param(90) # Process priority
os.sched_setscheduler(pid, sched, param) # Update priority settings
print(
f"Master board task with pid: {os.getpid()} changed to priority: {os.sched_getparam(os.getpid())}\n"
)
# Local variables
dt = config.dt
timeout = False
current_time = 0.0
cpt = 0
# Troubleshooting arrays (Normally not used)
plot_duration = 1000
plot_array_size = int(plot_duration / dt)
time_per_timestep_array = np.zeros(plot_array_size + 1)
cycle_time_array = np.zeros(plot_array_size + 1)
ic_time_array = np.zeros(plot_array_size + 1)
# Trajectory and Queue initiation
self.trajectory_queue_init()
q, q_prime = self.master_board.get_state()
trajectory = None
# Timer initiation
cycle_time = 0
program_start = time.perf_counter()
timestep_start = time.perf_counter()
timestep_end = time.perf_counter()
cycle_start = time.perf_counter()
# Communication with MasterBoard
while (self._running and not timeout):
if trajectory == None:
time_before_initial_condition = time.perf_counter()
if not self.message_queue.empty():
input_message = self.message_queue.get(block=False)
self.handle_input(input_message, q)
# No trajectory, try to get new from queue.
elif not self.trajectory_queue.empty():
trajectory = self.trajectory_queue.get(
block=False) # Check queue
current_position = self.master_board.get_state()[
0] # Update joints current position
trajectory.set_initial_conditions(
q,
current_time) # Set initial condition for trajectory
else:
if self._terminating:
self._running = False
ic_time_array[cpt] = time.perf_counter(
) - time_before_initial_condition
else:
# Follow current trajectory
q, q_prime = trajectory.step(current_time)
timeout = self.master_board.set_reference(q, q_prime)
if trajectory.Done():
if self._terminating:
self.terminate()
else:
trajectory = None
#Shared position. This allows InputTask to read the current position from a shared array
current_position = self.master_board.get_actual_state()[0]
with self.shared_position.get_lock():
self.shared_position[:] = current_position
# Update current reference to each joint.
timeout = self.master_board.track_reference()
self.plot.add_time(current_time)
# Cycle time is time spent calculating trajectory
cycle_time = time.perf_counter() - cycle_start
cycle_time_array[cpt] = cycle_time
if cycle_time >= dt:
cycle_time = dt
# Sleep MasterBoard for 1ms.
time.sleep(dt - cycle_time) # ideally: dt - cycle_time = 1ms
cycle_start = time.perf_counter() # Start time for next cycle
timestep_end = cycle_start
time_per_timestep_array[cpt] = timestep_end - timestep_start
timestep_start = cycle_start
current_time = current_time + dt
if cpt < plot_array_size:
cpt = cpt + 1
self.master_board.terminate()
#Reset prio
sched = os.SCHED_OTHER
param = os.sched_param(os.sched_get_priority_min(sched))
os.sched_setscheduler(pid, sched, param)
print(
f"Master board task with pid: {os.getpid()} changed to priority: {os.sched_getparam(os.getpid())}"
)
plt.plot(time_per_timestep_array[0:cpt - 1])
plt.show()
plt.plot(cycle_time_array[0:cpt - 1])
plt.show()
plt.plot(ic_time_array[0:cpt - 1])
plt.show()
for i in range(config.num_joints // 2):
self.plot.create_axis([
f"Joint {2 * i}, Position Reference",
f"Joint {2 * i}, Position Reading"
])
self.plot.create_axis([
f"Joint {2 * i + 1}, Position Reference",
f"Joint {2 * i + 1}, Position Reading"
])
self.plot.plot()
