def get_command(self, state, do_print=False):
try:
msg = self.udp_handle.get()
#Gotwitty
print("JoystickInterface.get_command msg = self.udp_handle.get()")
print(msg)
command = Command()
####### Handle discrete commands ########
# Check if requesting a state transition to trotting, or from trotting to resting
gait_toggle = msg["R1"]
command.trot_event = (gait_toggle == 1
and self.previous_gait_toggle == 0)
# Check if requesting a state transition to hopping, from trotting or resting
hop_toggle = msg["x"]
command.hop_event = (hop_toggle == 1
and self.previous_hop_toggle == 0)
activate_toggle = msg["L1"]
command.activate_event = (activate_toggle == 1
and self.previous_activate_toggle == 0)
# Update previous values for toggles and state
self.previous_gait_toggle = gait_toggle
self.previous_hop_toggle = hop_toggle
self.previous_activate_toggle = activate_toggle
####### Handle continuous commands ########
x_vel = msg["ly"] * self.config.max_x_velocity
y_vel = msg["lx"] * -self.config.max_y_velocity
command.horizontal_velocity = np.array([x_vel, y_vel])
command.yaw_rate = msg["rx"] * -self.config.max_yaw_rate
message_rate = msg["message_rate"]
message_dt = 1.0 / message_rate
pitch = msg["ry"] * self.config.max_pitch
deadbanded_pitch = deadband(pitch, self.config.pitch_deadband)
pitch_rate = clipped_first_order_filter(
state.pitch,
deadbanded_pitch,
self.config.max_pitch_rate,
self.config.pitch_time_constant,
)
command.pitch = state.pitch + message_dt * pitch_rate
height_movement = msg["dpady"]
command.height = state.height - message_dt * self.config.z_speed * height_movement
roll_movement = -msg["dpadx"]
command.roll = state.roll + message_dt * self.config.roll_speed * roll_movement
return command
except UDPComms.timeout:
if do_print:
print("UDP Timed out")
return Command()
def get_command_from_pipe(self, state, do_print=False):
"""
Returns a command object created based on the lastest message sendt from an ActionLoop queue.
"""
msg = self.pipe.recv()
command = Command()
####### Handle discrete commands ########
# Check if requesting a state transition to trotting, or from trotting to resting
gait_toggle = msg["trot"]
command.trot_event = (gait_toggle == 1 and self.previous_gait_toggle == 0)
# Check if requesting a state transition to hopping, from trotting or resting
hop_toggle = msg["hop"]
command.hop_event = (hop_toggle == 1 and self.previous_hop_toggle == 0)
activate_toggle = msg["activation"]
command.activate_event = (activate_toggle == 1 and self.previous_activate_toggle == 0)
# Update previous values for toggles and state
self.previous_gait_toggle = gait_toggle
self.previous_hop_toggle = hop_toggle
self.previous_activate_toggle = activate_toggle
####### Handle continuous commands ########
## CreateLab Comment: It looks wierd that x_vel uses y_axis_velocity, i don't know why.
# I have checked if it is the same in Stanfords Software, and it is.
x_vel = msg["y_axis_velocity"] * self.config.max_x_velocity
y_vel = msg["x_axis_velocity"] * -self.config.max_y_velocity
command.horizontal_velocity = np.array([x_vel, y_vel])
command.yaw_rate = msg["yaw"] * -self.config.max_yaw_rate
message_rate = msg["message_rate"]
message_dt = 1.0 / message_rate
pitch = msg["pitch"] * self.config.max_pitch
deadbanded_pitch = deadband(
pitch, self.config.pitch_deadband
)
pitch_rate = clipped_first_order_filter(
state.pitch,
deadbanded_pitch,
self.config.max_pitch_rate,
self.config.pitch_time_constant,
)
command.pitch = state.pitch + message_dt * pitch_rate
height_movement = msg["height"]
command.height = state.height - message_dt * self.config.z_speed * height_movement
roll_movement = - msg["roll"]
command.roll = state.roll + message_dt * self.config.roll_speed * roll_movement
return command
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(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 get_command(self, state, do_print=False):
try:
#msg = self.udp_handle.get()
command = Command()
####### Handle discrete commands ########
# Check if requesting a state transition to trotting, or from trotting to resting
gait_toggle = self.joy_state.btn1
command.trot_event = (gait_toggle == 1
and self.previous_gait_toggle == 0)
# Check if requesting a state transition to hopping, from trotting or resting
hop_toggle = self.joy_state.btn2
command.hop_event = (hop_toggle == 1
and self.previous_hop_toggle == 0)
activate_toggle = self.joy_state.btn3
command.activate_event = (activate_toggle == 1
and self.previous_activate_toggle == 0)
# Update previous values for toggles and state
self.previous_gait_toggle = gait_toggle
self.previous_hop_toggle = hop_toggle
self.previous_activate_toggle = activate_toggle
####### Handle continuous commands ########
x_vel = np.clip(self.twist.linear.x, -self.config.max_x_velocity,
self.config.max_x_velocity)
y_vel = np.clip(
self.twist.linear.y, -self.config.max_y_velocity,
self.config.max_y_velocity) #check for "-" if needed
command.horizontal_velocity = np.array([x_vel, y_vel])
command.yaw_rate = np.clip(self.twist.angular.z,
-self.config.max_yaw_rate,
self.config.max_yaw_rate)
#message_rate = msg["message_rate"]
message_dt = 1.0 / self.message_rate
pitch = self.joy_state.right_x / 512.0 * self.config.max_pitch
deadbanded_pitch = deadband(pitch, self.config.pitch_deadband)
pitch_rate = clipped_first_order_filter(
state.pitch,
deadbanded_pitch,
self.config.max_pitch_rate,
self.config.pitch_time_constant,
)
#command.pitch = state.pitch + message_dt * pitch_rate
command.pitch = 0
command.height = state.height
command.roll = state.roll
"""
height_movement = msg["dpady"]
command.height = state.height - message_dt * self.config.z_speed * height_movement
roll_movement = - msg["dpadx"]
command.roll = state.roll + message_dt * self.config.roll_speed * roll_movement
"""
rospy.loginfo(command.horizontal_velocity)
return command
except rospy.is_shutdown():
print("ros is down")
return Command()