def test_synchro_sit_command():
command = RobotCommandBuilder.synchro_sit_command()
_test_has_synchronized(command)
_test_has_mobility(command.synchronized_command)
assert command.synchronized_command.mobility_command.HasField("sit_request")
# with a build_on_command
arm_command = RobotCommandBuilder.arm_stow_command()
command = RobotCommandBuilder.synchro_sit_command(build_on_command=arm_command)
_test_has_synchronized(command)
_test_has_mobility(command.synchronized_command)
_test_has_arm(command.synchronized_command)
def test_arm_pose_command():
x = 0.75
y = 0
z = 0.25
qw = 1
qx = 0
qy = 0
qz = 0
command = RobotCommandBuilder.arm_pose_command(x, y, z, qw, qx, qy, qz, BODY_FRAME_NAME)
_test_has_synchronized(command)
_test_has_arm(command.synchronized_command)
assert command.synchronized_command.arm_command.HasField("arm_cartesian_command")
arm_cartesian_command = command.synchronized_command.arm_command.arm_cartesian_command
assert arm_cartesian_command.root_frame_name == BODY_FRAME_NAME
assert arm_cartesian_command.pose_trajectory_in_task.points[0].pose.position.x == x
assert arm_cartesian_command.pose_trajectory_in_task.points[0].pose.position.y == y
assert arm_cartesian_command.pose_trajectory_in_task.points[0].pose.position.z == z
assert arm_cartesian_command.pose_trajectory_in_task.points[0].pose.rotation.x == qx
assert arm_cartesian_command.pose_trajectory_in_task.points[0].pose.rotation.y == qy
assert arm_cartesian_command.pose_trajectory_in_task.points[0].pose.rotation.z == qz
assert arm_cartesian_command.pose_trajectory_in_task.points[0].pose.rotation.w == qw
# with a build_on_command
mobility_command = RobotCommandBuilder.synchro_sit_command()
command = RobotCommandBuilder.arm_pose_command(x, y, z, qw, qx, qy, qz, BODY_FRAME_NAME,
build_on_command=mobility_command)
_test_has_synchronized(command)
_test_has_mobility(command.synchronized_command)
_test_has_arm(command.synchronized_command)
def _sit(self):
"""Sets robot in Sit mode.
"""
if self.mode is not RobotMode.Sit:
self.mode = RobotMode.Sit
self.mobility_params = spot_command_pb2.MobilityParams(
locomotion_hint=spot_command_pb2.HINT_AUTO, stair_hint=0)
cmd = RobotCommandBuilder.synchro_sit_command(
params=self.mobility_params)
self._issue_robot_command(cmd)
def sit_down(self):
"""
Ask Spot to sit down
"""
if self._robot is None:
return
if self._prep_for_motion():
command_client = self._robot.ensure_client(
RobotCommandClient.default_service_name)
cmd = RobotCommandBuilder.synchro_sit_command()
command_client.robot_command(cmd)
def test_claw_gripper_close_command():
command = RobotCommandBuilder.claw_gripper_close_command()
_test_has_synchronized(command)
_test_has_gripper(command.synchronized_command)
assert (command.synchronized_command.gripper_command.WhichOneof("command") ==
"claw_gripper_command")
# with a build_on_command
mobility_command = RobotCommandBuilder.synchro_sit_command()
command = RobotCommandBuilder.claw_gripper_close_command(build_on_command=mobility_command)
_test_has_synchronized(command)
_test_has_mobility(command.synchronized_command)
_test_has_gripper(command.synchronized_command)
def test_arm_carry_command():
command = RobotCommandBuilder.arm_ready_command()
_test_has_synchronized(command)
_test_has_arm(command.synchronized_command)
assert (command.synchronized_command.arm_command.WhichOneof("command") ==
'named_arm_position_command')
# with a build_on_command
mobility_command = RobotCommandBuilder.synchro_sit_command()
command = RobotCommandBuilder.arm_carry_command(build_on_command=mobility_command)
_test_has_synchronized(command)
_test_has_mobility(command.synchronized_command)
_test_has_arm(command.synchronized_command)
def _sit(self):
self._start_robot_command('sit',
RobotCommandBuilder.synchro_sit_command())
def sit(self):
"""Stop the robot's motion and sit down if able."""
response = self._robot_command(RobotCommandBuilder.synchro_sit_command())
self._last_sit_command = response[2]
return response[0], response[1]
def build_sit_command(deprecated):
if (deprecated):
cmd = RobotCommandBuilder.sit_command()
else:
cmd = RobotCommandBuilder.synchro_sit_command()
return cmd
def sit(self):
self.__execute_command("sit",
RobotCommandBuilder.synchro_sit_command())
def test_synchro_sit_command():
command = RobotCommandBuilder.synchro_sit_command()
_test_has_synchronized(command)
_test_has_mobility(command.synchronized_command)
assert command.synchronized_command.mobility_command.HasField(
"sit_request")
