def test_synchro_stand_command(): command = RobotCommandBuilder.synchro_stand_command() _test_has_synchronized(command) _test_has_mobility(command.synchronized_command) assert command.synchronized_command.mobility_command.HasField("stand_request") # basic check with a build_on_command arm_command = RobotCommandBuilder.arm_stow_command() command = RobotCommandBuilder.synchro_stand_command(build_on_command=arm_command) _test_has_synchronized(command) _test_has_mobility(command.synchronized_command) _test_has_arm(command.synchronized_command)
def pitch_up(robot): """Pitch robot up to look for door handle. Args: robot: (Robot) Interface to Spot robot. """ robot.logger.info("Pitching robot up...") command_client = robot.ensure_client( RobotCommandClient.default_service_name) footprint_R_body = geometry.EulerZXY(0.0, 0.0, -1 * math.pi / 6.0) cmd = RobotCommandBuilder.synchro_stand_command( footprint_R_body=footprint_R_body) cmd_id = command_client.robot_command(cmd) timeout_sec = 10.0 end_time = time.time() + timeout_sec while time.time() < end_time: response = command_client.robot_command_feedback(cmd_id) synchronized_feedback = response.feedback.synchronized_feedback status = synchronized_feedback.mobility_command_feedback.stand_feedback.status if (status == basic_command_pb2.StandCommand.Feedback.STATUS_IS_STANDING): robot.logger.info("Robot pitched.") return time.sleep(1.0) raise Exception("Failed to pitch robot.")
def stand(self, monitor_command=True): """If the e-stop is enabled, and the motor power is enabled, stand the robot up.""" response = self._robot_command( RobotCommandBuilder.synchro_stand_command( params=self._mobility_params)) if monitor_command: self._last_stand_command = response[2] return response[0], response[1]
def _amble(self): """Sets robot in Amble mode. """ if self.mode is not RobotMode.Amble: self.mode = RobotMode.Amble self.mobility_params = spot_command_pb2.MobilityParams( locomotion_hint=spot_command_pb2.HINT_AMBLE, stair_hint=0) cmd = RobotCommandBuilder.synchro_stand_command( params=self.mobility_params) self._issue_robot_command(cmd)
def _crawl(self): """Sets robot in Crawl mode. """ if self.mode is not RobotMode.Crawl: self.mode = RobotMode.Crawl self.mobility_params = spot_command_pb2.MobilityParams( locomotion_hint=spot_command_pb2.HINT_CRAWL, stair_hint=0) cmd = RobotCommandBuilder.synchro_stand_command( params=self.mobility_params) self._issue_robot_command(cmd)
def _stand(self): """Sets robot in Stand mode. """ if self.mode is not RobotMode.Stand: self.mode = RobotMode.Stand self.mobility_params = spot_command_pb2.MobilityParams( locomotion_hint=spot_command_pb2.HINT_AUTO, stair_hint=0) cmd = RobotCommandBuilder.synchro_stand_command( params=self.mobility_params) self._issue_robot_command(cmd)
def _hop(self): """Sets robot in Hop mode. """ if self.mode is not RobotMode.Hop: self.mode = RobotMode.Hop self._reset_height() self.mobility_params = spot_command_pb2.MobilityParams( locomotion_hint=spot_command_pb2.HINT_HOP, stair_hint=0) cmd = RobotCommandBuilder.synchro_stand_command( params=self.mobility_params) self._issue_robot_command(cmd)
def _walk(self): """Sets robot in Walk mode. """ if self.mode is not RobotMode.Walk: self.mode = RobotMode.Walk self.mobility_params = spot_command_pb2.MobilityParams( locomotion_hint=spot_command_pb2.HINT_SPEED_SELECT_TROT, stair_hint=0) cmd = RobotCommandBuilder.synchro_stand_command( params=self.mobility_params) self._issue_robot_command(cmd)
def orient(self, yaw=0.0, pitch=0.0, roll=0.0): """ Ask Spot to orient its body (e.g. yaw, pitch, roll) @param[in] yaw Rotation about Z (+up/down) [rad] @param[in] pitch Rotation about Y (+left/right) [rad] @param[in] roll Rotation about X (+front/back) [rad] """ if self._robot is None: return if self._prep_for_motion(): rotation = geometry.EulerZXY(yaw=yaw, pitch=pitch, roll=roll) command_client = self._robot.ensure_client( RobotCommandClient.default_service_name) cmd = RobotCommandBuilder.synchro_stand_command( footprint_R_body=rotation) command_client.robot_command(cmd)
def _orientation_cmd_helper(self, yaw=0.0, roll=0.0, pitch=0.0, height=0.0): """Helper function that commands the robot with an orientation command; Used by the other orientation functions. Args: yaw: Yaw of the robot body. Defaults to 0.0. roll: Roll of the robot body. Defaults to 0.0. pitch: Pitch of the robot body. Defaults to 0.0. height: Height of the robot body from normal stand height. Defaults to 0.0. """ if not self.motors_powered: return orientation = EulerZXY(yaw, roll, pitch) cmd = RobotCommandBuilder.synchro_stand_command( body_height=height, footprint_R_body=orientation) self._issue_robot_command(cmd, endtime=time.time() + VELOCITY_CMD_DURATION)
def test_build_synchro_command(): # two synchro subcommands of the same type: arm_command_1 = RobotCommandBuilder.arm_ready_command() arm_command_2 = RobotCommandBuilder.arm_stow_command() command = RobotCommandBuilder.build_synchro_command(arm_command_1, arm_command_2) _test_has_synchronized(command) _test_has_arm(command.synchronized_command) assert not command.synchronized_command.HasField("gripper_command") assert not command.synchronized_command.HasField("mobility_command") command_position = command.synchronized_command.arm_command.named_arm_position_command.position assert command_position == arm_command_pb2.NamedArmPositionsCommand.POSITIONS_STOW # two synchro subcommands of a different type: arm_command = RobotCommandBuilder.arm_ready_command() mobility_command = RobotCommandBuilder.synchro_stand_command() command = RobotCommandBuilder.build_synchro_command(arm_command, mobility_command) _test_has_synchronized(command) _test_has_mobility(command.synchronized_command) _test_has_arm(command.synchronized_command) assert not command.synchronized_command.HasField("gripper_command") assert command.synchronized_command.mobility_command.HasField("stand_request") command_position = command.synchronized_command.arm_command.named_arm_position_command.position assert command_position == arm_command_pb2.NamedArmPositionsCommand.POSITIONS_READY # one synchro command and one deprecated mobility command: deprecated_mobility_command = RobotCommandBuilder.sit_command() command = RobotCommandBuilder.build_synchro_command(mobility_command, deprecated_mobility_command) _test_has_synchronized(command) _test_has_mobility(command.synchronized_command) assert command.synchronized_command.mobility_command.HasField("sit_request") # fullbody command is rejected full_body_command = RobotCommandBuilder.selfright_command() with pytest.raises(Exception): command = RobotCommandBuilder.build_synchro_command(arm_command, full_body_command)
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') 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) 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 robot.logger.info('Commanding robot to stand...') stand_command = RobotCommandBuilder.synchro_stand_command() command_client.robot_command(stand_command) 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) # 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 test_synchro_stand_command(): command = RobotCommandBuilder.synchro_stand_command() _test_has_synchronized(command) _test_has_mobility(command.synchronized_command) assert command.synchronized_command.mobility_command.HasField( "stand_request")
def stand(self): self.__execute_command("stand", RobotCommandBuilder.synchro_stand_command())
def build_stand_command(deprecated, **kwargs): if (deprecated): cmd = RobotCommandBuilder.stand_command(**kwargs) else: cmd = RobotCommandBuilder.synchro_stand_command(**kwargs) return cmd
def _stand(self): self._start_robot_command('stand', RobotCommandBuilder.synchro_stand_command())
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 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 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)