def _move(self, left_x, left_y, right_x):
"""Commands the robot with a velocity command based on left/right stick values.
Args:
left_x: X value of left stick.
left_y: Y value of left stick.
right_x: X value of right stick.
"""
# Stick left_x controls robot v_y
v_y = -left_x * VELOCITY_BASE_SPEED
# Stick left_y controls robot v_x
v_x = left_y * VELOCITY_BASE_SPEED
# Stick right_x controls robot v_rot
v_rot = -right_x * VELOCITY_BASE_ANGULAR
# Recreate mobility_params with the latest information
self.mobility_params = RobotCommandBuilder.mobility_params(
body_height=self.body_height,
locomotion_hint=self.mobility_params.locomotion_hint,
stair_hint=self.mobility_params.stair_hint)
cmd = RobotCommandBuilder.synchro_velocity_command(
v_x=v_x, v_y=v_y, v_rot=v_rot, params=self.mobility_params)
self._issue_robot_command(cmd,
endtime=time.time() + VELOCITY_CMD_DURATION)
def _velocity_cmd_helper(self, desc='', v_x=0.0, v_y=0.0, v_rot=0.0):
self._start_robot_command(
desc,
RobotCommandBuilder.synchro_velocity_command(v_x=v_x,
v_y=v_y,
v_rot=v_rot),
end_time_secs=time.time() + VELOCITY_CMD_DURATION)
def __execute_velocity(self, description, v_x=0.0, v_y=0.0, v_rot=0.0):
self.__execute_command(
description,
RobotCommandBuilder.synchro_velocity_command(v_x=v_x,
v_y=v_y,
v_rot=v_rot),
time.time() + self.__VELOCITY_CMD_DURATION)
def test_synchro_velocity_command():
v_x = 1.0
v_y = 2.0
v_rot = 3.0
command = RobotCommandBuilder.synchro_velocity_command(v_x, v_y, v_rot)
_test_has_synchronized(command)
_test_has_mobility(command.synchronized_command)
assert command.synchronized_command.mobility_command.HasField("se2_velocity_request")
vel_cmd = command.synchronized_command.mobility_command.se2_velocity_request
assert vel_cmd.velocity.linear.x == v_x
assert vel_cmd.velocity.linear.y == v_y
assert vel_cmd.velocity.angular == v_rot
assert vel_cmd.se2_frame_name == BODY_FRAME_NAME
# with a build_on_command
arm_command = RobotCommandBuilder.arm_stow_command()
command = RobotCommandBuilder.synchro_velocity_command(v_x, v_y, v_rot,
build_on_command=arm_command)
_test_has_synchronized(command)
_test_has_mobility(command.synchronized_command)
_test_has_arm(command.synchronized_command)
def velocity_cmd(self, v_x, v_y, v_rot, cmd_duration=0.1):
"""Send a velocity motion command to the robot.
Args:
v_x: Velocity in the X direction in meters
v_y: Velocity in the Y direction in meters
v_rot: Angular velocity around the Z axis in radians
cmd_duration: (optional) Time-to-live for the command in seconds. Default is 125ms (assuming 10Hz command rate).
"""
end_time = time.time() + cmd_duration
self._robot_command(RobotCommandBuilder.synchro_velocity_command(
v_x=v_x, v_y=v_y, v_rot=v_rot, params=self._mobility_params),
end_time_secs=end_time)
self._last_motion_command_time = end_time
