def run_docking(config): """A simple example of using the Boston Dynamics API to use the docking service. Copied from the hello_spot.py example""" bosdyn.client.util.setup_logging(config.verbose) sdk = bosdyn.client.create_standard_sdk('DockingClient') robot = sdk.create_robot(config.hostname) robot.authenticate(config.username, config.password) robot.time_sync.wait_for_sync() lease_client = robot.ensure_client(bosdyn.client.lease.LeaseClient.default_service_name) # To steal the lease on a running robot you'd like to dock, change this to `lease_client.take()` lease = lease_client.acquire() command_client = robot.ensure_client(robot_command.RobotCommandClient.default_service_name) try: with bosdyn.client.lease.LeaseKeepAlive(lease_client): # make sure we're powered on and standing robot.power_on() robot_command.blocking_stand(command_client) # Dock the robot blocking_dock_robot(robot, config.dock_id) print("Docking Success") finally: lease_client.return_lease(lease)
def connect(self, app_token, hostname, username, password, initialize_motion=True): # Create robot object. sdk = bosdyn.client.create_standard_sdk('RobotCommandMaster') sdk.load_app_token(app_token) robot = sdk.create_robot(hostname) robot.authenticate(username, password) # Check that an estop is connected with the robot so that the robot commands can be executed. verify_estop(robot) # Create the lease client. lease_client = robot.ensure_client(LeaseClient.default_service_name) lease = lease_client.acquire() robot.time_sync.wait_for_sync() lk = bosdyn.client.lease.LeaseKeepAlive(lease_client) # Setup clients for the robot state and robot command services. robot_state_client = robot.ensure_client( RobotStateClient.default_service_name) robot_command_client = robot.ensure_client( RobotCommandClient.default_service_name) # Power on the robot and stand it up. robot.power_on() if initialize_motion: blocking_stand(robot_command_client)
def start(self): """Claim lease of robot and start the fiducial follower.""" self._lease = self._lease_client.acquire() self._robot.time_sync.wait_for_sync() self._lease_keepalive = bosdyn.client.lease.LeaseKeepAlive( self._lease_client) # Stand the robot up. if self._standup: self.power_on() blocking_stand(self._robot_command_client) # Delay grabbing image until spot is standing (or close enough to upright). time.sleep(.35) while self._attempts <= self._max_attempts: detected_fiducial = False fiducial_rt_world = None if self._use_world_object_service: # Get the first fiducial object Spot detects with the world object service. fiducial = self.get_fiducial_objects() if fiducial is not None: vision_tform_fiducial = get_a_tform_b( fiducial.transforms_snapshot, VISION_FRAME_NAME, fiducial.apriltag_properties.frame_name_fiducial ).to_proto() if vision_tform_fiducial is not None: detected_fiducial = True fiducial_rt_world = vision_tform_fiducial.position else: # Detect the april tag in the images from Spot using the apriltag library. bboxes, source_name = self.image_to_bounding_box() if bboxes: self._previous_source = source_name (tvec, _, source_name) = self.pixel_coords_to_camera_coords( bboxes, self._intrinsics, source_name) vision_tform_fiducial_position = self.compute_fiducial_in_world_frame( tvec) fiducial_rt_world = geometry_pb2.Vec3( x=vision_tform_fiducial_position[0], y=vision_tform_fiducial_position[1], z=vision_tform_fiducial_position[2]) detected_fiducial = True if detected_fiducial: # Go to the tag and stop within a certain distance self.go_to_tag(fiducial_rt_world) else: print("No fiducials found") self._attempts += 1 #increment attempts at finding a fiducial # Power off at the conclusion of the example. if self._powered_on: self.power_off()
def stand(robot): """Stand robot. Args: robot: (Robot) Interface to Spot robot. """ robot.logger.info("Commanding robot to stand...") command_client = robot.ensure_client( RobotCommandClient.default_service_name) blocking_stand(command_client, timeout_sec=10) robot.logger.info("Robot standing.")
def stand_up(self): """ Ask Spot to stand up """ if self._robot is None: return if self._prep_for_motion(): command_client = self._robot.ensure_client( RobotCommandClient.default_service_name) blocking_stand(command_client, timeout_sec=10)
def run_camera_calibration_routine(robot): # Take lease. lease_client = robot.ensure_client(LeaseClient.default_service_name) lease = lease_client.take() with LeaseKeepAlive(lease_client): # Power on the robot. robot.power_on(timeout_sec=POWER_TIMEOUT) # Command the robot to stand. command_client = robot.ensure_client( RobotCommandClient.default_service_name) blocking_stand(command_client, timeout_sec=STAND_TIMEOUT) # Return this lease and get a new one. lease_client.return_lease(lease) lease = lease_client.take() # Run camera calibration. spot_check_client = robot.ensure_client( SpotCheckClient.default_service_name) run_camera_calibration(spot_check_client, lease, verbose=True) robot.logger.info("CameraCaliration ran with no errors!") lease_client.return_lease(lease)
def hello_arm(config): """A simple example of using the Boston Dynamics API to command Spot's arm.""" # See hello_spot.py for an explanation of these lines. bosdyn.client.util.setup_logging(config.verbose) sdk = bosdyn.client.create_standard_sdk('StowUnstowClient') robot = sdk.create_robot(config.hostname) robot.authenticate(config.username, config.password) robot.time_sync.wait_for_sync() assert robot.has_arm(), "Robot requires an arm to run this example." # Verify the robot is not estopped and that an external application has registered and holds # an estop endpoint. verify_estop(robot) lease_client = robot.ensure_client(bosdyn.client.lease.LeaseClient.default_service_name) lease = lease_client.acquire() try: with bosdyn.client.lease.LeaseKeepAlive(lease_client): # Now, we are ready to power on the robot. This call will block until the power # is on. Commands would fail if this did not happen. We can also check that the robot is # powered at any point. robot.logger.info("Powering on robot... This may take a several seconds.") robot.power_on(timeout_sec=20) assert robot.is_powered_on(), "Robot power on failed." robot.logger.info("Robot powered on.") # Tell the robot to stand up. The command service is used to issue commands to a robot. # The set of valid commands for a robot depends on hardware configuration. See # SpotCommandHelper for more detailed examples on command building. The robot # command service requires timesync between the robot and the client. robot.logger.info("Commanding robot to stand...") command_client = robot.ensure_client(RobotCommandClient.default_service_name) blocking_stand(command_client, timeout_sec=10) robot.logger.info("Robot standing.") time.sleep(2.0) # Unstow the arm unstow = RobotCommandBuilder.arm_ready_command() # Issue the command via the RobotCommandClient command_client.robot_command(unstow) robot.logger.info("Unstow command issued.") time.sleep(3.0) # Stow the arm # Build the stow command using RobotCommandBuilder stow = RobotCommandBuilder.arm_stow_command() # Issue the command via the RobotCommandClient command_client.robot_command(stow) robot.logger.info("Stow command issued.") time.sleep(3.0) # Power the robot off. By specifying "cut_immediately=False", a safe power off command # is issued to the robot. This will attempt to sit the robot before powering off. robot.power_off(cut_immediately=False, timeout_sec=20) assert not robot.is_powered_on(), "Robot power off failed." robot.logger.info("Robot safely powered off.") finally: # If we successfully acquired a lease, return it. lease_client.return_lease(lease)
def main(): '''Replay stored mission''' body_lease = None # Configure logging bosdyn.client.util.setup_logging() # Parse command-line arguments parser = argparse.ArgumentParser() bosdyn.client.util.add_common_arguments(parser) # If the map directory is omitted, we assume that everything the user wants done is encoded in # the mission itself. parser.add_argument('--map_directory', nargs='?', help='Optional path to map directory') parser.add_argument('--mission', dest='mission_file', help='Optional path to mission file') parser.add_argument('--timeout', type=float, default=3.0, dest='timeout', help='Mission client timeout (s).') parser.add_argument('--noloc', action='store_true', default=False, dest='noloc', help='Skip initial localization') parser.add_argument('--disable_alternate_route_finding', action='store_true', default=False, dest='disable_alternate_route_finding', help='Disable creating alternate-route-finding graph structure') group = parser.add_mutually_exclusive_group() group.add_argument('--time', type=float, default=0.0, dest='duration', help='Time to repeat mission (sec)') group.add_argument('--static', action='store_true', default=False, dest='static_mode', help='Stand, but do not run robot') args = parser.parse_args() # Use the optional map_directory argument as a proxy for these other tasks we normally do. do_localization = (args.map_directory is not None) and (not args.noloc) do_map_load = args.map_directory is not None fail_on_question = args.map_directory is not None if not args.mission_file: if not args.map_directory: raise Exception('Must specify at least one of map_directory or --mission.') args.mission_file = os.path.join(args.map_directory, 'missions', 'autogenerated') print('[ REPLAYING MISSION {} : MAP {} : HOSTNAME {} ]'.format(args.mission_file, args.map_directory, args.hostname)) # Initialize robot object robot = init_robot(args.hostname, args.username, args.password) # Acquire robot lease robot.logger.info('Acquiring lease...') lease_client = robot.ensure_client(bosdyn.client.lease.LeaseClient.default_service_name) body_lease = lease_client.acquire() if body_lease is None: raise Exception('Lease not acquired.') robot.logger.info('Lease acquired: %s', str(body_lease)) try: with bosdyn.client.lease.LeaseKeepAlive(lease_client): # Initialize clients robot_state_client, command_client, mission_client, graph_nav_client = init_clients( robot, body_lease, args.mission_file, args.map_directory, do_map_load, args.disable_alternate_route_finding) assert not robot.is_estopped(), "Robot is estopped. " \ "Please use an external E-Stop client, " \ "such as the estop SDK example, to configure E-Stop." # Ensure robot is powered on assert ensure_power_on(robot), 'Robot power on failed.' # Stand up and wait for the perception system to stabilize robot.logger.info('Commanding robot to stand...') blocking_stand(command_client, timeout_sec=10) countdown(5) robot.logger.info('Robot standing.') # Localize robot localization_error = False if do_localization: graph = graph_nav_client.download_graph() robot.logger.info('Localizing robot...') robot_state = robot_state_client.get_robot_state() localization = nav_pb2.Localization() # Attempt to localize using any visible fiducial graph_nav_client.set_localization( initial_guess_localization=localization, ko_tform_body=None, max_distance=None, max_yaw=None, fiducial_init=graph_nav_pb2.SetLocalizationRequest.FIDUCIAL_INIT_NEAREST) # Run mission if not args.static_mode and not localization_error: if args.duration == 0.0: run_mission(robot, mission_client, lease_client, fail_on_question, args.timeout) else: repeat_mission(robot, mission_client, lease_client, args.duration, fail_on_question, args.timeout) finally: # Ensure robot is powered off lease_client.lease_wallet.advance() robot.logger.info('Powering off...') power_off_success = ensure_power_off(robot) # Return lease robot.logger.info('Returning lease...') lease_client.return_lease(body_lease)
def force_wrench(config): """Commanding a force / wrench with Spot's arm.""" # See hello_spot.py for an explanation of these lines. bosdyn.client.util.setup_logging(config.verbose) sdk = bosdyn.client.create_standard_sdk('ForceWrenchClient') robot = sdk.create_robot(config.hostname) robot.authenticate(config.username, config.password) robot.time_sync.wait_for_sync() assert robot.has_arm(), "Robot requires an arm to run this example." # Verify the robot is not estopped and that an external application has registered and holds # an estop endpoint. assert not robot.is_estopped(), "Robot is estopped. Please use an external E-Stop client, " \ "such as the estop SDK example, to configure E-Stop." robot_state_client = robot.ensure_client( RobotStateClient.default_service_name) lease_client = robot.ensure_client( bosdyn.client.lease.LeaseClient.default_service_name) lease = lease_client.acquire() try: with bosdyn.client.lease.LeaseKeepAlive(lease_client): # Now, we are ready to power on the robot. This call will block until the power # is on. Commands would fail if this did not happen. We can also check that the robot is # powered at any point. robot.logger.info( "Powering on robot... This may take a several seconds.") robot.power_on(timeout_sec=20) assert robot.is_powered_on(), "Robot power on failed." robot.logger.info("Robot powered on.") # Tell the robot to stand up. The command service is used to issue commands to a robot. # The set of valid commands for a robot depends on hardware configuration. See # SpotCommandHelper for more detailed examples on command building. The robot # command service requires timesync between the robot and the client. robot.logger.info("Commanding robot to stand...") command_client = robot.ensure_client( RobotCommandClient.default_service_name) blocking_stand(command_client, timeout_sec=10) robot.logger.info("Robot standing.") # Send a second stand command with a lowered body height to allow the arm to reach the ground. stand_command = RobotCommandBuilder.synchro_stand_command( body_height=-0.15) command_id = command_client.robot_command(stand_command, timeout=2) time.sleep(2.0) # Unstow the arm unstow = RobotCommandBuilder.arm_ready_command() # Issue the command via the RobotCommandClient command_client.robot_command(unstow) robot.logger.info("Unstow command issued.") time.sleep(3.0) # Start in gravity-compensation mode (but zero force) f_x = 0 # Newtons f_y = 0 f_z = 0 # We won't have any rotational torques torque_x = 0 torque_y = 0 torque_z = 0 # Duration in seconds. seconds = 1000 # Use the helper function to build a single wrench command = RobotCommandBuilder.arm_wrench_command( f_x, f_y, f_z, torque_x, torque_y, torque_z, BODY_FRAME_NAME, seconds) # Send the request command_client.robot_command(command) robot.logger.info('Zero force commanded...') time.sleep(5.0) # Drive the arm into the ground with a specified force: f_z = -5 # Newtons command = RobotCommandBuilder.arm_wrench_command( f_x, f_y, f_z, torque_x, torque_y, torque_z, BODY_FRAME_NAME, seconds) # Send the request command_client.robot_command(command) time.sleep(5.0) # --------------- # # Hybrid position-force mode and trajectories. # # We'll move the arm in an X/Y trajectory while maintaining some force on the ground. # Hand will start to the left and move to the right. hand_x = 0.75 # in front of the robot. hand_y_start = 0.25 # to the left hand_y_end = -0.25 # to the right hand_z = 0 # will be ignored since we'll have a force in the Z axis. f_z = -5 # Newtons hand_vec3_start = geometry_pb2.Vec3(x=hand_x, y=hand_y_start, z=hand_z) hand_vec3_end = geometry_pb2.Vec3(x=hand_x, y=hand_y_end, z=hand_z) qw = 1 qx = 0 qy = 0 qz = 0 quat = geometry_pb2.Quaternion(w=qw, x=qx, y=qy, z=qz) # Build a position trajectory hand_pose1_in_flat_body = geometry_pb2.SE3Pose( position=hand_vec3_start, rotation=quat) hand_pose2_in_flat_body = geometry_pb2.SE3Pose( position=hand_vec3_end, rotation=quat) # Convert the poses to the odometry frame. robot_state = robot_state_client.get_robot_state() odom_T_flat_body = get_a_tform_b( robot_state.kinematic_state.transforms_snapshot, ODOM_FRAME_NAME, GRAV_ALIGNED_BODY_FRAME_NAME) hand_pose1 = odom_T_flat_body * math_helpers.SE3Pose.from_obj( hand_pose1_in_flat_body) hand_pose2 = odom_T_flat_body * math_helpers.SE3Pose.from_obj( hand_pose2_in_flat_body) traj_time = 5.0 time_since_reference = seconds_to_duration(traj_time) traj_point1 = trajectory_pb2.SE3TrajectoryPoint( pose=hand_pose1.to_proto(), time_since_reference=seconds_to_duration(0)) traj_point2 = trajectory_pb2.SE3TrajectoryPoint( pose=hand_pose2.to_proto(), time_since_reference=time_since_reference) hand_traj = trajectory_pb2.SE3Trajectory( points=[traj_point1, traj_point2]) # We'll use a constant wrench here. Build a wrench trajectory with a single point. # Note that we only need to fill out Z-force because that is the only axis that will # be in force mode. force_tuple = odom_T_flat_body.rotation.transform_point(x=0, y=0, z=f_z) force = geometry_pb2.Vec3(x=force_tuple[0], y=force_tuple[1], z=force_tuple[2]) torque = geometry_pb2.Vec3(x=0, y=0, z=0) wrench = geometry_pb2.Wrench(force=force, torque=torque) # We set this point to happen at 0 seconds. The robot will hold the wrench past that # time, so we'll end up with a constant value. duration = seconds_to_duration(0) traj_point = trajectory_pb2.WrenchTrajectoryPoint( wrench=wrench, time_since_reference=duration) trajectory = trajectory_pb2.WrenchTrajectory(points=[traj_point]) arm_cartesian_command = arm_command_pb2.ArmCartesianCommand.Request( pose_trajectory_in_task=hand_traj, root_frame_name=ODOM_FRAME_NAME, wrench_trajectory_in_task=trajectory, x_axis=arm_command_pb2.ArmCartesianCommand.Request. AXIS_MODE_POSITION, y_axis=arm_command_pb2.ArmCartesianCommand.Request. AXIS_MODE_POSITION, z_axis=arm_command_pb2.ArmCartesianCommand.Request. AXIS_MODE_FORCE, rx_axis=arm_command_pb2.ArmCartesianCommand.Request. AXIS_MODE_POSITION, ry_axis=arm_command_pb2.ArmCartesianCommand.Request. AXIS_MODE_POSITION, rz_axis=arm_command_pb2.ArmCartesianCommand.Request. AXIS_MODE_POSITION) arm_command = arm_command_pb2.ArmCommand.Request( arm_cartesian_command=arm_cartesian_command) synchronized_command = synchronized_command_pb2.SynchronizedCommand.Request( arm_command=arm_command) robot_command = robot_command_pb2.RobotCommand( synchronized_command=synchronized_command) # Send the request command_client.robot_command(robot_command) robot.logger.info('Running mixed position and force mode.') time.sleep(traj_time + 5.0) # Power the robot off. By specifying "cut_immediately=False", a safe power off command # is issued to the robot. This will attempt to sit the robot before powering off. robot.power_off(cut_immediately=False, timeout_sec=20) assert not robot.is_powered_on(), "Robot power off failed." robot.logger.info("Robot safely powered off.") finally: # If we successfully acquired a lease, return it. lease_client.return_lease(lease)
def force_wrench(config): """Commanding a force / wrench with Spot's arm.""" # See hello_spot.py for an explanation of these lines. bosdyn.client.util.setup_logging(config.verbose) sdk = bosdyn.client.create_standard_sdk('ForceTrajectoryClient') robot = sdk.create_robot(config.hostname) robot.authenticate(config.username, config.password) robot.time_sync.wait_for_sync() assert robot.has_arm(), "Robot requires an arm to run this example." # Verify the robot is not estopped and that an external application has registered and holds # an estop endpoint. assert not robot.is_estopped(), "Robot is estopped. Please use an external E-Stop client, " \ "such as the estop SDK example, to configure E-Stop." lease_client = robot.ensure_client( bosdyn.client.lease.LeaseClient.default_service_name) lease = lease_client.acquire() try: with bosdyn.client.lease.LeaseKeepAlive(lease_client): # Now, we are ready to power on the robot. This call will block until the power # is on. Commands would fail if this did not happen. We can also check that the robot is # powered at any point. robot.logger.info( "Powering on robot... This may take a several seconds.") robot.power_on(timeout_sec=20) assert robot.is_powered_on(), "Robot power on failed." robot.logger.info("Robot powered on.") # Tell the robot to stand up. The command service is used to issue commands to a robot. # The set of valid commands for a robot depends on hardware configuration. See # SpotCommandHelper for more detailed examples on command building. The robot # command service requires timesync between the robot and the client. robot.logger.info("Commanding robot to stand...") command_client = robot.ensure_client( RobotCommandClient.default_service_name) blocking_stand(command_client, timeout_sec=10) robot.logger.info("Robot standing.") time.sleep(2.0) # Unstow the arm unstow = RobotCommandBuilder.arm_ready_command() # Issue the command via the RobotCommandClient command_client.robot_command(unstow) robot.logger.info("Unstow command issued.") time.sleep(3.0) # Demonstrate an example force trajectory by ramping up and down a vertical force over # 10 seconds f_x0 = 0 # Newtons f_y0 = 0 f_z0 = 0 f_x1 = 0 # Newtons f_y1 = 0 f_z1 = -10 # push down # We won't have any rotational torques torque_x = 0 torque_y = 0 torque_z = 0 # Duration in seconds. traj_duration = 5 # First point on the trajectory force0 = geometry_pb2.Vec3(x=f_x0, y=f_y0, z=f_z0) torque0 = geometry_pb2.Vec3(x=torque_x, y=torque_y, z=torque_z) wrench0 = geometry_pb2.Wrench(force=force0, torque=torque0) t0 = seconds_to_duration(0) traj_point0 = trajectory_pb2.WrenchTrajectoryPoint( wrench=wrench0, time_since_reference=t0) # Second point on the trajectory force1 = geometry_pb2.Vec3(x=f_x1, y=f_y1, z=f_z1) torque1 = geometry_pb2.Vec3(x=torque_x, y=torque_y, z=torque_z) wrench1 = geometry_pb2.Wrench(force=force1, torque=torque1) t1 = seconds_to_duration(traj_duration) traj_point1 = trajectory_pb2.WrenchTrajectoryPoint( wrench=wrench1, time_since_reference=t1) # Build the trajectory trajectory = trajectory_pb2.WrenchTrajectory( points=[traj_point0, traj_point1]) # Build the full request, putting all axes into force mode. arm_cartesian_command = arm_command_pb2.ArmCartesianCommand.Request( root_frame_name=ODOM_FRAME_NAME, wrench_trajectory_in_task=trajectory, x_axis=arm_command_pb2.ArmCartesianCommand.Request. AXIS_MODE_FORCE, y_axis=arm_command_pb2.ArmCartesianCommand.Request. AXIS_MODE_FORCE, z_axis=arm_command_pb2.ArmCartesianCommand.Request. AXIS_MODE_FORCE, rx_axis=arm_command_pb2.ArmCartesianCommand.Request. AXIS_MODE_FORCE, ry_axis=arm_command_pb2.ArmCartesianCommand.Request. AXIS_MODE_FORCE, rz_axis=arm_command_pb2.ArmCartesianCommand.Request. AXIS_MODE_FORCE) arm_command = arm_command_pb2.ArmCommand.Request( arm_cartesian_command=arm_cartesian_command) synchronized_command = synchronized_command_pb2.SynchronizedCommand.Request( arm_command=arm_command) robot_command = robot_command_pb2.RobotCommand( synchronized_command=synchronized_command) # Send the request command_client.robot_command(robot_command) robot.logger.info('Force trajectory command issued...') time.sleep(5.0 + traj_duration) # Power the robot off. By specifying "cut_immediately=False", a safe power off command # is issued to the robot. This will attempt to sit the robot before powering off. robot.power_off(cut_immediately=False, timeout_sec=20) assert not robot.is_powered_on(), "Robot power off failed." robot.logger.info("Robot safely powered off.") finally: # If we successfully acquired a lease, return it. lease_client.return_lease(lease)
def run(config): """Testing API Stance This example will cause the robot to power on, stand and reposition its feet (Stance) at the location it's already standing at. * Use sw-estop running on tablet/python etc. * Have ~1m of free space all around the robot * Ctrl-C to exit and return lease. """ bosdyn.client.util.setup_logging(config.verbose) sdk = bosdyn.client.create_standard_sdk('StanceClient') robot = sdk.create_robot(config.hostname) robot.authenticate(config.username, config.password) robot.time_sync.wait_for_sync() # Acquire lease lease_client = robot.ensure_client( bosdyn.client.lease.LeaseClient.default_service_name) lease = lease_client.acquire() try: with bosdyn.client.lease.LeaseKeepAlive(lease_client): command_client = robot.ensure_client( RobotCommandClient.default_service_name) robot_state_client = robot.ensure_client( RobotStateClient.default_service_name) state = robot_state_client.get_robot_state() # This example ues the current body position, but you can specify any position. # A common use is to specify it relative to something you know, like a fiducial. vo_T_body = frame_helpers.get_se2_a_tform_b( state.kinematic_state.transforms_snapshot, frame_helpers.VISION_FRAME_NAME, frame_helpers.GRAV_ALIGNED_BODY_FRAME_NAME) # Power On robot.power_on() assert robot.is_powered_on(), "Robot power on failed." # Stand robot_command.blocking_stand(command_client) #### Example stance offsets from body position. #### x_offset = config.x_offset y_offset = config.y_offset pos_fl_rt_vision = vo_T_body * math_helpers.SE2Pose( x_offset, y_offset, 0) pos_fr_rt_vision = vo_T_body * math_helpers.SE2Pose( x_offset, -y_offset, 0) pos_hl_rt_vision = vo_T_body * math_helpers.SE2Pose( -x_offset, y_offset, 0) pos_hr_rt_vision = vo_T_body * math_helpers.SE2Pose( -x_offset, -y_offset, 0) stance_cmd = RobotCommandBuilder.stance_command( frame_helpers.VISION_FRAME_NAME, pos_fl_rt_vision.position, pos_fr_rt_vision.position, pos_hl_rt_vision.position, pos_hr_rt_vision.position) print( "After stance adjustment, press Ctrl-C to sit Spot and turn off motors." ) while True: # Update end time stance_cmd.synchronized_command.mobility_command.stance_request.end_time.CopyFrom( robot.time_sync.robot_timestamp_from_local_secs( time.time() + 5)) # Send the command command_client.robot_command(stance_cmd) time.sleep(1) finally: lease_client.return_lease(lease)
def arm_trajectory(config): # See hello_spot.py for an explanation of these lines. bosdyn.client.util.setup_logging(config.verbose) sdk = bosdyn.client.create_standard_sdk('ArmTrajectory') robot = sdk.create_robot(config.hostname) robot.authenticate(config.username, config.password) robot.time_sync.wait_for_sync() assert robot.has_arm(), "Robot requires an arm to run this example." # Verify the robot is not estopped and that an external application has registered and holds # an estop endpoint. assert not robot.is_estopped(), "Robot is estopped. Please use an external E-Stop client, " \ "such as the estop SDK example, to configure E-Stop." robot_state_client = robot.ensure_client( RobotStateClient.default_service_name) lease_client = robot.ensure_client( bosdyn.client.lease.LeaseClient.default_service_name) lease = lease_client.acquire() try: with bosdyn.client.lease.LeaseKeepAlive(lease_client): # Now, we are ready to power on the robot. This call will block until the power # is on. Commands would fail if this did not happen. We can also check that the robot is # powered at any point. robot.logger.info( "Powering on robot... This may take a several seconds.") robot.power_on(timeout_sec=20) assert robot.is_powered_on(), "Robot power on failed." robot.logger.info("Robot powered on.") # Tell the robot to stand up. The command service is used to issue commands to a robot. # The set of valid commands for a robot depends on hardware configuration. See # SpotCommandHelper for more detailed examples on command building. The robot # command service requires timesync between the robot and the client. robot.logger.info("Commanding robot to stand...") command_client = robot.ensure_client( RobotCommandClient.default_service_name) blocking_stand(command_client, timeout_sec=10) robot.logger.info("Robot standing.") time.sleep(2.0) # Move the arm along a simple trajectory vo_T_flat_body = get_a_tform_b( robot_state_client.get_robot_state( ).kinematic_state.transforms_snapshot, VISION_FRAME_NAME, GRAV_ALIGNED_BODY_FRAME_NAME) x = 0.75 # a reasonable position in front of the robot y1 = 0 # centered y2 = 0.4 # 0.4 meters to the robot's left y3 = -0.4 # 0.4 meters to the robot's right z = 0 # at the body's height # Use the same rotation as the robot's body. rotation = math_helpers.Quat() t_first_point = 0 # first point starts at t = 0 for the trajectory. t_second_point = 4.0 # trajectory will last 1.0 seconds t_third_point = 8.0 # trajectory will last 1.0 seconds # Build the two points in the trajectory hand_pose1 = math_helpers.SE3Pose(x=x, y=y1, z=z, rot=rotation) hand_pose2 = math_helpers.SE3Pose(x=x, y=y2, z=z, rot=rotation) hand_pose3 = math_helpers.SE3Pose(x=x, y=y3, z=z, rot=rotation) # Build the points by combining the pose and times into protos. traj_point1 = trajectory_pb2.SE3TrajectoryPoint( pose=hand_pose1.to_proto(), time_since_reference=seconds_to_duration(t_first_point)) traj_point2 = trajectory_pb2.SE3TrajectoryPoint( pose=hand_pose2.to_proto(), time_since_reference=seconds_to_duration(t_second_point)) traj_point3 = trajectory_pb2.SE3TrajectoryPoint( pose=hand_pose3.to_proto(), time_since_reference=seconds_to_duration(t_third_point)) # Build the trajectory proto by combining the two points hand_traj = trajectory_pb2.SE3Trajectory( points=[traj_point1, traj_point2, traj_point3]) # Build the command by taking the trajectory and specifying the frame it is expressed # in. # # In this case, we want to specify the trajectory in the body's frame, so we set the # root frame name to the the flat body frame. arm_cartesian_command = arm_command_pb2.ArmCartesianCommand.Request( pose_trajectory_in_task=hand_traj, root_frame_name=GRAV_ALIGNED_BODY_FRAME_NAME) # Pack everything up in protos. arm_command = arm_command_pb2.ArmCommand.Request( arm_cartesian_command=arm_cartesian_command) synchronized_command = synchronized_command_pb2.SynchronizedCommand.Request( arm_command=arm_command) robot_command = robot_command_pb2.RobotCommand( synchronized_command=synchronized_command) # Keep the gripper closed the whole time. robot_command = RobotCommandBuilder.claw_gripper_open_fraction_command( 0, build_on_command=robot_command) robot.logger.info("Sending trajectory command...") # Send the trajectory to the robot. cmd_id = command_client.robot_command(robot_command) # Wait until the arm arrives at the goal. while True: feedback_resp = command_client.robot_command_feedback(cmd_id) robot.logger.info( 'Distance to final point: ' + '{:.2f} meters'.format( feedback_resp.feedback.synchronized_feedback. arm_command_feedback.arm_cartesian_feedback. measured_pos_distance_to_goal) + ', {:.2f} radians'.format( feedback_resp.feedback.synchronized_feedback. arm_command_feedback.arm_cartesian_feedback. measured_rot_distance_to_goal)) if feedback_resp.feedback.synchronized_feedback.arm_command_feedback.arm_cartesian_feedback.status == arm_command_pb2.ArmCartesianCommand.Feedback.STATUS_TRAJECTORY_COMPLETE: robot.logger.info('Move complete.') break time.sleep(0.1) # Power the robot off. By specifying "cut_immediately=False", a safe power off command # is issued to the robot. This will attempt to sit the robot before powering off. robot.power_off(cut_immediately=False, timeout_sec=20) assert not robot.is_powered_on(), "Robot power off failed." robot.logger.info("Robot safely powered off.") finally: # If we successfully acquired a lease, return it. lease_client.return_lease(lease)
def main(): import argparse parser = argparse.ArgumentParser() bosdyn.client.util.add_common_arguments(parser) options = parser.parse_args() # Create robot object. sdk = bosdyn.client.create_standard_sdk('RobotCommandMaster') sdk.load_app_token(options.app_token) robot = sdk.create_robot(options.hostname) robot.authenticate(options.username, options.password) # Check that an estop is connected with the robot so that the robot commands can be executed. verify_estop(robot) # Create the lease client. lease_client = robot.ensure_client(LeaseClient.default_service_name) lease = lease_client.acquire() robot.time_sync.wait_for_sync() lk = bosdyn.client.lease.LeaseKeepAlive(lease_client) # Setup clients for the robot state and robot command services. robot_state_client = robot.ensure_client( RobotStateClient.default_service_name) robot_command_client = robot.ensure_client( RobotCommandClient.default_service_name) # Power on the robot and stand it up. robot.power_on() blocking_stand(robot_command_client) # Get robot state information. Specifically, we are getting the vision_tform_body transform to understand # the robot's current position in the vision frame. vision_tform_body = get_vision_tform_body( robot_state_client.get_robot_state( ).kinematic_state.transforms_snapshot) # We want to command a trajectory to go forward one meter in the x-direction of the body. # It is simple to define this trajectory relative to the body frame, since we know that will be # just 1 meter forward in the x-axis of the body. # Note that the rotation is just math_helpers.Quat(), which is the identity quaternion. We want the # rotation of the body at the goal to match the rotation of the body currently, so we do not need # to transform the rotation. body_tform_goal = math_helpers.SE3Pose(x=1, y=0, z=0, rot=math_helpers.Quat()) # We can then transform this transform to get the goal position relative to the vision frame. vision_tform_goal = vision_tform_body * body_tform_goal # Command the robot to go to the goal point in the vision frame. The command will stop at the new # position in the vision frame. robot_cmd = RobotCommandBuilder.trajectory_command( goal_x=vision_tform_goal.x, goal_y=vision_tform_goal.y, goal_heading=vision_tform_goal.rot.to_yaw(), frame_name=VISION_FRAME_NAME) end_time = 2.0 robot_command_client.robot_command(lease=None, command=robot_cmd, end_time_secs=time.time() + end_time) time.sleep(end_time) # Get new robot state information after moving the robot. Here we are getting the transform odom_tform_body, # which describes the robot body's position in the odom frame. odom_tform_body = get_odom_tform_body(robot_state_client.get_robot_state(). kinematic_state.transforms_snapshot) # We want to command a trajectory to go backwards one meter and to the left one meter. # It is simple to define this trajectory relative to the body frame, since we know that will be # just 1 meter backwards (negative-value) in the x-axis of the body and one meter left (positive-value) # in the y-axis of the body. body_tform_goal = math_helpers.SE3Pose(x=-1, y=1, z=0, rot=math_helpers.Quat()) # We can then transform this transform to get the goal position relative to the odom frame. odom_tform_goal = odom_tform_body * body_tform_goal # Command the robot to go to the goal point in the odom frame. The command will stop at the new # position in the odom frame. robot_cmd = RobotCommandBuilder.trajectory_command( goal_x=odom_tform_goal.x, goal_y=odom_tform_goal.y, goal_heading=odom_tform_goal.rot.to_yaw(), frame_name=ODOM_FRAME_NAME) end_time = 5.0 robot_command_client.robot_command(lease=None, command=robot_cmd, end_time_secs=time.time() + end_time) time.sleep(end_time) return True
def manual_control_loop(self): # Create robot object. if self.config.norobot: robot_command_client = None robot_state_client = None else: sdk = bosdyn.client.create_standard_sdk('RobotCommandMaster') sdk.load_app_token(self.config.app_token) robot = sdk.create_robot(self.config.hostname) robot.authenticate(self.config.username, self.config.password) # Check that an estop is connected with the robot so that the robot commands can be executed. verify_estop(robot) # Create the lease client. lease_client = robot.ensure_client( LeaseClient.default_service_name) lease = lease_client.acquire() robot.time_sync.wait_for_sync() lk = bosdyn.client.lease.LeaseKeepAlive(lease_client) # Setup clients for the robot state and robot command services. robot_state_client = robot.ensure_client( RobotStateClient.default_service_name) robot_command_client = robot.ensure_client( RobotCommandClient.default_service_name) # Power on the robot and stand it up. if not self.config.nopower: robot.power_on() blocking_stand(robot_command_client) # Initialize camera camera_pipeline = camera.init_camera() # Print pose self.get_pose(robot_state_client) is_server_on = False # Control loop while True: cmd = input('Command (wasd/qe/c/r):') if cmd == 'c': break elif cmd == 'r': print("Remote control..") try: self.reset_last_frame() if is_server_on: asyncio.get_event_loop().run_forever() else: is_server_on = True self.remote_control_loop( robot_command_client, robot_state_client, camera_pipeline=camera_pipeline, dummy=(robot_command_client is None)) except KeyboardInterrupt: print("ending remote control") pass else: self.reset_last_frame() pos = self.execute_command(robot_command_client, robot_state_client, cmd, wait_to_stabilize=False) # move_relative(robot_command_client, robot_state_client, timeout=5.0) robot_cmd = RobotCommandBuilder.safe_power_off_command() robot_command_client.robot_command(lease=None, command=robot_cmd, end_time_secs=time.time() + 2.) time.sleep(2.) return True
def arm_surface_contact(config): # See hello_spot.py for an explanation of these lines. bosdyn.client.util.setup_logging(config.verbose) sdk = bosdyn.client.create_standard_sdk('ArmSurfaceContactExample') robot = sdk.create_robot(config.hostname) robot.authenticate(config.username, config.password) robot.time_sync.wait_for_sync() assert robot.has_arm(), "Robot requires an arm to run this example." arm_surface_contact_client = robot.ensure_client( ArmSurfaceContactClient.default_service_name) # Verify the robot is not estopped and that an external application has registered and holds # an estop endpoint. assert not robot.is_estopped(), "Robot is estopped. Please use an external E-Stop client, " \ "such as the estop SDK example, to configure E-Stop." robot_state_client = robot.ensure_client( RobotStateClient.default_service_name) lease_client = robot.ensure_client( bosdyn.client.lease.LeaseClient.default_service_name) lease = lease_client.acquire() try: with bosdyn.client.lease.LeaseKeepAlive(lease_client): # Now, we are ready to power on the robot. This call will block until the power # is on. Commands would fail if this did not happen. We can also check that the robot is # powered at any point. robot.logger.info( "Powering on robot... This may take a several seconds.") robot.power_on(timeout_sec=20) assert robot.is_powered_on(), "Robot power on failed." robot.logger.info("Robot powered on.") # Tell the robot to stand up. The command service is used to issue commands to a robot. # The set of valid commands for a robot depends on hardware configuration. See # SpotCommandHelper for more detailed examples on command building. The robot # command service requires timesync between the robot and the client. robot.logger.info("Commanding robot to stand...") command_client = robot.ensure_client( RobotCommandClient.default_service_name) blocking_stand(command_client, timeout_sec=10) robot.logger.info("Robot standing.") time.sleep(2.0) # Unstow the arm unstow = RobotCommandBuilder.arm_ready_command() # Issue the command via the RobotCommandClient command_client.robot_command(unstow) robot.logger.info("Unstow command issued.") time.sleep(3.0) # ---------- # # Now we'll use the arm_surface_contact service to do an accurate position move with # some amount of force. # # Position of the hand: hand_x = 0.75 # in front of the robot. hand_y_start = 0 # centered hand_y_end = -0.5 # to the right hand_z = 0 # will be ignored since we'll have a force in the Z axis. f_z = -0.05 # percentage of maximum press force, negative to press down # be careful setting this too large, you can knock the robot over percentage_press = geometry_pb2.Vec3(x=0, y=0, z=f_z) hand_vec3_start_rt_body = geometry_pb2.Vec3(x=hand_x, y=hand_y_start, z=hand_z) hand_vec3_end_rt_body = geometry_pb2.Vec3(x=hand_x, y=hand_y_end, z=hand_z) # We want to point the hand straight down the entire time. qw = 0.707 qx = 0 qy = 0.707 qz = 0 body_Q_hand = geometry_pb2.Quaternion(w=qw, x=qx, y=qy, z=qz) # Build a position trajectory body_T_hand1 = geometry_pb2.SE3Pose( position=hand_vec3_start_rt_body, rotation=body_Q_hand) body_T_hand2 = geometry_pb2.SE3Pose(position=hand_vec3_end_rt_body, rotation=body_Q_hand) robot_state = robot_state_client.get_robot_state() odom_T_flat_body = get_a_tform_b( robot_state.kinematic_state.transforms_snapshot, ODOM_FRAME_NAME, GRAV_ALIGNED_BODY_FRAME_NAME) odom_T_hand1 = odom_T_flat_body * math_helpers.SE3Pose.from_obj( body_T_hand1) odom_T_hand2 = odom_T_flat_body * math_helpers.SE3Pose.from_obj( body_T_hand2) # Trajectory length traj_time = 5.0 # in seconds time_since_reference = seconds_to_duration(traj_time) traj_point1 = trajectory_pb2.SE3TrajectoryPoint( pose=odom_T_hand1.to_proto(), time_since_reference=seconds_to_duration(0)) traj_point2 = trajectory_pb2.SE3TrajectoryPoint( pose=odom_T_hand2.to_proto(), time_since_reference=time_since_reference) hand_traj = trajectory_pb2.SE3Trajectory( points=[traj_point1, traj_point2]) # Open the gripper gripper_cmd_packed = RobotCommandBuilder.claw_gripper_open_fraction_command( 0) gripper_command = gripper_cmd_packed.synchronized_command.gripper_command.claw_gripper_command cmd = arm_surface_contact_pb2.ArmSurfaceContact.Request( pose_trajectory_in_task=hand_traj, root_frame_name=ODOM_FRAME_NAME, press_force_percentage=percentage_press, x_axis=arm_surface_contact_pb2.ArmSurfaceContact.Request. AXIS_MODE_POSITION, y_axis=arm_surface_contact_pb2.ArmSurfaceContact.Request. AXIS_MODE_POSITION, z_axis=arm_surface_contact_pb2.ArmSurfaceContact.Request. AXIS_MODE_FORCE, z_admittance=arm_surface_contact_pb2.ArmSurfaceContact.Request. ADMITTANCE_SETTING_LOOSE, # Enable the cross term so that if the arm gets stuck in a rut, it will retract # upwards slightly, preventing excessive lateral forces. xy_to_z_cross_term_admittance=arm_surface_contact_pb2. ArmSurfaceContact.Request.ADMITTANCE_SETTING_VERY_STIFF, gripper_command=gripper_command) # Enable walking cmd.is_robot_following_hand = True # A bias force (in this case, leaning forward) can help improve stability. bias_force_x = -25 cmd.bias_force_ewrt_body.CopyFrom( geometry_pb2.Vec3(x=bias_force_x, y=0, z=0)) proto = arm_surface_contact_service_pb2.ArmSurfaceContactCommand( request=cmd) # Send the request robot.logger.info('Running arm surface contact...') arm_surface_contact_client.arm_surface_contact_command(proto) time.sleep(traj_time + 5.0) robot.logger.info('Turning off...') # Power the robot off. By specifying "cut_immediately=False", a safe power off command # is issued to the robot. This will attempt to sit the robot before powering off. robot.power_off(cut_immediately=False, timeout_sec=20) assert not robot.is_powered_on(), "Robot power off failed." robot.logger.info("Robot safely powered off.") finally: # If we successfully acquired a lease, return it. lease_client.return_lease(lease)
def hello_arm(config): """A simple example of using the Boston Dynamics API to command Spot's arm and body at the same time. Please be aware that this demo causes the robot to walk and move its arm. You can have some control over how much the robot moves -- see _L_ROBOT_SQUARE and _L_ARM_CIRCLE -- but regardless, the robot should have at least (_L_ROBOT_SQUARE + 3) m of space in each direction when this demo is used.""" # See hello_spot.py for an explanation of these lines. bosdyn.client.util.setup_logging(config.verbose) sdk = bosdyn.client.create_standard_sdk('HelloSpotClient') robot = sdk.create_robot(config.hostname) robot.authenticate(config.username, config.password) robot.time_sync.wait_for_sync() assert robot.has_arm(), "Robot requires an arm to run this example." # Verify the robot is not estopped and that an external application has registered and holds # an estop endpoint. assert not robot.is_estopped(), "Robot is estopped. Please use an external E-Stop client, " \ "such as the estop SDK example, to configure E-Stop." lease_client = robot.ensure_client( bosdyn.client.lease.LeaseClient.default_service_name) lease = lease_client.acquire() robot_state_client = robot.ensure_client( RobotStateClient.default_service_name) try: with bosdyn.client.lease.LeaseKeepAlive(lease_client): # Now, we are ready to power on the robot. This call will block until the power # is on. Commands would fail if this did not happen. We can also check that the robot is # powered at any point. robot.logger.info( "Powering on robot... This may take a several seconds.") robot.power_on(timeout_sec=20) assert robot.is_powered_on(), "Robot power on failed." robot.logger.info("Robot powered on.") # Tell the robot to stand up. The command service is used to issue commands to a robot. robot.logger.info("Commanding robot to stand...") command_client = robot.ensure_client( RobotCommandClient.default_service_name) blocking_stand(command_client, timeout_sec=10) robot.logger.info("Robot standing.") time.sleep(1.0) # Unstow the arm # Build the unstow command using RobotCommandBuilder unstow = RobotCommandBuilder.arm_ready_command() # Issue the command via the RobotCommandClient command_client.robot_command(unstow) robot.logger.info("Unstow command issued.") time.sleep(1.0) # Get robot pose in vision frame from robot state (we want to send commands in vision # frame relative to where the robot stands now) robot_state = robot_state_client.get_robot_state() vision_T_world = get_vision_tform_body( robot_state.kinematic_state.transforms_snapshot) # In this demo, the robot will walk in a square while moving its arm in a circle. # There are some parameters that you can set below: # Initialize a robot command message, which we will build out below command = robot_command_pb2.RobotCommand() # points in the square x_vals = np.array([0, 1, 1, 0, 0]) y_vals = np.array([0, 0, 1, 1, 0]) # duration in seconds for each move seconds_arm = _SECONDS_FULL / (_N_POINTS + 1) seconds_body = _SECONDS_FULL / x_vals.size # Commands will be sent in the visual odometry ("vision") frame frame_name = VISION_FRAME_NAME # Build an arm trajectory by assembling points (in meters) # x will be the same for each point x = _L_ROBOT_SQUARE + 0.5 for ii in range(_N_POINTS + 1): # Get coordinates relative to the robot's body y = (_L_ROBOT_SQUARE / 2) - _L_ARM_CIRCLE * (np.cos( 2 * ii * math.pi / _N_POINTS)) z = _VERTICAL_SHIFT + _L_ARM_CIRCLE * (np.sin( 2 * ii * math.pi / _N_POINTS)) # Using the transform we got earlier, transform the points into the world frame x_ewrt_vision, y_ewrt_vision, z_ewrt_vision = vision_T_world.transform_point( x, y, z) # Add a new point to the robot command's arm cartesian command se3 trajectory # This will be an se3 trajectory point point = command.synchronized_command.arm_command.arm_cartesian_command.pose_trajectory_in_task.points.add( ) # Populate this point with the desired position, rotation, and duration information point.pose.position.x = x_ewrt_vision point.pose.position.y = y_ewrt_vision point.pose.position.z = z_ewrt_vision point.pose.rotation.x = vision_T_world.rot.x point.pose.rotation.y = vision_T_world.rot.y point.pose.rotation.z = vision_T_world.rot.z point.pose.rotation.w = vision_T_world.rot.w traj_time = (ii + 1) * seconds_arm duration = seconds_to_duration(traj_time) point.time_since_reference.CopyFrom(duration) # set the frame for the hand trajectory command.synchronized_command.arm_command.arm_cartesian_command.root_frame_name = frame_name # Build a body se2trajectory by first assembling points for ii in range(x_vals.size): # Pull the point in the square relative to the robot and scale according to param x = _L_ROBOT_SQUARE * x_vals[ii] y = _L_ROBOT_SQUARE * y_vals[ii] # Transform desired position into world frame x_ewrt_vision, y_ewrt_vision, z_ewrt_vision = vision_T_world.transform_point( x, y, 0) # Add a new point to the robot command's arm cartesian command se3 trajectory # This will be an se2 trajectory point point = command.synchronized_command.mobility_command.se2_trajectory_request.trajectory.points.add( ) # Populate this point with the desired position, angle, and duration information point.pose.position.x = x_ewrt_vision point.pose.position.y = y_ewrt_vision point.pose.angle = vision_T_world.rot.to_yaw() traj_time = (ii + 1) * seconds_body duration = seconds_to_duration(traj_time) point.time_since_reference.CopyFrom(duration) # set the frame for the body trajectory command.synchronized_command.mobility_command.se2_trajectory_request.se2_frame_name = frame_name # Constrain the robot not to turn, forcing it to strafe laterally. speed_limit = SE2VelocityLimit( max_vel=SE2Velocity(linear=Vec2(x=2, y=2), angular=0), min_vel=SE2Velocity(linear=Vec2(x=-2, y=-2), angular=0)) mobility_params = spot_command_pb2.MobilityParams( vel_limit=speed_limit) command.synchronized_command.mobility_command.params.CopyFrom( RobotCommandBuilder._to_any(mobility_params)) # Send the command using the command client # The SE2TrajectoryRequest requires an end_time, which is set # during the command client call robot.logger.info("Sending arm and body trajectory commands.") command_client.robot_command(command, end_time_secs=time.time() + _SECONDS_FULL) time.sleep(_SECONDS_FULL + 2) # Power the robot off. By specifying "cut_immediately=False", a safe power off command # is issued to the robot. This will attempt to sit the robot before powering off. robot.power_off(cut_immediately=False, timeout_sec=20) assert not robot.is_powered_on(), "Robot power off failed." robot.logger.info("Robot safely powered off.") finally: # If we successfully acquired a lease, return it. lease_client.return_lease(lease)
def main(): import argparse parser = argparse.ArgumentParser() bosdyn.client.util.add_common_arguments(parser) parser.add_argument('--dx', default=0, type=float, help='Position offset in body frame (meters forward).') parser.add_argument('--dy', default=0, type=float, help='Position offset in body frame (meters left).') parser.add_argument('--dyaw', default=0, type=float, help='Position offset in body frame (degrees ccw).') parser.add_argument('--frame', choices=[VISION_FRAME_NAME, ODOM_FRAME_NAME], default=ODOM_FRAME_NAME, help='Send the command in this frame.') parser.add_argument('--stairs', action='store_true', help='Move the robot in stairs mode.') options = parser.parse_args() # Create robot object. sdk = bosdyn.client.create_standard_sdk('RobotCommandMaster') robot = sdk.create_robot(options.hostname) robot.authenticate(options.username, options.password) # Check that an estop is connected with the robot so that the robot commands can be executed. assert not robot.is_estopped(), "Robot is estopped. Please use an external E-Stop client, " \ "such as the estop SDK example, to configure E-Stop." # Create the lease client. lease_client = robot.ensure_client(LeaseClient.default_service_name) # Setup clients for the robot state and robot command services. robot_state_client = robot.ensure_client( RobotStateClient.default_service_name) robot_command_client = robot.ensure_client( RobotCommandClient.default_service_name) lease_client.acquire() with LeaseKeepAlive(lease_client, return_at_exit=True): # Power on the robot and stand it up. robot.time_sync.wait_for_sync() robot.power_on() blocking_stand(robot_command_client) try: return relative_move(options.dx, options.dy, math.radians(options.dyaw), options.frame, robot_command_client, robot_state_client, stairs=options.stairs) finally: # Send a Stop at the end, regardless of what happened. robot_command_client.robot_command( RobotCommandBuilder.stop_command())
def arm_with_body_follow(config): """A simple example that moves the arm and asks the body to move to a good position based on the arm's location.""" # See hello_spot.py for an explanation of these lines. bosdyn.client.util.setup_logging(config.verbose) sdk = bosdyn.client.create_standard_sdk('ArmWithBodyFollowClient') robot = sdk.create_robot(config.hostname) robot.authenticate(config.username, config.password) robot.time_sync.wait_for_sync() assert robot.has_arm(), "Robot requires an arm to run this example." # Verify the robot is not estopped and that an external application has registered and holds # an estop endpoint. assert not robot.is_estopped(), "Robot is estopped. Please use an external E-Stop client, " \ "such as the estop SDK example, to configure E-Stop." robot_state_client = robot.ensure_client( RobotStateClient.default_service_name) lease_client = robot.ensure_client( bosdyn.client.lease.LeaseClient.default_service_name) lease = lease_client.acquire() try: with bosdyn.client.lease.LeaseKeepAlive(lease_client): # Now, we are ready to power on the robot. This call will block until the power # is on. Commands would fail if this did not happen. We can also check that the robot is # powered at any point. robot.logger.info( "Powering on robot... This may take a several seconds.") robot.power_on(timeout_sec=20) assert robot.is_powered_on(), "Robot power on failed." robot.logger.info("Robot powered on.") # Tell the robot to stand up. The command service is used to issue commands to a robot. # The set of valid commands for a robot depends on hardware configuration. See # SpotCommandHelper for more detailed examples on command building. The robot # command service requires timesync between the robot and the client. robot.logger.info("Commanding robot to stand...") command_client = robot.ensure_client( RobotCommandClient.default_service_name) blocking_stand(command_client, timeout_sec=10) robot.logger.info("Robot standing.") time.sleep(2.0) # Move the arm to a spot in front of the robot, and command the body to follow the hand. # Build a position to move the arm to (in meters, relative to the body frame origin.) x = 1.25 y = 0 z = 0.25 hand_pos_rt_body = geometry_pb2.Vec3(x=x, y=y, z=z) # Rotation as a quaternion. qw = 1 qx = 0 qy = 0 qz = 0 body_Q_hand = geometry_pb2.Quaternion(w=qw, x=qx, y=qy, z=qz) # Build the SE(3) pose of the desired hand position in the moving body frame. body_T_hand = geometry_pb2.SE3Pose(position=hand_pos_rt_body, rotation=body_Q_hand) # Transform the desired from the moving body frame to the odom frame. robot_state = robot_state_client.get_robot_state() odom_T_body = get_a_tform_b( robot_state.kinematic_state.transforms_snapshot, ODOM_FRAME_NAME, GRAV_ALIGNED_BODY_FRAME_NAME) odom_T_hand = odom_T_body * math_helpers.SE3Pose.from_obj( body_T_hand) # duration in seconds seconds = 5 # Create the arm command. arm_command = RobotCommandBuilder.arm_pose_command( odom_T_hand.x, odom_T_hand.y, odom_T_hand.z, odom_T_hand.rot.w, odom_T_hand.rot.x, odom_T_hand.rot.y, odom_T_hand.rot.z, ODOM_FRAME_NAME, seconds) # Tell the robot's body to follow the arm follow_arm_command = RobotCommandBuilder.follow_arm_command() # Combine the arm and mobility commands into one synchronized command. command = RobotCommandBuilder.build_synchro_command( follow_arm_command, arm_command) # Send the request command_client.robot_command(command) robot.logger.info('Moving arm to position.') time.sleep(6.0) # Power the robot off. By specifying "cut_immediately=False", a safe power off command # is issued to the robot. This will attempt to sit the robot before powering off. robot.power_off(cut_immediately=False, timeout_sec=20) assert not robot.is_powered_on(), "Robot power off failed." robot.logger.info("Robot safely powered off.") finally: # If we successfully acquired a lease, return it. lease_client.return_lease(lease)
def joint_move_example(config): """A simple example of using the Boston Dynamics API to command Spot's arm to perform joint moves.""" # See hello_spot.py for an explanation of these lines. bosdyn.client.util.setup_logging(config.verbose) sdk = bosdyn.client.create_standard_sdk('ArmJointMoveClient') robot = sdk.create_robot(config.hostname) robot.authenticate(config.username, config.password) robot.time_sync.wait_for_sync() assert robot.has_arm(), "Robot requires an arm to run this example." # Verify the robot is not estopped and that an external application has registered and holds # an estop endpoint. verify_estop(robot) robot_state_client = robot.ensure_client( RobotStateClient.default_service_name) lease_client = robot.ensure_client( bosdyn.client.lease.LeaseClient.default_service_name) lease = lease_client.acquire() try: with bosdyn.client.lease.LeaseKeepAlive(lease_client): # Now, we are ready to power on the robot. This call will block until the power # is on. Commands would fail if this did not happen. We can also check that the robot is # powered at any point. robot.logger.info( "Powering on robot... This may take a several seconds.") robot.power_on(timeout_sec=20) assert robot.is_powered_on(), "Robot power on failed." robot.logger.info("Robot powered on.") # Tell the robot to stand up. The command service is used to issue commands to a robot. # The set of valid commands for a robot depends on hardware configuration. See # SpotCommandHelper for more detailed examples on command building. The robot # command service requires timesync between the robot and the client. robot.logger.info("Commanding robot to stand...") command_client = robot.ensure_client( RobotCommandClient.default_service_name) blocking_stand(command_client, timeout_sec=10) robot.logger.info("Robot standing.") time.sleep(2.0) # Example 1: issue a single point trajectory without a time_since_reference in order to perform # a minimum time joint move to the goal obeying the default acceleration and velocity limits. sh0 = 0.0692 sh1 = -1.882 el0 = 1.652 el1 = -0.0691 wr0 = 1.622 wr1 = 1.550 traj_point = RobotCommandBuilder.create_arm_joint_trajectory_point( sh0, sh1, el0, el1, wr0, wr1) arm_joint_traj = arm_command_pb2.ArmJointTrajectory( points=[traj_point]) # Make a RobotCommand command = make_robot_command(arm_joint_traj) # Send the request cmd_id = command_client.robot_command(command) robot.logger.info('Moving arm to position 1.') # Query for feedback to determine how long the goto will take. feedback_resp = command_client.robot_command_feedback(cmd_id) robot.logger.info("Feedback for Example 1: single point goto") time_to_goal = print_feedback(feedback_resp, robot.logger) time.sleep(time_to_goal) # Example 2: Single point trajectory with maximum acceleration/velocity constraints specified such # that the solver has to modify the desired points to honor the constraints sh0 = 0.0 sh1 = -2.0 el0 = 2.6 el1 = 0.0 wr0 = -0.6 wr1 = 0.0 max_vel = wrappers_pb2.DoubleValue(value=1) max_acc = wrappers_pb2.DoubleValue(value=5) traj_point = RobotCommandBuilder.create_arm_joint_trajectory_point( sh0, sh1, el0, el1, wr0, wr1, time_since_reference_secs=1.5) arm_joint_traj = arm_command_pb2.ArmJointTrajectory( points=[traj_point], maximum_velocity=max_vel, maximum_acceleration=max_acc) # Make a RobotCommand command = make_robot_command(arm_joint_traj) # Send the request cmd_id = command_client.robot_command(command) robot.logger.info( 'Requesting a single point trajectory with unsatisfiable constraints.' ) # Query for feedback feedback_resp = command_client.robot_command_feedback(cmd_id) robot.logger.info( "Feedback for Example 2: planner modifies trajectory") time_to_goal = print_feedback(feedback_resp, robot.logger) time.sleep(time_to_goal) # Example 3: Single point trajectory with default acceleration/velocity constraints and # time_since_reference_secs large enough such that the solver can plan a solution to the # points that also satisfies the constraints. sh0 = 0.0692 sh1 = -1.882 el0 = 1.652 el1 = -0.0691 wr0 = 1.622 wr1 = 1.550 traj_point = RobotCommandBuilder.create_arm_joint_trajectory_point( sh0, sh1, el0, el1, wr0, wr1, time_since_reference_secs=1.5) arm_joint_traj = arm_command_pb2.ArmJointTrajectory( points=[traj_point]) # Make a RobotCommand command = make_robot_command(arm_joint_traj) # Send the request cmd_id = command_client.robot_command(command) robot.logger.info( 'Requesting a single point trajectory with satisfiable constraints.' ) # Query for feedback feedback_resp = command_client.robot_command_feedback(cmd_id) robot.logger.info("Feedback for Example 3: unmodified trajectory") time_to_goal = print_feedback(feedback_resp, robot.logger) time.sleep(time_to_goal) # ----- Do a two-point joint move trajectory ------ # First stow the arm. # Build the stow command using RobotCommandBuilder stow = RobotCommandBuilder.arm_stow_command() # Issue the command via the RobotCommandClient command_client.robot_command(stow) robot.logger.info("Stow command issued.") time.sleep(2.0) # First point position sh0 = -1.5 sh1 = -0.8 el0 = 1.7 el1 = 0.0 wr0 = 0.5 wr1 = 0.0 # First point time (seconds) first_point_t = 2.0 # Build the proto for the trajectory point. traj_point1 = RobotCommandBuilder.create_arm_joint_trajectory_point( sh0, sh1, el0, el1, wr0, wr1, first_point_t) # Second point position sh0 = 1.0 sh1 = -0.2 el0 = 1.3 el1 = -1.3 wr0 = -1.5 wr1 = 1.5 # Second point time (seconds) second_point_t = 4.0 # Build the proto for the second trajectory point. traj_point2 = RobotCommandBuilder.create_arm_joint_trajectory_point( sh0, sh1, el0, el1, wr0, wr1, second_point_t) # Optionally, set the maximum allowable velocity in rad/s that a joint is allowed to # travel at. Also set the maximum allowable acceleration in rad/s^2 that a joint is # allowed to accelerate at. If these values are not set, a default will be used on # the robot. # Note that if these values are set too low and the trajectories are too aggressive # in terms of time, the desired joint angles will not be hit at the specified time. # Some other ways to help the robot actually hit the specified trajectory is to # increase the time between trajectory points, or to only specify joint position # goals without specifying velocity goals. max_vel = wrappers_pb2.DoubleValue(value=2.5) max_acc = wrappers_pb2.DoubleValue(value=15) # Build up a proto. arm_joint_traj = arm_command_pb2.ArmJointTrajectory( points=[traj_point1, traj_point2], maximum_velocity=max_vel, maximum_acceleration=max_acc) # Make a RobotCommand command = make_robot_command(arm_joint_traj) # Send the request cmd_id = command_client.robot_command(command) robot.logger.info('Moving arm along 2-point joint trajectory.') # Query for feedback to determine exactly what the planned trajectory is. feedback_resp = command_client.robot_command_feedback(cmd_id) robot.logger.info("Feedback for 2-point joint trajectory") print_feedback(feedback_resp, robot.logger) # Wait until the move completes before powering off. block_until_arm_arrives(command_client, cmd_id, second_point_t + 3.0) # Power the robot off. By specifying "cut_immediately=False", a safe power off command # is issued to the robot. This will attempt to sit the robot before powering off. robot.power_off(cut_immediately=False, timeout_sec=20) assert not robot.is_powered_on(), "Robot power off failed." robot.logger.info("Robot safely powered off.") finally: # If we successfully acquired a lease, return it. lease_client.return_lease(lease)
def gaze_control(config): """Commanding a gaze with Spot's arm.""" # See hello_spot.py for an explanation of these lines. bosdyn.client.util.setup_logging(config.verbose) sdk = bosdyn.client.create_standard_sdk('GazeDemoClient') robot = sdk.create_robot(config.hostname) robot.authenticate(config.username, config.password) robot.time_sync.wait_for_sync() assert robot.has_arm(), "Robot requires an arm to run this example." # Verify the robot is not estopped and that an external application has registered and holds # an estop endpoint. assert not robot.is_estopped(), "Robot is estopped. Please use an external E-Stop client, " \ "such as the estop SDK example, to configure E-Stop." robot_state_client = robot.ensure_client( RobotStateClient.default_service_name) lease_client = robot.ensure_client( bosdyn.client.lease.LeaseClient.default_service_name) lease = lease_client.acquire() try: with bosdyn.client.lease.LeaseKeepAlive(lease_client): # Now, we are ready to power on the robot. This call will block until the power # is on. Commands would fail if this did not happen. We can also check that the robot is # powered at any point. robot.logger.info( "Powering on robot... This may take a several seconds.") robot.power_on(timeout_sec=20) assert robot.is_powered_on(), "Robot power on failed." robot.logger.info("Robot powered on.") # Tell the robot to stand up. The command service is used to issue commands to a robot. # The set of valid commands for a robot depends on hardware configuration. See # SpotCommandHelper for more detailed examples on command building. The robot # command service requires timesync between the robot and the client. robot.logger.info("Commanding robot to stand...") command_client = robot.ensure_client( RobotCommandClient.default_service_name) blocking_stand(command_client, timeout_sec=10) robot.logger.info("Robot standing.") time.sleep(2.0) # Unstow the arm unstow = RobotCommandBuilder.arm_ready_command() # Issue the command via the RobotCommandClient command_client.robot_command(unstow) robot.logger.info("Unstow command issued.") time.sleep(3.0) # Convert the location from the moving base frame to the world frame. robot_state = robot_state_client.get_robot_state() odom_T_flat_body = get_a_tform_b( robot_state.kinematic_state.transforms_snapshot, ODOM_FRAME_NAME, GRAV_ALIGNED_BODY_FRAME_NAME) # Look at a point 3 meters in front and 4 meters to the left. # We are not specifying a hand location, the robot will pick one. gaze_target_in_odom = odom_T_flat_body.transform_point(x=3.0, y=4.0, z=0) gaze_command = RobotCommandBuilder.arm_gaze_command( gaze_target_in_odom[0], gaze_target_in_odom[1], gaze_target_in_odom[2], ODOM_FRAME_NAME) # Make the open gripper RobotCommand gripper_command = RobotCommandBuilder.claw_gripper_open_command() # Combine the arm and gripper commands into one RobotCommand synchro_command = RobotCommandBuilder.build_synchro_command( gripper_command, gaze_command) # Send the request robot.logger.info("Requesting gaze.") command_client.robot_command(synchro_command) time.sleep(4.0) # Look to the left and the right with the hand. # Robot's frame is X+ forward, Z+ up, so left and right is +/- in Y. x = 4.0 # look 4 meters ahead start_y = 2.0 end_y = -2.0 z = 0 # Look ahead, not up or down traj_time = 5.5 # take 5.5 seconds to look from left to right. start_pos_in_odom_tuple = odom_T_flat_body.transform_point( x=x, y=start_y, z=z) start_pos_in_odom = geometry_pb2.Vec3(x=start_pos_in_odom_tuple[0], y=start_pos_in_odom_tuple[1], z=start_pos_in_odom_tuple[2]) end_pos_in_odom_tuple = odom_T_flat_body.transform_point(x=x, y=end_y, z=z) end_pos_in_odom = geometry_pb2.Vec3(x=end_pos_in_odom_tuple[0], y=end_pos_in_odom_tuple[1], z=end_pos_in_odom_tuple[2]) # Create the trajectory points point1 = trajectory_pb2.Vec3TrajectoryPoint( point=start_pos_in_odom) duration_seconds = int(traj_time) duration_nanos = int((traj_time - duration_seconds) * 1e9) point2 = trajectory_pb2.Vec3TrajectoryPoint( point=end_pos_in_odom, time_since_reference=duration_pb2.Duration( seconds=duration_seconds, nanos=duration_nanos)) # Build the trajectory proto traj_proto = trajectory_pb2.Vec3Trajectory(points=[point1, point2]) # Build the proto gaze_cmd = arm_command_pb2.GazeCommand.Request( target_trajectory_in_frame1=traj_proto, frame1_name=ODOM_FRAME_NAME, frame2_name=ODOM_FRAME_NAME) arm_command = arm_command_pb2.ArmCommand.Request( arm_gaze_command=gaze_cmd) synchronized_command = synchronized_command_pb2.SynchronizedCommand.Request( arm_command=arm_command) command = robot_command_pb2.RobotCommand( synchronized_command=synchronized_command) # Make the open gripper RobotCommand gripper_command = RobotCommandBuilder.claw_gripper_open_command() # Combine the arm and gripper commands into one RobotCommand synchro_command = RobotCommandBuilder.build_synchro_command( gripper_command, command) # Send the request gaze_cmd_id = command_client.robot_command(command) robot.logger.info('Sending gaze trajectory.') # Wait until the robot completes the gaze before issuing the next command. block_until_arm_arrives(command_client, gaze_cmd_id, timeout_sec=traj_time + 3.0) # ------------- # # Now make a gaze trajectory that moves the hand around while maintaining the gaze. # We'll use the same trajectory as before, but add a trajectory for the hand to move to. # Hand will start to the left (while looking left) and move to the right. hand_x = 0.75 # in front of the robot. hand_y = 0 # centered hand_z_start = 0 # body height hand_z_end = 0.25 # above body height hand_vec3_start = geometry_pb2.Vec3(x=hand_x, y=hand_y, z=hand_z_start) hand_vec3_end = geometry_pb2.Vec3(x=hand_x, y=hand_y, z=hand_z_end) # We specify an orientation for the hand, which the robot will use its remaining degree # of freedom to achieve. Most of it will be ignored in favor of the gaze direction. qw = 1 qx = 0 qy = 0 qz = 0 quat = geometry_pb2.Quaternion(w=qw, x=qx, y=qy, z=qz) # Build a trajectory hand_pose1_in_flat_body = geometry_pb2.SE3Pose( position=hand_vec3_start, rotation=quat) hand_pose2_in_flat_body = geometry_pb2.SE3Pose( position=hand_vec3_end, rotation=quat) hand_pose1_in_odom = odom_T_flat_body * math_helpers.SE3Pose.from_obj( hand_pose1_in_flat_body) hand_pose2_in_odom = odom_T_flat_body * math_helpers.SE3Pose.from_obj( hand_pose2_in_flat_body) traj_point1 = trajectory_pb2.SE3TrajectoryPoint( pose=hand_pose1_in_odom.to_proto()) # We'll make this trajectory the same length as the one above. traj_point2 = trajectory_pb2.SE3TrajectoryPoint( pose=hand_pose2_in_odom.to_proto(), time_since_reference=duration_pb2.Duration( seconds=duration_seconds, nanos=duration_nanos)) hand_traj = trajectory_pb2.SE3Trajectory( points=[traj_point1, traj_point2]) # Build the proto gaze_cmd = arm_command_pb2.GazeCommand.Request( target_trajectory_in_frame1=traj_proto, frame1_name=ODOM_FRAME_NAME, tool_trajectory_in_frame2=hand_traj, frame2_name=ODOM_FRAME_NAME) arm_command = arm_command_pb2.ArmCommand.Request( arm_gaze_command=gaze_cmd) synchronized_command = synchronized_command_pb2.SynchronizedCommand.Request( arm_command=arm_command) command = robot_command_pb2.RobotCommand( synchronized_command=synchronized_command) # Make the open gripper RobotCommand gripper_command = RobotCommandBuilder.claw_gripper_open_command() # Combine the arm and gripper commands into one RobotCommand synchro_command = RobotCommandBuilder.build_synchro_command( gripper_command, command) # Send the request gaze_cmd_id = command_client.robot_command(synchro_command) robot.logger.info('Sending gaze trajectory with hand movement.') # Wait until the robot completes the gaze before powering off. block_until_arm_arrives(command_client, gaze_cmd_id, timeout_sec=traj_time + 3.0) # Power the robot off. By specifying "cut_immediately=False", a safe power off command # is issued to the robot. This will attempt to sit the robot before powering off. robot.power_off(cut_immediately=False, timeout_sec=20) assert not robot.is_powered_on(), "Robot power off failed." robot.logger.info("Robot safely powered off.") finally: # If we successfully acquired a lease, return it. lease_client.return_lease(lease)
def run_gcode_program(config): """A simple example of using the Boston Dynamics API to command a Spot robot.""" config_parser = configparser.ConfigParser() config_parser.read_file(open('gcode.cfg')) gcode_file = config_parser.get("General", "gcode_file") scale = config_parser.getfloat("General", "scale") min_dist_to_goal = config_parser.getfloat("General", "min_dist_to_goal") allow_walking = config_parser.getboolean("General", "allow_walking") velocity = config_parser.getfloat("General", "velocity") press_force_percent = config_parser.getfloat("General", "press_force_percent") below_z_is_admittance = config_parser.getfloat("General", "below_z_is_admittance") travel_z = config_parser.getfloat("General", "travel_z") gcode_start_x = config_parser.getfloat("General", "gcode_start_x") gcode_start_y = config_parser.getfloat("General", "gcode_start_y") draw_on_wall = config_parser.getboolean("General", "draw_on_wall") use_vision_frame = config_parser.getboolean("General", "use_vision_frame") use_xy_to_z_cross_term = config_parser.getboolean("General", "use_xy_to_z_cross_term") bias_force_x = config_parser.getfloat("General", "bias_force_x") if config_parser.has_option("General", "walk_to_at_end_rt_gcode_origin_x") and config_parser.has_option( "General", "walk_to_at_end_rt_gcode_origin_y"): walk_to_at_end_rt_gcode_origin_x = config_parser.getfloat( "General", "walk_to_at_end_rt_gcode_origin_x") walk_to_at_end_rt_gcode_origin_y = config_parser.getfloat( "General", "walk_to_at_end_rt_gcode_origin_y") else: walk_to_at_end_rt_gcode_origin_x = None walk_to_at_end_rt_gcode_origin_y = None if velocity <= 0: print('Velocity must be greater than 0. Currently is: ', velocity) return if use_vision_frame: api_send_frame = VISION_FRAME_NAME else: api_send_frame = ODOM_FRAME_NAME # The Boston Dynamics Python library uses Python's logging module to # generate output. Applications using the library can specify how # the logging information should be output. bosdyn.client.util.setup_logging(config.verbose) # The SDK object is the primary entry point to the Boston Dynamics API. # create_standard_sdk will initialize an SDK object with typical default # parameters. The argument passed in is a string identifying the client. sdk = bosdyn.client.create_standard_sdk('GcodeClient') # A Robot object represents a single robot. Clients using the Boston # Dynamics API can manage multiple robots, but this tutorial limits # access to just one. The network address of the robot needs to be # specified to reach it. This can be done with a DNS name # (e.g. spot.intranet.example.com) or an IP literal (e.g. 10.0.63.1) robot = sdk.create_robot(config.hostname) # Clients need to authenticate to a robot before being able to use it. robot.authenticate(config.username, config.password) # Establish time sync with the robot. This kicks off a background thread to establish time sync. # Time sync is required to issue commands to the robot. After starting time sync thread, block # until sync is established. robot.time_sync.wait_for_sync() # Verify the robot has an arm. assert robot.has_arm(), "Robot requires an arm to run the gcode example." # Verify the robot is not estopped and that an external application has registered and holds # an estop endpoint. assert not robot.is_estopped(), "Robot is estopped. Please use an external E-Stop client, " \ "such as the estop SDK example, to configure E-Stop." arm_surface_contact_client = robot.ensure_client(ArmSurfaceContactClient.default_service_name) # Only one client at a time can operate a robot. Clients acquire a lease to # indicate that they want to control a robot. Acquiring may fail if another # client is currently controlling the robot. When the client is done # controlling the robot, it should return the lease so other clients can # control it. Note that the lease is returned as the "finally" condition in this # try-catch-finally block. lease_client = robot.ensure_client(bosdyn.client.lease.LeaseClient.default_service_name) lease = lease_client.acquire() try: with bosdyn.client.lease.LeaseKeepAlive(lease_client): # Now, we are ready to power on the robot. This call will block until the power # is on. Commands would fail if this did not happen. We can also check that the robot is # powered at any point. robot.logger.info("Powering on robot... This may take a several seconds.") robot.power_on(timeout_sec=20) assert robot.is_powered_on(), "Robot power on failed." robot.logger.info("Robot powered on.") # Tell the robot to stand up. The command service is used to issue commands to a robot. # The set of valid commands for a robot depends on hardware configuration. See # SpotCommandHelper for more detailed examples on command building. The robot # command service requires timesync between the robot and the client. robot.logger.info("Commanding robot to stand...") command_client = robot.ensure_client(RobotCommandClient.default_service_name) blocking_stand(command_client, timeout_sec=10) robot.logger.info("Robot standing.") robot_state_client = robot.ensure_client(RobotStateClient.default_service_name) # Update state robot_state = robot_state_client.get_robot_state() gcode = GcodeReader(gcode_file, scale, robot.logger, below_z_is_admittance, travel_z, draw_on_wall, gcode_start_x, gcode_start_y) # Prep arm # Build a position to move the arm to (in meters, relative to the body frame's origin) x = 0.75 y = 0 if not draw_on_wall: z = -0.35 qw = .707 qx = 0 qy = .707 qz = 0 else: z = -0.25 qw = 1 qx = 0 qy = 0 qz = 0 flat_body_T_hand = math_helpers.SE3Pose(x, y, z, math_helpers.Quat(w=qw, x=qx, y=qy, z=qz)) odom_T_flat_body = get_a_tform_b(robot_state.kinematic_state.transforms_snapshot, ODOM_FRAME_NAME, GRAV_ALIGNED_BODY_FRAME_NAME) odom_T_hand = odom_T_flat_body * flat_body_T_hand robot.logger.info('Moving arm to starting position.') # Send the request odom_T_hand_obj = odom_T_hand.to_proto() move_time = 0.000001 # move as fast as possible because we will use (default) velocity/accel limiting. arm_command = RobotCommandBuilder.arm_pose_command( odom_T_hand_obj.position.x, odom_T_hand_obj.position.y, odom_T_hand_obj.position.z, odom_T_hand_obj.rotation.w, odom_T_hand_obj.rotation.x, odom_T_hand_obj.rotation.y, odom_T_hand_obj.rotation.z, ODOM_FRAME_NAME, move_time) command = RobotCommandBuilder.build_synchro_command(arm_command) cmd_id = command_client.robot_command(command) # Wait for the move to complete block_until_arm_arrives(command_client, cmd_id) # Update state and Get the hand position robot_state = robot_state_client.get_robot_state() (world_T_body, body_T_hand, world_T_hand, odom_T_body) = get_transforms( use_vision_frame, robot_state) world_T_admittance_frame = geometry_pb2.SE3Pose( position=geometry_pb2.Vec3(x=0, y=0, z=0), rotation=geometry_pb2.Quaternion(w=1, x=0, y=0, z=0)) if draw_on_wall: # Create an admittance frame that has Z- along the robot's X axis xhat_ewrt_robot = [0, 0, 1] xhat_ewrt_vo = [0, 0, 0] (xhat_ewrt_vo[0], xhat_ewrt_vo[1], xhat_ewrt_vo[2]) = world_T_body.rot.transform_point( xhat_ewrt_robot[0], xhat_ewrt_robot[1], xhat_ewrt_robot[2]) (z1, z2, z3) = world_T_body.rot.transform_point(-1, 0, 0) zhat_temp = [z1, z2, z3] zhat = make_orthogonal(xhat_ewrt_vo, zhat_temp) yhat = np.cross(zhat, xhat_ewrt_vo) mat = np.array([xhat_ewrt_vo, yhat, zhat]).transpose() q_wall = Quat.from_matrix(mat) zero_vec3 = geometry_pb2.Vec3(x=0, y=0, z=0) q_wall_proto = geometry_pb2.Quaternion(w=q_wall.w, x=q_wall.x, y=q_wall.y, z=q_wall.z) world_T_admittance_frame = geometry_pb2.SE3Pose(position=zero_vec3, rotation=q_wall_proto) # Touch the ground/wall move_arm(robot_state, True, [world_T_hand], arm_surface_contact_client, velocity, allow_walking, world_T_admittance_frame, press_force_percent, api_send_frame, use_xy_to_z_cross_term, bias_force_x) time.sleep(4.0) last_admittance = True # Update state robot_state = robot_state_client.get_robot_state() # Get the hand position (world_T_body, body_T_hand, world_T_hand, odom_T_body) = get_transforms( use_vision_frame, robot_state) odom_T_ground_plane = get_a_tform_b(robot_state.kinematic_state.transforms_snapshot, "odom", "gpe") world_T_odom = world_T_body * odom_T_body.inverse() (gx, gy, gz) = world_T_odom.transform_point(odom_T_ground_plane.x, odom_T_ground_plane.y, odom_T_ground_plane.z) ground_plane_rt_vo = [gx, gy, gz] # Compute the robot's position on the ground plane. #ground_plane_T_robot = odom_T_ground_plane.inverse() * # Compute an origin. if not draw_on_wall: # For on the ground: # xhat = body x # zhat = (0,0,1) # Ensure the origin is gravity aligned, otherwise we get some height drift. zhat = [0.0, 0.0, 1.0] (x1, x2, x3) = world_T_body.rot.transform_point(1.0, 0.0, 0.0) xhat_temp = [x1, x2, x3] xhat = make_orthogonal(zhat, xhat_temp) yhat = np.cross(zhat, xhat) mat = np.array([xhat, yhat, zhat]).transpose() vo_Q_origin = Quat.from_matrix(mat) world_T_origin = SE3Pose(world_T_hand.x, world_T_hand.y, world_T_hand.z, vo_Q_origin) else: # todo should I use the same one? world_T_origin = world_T_hand gcode.set_origin(world_T_origin, world_T_admittance_frame) robot.logger.info('Origin set') (is_admittance, world_T_goals, is_pause) = gcode.get_next_world_T_goals(ground_plane_rt_vo) while is_pause: do_pause() (is_admittance, world_T_goals, is_pause) = gcode.get_next_world_T_goals(ground_plane_rt_vo) if world_T_goals is None: # we're done! done = True move_arm(robot_state, is_admittance, world_T_goals, arm_surface_contact_client, velocity, allow_walking, world_T_admittance_frame, press_force_percent, api_send_frame, use_xy_to_z_cross_term, bias_force_x) odom_T_hand_goal = world_T_odom.inverse() * world_T_goals[-1] last_admittance = is_admittance done = False while not done: # Update state robot_state = robot_state_client.get_robot_state() # Determine if we are at the goal point (world_T_body, body_T_hand, world_T_hand, odom_T_body) = get_transforms( use_vision_frame, robot_state) (gx, gy, gz) = world_T_odom.transform_point(odom_T_ground_plane.x, odom_T_ground_plane.y, odom_T_ground_plane.z) ground_plane_rt_vo = [gx, gy, gz] world_T_odom = world_T_body * odom_T_body.inverse() odom_T_hand = odom_T_body * body_T_hand admittance_frame_T_world = math_helpers.SE3Pose.from_obj( world_T_admittance_frame).inverse() admit_frame_T_hand = admittance_frame_T_world * world_T_odom * odom_T_body * body_T_hand admit_frame_T_hand_goal = admittance_frame_T_world * world_T_odom * odom_T_hand_goal if is_admittance: dist = math.sqrt((admit_frame_T_hand.x - admit_frame_T_hand_goal.x)**2 + (admit_frame_T_hand.y - admit_frame_T_hand_goal.y)**2) #+ (admit_frame_T_hand.z - admit_frame_T_hand_goal.z)**2 ) else: dist = math.sqrt((admit_frame_T_hand.x - admit_frame_T_hand_goal.x)**2 + (admit_frame_T_hand.y - admit_frame_T_hand_goal.y)**2 + (admit_frame_T_hand.z - admit_frame_T_hand_goal.z)**2) arm_near_goal = dist < min_dist_to_goal if arm_near_goal: # Compute where to go. (is_admittance, world_T_goals, is_pause) = gcode.get_next_world_T_goals(ground_plane_rt_vo) while is_pause: do_pause() (is_admittance, world_T_goals, is_pause) = gcode.get_next_world_T_goals(ground_plane_rt_vo) if world_T_goals is None: # we're done! done = True robot.logger.info("Gcode program finished.") break move_arm(robot_state, is_admittance, world_T_goals, arm_surface_contact_client, velocity, allow_walking, world_T_admittance_frame, press_force_percent, api_send_frame, use_xy_to_z_cross_term, bias_force_x) odom_T_hand_goal = world_T_odom.inverse() * world_T_goals[-1] if is_admittance != last_admittance: if is_admittance: print('Waiting for touchdown...') time.sleep(3.0) # pause to wait for touchdown else: time.sleep(1.0) last_admittance = is_admittance elif not is_admittance: # We are in a travel move, so we'll keep updating to account for a changing # ground plane. (is_admittance, world_T_goals, is_pause) = gcode.get_next_world_T_goals( ground_plane_rt_vo, read_new_line=False) # At the end, walk back to the start. robot.logger.info('Done with gcode, going to stand...') blocking_stand(command_client, timeout_sec=10) robot.logger.info("Robot standing") # Compute walking location if walk_to_at_end_rt_gcode_origin_x is not None and walk_to_at_end_rt_gcode_origin_y is not None: robot.logger.info("Walking to end position...") gcode_origin_T_walk = SE3Pose(walk_to_at_end_rt_gcode_origin_x * scale, walk_to_at_end_rt_gcode_origin_y * scale, 0, Quat(1, 0, 0, 0)) odom_T_walk = world_T_odom.inverse() * gcode.world_T_origin * gcode_origin_T_walk odom_T_walk_se2 = SE2Pose.flatten(odom_T_walk) # Command the robot to go to the end point. walk_cmd = RobotCommandBuilder.trajectory_command( goal_x=odom_T_walk_se2.x, goal_y=odom_T_walk_se2.y, goal_heading=odom_T_walk_se2.angle, frame_name="odom") end_time = 15.0 #Issue the command to the robot command_client.robot_command(command=walk_cmd, end_time_secs=time.time() + end_time) time.sleep(end_time) robot.logger.info('Done.') # Power the robot off. By specifying "cut_immediately=False", a safe power off command # is issued to the robot. This will attempt to sit the robot before powering off. robot.power_off(cut_immediately=False, timeout_sec=20) assert not robot.is_powered_on(), "Robot power off failed." robot.logger.info("Robot safely powered off.") finally: # If we successfully acquired a lease, return it. lease_client.return_lease(lease)
def walk_to_object(config): """Get an image and command the robot to walk up to a selected object. We'll walk "up to" the object, not on top of it. The idea is that you want to interact or manipulate the object.""" # See hello_spot.py for an explanation of these lines. bosdyn.client.util.setup_logging(config.verbose) sdk = bosdyn.client.create_standard_sdk('WalkToObjectClient') robot = sdk.create_robot(config.hostname) robot.authenticate(config.username, config.password) robot.time_sync.wait_for_sync() assert robot.has_arm(), "Robot requires an arm to run this example." # Verify the robot is not estopped and that an external application has registered and holds # an estop endpoint. assert not robot.is_estopped(), "Robot is estopped. Please use an external E-Stop client, " \ "such as the estop SDK example, to configure E-Stop." lease_client = robot.ensure_client( bosdyn.client.lease.LeaseClient.default_service_name) image_client = robot.ensure_client(ImageClient.default_service_name) manipulation_api_client = robot.ensure_client( ManipulationApiClient.default_service_name) lease = lease_client.acquire() try: with bosdyn.client.lease.LeaseKeepAlive(lease_client): # Now, we are ready to power on the robot. This call will block until the power # is on. Commands would fail if this did not happen. We can also check that the robot is # powered at any point. robot.logger.info( "Powering on robot... This may take a several seconds.") robot.power_on(timeout_sec=20) assert robot.is_powered_on(), "Robot power on failed." robot.logger.info("Robot powered on.") # Tell the robot to stand up. The command service is used to issue commands to a robot. # The set of valid commands for a robot depends on hardware configuration. See # SpotCommandHelper for more detailed examples on command building. The robot # command service requires timesync between the robot and the client. robot.logger.info("Commanding robot to stand...") command_client = robot.ensure_client( RobotCommandClient.default_service_name) blocking_stand(command_client, timeout_sec=10) robot.logger.info("Robot standing.") time.sleep(2.0) # Take a picture with a camera robot.logger.info('Getting an image from: ' + config.image_source) image_responses = image_client.get_image_from_sources( [config.image_source]) if len(image_responses) != 1: print('Got invalid number of images: ' + str(len(image_responses))) print(image_responses) assert False image = image_responses[0] if image.shot.image.pixel_format == image_pb2.Image.PIXEL_FORMAT_DEPTH_U16: dtype = np.uint16 else: dtype = np.uint8 img = np.fromstring(image.shot.image.data, dtype=dtype) if image.shot.image.format == image_pb2.Image.FORMAT_RAW: img = img.reshape(image.shot.image.rows, image.shot.image.cols) else: img = cv2.imdecode(img, -1) # Show the image to the user and wait for them to click on a pixel robot.logger.info('Click on an object to walk up to...') image_title = 'Click to walk up to something' cv2.namedWindow(image_title) cv2.setMouseCallback(image_title, cv_mouse_callback) global g_image_click, g_image_display g_image_display = img cv2.imshow(image_title, g_image_display) while g_image_click is None: key = cv2.waitKey(1) & 0xFF if key == ord('q') or key == ord('Q'): # Quit print('"q" pressed, exiting.') exit(0) robot.logger.info('Walking to object at image location (' + str(g_image_click[0]) + ', ' + str(g_image_click[1]) + ')') walk_vec = geometry_pb2.Vec2(x=g_image_click[0], y=g_image_click[1]) # Optionally populate the offset distance parameter. if config.distance is None: offset_distance = None else: offset_distance = wrappers_pb2.FloatValue( value=config.distance) # Build the proto walk_to = manipulation_api_pb2.WalkToObjectInImage( pixel_xy=walk_vec, transforms_snapshot_for_camera=image.shot.transforms_snapshot, frame_name_image_sensor=image.shot.frame_name_image_sensor, camera_model=image.source.pinhole, offset_distance=offset_distance) # Ask the robot to pick up the object walk_to_request = manipulation_api_pb2.ManipulationApiRequest( walk_to_object_in_image=walk_to) # Send the request cmd_response = manipulation_api_client.manipulation_api_command( manipulation_api_request=walk_to_request) # Get feedback from the robot while True: time.sleep(0.25) feedback_request = manipulation_api_pb2.ManipulationApiFeedbackRequest( manipulation_cmd_id=cmd_response.manipulation_cmd_id) # Send the request response = manipulation_api_client.manipulation_api_feedback_command( manipulation_api_feedback_request=feedback_request) print( 'Current state: ', manipulation_api_pb2.ManipulationFeedbackState.Name( response.current_state)) if response.current_state == manipulation_api_pb2.MANIP_STATE_DONE: break robot.logger.info('Finished.') time.sleep(4.0) robot.logger.info('Sitting down and turning off.') # Power the robot off. By specifying "cut_immediately=False", a safe power off command # is issued to the robot. This will attempt to sit the robot before powering off. robot.power_off(cut_immediately=False, timeout_sec=20) assert not robot.is_powered_on(), "Robot power off failed." robot.logger.info("Robot safely powered off.") finally: # If we successfully acquired a lease, return it. lease_client.return_lease(lease)
def hello_spot(config): """A simple example of using the Boston Dynamics API to command a Spot robot.""" # The Boston Dynamics Python library uses Python's logging module to # generate output. Applications using the library can specify how # the logging information should be output. bosdyn.client.util.setup_logging(config.verbose) # The SDK object is the primary entry point to the Boston Dynamics API. # create_standard_sdk will initialize an SDK object with typical default # parameters. The argument passed in is a string identifying the client. sdk = bosdyn.client.create_standard_sdk('HelloSpotClient') # A Robot object represents a single robot. Clients using the Boston # Dynamics API can manage multiple robots, but this tutorial limits # access to just one. The network address of the robot needs to be # specified to reach it. This can be done with a DNS name # (e.g. spot.intranet.example.com) or an IP literal (e.g. 10.0.63.1) robot = sdk.create_robot(config.hostname) # Clients need to authenticate to a robot before being able to use it. robot.authenticate(config.username, config.password) # Establish time sync with the robot. This kicks off a background thread to establish time sync. # Time sync is required to issue commands to the robot. After starting time sync thread, block # until sync is established. robot.time_sync.wait_for_sync() # Verify the robot is not estopped and that an external application has registered and holds # an estop endpoint. assert not robot.is_estopped(), "Robot is estopped. Please use an external E-Stop client, " \ "such as the estop SDK example, to configure E-Stop." # Only one client at a time can operate a robot. Clients acquire a lease to # indicate that they want to control a robot. Acquiring may fail if another # client is currently controlling the robot. When the client is done # controlling the robot, it should return the lease so other clients can # control it. Note that the lease is returned as the "finally" condition in this # try-catch-finally block. lease_client = robot.ensure_client(bosdyn.client.lease.LeaseClient.default_service_name) lease = lease_client.acquire() try: with bosdyn.client.lease.LeaseKeepAlive(lease_client): # Now, we are ready to power on the robot. This call will block until the power # is on. Commands would fail if this did not happen. We can also check that the robot is # powered at any point. robot.logger.info("Powering on robot... This may take several seconds.") robot.power_on(timeout_sec=20) assert robot.is_powered_on(), "Robot power on failed." robot.logger.info("Robot powered on.") # Tell the robot to stand up. The command service is used to issue commands to a robot. # The set of valid commands for a robot depends on hardware configuration. See # SpotCommandHelper for more detailed examples on command building. The robot # command service requires timesync between the robot and the client. robot.logger.info("Commanding robot to stand...") command_client = robot.ensure_client(RobotCommandClient.default_service_name) blocking_stand(command_client, timeout_sec=10) robot.logger.info("Robot standing.") time.sleep(3) # Tell the robot to stand in a twisted position. # # The RobotCommandBuilder constructs command messages, which are then # issued to the robot using "robot_command" on the command client. # # In this example, the RobotCommandBuilder generates a stand command # message with a non-default rotation in the footprint frame. The footprint # frame is a gravity aligned frame with its origin located at the geometric # center of the feet. The X axis of the footprint frame points forward along # the robot's length, the Z axis points up aligned with gravity, and the Y # axis is the cross-product of the two. footprint_R_body = bosdyn.geometry.EulerZXY(yaw=0.4, roll=0.0, pitch=0.0) cmd = RobotCommandBuilder.synchro_stand_command(footprint_R_body=footprint_R_body) command_client.robot_command(cmd) robot.logger.info("Robot standing twisted.") time.sleep(3) # Now tell the robot to stand taller, using the same approach of constructing # a command message with the RobotCommandBuilder and issuing it with # robot_command. cmd = RobotCommandBuilder.synchro_stand_command(body_height=0.1) command_client.robot_command(cmd) robot.logger.info("Robot standing tall.") time.sleep(3) # Capture an image. # Spot has five sensors around the body. Each sensor consists of a stereo pair and a # fisheye camera. The list_image_sources RPC gives a list of image sources which are # available to the API client. Images are captured via calls to the get_image RPC. # Images can be requested from multiple image sources in one call. image_client = robot.ensure_client(ImageClient.default_service_name) sources = image_client.list_image_sources() image_response = image_client.get_image_from_sources(['frontleft_fisheye_image']) _maybe_display_image(image_response[0].shot.image) if config.save or config.save_path is not None: _maybe_save_image(image_response[0].shot.image, config.save_path) # Log a comment. # Comments logged via this API are written to the robots test log. This is the best way # to mark a log as "interesting". These comments will be available to Boston Dynamics # devs when diagnosing customer issues. log_comment = "HelloSpot tutorial user comment." robot.operator_comment(log_comment) robot.logger.info('Added comment "%s" to robot log.', log_comment) # Power the robot off. By specifying "cut_immediately=False", a safe power off command # is issued to the robot. This will attempt to sit the robot before powering off. robot.power_off(cut_immediately=False, timeout_sec=20) assert not robot.is_powered_on(), "Robot power off failed." robot.logger.info("Robot safely powered off.") finally: # If we successfully acquired a lease, return it. lease_client.return_lease(lease)
def main(argv): """Command line interface. Args: argv: List of command-line arguments passed to the program. """ parser = argparse.ArgumentParser() parser.add_argument( "--model-path", required=True, help= "Local file path to the Tensorflow model, example pre-trained models \ can be found at \ https://github.com/tensorflow/models/blob/master/research/object_detection/g3doc/detection_model_zoo.md" ) parser.add_argument( "--classes", default='/classes.json', type=str, help="File containing json mapping of object class IDs to class names") parser.add_argument( "--number-tensorflow-processes", default=1, type=int, help="Number of Tensorflow processes to run in parallel") parser.add_argument( "--detection-threshold", default=0.7, type=float, help="Detection threshold to use for Tensorflow detections") parser.add_argument( "--sleep-between-capture", default=1.0, type=float, help= "Seconds to sleep between each image capture loop iteration, which captures " + "an image from all cameras") parser.add_argument( "--detection-class", default=1, type=int, help="Detection classes to use in the" + "Tensorflow model; Default is to use 1, which is a person in the Coco dataset" ) parser.add_argument( "--max-processing-delay", default=7.0, type=float, help="Maximum allowed delay for processing an image; " + "any image older than this value will be skipped") bosdyn.client.util.add_common_arguments(parser) options = parser.parse_args(argv) try: # Make sure the model path is a valid file if not _check_model_path(options.model_path): return False # Check for classes json file, otherwise use the COCO class dictionary _check_and_load_json_classes(options.classes) global TENSORFLOW_PROCESS_BARRIER # pylint: disable=global-statement TENSORFLOW_PROCESS_BARRIER = Barrier( options.number_tensorflow_processes + 1) # Start Tensorflow processes start_tensorflow_processes(options.number_tensorflow_processes, options.model_path, options.detection_class, options.detection_threshold, options.max_processing_delay) # sleep to give the Tensorflow processes time to initialize try: TENSORFLOW_PROCESS_BARRIER.wait() except BrokenBarrierError as exc: print( f'Error waiting for Tensorflow processes to initialize: {exc}') return False # Start the API related things # Create robot object with a world object client sdk = bosdyn.client.create_standard_sdk('SpotFollowClient') robot = sdk.create_robot(options.hostname) robot.authenticate(options.username, options.password) #Time sync is necessary so that time-based filter requests can be converted robot.time_sync.wait_for_sync() # Verify the robot is not estopped and that an external application has registered and holds # an estop endpoint. verify_estop(robot) # Create the sdk clients robot_state_client = robot.ensure_client( RobotStateClient.default_service_name) robot_command_client = robot.ensure_client( RobotCommandClient.default_service_name) lease_client = robot.ensure_client(LeaseClient.default_service_name) image_client = robot.ensure_client(ImageClient.default_service_name) source_list = get_source_list(image_client) image_task = AsyncImage(image_client, source_list) robot_state_task = AsyncRobotState(robot_state_client) task_list = [image_task, robot_state_task] _async_tasks = AsyncTasks(task_list) print('Detect and follow client connected.') lease = lease_client.take() lease_keep = LeaseKeepAlive(lease_client) # Power on the robot and stand it up resp = robot.power_on() try: blocking_stand(robot_command_client) except CommandFailedError as exc: print( f'Error ({exc}) occurred while trying to stand. Check robot surroundings.' ) return False except CommandTimedOutError as exc: print(f'Stand command timed out: {exc}') return False print('Robot powered on and standing.') params_set = get_mobility_params() # This thread starts the async tasks for image and robot state retrieval update_thread = Thread(target=_update_thread, args=[_async_tasks]) update_thread.daemon = True update_thread.start() # Wait for the first responses. while any(task.proto is None for task in task_list): time.sleep(0.1) # Start image capture process image_capture_thread = Thread(target=capture_images, args=( image_task, options.sleep_between_capture, )) image_capture_thread.start() while True: # This comes from the tensorflow processes and limits the rate of this loop entry = PROCESSED_BOXES_QUEUE.get() # find the highest confidence bounding box highest_conf_source = _find_highest_conf_source(entry) if highest_conf_source is None: # no boxes or scores found continue capture_to_use = entry[highest_conf_source] raw_time = capture_to_use['raw_image_time'] time_gap = time.time() - raw_time if time_gap > options.max_processing_delay: continue # Skip image due to delay # Find the transform to highest confidence object using the depth sensor world_tform_object = get_object_position( capture_to_use['world_tform_cam'], capture_to_use['visual_image'], capture_to_use['depth_image'], capture_to_use['boxes'][0], ROTATION_ANGLES[capture_to_use['source']]) # get_object_position can fail if there is insufficient depth sensor information if not world_tform_object: continue scores = capture_to_use['scores'] print( f'Transform for object with confidence {scores[0]}: {world_tform_object}' ) print( f'Process latency: {time.time() - capture_to_use["system_cap_time"]}' ) tag_cmd = get_go_to(world_tform_object, robot_state_task.proto, params_set) end_time = 15.0 if tag_cmd is not None: robot_command_client.robot_command(lease=None, command=tag_cmd, end_time_secs=time.time() + end_time) return True except Exception as exc: # pylint: disable=broad-except LOGGER.error("Spot Tensorflow Detector threw an exception: %s", exc) return False
def gesture_control(config): """A simple example of using the Boston Dynamics API to command a Spot robot.""" # The Boston Dynamics Python library uses Python's logging module to # generate output. Applications using the library can specify how # the logging information should be output. bosdyn.client.util.setup_logging(config.verbose) # The SDK object is the primary entry point to the Boston Dynamics API. # create_standard_sdk will initialize an SDK object with typical default # parameters. The argument passed in is a string identifying the client. sdk = bosdyn.client.create_standard_sdk('HelloSpotClient') # A Robot object represents a single robot. Clients using the Boston # Dynamics API can manage multiple robots, but this tutorial limits # access to just one. The network address of the robot needs to be # specified to reach it. This can be done with a DNS name # (e.g. spot.intranet.example.com) or an IP literal (e.g. 10.0.63.1) robot = sdk.create_robot(config.hostname) # Clients need to authenticate to a robot before being able to use it. robot.authenticate(config.username, config.password) # Establish time sync with the robot. This kicks off a background thread to establish time sync. # Time sync is required to issue commands to the robot. After starting time sync thread, block # until sync is established. robot.time_sync.wait_for_sync() # Verify the robot is not estopped and that an external application has registered and holds # an estop endpoint. assert not robot.is_estopped(), "Robot is estopped. Please use an external E-Stop client, " \ "such as the estop SDK example, to configure E-Stop." # Only one client at a time can operate a robot. Clients acquire a lease to # indicate that they want to control a robot. Acquiring may fail if another # client is currently controlling the robot. When the client is done # controlling the robot, it should return the lease so other clients can # control it. Note that the lease is returned as the "finally" condition in this # try-catch-finally block. lease_client = robot.ensure_client( bosdyn.client.lease.LeaseClient.default_service_name) lease = lease_client.acquire() try: with bosdyn.client.lease.LeaseKeepAlive(lease_client): # Now, we are ready to power on the robot. This call will block until the power # is on. Commands would fail if this did not happen. We can also check that the robot is # powered at any point. robot.logger.info( "Powering on robot... This may take several seconds.") robot.power_on(timeout_sec=20) assert robot.is_powered_on(), "Robot power on failed." robot.logger.info("Robot powered on.") # _robot_command_client = robot.ensure_client(RobotCommandClient.default_service_name) fileHands = mp.solutions.hands hands = fileHands.Hands(max_num_hands=1, min_detection_confidence=0.98) drawHands = mp.solutions.drawing_utils gestureRecognizer = load_model("mp_hand_gesture") print("here") file = open("gesture.names", "r") gestureNames = file.read().split('\n') file.close() vid = cv.VideoCapture(0) while True: _, frame = vid.read() x, y, z = frame.shape frame = cv.flip(frame, 1) frameColored = cv.cvtColor(frame, cv.COLOR_BGR2RGB) gesturePrediction = hands.process(frameColored) gestureName = "" if (gesturePrediction.multi_hand_landmarks): handLandmarks = [] for handDetails in gesturePrediction.multi_hand_landmarks: for landmark in handDetails.landmark: landmarkX = int(landmark.x * x) landmarkY = int(landmark.y * y) handLandmarks.append([landmarkX, landmarkY]) drawHands.draw_landmarks(frame, handDetails, fileHands.HAND_CONNECTIONS) gesture = gestureRecognizer.predict([handLandmarks]) gestureIndex = np.argmax(gesture) gestureName = gestureNames[gestureIndex] print(gestureName) if (gestureName == "walkForward"): print("Forward") # call function for Spot to walk forward if (gestureName == "walkBackward"): robot.power_off(cut_immediately=False, timeout_sec=20) assert not robot.is_powered_on( ), "Robot power off failed." robot.logger.info("Robot safely powered off.") print("Spot, shut down!") # call function for Spot to walk backward if (gestureName == "walkLeft"): print("Spot, take a step to the left!") # call function for Spot to walk left if (gestureName == "walkRight"): print("Spot, take a step to the right!") # call function for Spot to walk right if (gestureName == "turnLeft"): print("Spot, turn to the left!") # call function for Spot to turn left if (gestureName == "turnRight"): print("Spot, turn to the right!") # call function for Spot to turn right if (gestureName == "tiltUp"): robot.logger.info("Commanding robot to stand...") command_client = robot.ensure_client( RobotCommandClient.default_service_name) blocking_stand(command_client, timeout_sec=10) robot.logger.info("Robot standing.") time.sleep(3) print("Spot, stand up!") # call function for Spot to tilt up if (gestureName == "tiltDown"): print("Spot, tilt down!") # call function for Spot to tilt down # potential implementation of time.sleep() cv.imshow("Gesture", frame) if (cv.waitKey(1) == ord("x")): break vid.release() cv.destroyAllWindows() # Tell the robot to stand up. The command service is used to issue commands to a robot. # The set of valid commands for a robot depends on hardware configuration. See # SpotCommandHelper for more detailed examples on command building. The robot # command service requires timesync between the robot and the client. # Tell the robot to stand in a twisted position. # # The RobotCommandBuilder constructs command messages, which are then # issued to the robot using "robot_command" on the command client. # # In this example, the RobotCommandBuilder generates a stand command # message with a non-default rotation in the footprint frame. The footprint # frame is a gravity aligned frame with its origin located at the geometric # center of the feet. The X axis of the footprint frame points forward along # the robot's length, the Z axis points up aligned with gravity, and the Y # axis is the cross-product of the two. footprint_R_body = bosdyn.geometry.EulerZXY(yaw=0.4, roll=0.0, pitch=0.0) cmd = RobotCommandBuilder.synchro_stand_command( footprint_R_body=footprint_R_body) command_client.robot_command(cmd) robot.logger.info("Robot standing twisted.") time.sleep(3) # Now tell the robot to stand taller, using the same approach of constructing # a command message with the RobotCommandBuilder and issuing it with # robot_command. cmd = RobotCommandBuilder.synchro_stand_command(body_height=0.1) command_client.robot_command(cmd) robot.logger.info("Robot standing tall.") time.sleep(3) # Capture an image. # Spot has five sensors around the body. Each sensor consists of a stereo pair and a # fisheye camera. The list_image_sources RPC gives a list of image sources which are # available to the API client. Images are captured via calls to the get_image RPC. # Images can be requested from multiple image sources in one call. image_client = robot.ensure_client( ImageClient.default_service_name) sources = image_client.list_image_sources() image_response = image_client.get_image_from_sources( ['frontleft_fisheye_image']) # _maybe_display_image(image_response[0].shot.image) # if config.save or config.save_path is not None: # maybe_save_image(image_response[0].shot.image, config.save_path) # Log a comment. # Comments logged via this API are written to the robots test log. This is the best way # to mark a log as "interesting". These comments will be available to Boston Dynamics # devs when diagnosing customer issues. log_comment = "HelloSpot tutorial user comment." robot.operator_comment(log_comment) robot.logger.info('Added comment "%s" to robot log.', log_comment) # Power the robot off. By specifying "cut_immediately=False", a safe power off command # is issued to the robot. This will attempt to sit the robot before powering off. finally: # If we successfully acquired a lease, return it. lease_client.return_lease(lease)
def hello_arm(config): """A simple example of using the Boston Dynamics API to command Spot's arm.""" # See hello_spot.py for an explanation of these lines. bosdyn.client.util.setup_logging(config.verbose) sdk = bosdyn.client.create_standard_sdk('HelloSpotClient') robot = sdk.create_robot(config.hostname) robot.authenticate(config.username, config.password) robot.time_sync.wait_for_sync() assert robot.has_arm(), "Robot requires an arm to run this example." # Verify the robot is not estopped and that an external application has registered and holds # an estop endpoint. assert not robot.is_estopped(), "Robot is estopped. Please use an external E-Stop client, " \ "such as the estop SDK example, to configure E-Stop." robot_state_client = robot.ensure_client( RobotStateClient.default_service_name) lease_client = robot.ensure_client( bosdyn.client.lease.LeaseClient.default_service_name) lease = lease_client.acquire() try: with bosdyn.client.lease.LeaseKeepAlive(lease_client): # Now, we are ready to power on the robot. This call will block until the power # is on. Commands would fail if this did not happen. We can also check that the robot is # powered at any point. robot.logger.info( "Powering on robot... This may take a several seconds.") robot.power_on(timeout_sec=20) assert robot.is_powered_on(), "Robot power on failed." robot.logger.info("Robot powered on.") # Tell the robot to stand up. The command service is used to issue commands to a robot. # The set of valid commands for a robot depends on hardware configuration. See # SpotCommandHelper for more detailed examples on command building. The robot # command service requires timesync between the robot and the client. robot.logger.info("Commanding robot to stand...") command_client = robot.ensure_client( RobotCommandClient.default_service_name) blocking_stand(command_client, timeout_sec=10) robot.logger.info("Robot standing.") time.sleep(2.0) # Move the arm to a spot in front of the robot, and open the gripper. # Make the arm pose RobotCommand # Build a position to move the arm to (in meters, relative to and expressed in the gravity aligned body frame). x = 0.75 y = 0 z = 0.25 hand_ewrt_flat_body = geometry_pb2.Vec3(x=x, y=y, z=z) # Rotation as a quaternion qw = 1 qx = 0 qy = 0 qz = 0 flat_body_Q_hand = geometry_pb2.Quaternion(w=qw, x=qx, y=qy, z=qz) flat_body_T_hand = geometry_pb2.SE3Pose( position=hand_ewrt_flat_body, rotation=flat_body_Q_hand) robot_state = robot_state_client.get_robot_state() odom_T_flat_body = get_a_tform_b( robot_state.kinematic_state.transforms_snapshot, ODOM_FRAME_NAME, GRAV_ALIGNED_BODY_FRAME_NAME) odom_T_hand = odom_T_flat_body * math_helpers.SE3Pose.from_obj( flat_body_T_hand) # duration in seconds seconds = 2 arm_command = RobotCommandBuilder.arm_pose_command( odom_T_hand.x, odom_T_hand.y, odom_T_hand.z, odom_T_hand.rot.w, odom_T_hand.rot.x, odom_T_hand.rot.y, odom_T_hand.rot.z, ODOM_FRAME_NAME, seconds) # Make the open gripper RobotCommand gripper_command = RobotCommandBuilder.claw_gripper_open_fraction_command( 1.0) # Combine the arm and gripper commands into one RobotCommand command = RobotCommandBuilder.build_synchro_command( gripper_command, arm_command) # Send the request cmd_id = command_client.robot_command(command) robot.logger.info('Moving arm to position 1.') # Wait until the arm arrives at the goal. block_until_arm_arrives_with_prints(robot, command_client, cmd_id) # Move the arm to a different position hand_ewrt_flat_body.z = 0 flat_body_Q_hand.w = 0.707 flat_body_Q_hand.x = 0.707 flat_body_Q_hand.y = 0 flat_body_Q_hand.z = 0 flat_body_T_hand2 = geometry_pb2.SE3Pose( position=hand_ewrt_flat_body, rotation=flat_body_Q_hand) odom_T_hand = odom_T_flat_body * math_helpers.SE3Pose.from_obj( flat_body_T_hand2) arm_command = RobotCommandBuilder.arm_pose_command( odom_T_hand.x, odom_T_hand.y, odom_T_hand.z, odom_T_hand.rot.w, odom_T_hand.rot.x, odom_T_hand.rot.y, odom_T_hand.rot.z, ODOM_FRAME_NAME, seconds) # Close the gripper gripper_command = RobotCommandBuilder.claw_gripper_open_fraction_command( 0.0) # Build the proto command = RobotCommandBuilder.build_synchro_command( gripper_command, arm_command) # Send the request cmd_id = command_client.robot_command(command) robot.logger.info('Moving arm to position 2.') # Wait until the arm arrives at the goal. # Note: here we use the helper function provided by robot_command. block_until_arm_arrives(command_client, cmd_id) robot.logger.info('Done.') # Power the robot off. By specifying "cut_immediately=False", a safe power off command # is issued to the robot. This will attempt to sit the robot before powering off. robot.power_off(cut_immediately=False, timeout_sec=20) assert not robot.is_powered_on(), "Robot power off failed." robot.logger.info("Robot safely powered off.") finally: # If we successfully acquired a lease, return it. lease_client.return_lease(lease)
def main(raw_args=None): """Replay stored mission""" body_lease = None # Configure logging bosdyn.client.util.setup_logging() # Parse command-line arguments parser = argparse.ArgumentParser() bosdyn.client.util.add_common_arguments(parser) parser.add_argument('--timeout', type=float, default=3.0, dest='timeout', help='Mission client timeout (s).') parser.add_argument('--noloc', action='store_true', default=False, dest='noloc', help='Skip initial localization') parser.add_argument( '--disable_alternate_route_finding', action='store_true', default=False, dest='disable_alternate_route_finding', help='Disable creating alternate-route-finding graph structure') parser.add_argument( '--disable_directed_exploration', action='store_true', default=False, dest='disable_directed_exploration', help='Disable directed exploration for skipped blocked branches') group = parser.add_mutually_exclusive_group() group.add_argument('--time', type=float, default=0.0, dest='duration', help='Time to repeat mission (sec)') group.add_argument('--static', action='store_true', default=False, dest='static_mode', help='Stand, but do not run robot') # Subparser for mission type subparsers = parser.add_subparsers(dest='mission_type', help='Mission type') subparsers.required = True # Subparser for simple mission simple_parser = subparsers.add_parser('simple', help='Simple mission (non-Autowalk)') simple_parser.add_argument('simple_mission_file', help='Mission file for non-Autowalk mission.') # Subparser for Autowalk mission autowalk_parser = subparsers.add_parser( 'autowalk', help='Autowalk mission using graph_nav') autowalk_parser.add_argument( 'map_directory', help= 'Directory containing graph_nav map and default Autowalk mission file.' ) autowalk_parser.add_argument( '--autowalk_mission', dest='autowalk_mission_file', help='Optional alternate Autowalk mission file.') args = parser.parse_args(raw_args) if args.mission_type == 'simple': do_map_load = False fail_on_question = False do_localization = False mission_file = args.simple_mission_file map_directory = None print('[ REPLAYING SIMPLE MISSION {} : HOSTNAME {} ]'.format( mission_file, args.hostname)) else: do_map_load = True fail_on_question = True if args.noloc: do_localization = False else: do_localization = True map_directory = args.map_directory if args.autowalk_mission_file: mission_file = args.autowalk_mission_file else: mission_file = map_directory + '/missions/autogenerated' print( '[ REPLAYING AUTOWALK MISSION {} : MAP DIRECTORY {} : HOSTNAME {} ]' .format(mission_file, map_directory, args.hostname)) # Initialize robot object robot = init_robot(args.hostname, args.username, args.password) # Acquire robot lease robot.logger.info('Acquiring lease...') lease_client = robot.ensure_client( bosdyn.client.lease.LeaseClient.default_service_name) body_lease = lease_client.acquire() if body_lease is None: raise Exception('Lease not acquired.') robot.logger.info('Lease acquired: %s', str(body_lease)) # Initialize power client robot.logger.info('Starting power client...') power_client = robot.ensure_client(PowerClient.default_service_name) # Initialize other clients robot_state_client, command_client, mission_client, graph_nav_client = init_clients( robot, body_lease, mission_file, map_directory, do_map_load, args.disable_alternate_route_finding) try: with bosdyn.client.lease.LeaseKeepAlive(lease_client): assert not robot.is_estopped(), "Robot is estopped. " \ "Please use an external E-Stop client, " \ "such as the estop SDK example, to configure E-Stop." # Turn on power power_on(power_client) # Stand up and wait for the perception system to stabilize robot.logger.info('Commanding robot to stand...') blocking_stand(command_client, timeout_sec=20) countdown(5) robot.logger.info('Robot standing.') # Localize robot localization_error = False if do_localization: graph = graph_nav_client.download_graph() robot.logger.info('Localizing robot...') robot_state = robot_state_client.get_robot_state() localization = nav_pb2.Localization() # Attempt to localize using any visible fiducial graph_nav_client.set_localization( initial_guess_localization=localization, ko_tform_body=None, max_distance=None, max_yaw=None, fiducial_init=graph_nav_pb2.SetLocalizationRequest. FIDUCIAL_INIT_NEAREST) # Run mission if not args.static_mode and not localization_error: if args.duration == 0.0: run_mission(robot, mission_client, lease_client, fail_on_question, args.timeout, args.disable_directed_exploration) else: repeat_mission(robot, mission_client, lease_client, args.duration, fail_on_question, args.timeout, args.disable_directed_exploration) finally: # Turn off power lease_client.lease_wallet.advance() robot.logger.info('Powering off...') safe_power_off(command_client, robot_state_client) # Return lease robot.logger.info('Returning lease...') lease_client.return_lease(body_lease)
def stand(self): """Stand the robot.""" blocking_stand(self._robot_command_client)