def __init__(self):
pygame.init()
self.background_color = (200, 200, 200)
self.main_screen = Screen(background_color=self.background_color)
self.controller = Controller()
self.player = Player(pygame.Vector2(0, 0))
self.clock = pygame.time.Clock()
self.running = False
self.action = ""
self.value = 0
self.level_one = LevelOne(30, self.background_color)
self.player.set_init_pos(self.level_one.player_init)
self.player.set_current_level(self.level_one)
enemy_mov_period = 1.5
self.enemies = [
Enemy(self.level_one.enemies_init[i],
self.level_one.enemies_init_direct[i], 'h', enemy_mov_period,
self.level_one.enemies_trajectory[i][0],
self.level_one.enemies_trajectory[i][1])
for i in range(len(self.level_one.enemies_init))
]
self.score = Score()
pygame.mixer.music.load("SoundTrack/hardestGameThemeSong.mp3")
pygame.mixer.music.play(-1)
pygame.mixer.music.set_volume(0.5)
def main():
"""Main program
"""
start_pigpiod()
pi_board = pigpio.pi()
pwm_params = PWMParams()
initialize_pwm(pi_board, pwm_params)
robot_config = PupperConfig()
servo_params = ServoParams()
controller = Controller(robot_config)
user_input = UserInputs()
last_loop = time.time()
while (True):
if time.time() - last_loop < controller.gait_params.dt:
continue
last_loop = time.time()
# Parse the udp joystick commands and then update the robot controller's parameters
get_input(user_input)
update_controller(controller, user_input)
# Step the controller forward by dt
step_controller(controller, robot_config)
# Update the pwm widths going to the servos
send_servo_commands(pi_board, pwm_params, servo_params,
controller.joint_angles)
def main():
""" Main. """
cf = ConfigHandler()
cs = cf.create_config_settings('../config/config.json')
c = Controller()
f = c.get_dir_file()
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 main():
try:
controller = Controller()
controller.mainView()
except:
messagebox.showerror(
title="Error",
message=
"The program has unexpectedly crashed. Please contact the developers"
)
def main():
"""Main program
"""
pi_board = pigpio.pi()
pwm_params = PWMParams()
servo_params = ServoParams()
controller = Controller()
controller.movement_reference = MovementReference()
controller.movement_reference.v_xy_ref = np.array([0.0, 0.0])
controller.movement_reference.wz_ref = 0
controller.movement_reference.pitch = 15.0 * np.pi / 180.0
controller.movement_reference.roll = 0
controller.swing_params = SwingParams()
controller.swing_params.z_clearance = 0.06
controller.stance_params = StanceParams()
controller.stance_params.delta_y = 0.08
controller.gait_params = GaitParams()
controller.gait_params.dt = 0.01
initialize_pwm(pi_board, pwm_params)
values = UDPComms.Subscriber(8830, timeout=0.3)
last_loop = time.time()
now = last_loop
start = time.time()
for i in range(60000):
last_loop = time.time()
step_controller(controller)
send_servo_commands(pi_board, pwm_params, servo_params,
controller.joint_angles)
try:
msg = values.get()
print(msg)
except UDPComms.timeout:
print("timout")
msg = {"x": 0, "y": 0, "twist": 0, "pitch": 0}
x_vel = msg["y"] / 7.0
y_vel = -msg["x"] / 7.0
yaw_rate = -msg["twist"] * 0.8
pitch = msg["pitch"] * 30.0 * np.pi / 180.0
print(pitch)
controller.movement_reference.v_xy_ref = np.array([x_vel, y_vel])
controller.movement_reference.wz_ref = yaw_rate
controller.movement_reference.pitch = pitch
while now - last_loop < controller.gait_params.dt:
now = time.time()
# print("Time since last loop: ", now - last_loop)
end = time.time()
print("seconds per loop: ", (end - start) / 1000.0)
def __init__(self, initial_pos: pygame.Vector2, controller=Controller()):
self.position = initial_pos
self.controller = controller
self.width = 20
self.height = 20
self.velocity = pygame.Vector2(100, 100)
self.color = (255, random.randint(0, 255), 0)
self.current_level = None
self.dead = False
self.poison = 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__(self, screen_size, roster, flags):
self.screen_size = screen_size
self.flags = flags
self.controller = Controller()
# build team roster
self.team = []
for hero in roster:
self.add_hero(hero_data=hero)
self.player = self.team[0]
self.center_box = pygame.Rect(self.screen_size[0] / 4,
self.screen_size[1] / 4,
self.screen_size[0] / 2,
self.screen_size[1] / 2)
self.cam_center = (self.screen_size[0] / 2, self.screen_size[1] / 2)
self.cam_offset_x = 0
self.cam_offset_y = 0
self.map = None
self.menu = None
self.show_menu = False
self.notification = None
self.destination_door = None
self.fade_alpha = 0
self.fade_speed = 5
self.fade_out = False
self.fade_in = False
self.text_box = self.build_text_box()
self.blackness = pygame.Surface(self.screen_size)
self.blackness.fill((0, 0, 0))
self.blackness.set_alpha(self.fade_alpha)
# battle
self.in_battle = False
self.battle = None
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()
from src.Controller import Controller
if __name__ == '__main__':
c = Controller()
c.run()
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)
from src.Controller import Controller
from src.Repository import Repository
from src.View import View
temperature_filename = "../data/temperature.dat"
humidity_filename = "../data/humidity.dat"
time_filename = "../data/time.dat"
table_filename = "../data/table.dat"
if __name__ == '__main__':
temperature_repository = Repository(temperature_filename)
controller = Controller(Repository(temperature_filename),
Repository(humidity_filename),
Repository(time_filename), table_filename)
view = View(controller)
view.run()
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(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 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)
from src.Controller import Controller
from src.Model import Model
from src.configuration.configParser import config_parser_result, save_type_key
if __name__ == "__main__":
result_dict = config_parser_result()
model = Model('savedata', result_dict[save_type_key])
controller = Controller(model)
controller.start()
import matplotlib
from src.Controller import Controller
from src.Repository import Repository
import matplotlib.pyplot as plt
if __name__ == '__main__':
# repo = Repository("../data/2c.dat")
repo = Repository("../data/3c.dat")
ctrl = Controller(repo)
total_accuracy = []
total_runs = 1000
total_sets = 0.15
for run in range(total_runs):
accuracy = ctrl.run(total_sets)
total_accuracy.append(accuracy)
print("Accuracy for run {0} = {1}".format(run, accuracy))
print("Accuracy for {0} runs with {1} sets is: {2}".format(total_runs, total_sets, sum(total_accuracy)/total_runs))
plt.plot(range(total_runs), total_accuracy)
plt.show()
def main():
controller = Controller()
controller.mainView()
def main():
"""Main program
"""
# Start pwm to servos
# start_pigpiod()
pi_board = pigpio.pi()
pwm_params = PWMParams()
initialize_pwm(pi_board, pwm_params)
# Create config
robot_config = PupperConfig()
servo_params = ServoParams()
# Create imu handle
imu = IMU(port="/dev/ttyACM0")
imu.flush_buffer()
# Create controller and user input handles
controller = Controller(robot_config)
input_params = UserInputParams()
user_input = UserInputs(max_x_velocity=input_params.max_x_velocity,
max_y_velocity=input_params.max_y_velocity,
max_yaw_rate=input_params.max_yaw_rate,
max_pitch=input_params.max_pitch)
last_loop = time.time()
print("Summary of gait parameters:")
print("overlap time: ", controller.gait_params.overlap_time)
print("swing time: ", controller.gait_params.swing_time)
print("z clearance: ", controller.swing_params.z_clearance)
print("x shift: ", controller.stance_params.x_shift)
# Wait until the activate button has been pressed
while True:
print("Waiting for L1 to activate robot.")
while True:
get_input(user_input)
if user_input.activate == 1 and user_input.last_activate == 0:
user_input.last_activate = 1
break
user_input.last_activate = user_input.activate
print("Robot activated.")
while True:
now = time.time()
if now - last_loop < controller.gait_params.dt:
continue
last_loop = time.time()
# Parse the udp joystick commands and then update the robot controller's parameters
get_input(user_input)
if user_input.activate == 1 and user_input.last_activate == 0:
user_input.last_activate = 1
break
else:
user_input.last_activate = user_input.activate
update_controller(controller, user_input)
# Read imu data. Orientation will be None if no data was available
quat_orientation = imu.read_orientation()
# Step the controller forward by dt
step_controller(controller, robot_config, quat_orientation)
# Update the pwm widths going to the servos
send_servo_commands(pi_board, pwm_params, servo_params,
controller.joint_angles)
deactivate_servos(pi_board, pwm_params)
from src.Controller import Controller
import matplotlib.pyplot as plt
if __name__ == '__main__':
chromosome_number = 200
gen_number = 30
ctrl = Controller(chromosome_number, gen_number, "../data/data_100.csv")
ctrl.run()
plt.plot(range(0, gen_number), ctrl.averages)
plt.axis([0, gen_number, min(ctrl.averages), max(ctrl.averages)])
plt.show()
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)
return temp
# Create environment objects
PUPPER_CONFIG = PupperConfig()
ENVIRONMENT_CONFIG = EnvironmentConfig()
SOLVER_CONFIG = SolverConfig()
# Initailize MuJoCo
PupperXMLParser.Parse(PUPPER_CONFIG, ENVIRONMENT_CONFIG, SOLVER_CONFIG)
model = load_model_from_path("src/pupper_out.xml")
sim = MjSim(model)
viewer = MjViewer(sim)
# Create pupper_controller
pupper_controller = Controller()
pupper_controller.movement_reference.v_xy_ref = np.array([0.2, 0.0])
pupper_controller.movement_reference.wz_ref = 0.0
pupper_controller.swing_params.z_clearance = 0.03
pupper_controller.gait_params.dt = 0.005
pupper_controller.stance_params.delta_y = 0.12
# Initialize joint angles
sim.data.qpos[7:] = parallel_to_serial_joint_angles(
pupper_controller.joint_angles).T.reshape(12)
# Set the robot to be above the floor to begin with
sim.data.qpos[2] = 0.5
# Initialize pwm and servo params
pwm_params = PWMParams()
servo_params = ServoParams()
# Create environment objects
PUPPER_CONFIG = PupperConfig()
PUPPER_CONFIG.XML_IN = "pupper_pybullet.xml"
PUPPER_CONFIG.XML_OUT = "pupper_pybullet_out.xml"
ENVIRONMENT_CONFIG = EnvironmentConfig()
SOLVER_CONFIG = SolverConfig()
# Initailize MuJoCo
PupperXMLParser.Parse(PUPPER_CONFIG, ENVIRONMENT_CONFIG, SOLVER_CONFIG)
# Create controller
robot_config = PupperConfig()
servo_params = ServoParams()
controller = Controller(robot_config)
user_input = UserInputs()
# Run the simulation
timesteps = 240*60*10 # simulate for a max of 10 minutes
# Sim seconds per sim step
sim_steps_per_sim_second = 240
sim_seconds_per_sim_step = 1.0 / sim_steps_per_sim_second
start = time.time()
last_control_update = 0
controller.gait_params.contact_phases = np.array(
[[1, 1, 1, 0], [1, 0, 1, 1], [1, 0, 1, 1], [1, 1, 1, 0]]
)
def main():
Controller()
# import flask
from flask import Flask
from flask import request
from flask_cors import CORS
# import controller
from src.Controller import Controller
# create app
app = Flask(__name__)
CORS(app)
# create controller
controller = Controller()
# *** server-client-interface ***
@app.route("/stock/pull", methods=['GET'])
def getstock():
return controller.stock_pullStock(request)
@app.route("/stock/get-providers", methods=["GET"])
def getproviders():
return controller.stock_getProviders(request)
@app.route("/search/queries/pull", methods=["GET"])
def getqueries():
return controller.search_pullQueries(request)
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():
#Create an instance of the controller object
test = Controller()
test.mainloop()