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(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(use_imu=False, default_velocity=np.zeros(2), default_yaw_rate=0.0, lock_frame_rate=True):
device = evdev.InputDevice(list_devices()[0])
print(device)
# Create config
sim = Sim()
hardware_interface = HardwareInterface(sim.model, sim.joint_indices)
# Create imu handle
if use_imu:
imu = IMU()
# Load hierarchy
pyaon.setNumThreads(4)
# lds = []
# for i in range(5):
# ld = pyaon.LayerDesc()
# ld.hiddenSize = Int3(4, 4, 16)
# ld.ffRadius = 4
# ld.pRadius = 4
# ld.aRadius = 4
# ld.ticksPerUpdate = 2
# ld.temporalHorizon = 2
# lds.append(ld)
# input_sizes = [ Int3(4, 3, ANGLE_RESOLUTION) ]
# input_types = [ pyaon.inputTypeAction ]
# input_sizes.append(Int3(3, 2, IMU_RESOLUTION))
# input_types.append(pyaon.inputTypeNone)
#h = pyaon.Hierarchy(cs, input_sizes, input_types, lds)
h = pyaon.Hierarchy("pupper.ohr")
#h = pyaon.Hierarchy("pupper_rltrained.ohr")
angles = 12 * [ 0.0 ]
# Sim seconds per sim step
sim_steps_per_sim_second = 240
sim_dt = 1.0 / sim_steps_per_sim_second
start_sim_time = time.time()
sim_time_elapsed = 0.0
reward = 0.0
control_reward_accum = 0.0
control_reward_accum_steps = 0
vels = ( [ 0, 0, 0 ], [ 0, 0, 0 ] )
steps = 0
actions = list(h.getPredictionCs(0))
# Create config
config = Configuration()
config.z_clearance = 0.05
# Create controller and user input handles
controller = Controller(
config,
four_legs_inverse_kinematics,
)
state = State()
state.behavior_state = BehaviorState.TROT
octaves = 3
smooth_chain = octaves * [ np.array([ 0.0, 0.0, 0.0 ]) ]
smooth_factor = 0.005
smooth_scale = 9.0
max_speed = 0.5
max_yaw_rate = 1.5
offsets = np.array([ -0.12295051, 0.12295051, -0.12295051, 0.12295051, 0.77062617, 0.77062617,
0.77062617, 0.77062617, -0.845151, -0.845151, -0.845151, -0.845151 ])
while True:
start_step_time = time.time()
sim_time_elapsed += sim_dt
if sim_time_elapsed > config.dt:
try:
for event in device.read():
if event.type == ecodes.EV_ABS:
if event.code == ecodes.ABS_X:
ls[0] = event.value / 32767.0
elif event.code == ecodes.ABS_Y:
ls[1] = event.value / 32767.0
elif event.code == ecodes.ABS_RX:
rs[0] = event.value / 32767.0
elif event.code == ecodes.ABS_RY:
rs[1] = event.value / 32767.0
except:
pass
sim_time_elapsed = sim_time_elapsed % config.dt
imu_vals = list(vels[0]) + list(vels[1])
imu_SDR = []
for i in range(len(imu_vals)):
imu_SDR.append(IMU_RESOLUTION // 2)#int((np.tanh(imu_vals[i] * IMU_SQUASH_SCALE) * 0.5 + 0.5) *(IMU_RESOLUTION - 1) + 0.5))
#direction = smoothed_result
#direction = np.array([ 0.75, 0.0, 0.0 ])
direction = np.array([ -ls[1], -ls[0], -rs[0] ])
sim.set_direction(np.array([ direction[0] * max_speed, direction[1] * max_speed, direction[2] * max_yaw_rate ]))
command_SDR = [ int((direction[i] * 0.5 + 0.5) * (COMMAND_RESOLUTION - 1) + 0.5) for i in range(3) ]
h.step([ actions, command_SDR, imu_SDR ], False, control_reward_accum / max(1, control_reward_accum_steps))
actions = list(h.getPredictionCs(0))
#actions = mutate(np.array(actions), 0.01, h.getInputSize(0).z).tolist()
control_reward_accum = 0.0
control_reward_accum_steps = 0
joint_angles = np.zeros((3, 4))
motor_index = 0
for segment_index in range(3):
for leg_index in range(4):
target_angle = (actions[motor_index] / float(ANGLE_RESOLUTION - 1) * 2.0 - 1.0) * (0.25 * np.pi) + offsets[motor_index]
delta = 0.3 * (target_angle - angles[motor_index])
max_delta = 0.05
if abs(delta) > max_delta:
delta = max_delta if delta > 0.0 else -max_delta
angles[motor_index] += delta
joint_angles[segment_index, leg_index] = angles[motor_index]
motor_index += 1
command = Command()
# Go forward at max speed
command.horizontal_velocity = direction[0 : 2] * max_speed
command.yaw_rate = direction[2] * max_yaw_rate
quat_orientation = (
np.array([1, 0, 0, 0])
)
state.quat_orientation = quat_orientation
# Step the controller forward by dt
controller.run(state, command)
#joint_angles = copy(state.joint_angles)
# Update the pwm widths going to the servos
hardware_interface.set_actuator_postions(joint_angles)
# Simulate physics for 1/240 seconds (the default timestep)
reward, vels = sim.step()
control_reward_accum += reward
control_reward_accum_steps += 1
if steps % 50000 == 49999:
print("Saving...")
h.save("pupper_rltrained.ohr")
steps += 1
# Performance testing
step_elapsed = time.time() - start_step_time
# Keep framerate
if lock_frame_rate:
time.sleep(max(0, sim_dt - step_elapsed))
pygame.quit()
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)
import numpy as np
import time
from src.IMU import IMU
from src.Controller import Controller
from JoystickInterface import JoystickInterface
from src.State import State
from pupper.HardwareInterface import HardwareInterface
from pupper.Config import Configuration
from pupper.Kinematics import four_legs_inverse_kinematics
import math
# from calibrate import app
# Create config
config = Configuration()
hardware_interface = HardwareInterface()
def main(use_imu=False):
"""Main program
"""
# 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,
)
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 __init__(self):
'''Constructor'''
# Create speed, body rate, and state command data variables
self.x_speed_cmd_mps = 0
self.y_speed_cmd_mps = 0
self.yaw_rate_cmd_rps = 0
self.trot_event_cmd = False
self.prev_trot_event_cmd = False
# Create and publish servo config message
# Initialize servo_config_msg
self._servo_config_msg = ServoConfigArray()
for s in servo_dict.values():
temp_servo = ServoConfig()
temp_servo.center = s['center']
temp_servo.range = s['range']
temp_servo.servo = s['num']
temp_servo.direction = s['direction']
# Append servo to servo config message
self._servo_config_msg.servos.append(temp_servo)
# Publish servo configuration
# rospy.loginfo("> Waiting for config_servos service...")
# rospy.wait_for_service('config_servos')
# rospy.loginfo("> Config_servos service found!")
# try:
# servoConfigService = rospy.ServiceProxy('config_servos',ServosConfig)
# resp = servoConfigService(self._servo_config_msg.servos)
# print("Config servos done!!, returned value: %i"%resp.error)
# except rospy.ServiceException, e:
# print "Service call failed: %s"%e
# Set up and initialize ros node
# rospy.loginfo("Setting Up the Spot Micro Simple Command Node...")
# Set up and title the ros node for this code
# rospy.init_node('spot_micro_walk')
# Create a servo command dictionary in the same order as angles are received back from
# SpotMicroStickFigure.get_leg_angles
self.servo_cmds_rad = {'RB_1':0,'RB_2':0,'RB_3':0,
'RF_1':0,'RF_2':0,'RF_3':0,
'LF_1':0,'LF_2':0,'LF_3':0,
'LB_1':0,'LB_2':0,'LB_3':0}
# Create empty ServoArray message with n number of Servos in its array
self._servo_msg = ServoArray()
for _ in range(len(servo_dict)):
self._servo_msg.servos.append(Servo())
# Create the servo array publisher
# self.ros_pub_servo_array = rospy.Publisher("/servos_proportional", ServoArray, queue_size=1)
# rospy.loginfo("> Publisher corrrectly initialized")
# Create subsribers for speed and body rate command topics, both using vector3
# rospy.Subscriber('x_speed_cmd',Float32,self.update_x_speed_cmd)
# rospy.Subscriber('/y_speed_cmd',Float32,self.update_y_speed_cmd)
# rospy.Subscriber('/yaw_rate_cmd',Float32,self.update_yaw_rate_cmd)
# rospy.Subscriber('/state_cmd',Bool,self.update_state_cmd)
# rospy.loginfo("Initialization complete")
# Create a spot micro stick figure object to encapsulate robot state
self.default_height = 0.18
self.sm = SpotMicroStickFigure(y=self.default_height)
# Set absolute foot positions for default stance,
# foot order: RB, RF, LF, LB
l = self.sm.body_length
w = self.sm.body_width
l1 = self.sm.hip_length
self.default_sm_foot_position = np.array([ [-l/2, 0, w/2 + l1],
[ l/2 , 0, w/2 + l1],
[ l/2 , 0, -w/2 - l1],
[-l/2 , 0, -w/2 - l1] ])
self.sm.set_absolute_foot_coordinates(self.default_sm_foot_position)
# Create configuration object and update values to reflect spot micro configuration
self.config = Configuration()
self.config.delta_x = l/2
self.config.delta_y = w/2 + l1
self.default_z_ref = -self.default_height
# Create controller object
self.controller = Controller(self.config)
# Create state object
self.state = State()
self.state.foot_locations = (self.config.default_stance + np.array([0,0,-self.default_height])[:, np.newaxis])
# Create Command object
self.command = Command()
self.command.height = -self.default_height
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)