def _move(self, left_x, left_y, right_x): """Commands the robot with a velocity command based on left/right stick values. Args: left_x: X value of left stick. left_y: Y value of left stick. right_x: X value of right stick. """ # Stick left_x controls robot v_y v_y = -left_x * VELOCITY_BASE_SPEED # Stick left_y controls robot v_x v_x = left_y * VELOCITY_BASE_SPEED # Stick right_x controls robot v_rot v_rot = -right_x * VELOCITY_BASE_ANGULAR # Recreate mobility_params with the latest information self.mobility_params = RobotCommandBuilder.mobility_params( body_height=self.body_height, locomotion_hint=self.mobility_params.locomotion_hint, stair_hint=self.mobility_params.stair_hint) cmd = RobotCommandBuilder.velocity_command(v_x=v_x, v_y=v_y, v_rot=v_rot, params=self.mobility_params) self.command_client.robot_command_async(cmd, end_time_secs=time.time() + VELOCITY_CMD_DURATION)
def get_walking_params(max_linear_vel, max_rotation_vel): max_vel_linear = geometry_pb2.Vec2(x=max_linear_vel, y=max_linear_vel) max_vel_se2 = geometry_pb2.SE2Velocity(linear=max_vel_linear, angular=max_rotation_vel) vel_limit = geometry_pb2.SE2VelocityLimit(max_vel=max_vel_se2) params = RobotCommandBuilder.mobility_params() params.vel_limit.CopyFrom(vel_limit) return params
def set_mobility_params(self, body_height=0, footprint_R_body=EulerZXY(), locomotion_hint=1, stair_hint=False, external_force_params=None): """Define body, locomotion, and stair parameters. 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 stair_hint: Boolean to define stair motion """ self._mobility_params = RobotCommandBuilder.mobility_params( body_height, footprint_R_body, locomotion_hint, stair_hint, external_force_params)
def relative_move(dx, dy, dyaw, frame_name, robot_command_client, robot_state_client, stairs=False): transforms = robot_state_client.get_robot_state( ).kinematic_state.transforms_snapshot # Build the transform for where we want the robot to be relative to where the body currently is. body_tform_goal = math_helpers.SE2Pose(x=dx, y=dy, angle=dyaw) # We do not want to command this goal in body frame because the body will move, thus shifting # our goal. Instead, we transform this offset to get the goal position in the output frame # (which will be either odom or vision). out_tform_body = get_se2_a_tform_b(transforms, frame_name, BODY_FRAME_NAME) out_tform_goal = out_tform_body * body_tform_goal # Command the robot to go to the goal point in the specified frame. The command will stop at the # new position. robot_cmd = RobotCommandBuilder.synchro_se2_trajectory_point_command( goal_x=out_tform_goal.x, goal_y=out_tform_goal.y, goal_heading=out_tform_goal.angle, frame_name=frame_name, params=RobotCommandBuilder.mobility_params(stair_hint=stairs)) end_time = 10.0 cmd_id = robot_command_client.robot_command(lease=None, command=robot_cmd, end_time_secs=time.time() + end_time) # Wait until the robot has reached the goal. while True: feedback = robot_command_client.robot_command_feedback(cmd_id) mobility_feedback = feedback.feedback.synchronized_feedback.mobility_command_feedback if mobility_feedback.status != RobotCommandFeedbackStatus.STATUS_PROCESSING: print("Failed to reach the goal") return False traj_feedback = mobility_feedback.se2_trajectory_feedback if (traj_feedback.status == traj_feedback.STATUS_AT_GOAL and traj_feedback.body_movement_status == traj_feedback.BODY_STATUS_SETTLED): print("Arrived at the goal.") return True time.sleep(1)
def __init__(self, username, password, hostname, logger, rates={}, callbacks={}): self._username = username self._password = password self._hostname = hostname self._logger = logger self._rates = rates self._callbacks = callbacks self._keep_alive = True self._valid = True self._mobility_params = RobotCommandBuilder.mobility_params() self._is_standing = False self._is_sitting = True self._is_moving = False self._last_stand_command = None self._last_sit_command = None self._last_motion_command = None self._last_motion_command_time = None self._front_image_requests = [] for source in front_image_sources: self._front_image_requests.append( build_image_request( source, image_format=image_pb2.Image.Format.FORMAT_RAW)) self._side_image_requests = [] for source in side_image_sources: self._side_image_requests.append( build_image_request( source, image_format=image_pb2.Image.Format.FORMAT_RAW)) self._rear_image_requests = [] for source in rear_image_sources: self._rear_image_requests.append( build_image_request( source, image_format=image_pb2.Image.Format.FORMAT_RAW)) try: self._sdk = create_standard_sdk('ros_spot') except Exception as e: self._logger.error("Error creating SDK object: %s", e) self._valid = False return self._robot = self._sdk.create_robot(self._hostname) try: self._robot.authenticate(self._username, self._password) self._robot.start_time_sync() except RpcError as err: self._logger.error("Failed to communicate with robot: %s", err) self._valid = False return if self._robot: # Clients try: self._robot_state_client = self._robot.ensure_client( RobotStateClient.default_service_name) self._robot_command_client = self._robot.ensure_client( RobotCommandClient.default_service_name) self._graph_nav_client = self._robot.ensure_client( GraphNavClient.default_service_name) self._power_client = self._robot.ensure_client( PowerClient.default_service_name) self._lease_client = self._robot.ensure_client( LeaseClient.default_service_name) self._lease_wallet = self._lease_client.lease_wallet self._image_client = self._robot.ensure_client( ImageClient.default_service_name) self._estop_client = self._robot.ensure_client( EstopClient.default_service_name) except Exception as e: self._logger.error("Unable to create client service: %s", e) self._valid = False return # Store the most recent knowledge of the state of the robot based on rpc calls. self._current_graph = None self._current_edges = dict( ) #maps to_waypoint to list(from_waypoint) self._current_waypoint_snapshots = dict( ) # maps id to waypoint snapshot self._current_edge_snapshots = dict() # maps id to edge snapshot self._current_annotation_name_to_wp_id = dict() # Async Tasks self._async_task_list = [] self._robot_state_task = AsyncRobotState( self._robot_state_client, self._logger, max(0.0, self._rates.get("robot_state", 0.0)), self._callbacks.get("robot_state", lambda: None)) self._robot_metrics_task = AsyncMetrics( self._robot_state_client, self._logger, max(0.0, self._rates.get("metrics", 0.0)), self._callbacks.get("metrics", lambda: None)) self._lease_task = AsyncLease( self._lease_client, self._logger, max(0.0, self._rates.get("lease", 0.0)), self._callbacks.get("lease", lambda: None)) self._front_image_task = AsyncImageService( self._image_client, self._logger, max(0.0, self._rates.get("front_image", 0.0)), self._callbacks.get("front_image", lambda: None), self._front_image_requests) self._side_image_task = AsyncImageService( self._image_client, self._logger, max(0.0, self._rates.get("side_image", 0.0)), self._callbacks.get("side_image", lambda: None), self._side_image_requests) self._rear_image_task = AsyncImageService( self._image_client, self._logger, max(0.0, self._rates.get("rear_image", 0.0)), self._callbacks.get("rear_image", lambda: None), self._rear_image_requests) self._idle_task = AsyncIdle(self._robot_command_client, self._logger, 10.0, self) self._estop_endpoint = None self._async_tasks = AsyncTasks([ self._robot_state_task, self._robot_metrics_task, self._lease_task, self._front_image_task, self._side_image_task, self._rear_image_task, self._idle_task ]) self._robot_id = None self._lease = None
def __init__(self, username, password, token, hostname, logger, rates={}, callbacks={}): self._username = username self._password = password self._token = token self._hostname = hostname self._logger = logger self._rates = rates self._callbacks = callbacks self._keep_alive = True self._valid = True self._mobility_params = RobotCommandBuilder.mobility_params() self._is_standing = False self._is_sitting = True self._is_moving = False self._last_stand_command = None self._last_sit_command = None self._last_motion_command = None self._last_motion_command_time = None self._front_image_requests = [] for source in front_image_sources: self._front_image_requests.append( build_image_request( source, image_format=image_pb2.Image.Format.FORMAT_RAW)) self._side_image_requests = [] for source in side_image_sources: self._side_image_requests.append( build_image_request( source, image_format=image_pb2.Image.Format.FORMAT_RAW)) self._rear_image_requests = [] for source in rear_image_sources: self._rear_image_requests.append( build_image_request( source, image_format=image_pb2.Image.Format.FORMAT_RAW)) try: self._sdk = create_standard_sdk('ros_spot') except Exception as e: self._logger.error("Error creating SDK object: %s", e) self._valid = False return try: self._sdk.load_app_token(self._token) except Exception as e: self._logger.error("Error loading developer token: %s", e) self._valid = False return self._robot = self._sdk.create_robot(self._hostname) try: self._robot.authenticate(self._username, self._password) self._robot.start_time_sync() except RpcError as err: self._logger.error("Failed to communicate with robot: %s", err) self._valid = False return if self._robot: # Clients try: self._robot_state_client = self._robot.ensure_client( RobotStateClient.default_service_name) self._robot_command_client = self._robot.ensure_client( RobotCommandClient.default_service_name) self._power_client = self._robot.ensure_client( PowerClient.default_service_name) self._lease_client = self._robot.ensure_client( LeaseClient.default_service_name) self._image_client = self._robot.ensure_client( ImageClient.default_service_name) self._estop_client = self._robot.ensure_client( EstopClient.default_service_name) except Exception as e: self._logger.error("Unable to create client service: %s", e) self._valid = False return # Async Tasks self._async_task_list = [] self._robot_state_task = AsyncRobotState( self._robot_state_client, self._logger, max(0.0, self._rates.get("robot_state", 0.0)), self._callbacks.get("robot_state", lambda: None)) self._robot_metrics_task = AsyncMetrics( self._robot_state_client, self._logger, max(0.0, self._rates.get("metrics", 0.0)), self._callbacks.get("metrics", lambda: None)) self._lease_task = AsyncLease( self._lease_client, self._logger, max(0.0, self._rates.get("lease", 0.0)), self._callbacks.get("lease", lambda: None)) self._front_image_task = AsyncImageService( self._image_client, self._logger, max(0.0, self._rates.get("front_image", 0.0)), self._callbacks.get("front_image", lambda: None), self._front_image_requests) self._side_image_task = AsyncImageService( self._image_client, self._logger, max(0.0, self._rates.get("side_image", 0.0)), self._callbacks.get("side_image", lambda: None), self._side_image_requests) self._rear_image_task = AsyncImageService( self._image_client, self._logger, max(0.0, self._rates.get("rear_image", 0.0)), self._callbacks.get("rear_image", lambda: None), self._rear_image_requests) self._idle_task = AsyncIdle(self._robot_command_client, self._logger, 10.0, self) self._estop_endpoint = EstopEndpoint(self._estop_client, 'ros', 9.0) self._async_tasks = AsyncTasks([ self._robot_state_task, self._robot_metrics_task, self._lease_task, self._front_image_task, self._side_image_task, self._rear_image_task, self._idle_task ]) self._robot_id = None self._lease = None
def resetMobilityParams(self): """ Resets the mobility parameters used for motion commands to the default values provided by the bosdyn api. Returns: """ self._mobility_params = RobotCommandBuilder.mobility_params()