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