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 update_controller(controller, user_input_obj):
controller.movement_reference.v_xy_ref = np.array(
[user_input_obj.x_vel, user_input_obj.y_vel]
)
controller.movement_reference.wz_ref = user_input_obj.yaw_rate
message_dt = 1.0 / user_input_obj.message_rate
# TODO: Put this filter code somewhere else
deadbanded_pitch = deadband(user_input_obj.pitch, controller.stance_params.pitch_deadband)
pitch_rate = clipped_first_order_filter(controller.movement_reference.pitch, deadbanded_pitch, controller.stance_params.max_pitch_rate, controller.stance_params.pitch_time_constant)
controller.movement_reference.pitch += message_dt * pitch_rate
controller.state = user_input_obj.current_state
# Note this is negative since it is the feet relative to the body
controller.movement_reference.z_ref -= (
controller.stance_params.z_speed * message_dt * user_input_obj.stance_movement
)
controller.movement_reference.roll += (
controller.stance_params.roll_speed * message_dt * user_input_obj.roll_movement
)
def parse_command(old_com, state, latest_data, config):
global remote_ctl_flag, previous_gait_toggle
command = old_com
lx = 0
ly = 0
rx = 0
ry = 0
dpadx = 0
dpady = 0
trot = 0
for entry in latest_data.split(b'@'):
raw_pair = entry.split(b'#')
if len(raw_pair) < 2: continue
key = raw_pair[0]
try:
value = float(raw_pair[1])
except:
print('Invalid data received. Skipping...')
print(entry)
continue
if value != 0:
remote_ctl_flag = True
if key == b'rx':
rx = value
elif key == b'ry':
ry = value
elif key == b'lx':
lx = value
elif key == b'ly':
ly = value
elif key == b'dpadx':
dpadx = value
elif key == b'dpady':
dpady = value
elif key == b'trot':
trot = value
####### Handle continuous commands ########
x_vel = ly * config.max_x_velocity # for back
# print(x_vel)
y_vel = lx * -config.max_y_velocity # left right
command.horizontal_velocity = np.array([x_vel, y_vel])
command.yaw_rate = rx * -config.max_yaw_rate # twist and shout
message_rate = 50
message_dt = 1.0 / message_rate
command.trot_event = (trot == 1 and previous_gait_toggle == 0)
previous_gait_toggle = trot
pitch = ry * config.max_pitch
deadbanded_pitch = deadband(pitch, config.pitch_deadband)
pitch_rate = clipped_first_order_filter(
state.pitch,
deadbanded_pitch,
config.max_pitch_rate,
config.pitch_time_constant,
)
command.pitch = state.pitch + message_dt * pitch_rate
height_movement = dpady
command.height = state.height - message_dt * config.z_speed * height_movement
roll_movement = -dpadx
command.roll = state.roll + message_dt * config.roll_speed * roll_movement
return command
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()
