def stand_command(params=None,
body_height=0.0,
footprint_R_body=geometry.EulerZXY()):
"""Command robot to stand. If the robot is sitting, it will stand up. If the robot is
moving, it will come to a stop. Params can specify a trajectory for the body to follow
while standing. In the simplest case, this can be a specific position+orientation which the
body will hold at. The arguments body_height and footprint_R_body are ignored if params
argument is passed.
Args:
params(spot.MobilityParams): Spot specific parameters for mobility commands. If not set,
this will be constructed using other args.
body_height(float): Height, meters, to stand at relative to a nominal stand height.
footprint_R_body(EulerZXY): The orientation of the body frame with respect to the
footprint frame (gravity aligned framed with yaw computed from the stance feet)
Returns:
RobotCommand, which can be issued to the robot command service.
"""
if not params:
params = RobotCommandBuilder.mobility_params(
body_height=body_height, footprint_R_body=footprint_R_body)
any_params = RobotCommandBuilder._to_any(params)
mobility_command = mobility_command_pb2.MobilityCommand.Request(
stand_request=basic_command_pb2.StandCommand.Request(),
params=any_params)
command = robot_command_pb2.RobotCommand(
mobility_command=mobility_command)
return command
def pitch_up(robot):
"""Pitch robot up to look for door handle.
Args:
robot: (Robot) Interface to Spot robot.
"""
robot.logger.info("Pitching robot up...")
command_client = robot.ensure_client(
RobotCommandClient.default_service_name)
footprint_R_body = geometry.EulerZXY(0.0, 0.0, -1 * math.pi / 6.0)
cmd = RobotCommandBuilder.synchro_stand_command(
footprint_R_body=footprint_R_body)
cmd_id = command_client.robot_command(cmd)
timeout_sec = 10.0
end_time = time.time() + timeout_sec
while time.time() < end_time:
response = command_client.robot_command_feedback(cmd_id)
synchronized_feedback = response.feedback.synchronized_feedback
status = synchronized_feedback.mobility_command_feedback.stand_feedback.status
if (status ==
basic_command_pb2.StandCommand.Feedback.STATUS_IS_STANDING):
robot.logger.info("Robot pitched.")
return
time.sleep(1.0)
raise Exception("Failed to pitch robot.")
def set_default_body_control():
"""Set default body control params to current body position"""
footprint_R_body = geometry.EulerZXY()
position = geo.Vec3(x=0.0, y=0.0, z=0.0)
rotation = footprint_R_body.to_quaternion()
pose = geo.SE3Pose(position=position, rotation=rotation)
point = trajectory_pb2.SE3TrajectoryPoint(pose=pose)
traj = trajectory_pb2.SE3Trajectory(points=[point])
return spot_command_pb2.BodyControlParams(base_offset_rt_footprint=traj)
def test_ypr_to_quaternion_proto(yrp, wxyz):
euler_zxy = geometry.EulerZXY(yaw=yrp[0], roll=yrp[1], pitch=yrp[2])
quat_initial = Quaternion(w=wxyz[0], x=wxyz[1], y=wxyz[2], z=wxyz[3])
quat = euler_zxy.to_quaternion()
assert (_is_near(abs(_dot(quat, quat_initial)), 1.0, 1e-6))
# Do another circular conversion, make sure everything looks ok.
euler_zxy_final = quat.to_euler_zxy()
quat_final = euler_zxy_final.to_quaternion()
assert (_is_near(abs(_dot(quat_final, quat_initial)), 1.0, 1e-6))
def orient(self, yaw=0.0, pitch=0.0, roll=0.0):
"""
Ask Spot to orient its body (e.g. yaw, pitch, roll)
@param[in] yaw Rotation about Z (+up/down) [rad]
@param[in] pitch Rotation about Y (+left/right) [rad]
@param[in] roll Rotation about X (+front/back) [rad]
"""
if self._robot is None:
return
if self._prep_for_motion():
rotation = geometry.EulerZXY(yaw=yaw, pitch=pitch, roll=roll)
command_client = self._robot.ensure_client(
RobotCommandClient.default_service_name)
cmd = RobotCommandBuilder.stand_command(footprint_R_body=rotation)
command_client.robot_command(cmd)
def mobility_params(body_height=0.0, footprint_R_body=geometry.EulerZXY(),
locomotion_hint=spot_command_pb2.HINT_AUTO, stair_hint=False,
external_force_params=None):
"""Helper to create Mobility params for spot mobility commands. This function is designed
to help get started issuing commands, but lots of options are not exposed via this
interface. See spot.robot_command_pb2 for more details. If unset, good defaults will be
chosen by the robot.
Returns:
spot.MobilityParams, params for spot mobility commands.
"""
# Simplified body control params
position = geometry_pb2.Vec3(z=body_height)
rotation = footprint_R_body.to_quaternion()
pose = geometry_pb2.SE3Pose(position=position, rotation=rotation)
point = trajectory_pb2.SE3TrajectoryPoint(pose=pose)
frame = geometry_pb2.Frame(base_frame=geometry_pb2.FRAME_BODY)
traj = trajectory_pb2.SE3Trajectory(points=[point], frame=frame)
body_control = spot_command_pb2.BodyControlParams(base_offset_rt_footprint=traj)
return spot_command_pb2.MobilityParams(body_control=body_control,
locomotion_hint=locomotion_hint,
stair_hint=stair_hint,
external_force_params=external_force_params)
def mobility_params(body_height=0.0,
footprint_R_body=geometry.EulerZXY(),
locomotion_hint=spot_command_pb2.HINT_AUTO,
stair_hint=False,
external_force_params=None):
"""Helper to create Mobility params for spot mobility commands. This function is designed
to help get started issuing commands, but lots of options are not exposed via this
interface. See spot.robot_command_pb2 for more details. If unset, good defaults will be
chosen by the robot.
Args:
body_height: Body height in meters.
footprint_R_body(EulerZXY): The orientation of the body frame with respect to the
footprint frame (gravity aligned framed with yaw computed
from the stance feet)
locomotion_hint: Locomotion hint to use for the command.
stair_hint: Boolean to specify if stair mode should be used.
external_force_params(spot.BodyExternalForceParams): Robot body external force
parameters.
Returns:
spot.MobilityParams, params for spot mobility commands.
"""
# Simplified body control params
position = geometry_pb2.Vec3(z=body_height)
rotation = footprint_R_body.to_quaternion()
pose = geometry_pb2.SE3Pose(position=position, rotation=rotation)
point = trajectory_pb2.SE3TrajectoryPoint(pose=pose)
traj = trajectory_pb2.SE3Trajectory(points=[point])
body_control = spot_command_pb2.BodyControlParams(
base_offset_rt_footprint=traj)
return spot_command_pb2.MobilityParams(
body_control=body_control,
locomotion_hint=locomotion_hint,
stair_hint=stair_hint,
external_force_params=external_force_params)
def _stand(self):
self._start_robot_command('stand', RobotCommandBuilder.stand_command(
body_height=self.height,
footprint_R_body=geometry.EulerZXY(roll=self.roll, yaw=self.yaw, pitch=self.pitch)))
