def __init__(self, problem, config_validator, out_path=None):
self.out_path = out_path
self.config_validator = config_validator
self.problem = problem
self.costs = []
self.state = State(self.problem)
self.best_state = self.state
self.camera_move_method = config_validator.getParameter(
'camera_move_method', ['local', 'random'], 'local')
self.r_count_method = config_validator.getParameter(
'r_count_method', ['average', 'max'], 'average')
try:
self.Tmax = config_validator.getIntegerParameter('t_max', 5000)
self.Tmin = config_validator.getIntegerParameter('t_min', 50)
self.steps = config_validator.getIntegerParameter(
'num_iterations', 100)
self.updates = config_validator.getIntegerParameter(
'num_updates', 10)
self.update_counter = 0
self.r_min = config_validator.getIntegerParameter('r_min', 100)
self.alpha = config_validator.getDoubleParameter('alpha', 100)
self.beta = config_validator.getDoubleParameter('beta', 100)
except KeyError as e:
print("Exception. Option {} is missing in config file".format(e))
super(SimulatedAnnealer, self).__init__(self.state)
def assign_model(self, variables: Variables) -> int:
logging.debug('Assign model into cfg...')
gas = 0
state = State()
for node in self.path:
for opcode in node.opcodes:
gas += state.simulate_with_model(opcode, self.model, variables)
self.model_gas = gas
def run_robot(PipeConnection: connection.Connection, printState=False):
"""
A loop function cabable of updating a pupper robots state object based of of commands recieved via pipe form the transmission loop.
"""
config = Configuration()
hardware_interface = HardwareInterface()
controller = Controller(
config,
four_legs_inverse_kinematics,
)
state = State()
msgHandler = MessageHandler(config, PipeConnection)
last_loop = time.time()
deactivate = False
while True:
if deactivate == True:
print("Robot loop terminated")
break
while True:
command = msgHandler.get_command_from_pipe(state)
if command.activate_event == 1:
break
time.sleep(0.1)
while True:
now = time.time()
if now - last_loop < config.dt:
continue
last_loop = time.time()
command = msgHandler.get_command_from_pipe(state)
if command.activate_event == 1:
deactivate = True
break
state.quat_orientation = np.array([1, 0, 0, 0])
# Step the controller forward by dt
controller.run(state, command)
if printState == True:
state.printSelf()
# Update the pwm widths going to the servos
hardware_interface.set_actuator_postions(state.joint_angles)
def keys_pressed_reaction(self):
if State.is_key_pressed(pygame.K_p):
# Play the game
self.state = State.GAME_STATE
pass
elif State.is_key_pressed(pygame.K_a):
# Start ai
self.state = State.AI_GAME_STATE
pass
def get_example_state(self):
if self.params.multi_agent:
num_agents = self.params.num_agents_range[0]
else:
num_agents = 1
state = State(self.map_image, num_agents, self.params.multi_agent)
state.device_map = np.zeros(self.shape, dtype=float)
state.collected = np.zeros(self.shape, dtype=float)
return state
def __init__(self, display, clock):
State.__init__(self, display, clock)
self.player = Player()
self.map = Map(self.player)
self.forwardData = {}
self.speed = getSettingForName(SettingNames.SPEED).val
self.initResources()
def symbolic_execution_from_other_head(self) -> None:
from_list = [edge.from_ for edge in self.cfg.edges]
to_list = [edge.to_ for edge in self.cfg.edges]
node_list = [
node for node in self.cfg.nodes
if node.tag in from_list and node.tag not in to_list
]
for node in node_list:
state = State()
state.init_with_var(self.variables)
self.symbolic_execution(node.tag, Path(), state)
def start(self):
for node in self.cfg.nodes:
if node.color == 'yellow':
try:
s = State()
s.stack = {'0': 0}
s.memory = {64: 128}
logging.debug('Start From Tag %s' % node.tag)
self.symbolic_execution(node.tag, Path(), s)
except:
continue
def __init__(self, tag: int, opcodes: [Opcode]):
self.tag = tag
self.opcodes = opcodes
self.init_state = State()
self.state = State()
self.gas = 0
self.path_constraint = None
self.color = 'black'
self.visited = False
self.count = 0
self.loop_condition = list()
def calculate_reward(self, state: State, action: GridActions,
next_state: State):
reward = self.calculate_motion_rewards(state, action, next_state)
# Reward the collected data
reward += self.params.data_multiplier * (
state.get_remaining_data() - next_state.get_remaining_data())
# Cumulative reward
self.cumulative_reward += reward
return reward
def __init__(self, agent, temp_it=CFG.temp_thresh + 1, player_name=None):
self.agent = agent
self.game = self.agent.game
self.logger = Logger()
self.temp_it = temp_it
self.player_name = player_name
self.tree = State(None, 1, self.game(), prior=1.0)
def main(id, command_status):
arduino = ArduinoSerial('COM4') # need to specify the serial port
# Create config
config = Configuration()
controller = Controller(
config,
four_legs_inverse_kinematics,
)
state = State()
last_loop = time.time()
command = Command()
while True:
now = time.time()
if now - last_loop < config.dt:
continue
last_loop = time.time()
# calculate robot step command from keyboard inputs
result_dict = command_status.get()
#print(result_dict)
command_status.put(result_dict)
x_vel = result_dict['IDstepLength']
y_vel = result_dict['IDstepWidth']
command.yaw_rate = result_dict['IDstepAlpha']
command.horizontal_velocity = np.array([x_vel, y_vel])
arduinoLoopTime, Xacc, Yacc, realRoll, realPitch = arduino.serialRecive(
) # recive serial(IMU part)
# Read imu data. Orientation will be None if no data was available
quat_orientation = (np.array([1, 0, 0, 0]))
state.quat_orientation = quat_orientation
# Step the controller forward by dt
controller.run(state, command)
deg_angle = np.rad2deg(
state.joint_angles) + angle_offset # make angle rad to deg
print(deg_angle)
arduino.serialSend(deg_angle[:, 0], deg_angle[:, 1], deg_angle[:, 2],
deg_angle[:, 3])
consoleClear()
def init_episode(self):
self.device_list = self.device_manager.generate_device_list(
self.device_positions)
if self.params.multi_agent:
self.num_agents = int(
np.random.randint(low=self.params.num_agents_range[0],
high=self.params.num_agents_range[1] + 1,
size=1))
else:
self.num_agents = 1
state = State(self.map_image, self.num_agents, self.params.multi_agent)
state.reset_devices(self.device_list)
if self.params.fixed_starting_idcs:
idx = self.params.starting_idcs
else:
# Replace False insures that starting positions of the agents are different
idx = np.random.choice(len(self.starting_vector),
size=self.num_agents,
replace=False)
state.positions = [self.starting_vector[i] for i in idx]
state.movement_budgets = np.random.randint(
low=self.params.movement_range[0],
high=self.params.movement_range[1] + 1,
size=self.num_agents)
state.initial_movement_budgets = state.movement_budgets.copy()
return state
def __init__(self, window, state=State.GAME_STATE):
State.__init__(self, state, window)
self.substate = state
self.penguin = Penguin(window.width / 3, window.height / 2)
self.entities = []
ice_boundary = IceBoundary(window.width, window.height)
self.entities = self.entities + ice_boundary.floor + ice_boundary.ceiling
self.game_count = 0
# AI needs this info
self.closest_gap_range = (0,window.height)
self.closest_x_distance_to_penguin = window.width - self.penguin.x
self.old_state = None
self.environment_state = None
self.reward = 0
# Score message
self.score = 0
self.score_text_surf, self.score_text_rect = Text.text_objects(str(self.score), Text.small_font)
self.score_text_rect.center = ((self.window.width * 4 / 5), (self.window.height / 6))
# Game start screen
self.game_started = False
self.spacebar_text_surf, self.spacebar_text_rect = Text.text_objects("To begin and to flap, hit the (Spacebar)", Text.tiny_font)
self.spacebar_text_rect.center = ((self.window.width / 2), (self.window.height / 3))
self.p_to_pause_text_surf, self.p_to_pause_text_rect = Text.text_objects("(P) to pause", Text.tiny_font)
self.p_to_pause_text_rect.center = ((self.window.width / 2), (self.window.height * 2 / 3))
# Quit text
self.quit_text_surf , self.quit_text_rect = Text.text_objects("(Q)uit to menu", Text.tiny_font)
self.quit_text_rect.center = ((self.window.width / 2), (self.window.height * 2 / 3))
# Unpause text
self.unpause_text_surf, self.unpause_text_rect = Text.text_objects("(U)npause", Text.small_font)
self.unpause_text_rect.center = ((self.window.width / 2), (self.window.height / 3))
# Restart text
self.restart_text_surf, self.restart_text_rect = Text.text_objects("(P)lay Again", Text.small_font)
self.restart_text_rect.center = ((self.window.width / 2), (self.window.height / 3))
self.update_score()
if self.substate == State.AI_GAME_STATE:
self.game_started = True
def play(self, board, player_color):
"""
To play against an opponent
:return:
"""
self.tree = State(None, player_color, board, prior=1.0)
self.search_()
next_state, _ = self.sample_move()
board.play_(player_color, next_state.action)
def __init__(self, window):
State.__init__(self, State.MENU_STATE, window)
center_x = self.window.width / 2
self.title_text_surf, self.title_text_rect = Text.text_objects(
"Penguino.ai", Text.large_font)
self.title_text_rect.center = (center_x, (self.window.height / 6))
self.instructions_text_surf, self.instructions_text_rect = Text.text_objects(
"Type the letter in () to select an option", Text.tiny_font)
self.instructions_text_rect.center = (center_x,
(self.window.height * 2 / 6))
self.play_text_surf, self.play_text_rect = Text.text_objects(
"(P)lay", Text.small_font)
self.play_text_rect.center = (center_x, (self.window.height * 3 / 6))
self.ai_text_surf, self.ai_text_rect = Text.text_objects(
"(A)I Model", Text.small_font)
self.ai_text_rect.center = (center_x, (self.window.height * 4 / 6))
def expand_leaf_(self, leaf, p):
available_moves = leaf.board.list_available_moves()
noise = iter(
np.random.dirichlet([CFG.dirichlet_alpha] * len(available_moves)))
for action in available_moves:
new_board = deepcopy(leaf.board)
new_board.play_(leaf.player_color, action)
prior = (1 - CFG.epsilon) * p[action] + CFG.epsilon * next(noise)
new_state = State(action, -leaf.player_color, new_board, prior,
leaf)
leaf.children.append(new_state)
def create_automaton(self, auto_id, states_list, symbols_list, init_state, final_states_list, transitions_list):
auto_id = auto_id[0]
states = {}
for id in states_list:
states[id] = State(id, False, {})
init_state = states[init_state]
for id in final_states_list:
states[id].is_final = True
for transition in transitions_list:
origin = transition[0]
symbol = transition[1]
destiny = transition[2]
if symbol not in states[origin].transitions:
states[origin].transitions[symbol] = []
states[origin].transitions[symbol].append(destiny)
return Automaton(auto_id, states, symbols_list, init_state)
def keys_pressed_reaction(self):
if self.substate == State.GAME_STATE:
if State.is_key_pressed(pygame.K_SPACE):
if not self.game_started:
self.game_started = True
self.penguin.flapped()
elif State.is_key_pressed(pygame.K_p) and self.game_started:
# Pause the game
self.substate = Game.PAUSE_STATE
elif self.substate == Game.PAUSE_STATE:
if State.is_key_pressed(pygame.K_u):
self.substate = State.GAME_STATE
elif State.is_key_pressed(pygame.K_q):
self.state = State.MENU_STATE
elif self.substate == Game.DEAD_STATE:
if State.is_key_pressed(pygame.K_p):
self.state = State.RESTART_STATE
elif State.is_key_pressed(pygame.K_q):
self.state = State.MENU_STATE
elif self.substate == Game.AI_DEAD_STATE:
self.state = State.AI_RESTART_STATE
def main(use_imu=False):
"""Main program
"""
rospy.init_node("pupper_js_pub")
pub = rospy.Publisher("/pupper_js", JointState, queue_size=1)
pupper_js = JointState()
pupper_js.name = [
"leg_f_r_joint", "leg_f_l_joint", "leg_b_r_joint", "leg_b_l_joint",
"shldr_f_r_joint", "shldr_f_l_joint", "shldr_b_r_joint",
"shldr_b_l_joint", "shldr_f_r_joint_inter", "shldr_f_l_joint_inter",
"shldr_b_r_joint_inter", "shldr_b_l_joint_inter"
]
# Create config
config = Configuration()
hardware_interface = HardwareInterface()
# Create imu handle
if use_imu:
imu = IMU(port="/dev/ttyACM0")
imu.flush_buffer()
# Create controller and user input handles
controller = Controller(
config,
four_legs_inverse_kinematics,
)
state = State()
print("Creating joystick listener...")
joystick_interface = JoystickInterface(config)
print("Done.")
last_loop = time.time()
print("Summary of gait parameters:")
print("overlap time: ", config.overlap_time)
print("swing time: ", config.swing_time)
print("z clearance: ", config.z_clearance)
print("x shift: ", config.x_shift)
# Wait until the activate button has been pressed
while True:
print("Waiting for L1 to activate robot.")
while True:
command = joystick_interface.get_command(state)
joystick_interface.set_color(config.ps4_deactivated_color)
if command.activate_event == 1:
break
time.sleep(0.1)
print("Robot activated.")
joystick_interface.set_color(config.ps4_color)
while True:
now = time.time()
if now - last_loop < config.dt:
continue
last_loop = time.time()
# Parse the udp joystick commands and then update the robot controller's parameters
command = joystick_interface.get_command(state)
if command.activate_event == 1:
print("Deactivating Robot")
break
# Read imu data. Orientation will be None if no data was available
quat_orientation = (imu.read_orientation()
if use_imu else np.array([1, 0, 0, 0]))
state.quat_orientation = quat_orientation
# Step the controller forward by dt
controller.run(state, command)
# Update the pwm widths going to the servos
pupper_js.position = np.concatenate(
(state.joint_angles[1], state.joint_angles[0],
state.joint_angles[2]),
axis=None)
pub.publish(pupper_js)
hardware_interface.set_actuator_postions(state.joint_angles)
def __init__(self, display, clock):
State.__init__(self, display, clock)
self.selectedId = 0
self.initResources()
def main(use_imu=False):
global remote_ctl_flag
"""Main program
"""
last_poll = time.time()
poll_cooldown = 0.05
# Create config
config = Configuration()
hardware_interface = HardwareInterface()
# Create imu handle
if use_imu:
imu = IMU(port="/dev/ttyACM0")
imu.flush_buffer()
# Create controller and user input handles
controller = Controller(
config,
four_legs_inverse_kinematics,
)
state = State()
print("Creating joystick listener...")
joystick_interface = JoystickInterface(config)
print("Done.")
last_loop = time.time()
print("Summary of gait parameters:")
print("overlap time: ", config.overlap_time)
print("swing time: ", config.swing_time)
print("z clearance: ", config.z_clearance)
print("x shift: ", config.x_shift)
# Wait until the activate button has been pressed
while True:
print("Waiting for L1 to activate robot.")
while True:
command = joystick_interface.get_command(state)
joystick_interface.set_color(config.ps4_deactivated_color)
if command.activate_event == 1:
break
time.sleep(0.1)
print("Robot activated.")
joystick_interface.set_color(config.ps4_color)
while True:
now = time.time()
if now - last_loop < config.dt:
continue
last_loop = time.time()
if time.time() - last_poll > poll_cooldown:
last_poll = time.time()
# print(latest_data)
events = sel.select(timeout=0.5)
for key, mask in events:
if key.data is None:
accept_wrapper(key.fileobj)
else:
service_connection(key, mask)
command = parse_command(command, state, latest_data, config)
# Parse the udp joystick commands and then update the robot controller's parameters
if not remote_ctl_flag:
command = joystick_interface.get_command(state)
remote_ctl_flag = False
if command.activate_event == 1:
print("Deactivating Robot")
break
# Read imu data. Orientation will be None if no data was available
quat_orientation = (imu.read_orientation()
if use_imu else np.array([1, 0, 0, 0]))
state.quat_orientation = quat_orientation
# Step the controller forward by dt
controller.run(state, command)
# Update the pwm widths going to the servos
hardware_interface.set_actuator_postions(state.joint_angles)
class SimulatedAnnealer(Annealer):
def __init__(self, problem, config_validator, out_path=None):
self.out_path = out_path
self.config_validator = config_validator
self.problem = problem
self.costs = []
self.state = State(self.problem)
self.best_state = self.state
self.camera_move_method = config_validator.getParameter(
'camera_move_method', ['local', 'random'], 'local')
self.r_count_method = config_validator.getParameter(
'r_count_method', ['average', 'max'], 'average')
try:
self.Tmax = config_validator.getIntegerParameter('t_max', 5000)
self.Tmin = config_validator.getIntegerParameter('t_min', 50)
self.steps = config_validator.getIntegerParameter(
'num_iterations', 100)
self.updates = config_validator.getIntegerParameter(
'num_updates', 10)
self.update_counter = 0
self.r_min = config_validator.getIntegerParameter('r_min', 100)
self.alpha = config_validator.getDoubleParameter('alpha', 100)
self.beta = config_validator.getDoubleParameter('beta', 100)
except KeyError as e:
print("Exception. Option {} is missing in config file".format(e))
super(SimulatedAnnealer, self).__init__(self.state)
# override
def move(self):
self.state = self.state.generateNeighbour(self.camera_move_method)
# override
def energy(self):
coverage_energy = self.alpha * self.getCoverage()
camera_cost = self.beta * self.getCameraCostRatio()
redundancy_cost = self.getRedundancyParameter() / self.r_min
energy = coverage_energy - camera_cost - redundancy_cost
energy *= -1 # use for maximization
self.costs.append(energy)
# save energy's components for output
self.state.energy = energy
self.state.coverage_energy = coverage_energy
self.state.camera_cost = camera_cost
self.state.redundancy_cost = redundancy_cost
if energy < self.best_state.energy:
self.best_state = self.state
return energy
# override
def update(self, *args, **kwargs):
print("========== Update annealing:", self.update_counter,
" ==========")
print("Total energy: ", self.state.energy)
print("coverage_energy: ", self.state.coverage_energy)
print("camera_cost: ", self.state.camera_cost)
print("redundancy_cost: ", self.state.redundancy_cost)
plot_path = os.path.join(self.out_path,
'state_' + str(self.update_counter))
PlotCreator.createStatePlot(plot_path, self.state, room_only=False)
self.update_counter += 1
def getCoverage(self):
covered_points = set()
for c in self.state.cameras:
covered_points.update(c.covered_points)
if len(self.problem.inside_points) == 0:
raise RuntimeError("number of room inside points cannot be 0!!")
coverage = len(covered_points) / float(len(self.problem.inside_points))
#print("Coverage: ", coverage)
return coverage
def getCameraCostRatio(self):
num_cameras = len(self.state.cameras)
ratio = max(0, num_cameras - self.problem.min_number_of_cams)
if self.problem.min_number_of_cams == 0:
raise RuntimeError("k_min cannot be 0!")
ratio /= float(self.problem.min_number_of_cams)
#print("k = ", ratio)
return ratio
def getRedundancyParameter(self):
def count_redundancy():
redundancy_list = []
for p in self.problem.inside_points:
r_x = 0
for c in self.state.cameras:
if p in c.covered_points:
r_x += 1
redundancy_list.append(max(0, self.r_min - r_x))
return redundancy_list
if len(self.problem.inside_points) == 0.0:
raise RuntimeError("number of room inside points cannot be 0!!")
if self.r_count_method == 'average':
r_sum = sum(count_redundancy())
return r_sum / float(len(self.problem.inside_points))
elif self.r_count_method == 'max':
return max(count_redundancy())
else:
raise RuntimeError("Unrecognized r_count_method!")
def get_state(self):
return State(self.id, self.is_final, self.transitions)
def main(stdscr):
"""Main program
"""
# Init Keyboard
stdscr.clear() # clear the screen
curses.noecho() # don't echo characters
curses.cbreak() # don't wait on CR
stdscr.keypad(True) # Map arrow keys to UpArrow, etc
stdscr.nodelay(True) # Don't block - do not wait on keypress
# Create config
config = Configuration()
# hardware_interface = HardwareInterface()
command = Command()
# Create controller and user input handles
controller = Controller(
config,
four_legs_inverse_kinematics,
)
state = State()
last_loop = time.time()
print("Summary of gait parameters:")
print("overlap time: ", config.overlap_time)
print("swing time: ", config.swing_time)
print("z clearance: ", config.z_clearance)
print("x shift: ", config.x_shift)
print("Waiting for L1 to activate robot.")
key_press = ''
while True:
if key_press == 'q':
break
# Wait until the activate button has been pressed
while True:
try:
key_press = stdscr.getkey()
except:
key_press = ''
if key_press == 'q':
break
elif key_press == 'a':
# command.activate_event = True
# command.yaw_rate = 0
# print(state.behavior_state.name)
# print("\r")
print("Robot Activated\r")
break
if key_press == 'q':
break
do_print_cmd = False
while True:
x_speed = command.horizontal_velocity[0]
y_speed = command.horizontal_velocity[1]
yaw_speed = command.yaw_rate
new_x_speed = x_speed
new_y_speed = y_speed
new_yaw_speed = yaw_speed
try:
key_press = stdscr.getkey()
except:
key_press = ''
if key_press == 'q':
break
elif key_press == 'a':
# command.activate_event = True
# controller.run(state, command)
# command.activate_event = False
# command.trot_event = False
print("Robot Deactivate\r")
break
elif key_press == 't':
command.trot_event = True
print("Trot Event\r")
do_print_cmd = True
elif key_press == ' ':
new_x_speed = 0
new_y_speed = 0
do_print_cmd = True
elif key_press == 'k':
new_yaw_speed = 0
do_print_cmd = True
elif key_press == 'i':
new_x_speed = min(config.max_x_velocity,
x_speed + config.max_x_velocity / 5.0)
do_print_cmd = True
elif key_press == ',':
new_x_speed = max(-1 * config.max_x_velocity,
x_speed - config.max_x_velocity / 5.0)
do_print_cmd = True
elif key_press == 'f':
new_y_speed = max(-1 * config.max_y_velocity,
y_speed - config.max_y_velocity / 5.0)
do_print_cmd = True
elif key_press == 's':
new_y_speed = min(config.max_y_velocity,
y_speed + config.max_y_velocity / 5.0)
do_print_cmd = True
elif key_press == 'd':
new_y_speed = 0
do_print_cmd = True
elif key_press == 'j':
new_yaw_speed = min(config.max_yaw_rate,
yaw_speed + config.max_yaw_rate / 5.0)
do_print_cmd = True
elif key_press == 'l':
new_yaw_speed = max(-1 * config.max_yaw_rate,
yaw_speed - config.max_yaw_rate / 5.0)
do_print_cmd = True
command.horizontal_velocity = np.array([new_x_speed, new_y_speed])
command.yaw_rate = new_yaw_speed
now = time.time()
if now - last_loop < config.dt:
continue
last_loop = time.time()
# Read imu data. Orientation will be None if no data was available
quat_orientation = (np.array([1, 0, 0, 0]))
state.quat_orientation = quat_orientation
# Step the controller forward by dt
controller.run(state, command)
if do_print_cmd:
print_cmd(command)
print_state(state)
do_print_cmd = False
# print(state.behavior_state.name + "\r")
# print_state(state)
# Update the pwm widths going to the servos
# hardware_interface.set_actuator_postions(state.joint_angles)
command.activate_event = False
command.trot_event = False
def symbolic_execution(self, tag: int, path: Path, state: State) -> None:
from src.Result import Result
# logging.debug('TAG: %s' % tag)
# if settings.DETECT_LOOP:
# return
node = self.cfg.get_node(tag)
if not node:
return
node.visit()
node.init_state = deepcopy(state)
gas = 0
if node.count % 10 == 0:
logging.debug('%s visit %s times' % (tag, node.count))
if ENABLE_MAX_NODE_VISITED_TIMES and node.count > MAX_NODE_VISITED_TIMES:
return
for opcode in node.opcodes:
# NOTE: state simulation
result = state.simulate(opcode, self.variables)
# if tag in [1133]:
# logging.debug('%s: %s' % (opcode.pc, opcode.name))
# logging.debug('Stack: %s\n' % self.to_string(state.stack))
# logging.debug('MEM: %s' % self.to_string(state.memory))
# logging.debug('STO: %s\n' % self.to_string(state.storage))
path.add_path_constraints(result.path_constraints)
gas += result.gas
gas = simplify(gas) if is_expr(gas) else gas
path.add_gas(result.gas)
path.add_memory_gas(result.memory_gas)
if opcode.name == 'JUMP':
# NOTE: set gas to node
node.set_gas(gas)
node.set_state(deepcopy(state))
# NOTE: add tag to the path list
path.add_node(deepcopy(node))
# NOTE: if edge is not in edges -> add edge into edges
self.__add_edge(Edge(node.tag, result.jump_tag, 'red'))
return self.symbolic_execution(result.jump_tag, path, state)
elif opcode.name == 'JUMPI':
tmp_cond = simplify(result.jump_condition) if is_expr(
result.jump_condition) else result.jump_condition
result.jump_condition = int(tmp_cond.as_long()) if isinstance(
tmp_cond, BitVecNumRef) else result.jump_condition
node.set_path_constraint(result.jump_condition)
# NOTE: Loop detection
detect_loop = False
if LOOP_DETECTION:
if path.count_specific_node_num(node.tag) > 0 and is_expr(
result.jump_condition):
jump_condition, jump_condition_n1 = path.handle_loop(
node, opcode.pc, self.variables, self.cfg)
if jump_condition is not None:
detect_loop = True
result.jump_condition = jump_condition if str(
jump_condition
) != 'same' else result.jump_condition
elif path.count_specific_node_num(
node.tag) >= MAX_LOOP_ITERATIONS - 1:
# LOG ERROR
err_result = Result()
err_message = 'Loop Error:[%s] %s' % (
tag, result.jump_condition)
err_result.log_error(settings.ADDRESS, err_message)
logging.error(err_message)
return
else:
path_cond = simplify(node.path_constraint) if is_expr(
node.path_constraint) else node.path_constraint
if path.count_specific_node_num(
node.tag) >= MAX_LOOP_ITERATIONS and is_expr(
path_cond):
for node in self.cfg.nodes:
if node.tag == tag:
node.loop_condition.append(
path.find_loop_condition(node))
return
# NOTE: if edge is not in edges -> add edge into edges
self.__add_edge(Edge(node.tag, result.jump_tag, 'red'))
self.__add_edge(Edge(node.tag, opcode.get_next_pc(), 'red'))
edge_true = self.cfg.get_edge(node.tag, result.jump_tag)
edge_false = self.cfg.get_edge(node.tag, opcode.get_next_pc())
if detect_loop:
edge_true.set_path_constraint(
self.to_string(simplify(result.jump_condition == 1)))
edge_false.set_path_constraint(
self.to_string(simplify(result.jump_condition == 0)))
if path.contain_node(result.jump_tag):
jump_condition_n1 = 0 if jump_condition_n1 is None else jump_condition_n1
path.add_path_constraints([
result.jump_condition == 1, jump_condition_n1 == 1
])
return self.symbolic_execution(opcode.get_next_pc(),
deepcopy(path),
deepcopy(state))
elif path.contain_node(opcode.get_next_pc()):
jump_condition_n1 = 1 if jump_condition_n1 is None else jump_condition_n1
path.add_path_constraints([
result.jump_condition == 0, jump_condition_n1 == 0
])
return self.symbolic_execution(result.jump_tag,
deepcopy(path),
deepcopy(state))
else:
# LOG ERROR
err_result = Result()
err_message = 'Loop Error:[%s] Both JUMPDEST tags have been executed' % tag
err_result.log_error(settings.ADDRESS, err_message)
raise ValueError(err_message)
else:
# NOTE: set gas to node
node.set_gas(gas)
node.set_state(deepcopy(state))
# NOTE: add tag to the path list
path.add_node(deepcopy(node))
# NOTE: Jump to two path
if isinstance(result.jump_condition,
int) and result.jump_condition == 1:
edge_true.set_path_constraint('True')
edge_false.set_path_constraint('False')
return self.symbolic_execution(result.jump_tag,
deepcopy(path),
deepcopy(state))
elif isinstance(result.jump_condition,
int) and result.jump_condition == 0:
edge_true.set_path_constraint('False')
edge_false.set_path_constraint('True')
return self.symbolic_execution(opcode.get_next_pc(),
deepcopy(path),
deepcopy(state))
elif isinstance(result.jump_condition, int):
return
else:
edge_true.set_path_constraint(
self.to_string(
simplify(result.jump_condition == 1)))
edge_false.set_path_constraint(
self.to_string(
simplify(result.jump_condition == 0)))
true_path, false_path = deepcopy(path), deepcopy(path)
true_state, false_state = deepcopy(state), deepcopy(
state)
true_path.add_path_constraints(
[result.jump_condition == 1])
false_path.add_path_constraints(
[result.jump_condition == 0])
self.symbolic_execution(result.jump_tag, true_path,
true_state)
self.symbolic_execution(opcode.get_next_pc(),
false_path, false_state)
return
elif opcode.name in [
'STOP', 'RETURN', 'REVERT', 'INVALID', 'SELFDESTRUCT'
]:
# NOTE: set gas to node
node.set_gas(gas)
node.set_state(deepcopy(state))
# NOTE: add tag to the path list
path.add_node(deepcopy(node))
# NOTE: simplify gas formula
path.gas = simplify(path.gas) if is_expr(path.gas) else int(
path.gas)
path.gas = int(path.gas.as_long()) if isinstance(
path.gas, BitVecNumRef) else path.gas
path.gas = int(path.gas) if isinstance(path.gas,
float) else path.gas
# NOTE: solve gas satisfiability & set gas type
if path.solve():
if isinstance(path.gas, int):
path.set_gas_type('constant')
elif 'loop' in str(path.gas) and path.solve_unbound():
settings.DETECT_LOOP = True
path.set_gas_type('unbound')
logging.debug('Detect loop')
else:
path.set_gas_type('bound')
self.paths.append(path)
self.count_path += 1
logging.debug('Finish one path...[%s]' % self.count_path)
return
"""
NOTE:
the end of the node is not in block ins -> jump to next node
"""
# NOTE: set gas to node
node.set_gas(gas)
# NOTE: add tag to the path list
path.add_node(deepcopy(node))
# NOTE: if edge is not in edges -> add edge into edges
self.__add_edge(Edge(node.tag, opcode.get_next_pc(), 'red'))
return self.symbolic_execution(opcode.get_next_pc(), path, state)
def main(use_imu=False):
"""Main program
"""
# Create config
config = Configuration()
hardware_interface = HardwareInterface()
# Create imu handle
if use_imu:
imu = IMU(port="/dev/ttyACM0")
imu.flush_buffer()
# Create controller and user input handles
controller = Controller(
config,
four_legs_inverse_kinematics,
)
state = State()
print("Creating joystick listener...")
eaa_interface = Eaainterface(config)
print("Done.")
last_loop = time.time()
print("Summary of gait parameters:")
print("overlap time: ", config.overlap_time)
print("swing time: ", config.swing_time)
print("z clearance: ", config.z_clearance)
print("x shift: ", config.x_shift)
# Wait until the activate button has been pressed
while True:
print("Waiting for L1 to activate robot.")
while True:
command = eaa_interface.get_command(state)
#brugbart?
joystick_interface.set_color(config.ps4_deactivated_color)
if command.activate_event == 1:
break
time.sleep(0.1)
print("Robot activated.")
#brugtbart?
joystick_interface.set_color(config.ps4_color)
while True:
now = time.time()
if now - last_loop < config.dt:
continue
last_loop = time.time()
# Parse the udp joystick commands and then update the robot controller's parameters
command = eaa_interface.get_command(state)
if command.activate_event == 1:
print("Deactivating Robot")
break
# Read imu data. Orientation will be None if no data was available
quat_orientation = (imu.read_orientation()
if use_imu else np.array([1, 0, 0, 0]))
state.quat_orientation = quat_orientation
# Step the controller forward by dt
controller.run(state, command)
# Update the pwm widths going to the servos
hardware_interface.set_actuator_postions(state.joint_angles)
def main(FLAGS):
"""Main program"""
data = Data()
# Create a robot joint state message of size 12
state_msg = JointState()
state_msg.name = [""] * 12
state_msg.position = [0] * 12
state_msg.velocity = [0] * 12
state_msg.effort = [0] * 12
pose_msg = Pose()
# Create pupper whisperer which will provide I/O comms through gazebo node
PupComm = whisperer()
# Set the names of the joints
for name, index in PupComm.joint_name_map.items():
state_msg.name[index] = name
# Define the message callback
def commandCallback(msg):
data.commands = list(msg.data)
# Create the subscriber and publisher
state_pub = rospy.Publisher("pupper_state", JointState, queue_size=1)
pose_pub = rospy.Publisher("pupper_pose", Pose, queue_size=1)
command_sub = rospy.Subscriber("pupper_commands",
Float64MultiArray,
commandCallback,
queue_size=1)
# Run at 1000 Hz
rate = rospy.Rate(1000)
# Create config
config = Configuration()
hardware_interface = HardwareInterface.HardwareInterface(port=SERIAL_PORT)
# Create controller and user input handles
controller = Controller(config, four_legs_inverse_kinematics)
state = State(height=config.default_z_ref)
print("Creating joystick listener...", end="")
joystick_interface = JoystickInterface(config)
print("Done.")
summarize_config(config)
if FLAGS.log:
#today_string = datetime.datetime.now().strftime("%Y%m%d-%H%M%S")
filename = os.path.join(DIRECTORY, FILE_DESCRIPTOR + ".csv")
log_file = open(filename, "w")
hardware_interface.write_logfile_header(log_file)
if FLAGS.zero:
# hardware_interface.set_joint_space_parameters(0, 4.0, 4.0)
hardware_interface.set_joint_space_parameters(
0, 4.0, 0.2) # mathew (changed max current to .2)
hardware_interface.set_actuator_postions(np.zeros((3, 4)))
input(
"Do you REALLY want to calibrate? Press enter to continue or ctrl-c to quit."
)
print("Zeroing motors...", end="")
hardware_interface.zero_motors()
hardware_interface.set_max_current_from_file()
print("Done.")
# The zero position is set with pupper laying down with elbows back.
# Follow this procedure: 1. Lay pupper flat
# 2. Rotate hip to raise leg
# 3. Rotate elbow to raise foot off ground
# 4. Rotate hip to lower leg until end of motor touches ground
# 5. Rotate elbow until foot touches ground (the motor should
# be rubbing against ground if done correctly)
# If done correctly, repeatability is < 1 degree
input("Press enter to ZERO MOTORS") # - mathew
hardware_interface.zero_motors()
print("Zeroing Done")
hardware_interface.set_max_current_from_file() # - mathew
# //////////////////////////// PD CONTROL ////////////////////////////////////////
input("Press enter to stand. MAKE SURE PUPPER IS LAYING DOWN ELBOWS BACK.")
print("Press q to start WBC")
hardware_interface.set_joint_space_parameters(0, 0, 7.0) #Set max current
PD_Kp = .6 # Gains on position error
PD_Kd = .1 # Gains on velocity error
PD_joint_pos = [0] * 12
PD_joint_vel = [0] * 12
PD_torque = [0] * 12
PD_des_pos = [
0, np.pi / 4, np.pi / 2, 0, -np.pi / 4, -np.pi / 2, 0, np.pi / 4,
np.pi / 2, 0, -np.pi / 4, -np.pi / 2
]
while state.activation == 0:
# print(time.time())
command = joystick_interface.get_command(state)
#Get joint states
PupComm.store_robot_states(hardware_interface.get_robot_states())
# Put into the correct form for us
for i, name in enumerate(PupComm.joint_name_map.keys()):
(joint_pos, joint_vel, _) = PupComm.get_joint_state(name)
PD_joint_pos[i] = joint_pos
PD_joint_vel[i] = joint_vel
# Calculate PD torque
for i in range(12):
if (i % 3 == 0):
PD_torque[i] = PD_Kp * (PD_des_pos[i] - PD_joint_pos[i]
) + PD_Kd * (-PD_joint_vel[i])
print("Joint Velocity ", i, ": ", PD_joint_vel[i])
print("Pos error ", i, ": ", (PD_des_pos[i] - PD_joint_pos[i]))
else:
PD_torque[i] = 0
print("PD torque[0]: {:+.3f}".format(PD_torque[0]),
" PD torque[3]: {:+.3f}".format(PD_torque[3]),
" PD torque[6]: {:+.3f}".format(PD_torque[6]),
" PD torque[9]: {:+.3f}".format(PD_torque[9]))
PD_torque_reordered = PupComm.reorder_torques(PD_torque)
hardware_interface.set_torque(PD_torque_reordered)
# Break loop when "q" is pressed
if command.activate_event == 1:
print("WBC STARTED. Press q to stop motors.")
break
# ///////////////////////////////////////////////////////////////////////////
# stand_position = np.array( [[ 0, 0, 0, 0],
# [np.pi/4,-np.pi/4,np.pi/4,-np.pi/4],
# [np.pi/2,-np.pi/2,np.pi/2,-np.pi/2]])
# hardware_interface.set_actuator_postions(stand_position)
# command = joystick_interface.get_command(state)
# controller.run(state, command)
# hardware_interface.set_max_current_from_file() # Uses HardwareConfig.py MAX_CURRENT
hardware_interface.set_joint_space_parameters(0, 0,
7.0) #Set max current #5.0
state.activation = 1 # Start automatically
try:
while not rospy.is_shutdown():
if state.activation == 0:
time.sleep(0.02)
joystick_interface.set_color(config.ps4_deactivated_color)
command = joystick_interface.get_command(state)
if command.activate_event == 1:
print("Robot activated.")
joystick_interface.set_color(config.ps4_color)
hardware_interface.serial_handle.reset_input_buffer()
hardware_interface.activate()
state.activation = 1
continue
elif state.activation == 1:
command = joystick_interface.get_command(state)
# controller.run(state, command)
#Printing states in function below
# if(True):
# # --------------------- Make sure we're reading info from pupper ---------------------------
# print("front right hip : {:+.5f}".format(PupComm.get_joint_state("front_right_hip")[0]), " "
# "front right shoulder : {:+.5f}".format(PupComm.get_joint_state("front_right_shoulder")[0]), " "
# "front right elbow : {:+.5f}".format(PupComm.get_joint_state("front_right_elbow")[0]))
# print("back left hip : {:+.5f}".format(PupComm.get_joint_state("back_left_hip")[0]), " "
# "back left shoulder : {:+.5f}".format(PupComm.get_joint_state("back_left_shoulder")[0]), " "
# "back left elbow : {:+.5f}".format(PupComm.get_joint_state("back_left_elbow")[0]))
# print("back right hip : {:+.5f}".format(PupComm.get_joint_state("back_right_hip")[0]), " "
# "back right shoulder : {:+.5f}".format(PupComm.get_joint_state("back_right_shoulder")[0]), " "
# "back right elbow : {:+.5f}".format(PupComm.get_joint_state("back_right_elbow")[0]))
# print("front left hip : {:+.5f}".format(PupComm.get_joint_state("front_left_hip")[0]), " "
# "front left shoulder : {:+.5f}".format(PupComm.get_joint_state("front_left_shoulder")[0]), " "
# "front left elbow : {:+.5f}".format(PupComm.get_joint_state("front_left_elbow")[0]))
# # ------------------------------------------- mathew
# dummy = 1
# Read data from pupper
PupComm.store_robot_states(
hardware_interface.get_robot_states())
# Put into the correct form
for i, name in enumerate(PupComm.joint_name_map.keys()):
(joint_pos, joint_vel,
joint_cur) = PupComm.get_joint_state(name)
state_msg.position[i] = joint_pos
state_msg.velocity[i] = joint_vel
state_msg.effort[i] = joint_cur
# Send the robot state to the C++ node
state_pub.publish(state_msg)
# Get Orientation
quaternion = PupComm.get_pupper_orientation()
pose_msg.position.x = 0 # Replace with function
pose_msg.position.y = 0
pose_msg.position.z = 0
pose_msg.orientation.w = quaternion[0] # Replace with function
pose_msg.orientation.x = quaternion[1]
pose_msg.orientation.y = quaternion[2]
pose_msg.orientation.z = quaternion[3]
# print("Quaternion: ", quaternion)
#Send Orientation to the C++ node
pose_pub.publish(pose_msg)
# Read torque command from ROS
WBC_commands_reordered = PupComm.reorder_torques(data.commands)
# -------------------------------------------
# -------- Send torques to pupper ----------
# -------------------------------------------
# WBC_commands_reordered is a list with order:
# [FR hip, FR shoulder, FR knee,
# FL hip, FL shoulder, FL knee,
# BR hip, BR shoulder, BR knee,
# BL hip, BL shoulder, BL knee]
# For testing, set desired torque:
# manual_torques = [0] * 12
# manual_torques[2] = 1 #make sure limits are obeyed
# WBC_commands_reordered = PupComm.reorder_torques(manual_torques)
# Scaling factors found in C610.cpp in Stanford's code
# print("looped")
for i in range(12):
# Convert torque to current
torque_cmd = WBC_commands_reordered[i]
vel = PupComm.robot_states_["vel"][i]
# if vel * torque_cmd <= 0:
WBC_commands_reordered[i] = 1 / 0.308 * (
torque_cmd + 0.0673 * np.sign(vel) + .00277 * vel)
# else:
# WBC_commands_reordered[i] = 1/0.179 * (torque_cmd + 0.0136 * np.sign(vel) + 0.00494 * vel)
# print("Motor ", i, " B")
# print("Curent ", i, ": {:+.3f}".format(WBC_commands_reordered[i]))
# if vel * torque_cmd > 0:
# print("B")
#Set_torque actually sets currents
hardware_interface.set_torque(
WBC_commands_reordered) # FR, FL, BR, BL
# -------------------------------------------
## Commented below -
# hardware_interface.set_cartesian_positions(
# state.final_foot_locations
# )
if FLAGS.log:
any_data = hardware_interface.log_incoming_data(log_file)
if any_data:
print(any_data["ts"])
# if command.activate_event == 1:
# print("Deactivating Robot")
# print("Waiting for L1 to activate robot.")
# hardware_interface.deactivate()
# state.activation = 0
# continue
# Sleep to maintain the desired frequency
rate.sleep()
# Break loop when "q" is pressed
if command.activate_event == 1:
hardware_interface.set_torque([0] * 12)
print("Stopping motors.")
break
except KeyboardInterrupt:
if FLAGS.log:
print("Closing log file")
log_file.close()
def main(use_imu=False):
"""Main program
"""
# Create config
config = Configuration()
training_interface = TrainingInterface()
# Create controller and user input handles
controller = Controller(
config,
four_legs_inverse_kinematics,
)
state = State()
# Behavior to learn
state.behavior_state = BehaviorState.TROT
print("Summary of gait parameters:")
print("overlap time: ", config.overlap_time)
print("swing time: ", config.swing_time)
print("z clearance: ", config.z_clearance)
print("x shift: ", config.x_shift)
amplitude = 0.0
amplitude_vel = 0.0
angle = 0.0
angle_vel = 0.0
yaw = 0.0
yaw_vel = 0.0
while True:
# Parse the udp joystick commands and then update the robot controller's parameters
command = Command()
amplitude_accel = np.random.randn() * 3.0
amplitude_vel += amplitude_accel * config.dt - 0.2 * amplitude_vel * config.dt
amplitude += amplitude_vel * config.dt
angle_accel = np.random.randn() * 3.0
angle_vel += angle_accel * config.dt - 0.2 * angle_vel * config.dt
angle += angle_vel * config.dt
yaw_accel = np.random.randn() * 3.0
yaw_vel += yaw_accel * config.dt - 0.2 * yaw_vel * config.dt
yaw += yaw_vel * config.dt
#print(str(amplitude) + " " + str(angle) + " " + str(yaw))
# Go forward at max speed
command.horizontal_velocity = np.array([
np.cos(angle) * np.sin(amplitude),
np.sin(angle) * np.sin(amplitude)
]) * 0.5
command.yaw_rate = np.sin(yaw) * 0.5
quat_orientation = (np.array([1, 0, 0, 0]))
state.quat_orientation = quat_orientation
# Step the controller forward by dt
controller.run(state, command)
training_interface.set_direction(
np.array([
command.horizontal_velocity[0], command.horizontal_velocity[1],
command.yaw_rate
]))
# Update the agent with the angles
training_interface.set_actuator_positions(state.joint_angles)
def main(use_imu=False):
"""Main program
"""
# Create config
config = Configuration()
hardware_interface = HardwareInterface()
# Create imu handle
if use_IMU:
imu = IMU(0x4A)
imu.begin()
time.sleep(0.5)
#Startup the IMU data reading thread
try:
_thread.start_new_thread( IMU_read, (use_IMU,imu,) )
except:
print ("Error: IMU thread could not startup!!!")
# Create controller and user input handles
controller = Controller(
config,
four_legs_inverse_kinematics,
)
state = State()
print("Creating joystick listener...")
joystick_interface = JoystickInterface(config)
print("Done.")
last_loop = time.time()
print("Summary of gait parameters:")
print("overlap time: ", config.overlap_time)
print("swing time: ", config.swing_time)
print("z clearance: ", config.z_clearance)
print("x shift: ", config.x_shift)
# Wait until the activate button has been pressed
while True:
print("Waiting for L1 to activate robot.")
while True:
command = joystick_interface.get_command(state)
joystick_interface.set_color(config.ps4_deactivated_color)
if command.activate_event == 1:
break
time.sleep(0.1)
print("Robot activated.")
joystick_interface.set_color(config.ps4_color)
while True:
now = time.time()
if now - last_loop < config.dt:
continue
last_loop = time.time()
# Parse the udp joystick commands and then update the robot controller's parameters
command = joystick_interface.get_command(state)
if command.activate_event == 1:
print("Deactivating Robot")
break
# Read imu data. Orientation will be None if no data was available
state.quat_orientation = quat_orientation
print(state.quat_orientation)
# Step the controller forward by dt
controller.run(state, command)
# Update the pwm widths going to the servos
hardware_interface.set_actuator_postions(state.joint_angles)