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