def set_mobility_params(self): """Set robot mobility params to disable obstacle aworldidance.""" obstacles = spot_command_pb2.ObstacleParams( disable_vision_body_obstacle_avoidance=True, disable_vision_foot_obstacle_avoidance=True, disable_vision_foot_constraint_avoidance=True, obstacle_avoidance_padding=.001) body_control = self.set_default_body_control() if self._limit_speed: speed_limit = SE2VelocityLimit(max_vel=SE2Velocity( linear=Vec2(x=self._max_x_vel, y=self._max_y_vel), angular=self._max_ang_vel)) if not self._avoid_obstacles: mobility_params = spot_command_pb2.MobilityParams( obstacle_params=obstacles, vel_limit=speed_limit, body_control=body_control, locomotion_hint=spot_command_pb2.HINT_AUTO) else: mobility_params = spot_command_pb2.MobilityParams( vel_limit=speed_limit, body_control=body_control, locomotion_hint=spot_command_pb2.HINT_AUTO) elif not self._avoid_obstacles: mobility_params = spot_command_pb2.MobilityParams( obstacle_params=obstacles, body_control=body_control, locomotion_hint=spot_command_pb2.HINT_AUTO) else: #When set to none, RobotCommandBuilder populates with good default values mobility_params = None return mobility_params
def initialize_robot_from_config(self, config): """Initializes SDK from command line arguments. Args: config: Command-line arguments as argparse object. """ sdk = bosdyn.client.create_standard_sdk(self.client_name) self.robot = sdk.create_robot(config.hostname) self.robot.authenticate(config.username, config.password) self.robot.time_sync.wait_for_sync() self.command_client = self.robot.ensure_client( RobotCommandClient.default_service_name) self.lease_client = self.robot.ensure_client( LeaseClient.default_service_name) self.estop_client = self.robot.ensure_client( EstopClient.default_service_name) self.mobility_params = spot_command_pb2.MobilityParams( locomotion_hint=spot_command_pb2.HINT_AUTO) # Print controls print( textwrap.dedent("""\ | Button Combination | Functionality | |--------------------|--------------------------| | A | Walk | | B | Stand | | X | Sit | | Y | Stairs | | LB + : | | | - D-pad up/down | Walk height | | - D-pad left | Battery-Change Pose | | - D-pad right | Self right | | - Y | Jog | | - A | Amble | | - B | Crawl | | - X | Hop | | | | | If Stand Mode | | | - Left Stick | | | -- X | Rotate body in roll axis | | -- Y | Control height | | - Right Stick | | | -- X | Turn body in yaw axis | | -- Y | Turn body in pitch axis | | Else | | | - Left Stick | Move | | - Right Stick | Turn | | | | | LB + RB + B | E-Stop | | Start | Motor power & Control | | Back | Exit | """)) # Describe the necessary steps before one can command the robot. print("Before you can command the robot: \n" + \ "\t1. Acquire a software E-Stop (Left Button + Right Button + B). \n" + \ "\t2. Obtain a lease and power on the robot's motors (Start button).")
def get_mobility_params(): """Gets mobility parameters for following""" vel_desired = .75 speed_limit = geo.SE2VelocityLimit( max_vel=geo.SE2Velocity(linear=geo.Vec2(x=vel_desired, y=vel_desired), angular=.25)) body_control = set_default_body_control() mobility_params = spot_command_pb2.MobilityParams(vel_limit=speed_limit, obstacle_params=None, body_control=body_control, locomotion_hint=spot_command_pb2.HINT_TROT) return mobility_params
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 _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 _sit(self): """Sets robot in Sit mode. """ if self.mode is not RobotMode.Sit: self.mode = RobotMode.Sit self.mobility_params = spot_command_pb2.MobilityParams( locomotion_hint=spot_command_pb2.HINT_AUTO, stair_hint=0) cmd = RobotCommandBuilder.synchro_sit_command( params=self.mobility_params) self._issue_robot_command(cmd)
def _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 _sit(self): """Sets robot in Sit mode. """ if not self.motors_powered: return if self.mode is not RobotMode.Sit: self.mode = RobotMode.Sit self.mobility_params = spot_command_pb2.MobilityParams( locomotion_hint=spot_command_pb2.HINT_AUTO, stair_hint=0) cmd = RobotCommandBuilder.sit_command(params=self.mobility_params) self.command_client.robot_command_async(cmd)
def _crawl(self): """Sets robot in Crawl mode. """ if not self.motors_powered: return 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.stand_command( params=self.mobility_params) self.command_client.robot_command_async(cmd)
def _amble(self): """Sets robot in Amble mode. """ if not self.motors_powered: return 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.stand_command( params=self.mobility_params) self.command_client.robot_command_async(cmd)
def _walk(self): """Sets robot in Walk mode. """ if not self.motors_powered: return 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.stand_command( params=self.mobility_params) self.command_client.robot_command_async(cmd)
def _hop(self): """Sets robot in Hop mode. """ if not self.motors_powered: return 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.stand_command( params=self.mobility_params) self.command_client.robot_command_async(cmd)
def initialize_robot_from_config(self, config): """Initializes SDK from command line arguments. Args: config: Command-line arguments as argparse object. """ sdk = bosdyn.client.create_standard_sdk(self.client_name) sdk.load_app_token(config.app_token) self.robot = sdk.create_robot(config.hostname) self.robot.authenticate(config.username, config.password) self.robot.time_sync.wait_for_sync() self.command_client = self.robot.ensure_client( RobotCommandClient.default_service_name) self.lease_client = self.robot.ensure_client( LeaseClient.default_service_name) self.estop_client = self.robot.ensure_client( EstopClient.default_service_name) self.mobility_params = spot_command_pb2.MobilityParams( locomotion_hint=spot_command_pb2.HINT_AUTO)
def mobility_params(body_height=0.0, footprint_R_body=geometry.EulerZXY(), locomotion_hint=spot_command_pb2.HINT_AUTO, stair_hint=False, external_force_params=None): """Helper to create Mobility params for spot mobility commands. This function is designed to help get started issuing commands, but lots of options are not exposed via this interface. See spot.robot_command_pb2 for more details. If unset, good defaults will be chosen by the robot. Returns: spot.MobilityParams, params for spot mobility commands. """ # Simplified body control params position = geometry_pb2.Vec3(z=body_height) rotation = footprint_R_body.to_quaternion() pose = geometry_pb2.SE3Pose(position=position, rotation=rotation) point = trajectory_pb2.SE3TrajectoryPoint(pose=pose) frame = geometry_pb2.Frame(base_frame=geometry_pb2.FRAME_BODY) traj = trajectory_pb2.SE3Trajectory(points=[point], frame=frame) body_control = spot_command_pb2.BodyControlParams(base_offset_rt_footprint=traj) return spot_command_pb2.MobilityParams(body_control=body_control, locomotion_hint=locomotion_hint, stair_hint=stair_hint, external_force_params=external_force_params)
def mobility_params(body_height=0.0, footprint_R_body=geometry.EulerZXY(), locomotion_hint=spot_command_pb2.HINT_AUTO, stair_hint=False, external_force_params=None): """Helper to create Mobility params for spot mobility commands. This function is designed to help get started issuing commands, but lots of options are not exposed via this interface. See spot.robot_command_pb2 for more details. If unset, good defaults will be chosen by the robot. Args: body_height: Body height in meters. footprint_R_body(EulerZXY): The orientation of the body frame with respect to the footprint frame (gravity aligned framed with yaw computed from the stance feet) locomotion_hint: Locomotion hint to use for the command. stair_hint: Boolean to specify if stair mode should be used. external_force_params(spot.BodyExternalForceParams): Robot body external force parameters. Returns: spot.MobilityParams, params for spot mobility commands. """ # Simplified body control params position = geometry_pb2.Vec3(z=body_height) rotation = footprint_R_body.to_quaternion() pose = geometry_pb2.SE3Pose(position=position, rotation=rotation) point = trajectory_pb2.SE3TrajectoryPoint(pose=pose) traj = trajectory_pb2.SE3Trajectory(points=[point]) body_control = spot_command_pb2.BodyControlParams( base_offset_rt_footprint=traj) return spot_command_pb2.MobilityParams( body_control=body_control, locomotion_hint=locomotion_hint, stair_hint=stair_hint, external_force_params=external_force_params)
def 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)