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