API Reference

def connect(self) -> bool:
    """
    Connect to the server if currently not connected.

    Directly calling this method is deprecated in favor of using the `with` statement:

    ```python
    # Use:
    with AIRBOTArm() as robot:
        ...

    # instead of
    robot = AIRBOTArm()
    robot.connect()
    ...
    robot.disconnect()
    ```

    This method is idempotent. Calling this method if SDK has already
    conencted to the server would cause no harm.

    Returns:
        result (bool): True if connected successfully
    """
    if self._control_stub is None:
        self._channel = grpc.insecure_channel(self.endpoint)
        self._control_stub = arm_pb2_grpc.ControlServiceStub(self._channel)
        self._feedback_stub = arm_pb2_grpc.FeedbackServiceStub(self._channel)
        self._utils_stub = utils_pb2_grpc.UtilsServiceStub(self._channel)

        self._feedbacking = True
        self._feedback_thread = threading.Thread(target=self._status)
        self._feedback_thread.start()
        start_time = time.time()
        while self._feedback_jointstates is None:
            time.sleep(0.01)
            if time.time() - start_time > 1:
                logging.error("Failed to connect to the server.")
                self._feedbacking = False
                self._feedback_thread.join()
                self._feedback_thread = None
                self._control_stub = None
                self._feedback_stub = None
                self._utils_stub = None
                if self._channel is not None:
                    self._channel.close()
                self._channel = None
                return False

        return True
    else:
        logging.warning("Already connected to the server.")
        return True

def disconnect(self) -> bool:
    """
    Disconnect from to the server if currently not connected.

    Directly calling this method is deprecated in favor of using the `with` statement:

    ```python
    # Use:
    with AIRBOTArm() as robot:
        ...

    # instead of
    robot = AIRBOTArm()
    robot.connect()
    ...
    robot.disconnect()
    ```

    This method is idempotent. Calling this method if SDK has already
    disconnected from the server would cause no harm.

    Returns:
        result(bool): True if connected successfully
    """
    if self._feedbacking:
        self._feedbacking = False
        if self._feedback_thread is not None:
            self._feedback_thread.join()
            self._feedback_thread = None

    self._servo_cmd_queue.clear()
    self._servo_stop_event.set()
    if self._servo_thread is not None:
        self._servo_thread.join()
        self._servo_thread = None
    self._servo_stop_event.clear()
    self._servo_condition = threading.Semaphore(0)

    if self._control_stub is not None:
        self._control_stub = None
        self._feedback_stub = None
        self._utils_stub = None
        self._channel.close()
        self._channel = None
        return True
    else:
        logging.warning("Already disconnected from the server.")
        return True

@grpc_safe()
def get_control_mode(self) -> Optional[RobotMode]:
    """
    Obtain current control mode of the driver service.

    Returns:
        mode (RobotMode): The current control mode of the driver service.
    """
    stamp = time.time_ns()
    request = arm_pb2.GetModeRequest()
    request.stamp.sec = int(stamp / 1e9)
    request.stamp.nanosec = int(stamp % 1e9)
    request.request_id = self.client_id
    request.component_names[:] = ["arm"]

    response = self._feedback_stub.GetMode(request)
    return RobotMode(response.modes[0]) if response.modes else None

@grpc_safe()
def get_eef_eff(self) -> Optional[List[float]]:
    """
    Read current end effector torque output (in Nm) from the SDK server.
    Currently only parallel two-finger end effector is supported.

    If no end effector is installed, an empty list is returned.

    If the SDK has not received joint states from the server, None is returned.

    Returns:
        eef_eff (list[float]): the current end effector torque output of the arm. \
        None if SDK has not received joint states or no end \
        effector is installed.
    """
    if self._feedback_jointstates is None:
        logging.error("No joint states received yet.")
        return None
    else:
        return [
            i[1]
            for i in sorted(
                [
                    (name, effort)
                    for name, effort in zip(
                        self._feedback_jointstates.name,
                        self._feedback_jointstates.effort,
                    )
                ],
                key=lambda x: x[0],
            )
            if i[0] == "gripper_mapping_controller"
        ]

@grpc_safe()
def get_eef_pos(self) -> Optional[List[float]]:
    """
    Read current end effector width (in meters) from the SDK server.
    Currently only parallel two-finger end effector is supported.

    If no end effector is installed, an empty list is returned.

    If the SDK has not received joint states from the server, None is returned.

    Returns:
        eef_pos (list[float] | None): the current end effector width of the arm. \
        None if SDK has not received joint states or no end \
        effector is installed.
    """
    if self._feedback_jointstates is None:
        logging.error("No joint states received yet.")
        return None
    else:
        return [
            i[1]
            for i in sorted(
                [
                    (name, position)
                    for name, position in zip(
                        self._feedback_jointstates.name,
                        self._feedback_jointstates.position,
                    )
                ],
                key=lambda x: x[0],
            )
            if i[0] == "gripper_mapping_controller"
        ]

@grpc_safe()
def get_end_pose(self) -> Optional[List[List[float]]]:
    """
    Read current end pose from the SDK server.

    The zero point of the reference frame for the end pose is the intersection point
    of the rotating axis of first joint and installing plane.
    The axes of reference frame for the end are x axis pointing forward, y axis
    pointing leftward and z axis pointing upward.

    If no end effector is installed, the pose of the end flange is returned.
    If any end effector is installed, the pose of the effecting point
    is returned. For example, the effecting point of AIRBOT G2 is the grasping
    point in the front.

    TODO: Replace with actual doc url
    For detailed explanation of the reference frame and the end link used
    for get_end_pose, please refer to [https://www.bing.com](https://www.bing.com)

    Returns:
        end_pose (list[list[float]]): A list of two float-list. The first list is \
        the cartesian translation of the pose in meters, and the second list \
        is the orientation quaternion (x, y, z, w) of the pose.

    """
    if self._feedback_end_poses is None:
        logging.error("No end poses received yet.")
        return None
    else:
        return [
            [
                self._feedback_end_poses.translation.x,
                self._feedback_end_poses.translation.y,
                self._feedback_end_poses.translation.z,
            ],
            [
                self._feedback_end_poses.orientation.x,
                self._feedback_end_poses.orientation.y,
                self._feedback_end_poses.orientation.z,
                self._feedback_end_poses.orientation.w,
            ],
        ]

@grpc_safe()
def get_joint_eff(self) -> Optional[List[float]]:
    """
    Read current joint efforts (in Nm) from the SDK server.

    The returned list only contains the joint efforts of the arm itself.
    The position of the end effector (if any) is not included.

    If SDK has not received joint states from the server, None is returned.

    Returns:
        joint_eff (float | None): the current efforts of the arm joints.
    """
    if self._feedback_jointstates is None:
        logging.error("No joint states received yet.")
        return None
    else:
        return [
            i[1]
            for i in sorted(
                [
                    (name, effort)
                    for name, effort in zip(
                        self._feedback_jointstates.name,
                        self._feedback_jointstates.effort,
                    )
                ],
                key=lambda x: x[0],
            )
            if i[0].startswith("joint")
        ]

@grpc_safe()
def get_joint_pos(self) -> Optional[List[float]]:
    """
    Read current joint positions (in radian) from the SDK server.

    The returned list only contains the joint positions of the arm itself.
    The position of the end effector (if any) is not included.

    If SDK has not received joint states from the server, None is returned

    Returns:
        joint_pos (list[float] | None): the current positions of the arm joints.
    """
    if self._feedback_jointstates is None:
        logging.error("No joint states received yet.")
        return None
    else:
        return [
            i[1]
            for i in sorted(
                [
                    (name, position)
                    for name, position in zip(
                        self._feedback_jointstates.name,
                        self._feedback_jointstates.position,
                    )
                ],
                key=lambda x: x[0],
            )
            if i[0].startswith("joint")
        ]

@grpc_safe()
def get_joint_vel(self) -> Optional[List[float]]:
    """
    Read current joint velocities (in radian/s) from the SDK server.

    The returned list only contains the joint velocities of the arm itself.
    The position of the end effector (if any) is not included.

    If SDK has not received joint states from the server, None is returned

    Returns:
        joint_vel (list[float] | None): the current velocities of the arm joints.
    """
    if self._feedback_jointstates is None:
        logging.error("No joint states received yet.")
        return None
    else:
        return [
            i[1]
            for i in sorted(
                [
                    (name, velocity)
                    for name, velocity in zip(
                        self._feedback_jointstates.name,
                        self._feedback_jointstates.velocity,
                    )
                ],
                key=lambda x: x[0],
            )
            if i[0].startswith("joint")
        ]

@grpc_safe()
def get_params(self, names: List[str]) -> Dict[str, Any]:
    """
    Get parameters from the driver server.

    Args:
        names (list[str]): The names of the parameters to get.
    Returns:
        params (dict): A dictionary of the parameters.
    """
    if len(names) == 0:
        logging.error("No parameter names provided.")
        return {}
    stamp = time.time_ns()
    request = arm_pb2.GetParamsRequest()
    request.stamp.sec = int(stamp / 1e9)
    request.stamp.nanosec = int(stamp % 1e9)
    request.request_id = self.client_id
    request.keys[:] = names

    response = self._feedback_stub.GetParams(request)
    return {
        k: getattr(v, v.WhichOneof("union"))
        for k, v in zip(response.keys, response.values)
    }

@grpc_safe()
def get_state(self) -> State:
    """
    Obtain current state of the driver service.

    Returns:
        state (State): The current state of the driver service.
    """
    stamp = time.time_ns()
    request = utils_pb2.GetStateRequest()
    request.stamp.sec = int(stamp / 1e9)
    request.stamp.nanosec = int(stamp % 1e9)
    request.request_id = self.client_id

    response = self._utils_stub.GetState(request)
    return State(response.state)

@grpc_safe()
def load_app(self, name: str, params: Optional[dict] = None) -> bool:
    """
    Load an app to the driver server.

    When an read-write app is loaded, the SDK would enter read-only state,
    in which get methods are available but set methods are blocked. See
    [available states](./#airbot_py.arm--internal-states-of-the-driver)

    Currently available apps:
    * `record_replay_app/record_replay_app::RecordReplayApp`

    Args:
        name (str): The name of the app to load.
        params (dict): The parameters for the app. Default: {}

    Returns:
        result (bool): True if the app was loaded successfully, False otherwise.
    """
    stamp = time.time_ns()
    request = utils_pb2.LoadAppRequest()
    request.stamp.sec = int(stamp / 1e9)
    request.stamp.nanosec = int(stamp % 1e9)
    request.request_id = self.client_id
    request.app_name = name
    if params is not None:
        for key, value in params.items():
            request.params_key.append(key)
            request.params_value.append(str(value))

    response = self._utils_stub.LoadApp(request)
    return response.ok

@grpc_safe()
def move_eef_pos(
    self,
    joint_pos: Union[List[float], float],
    blocking: bool = True,
) -> bool:
    """
    Send one joint position command to the end effector.

    Effective for parallel two-finger end effector only.

    Before calling this method, the robot should be in `PLANNING_POS` mode,
    otherwise the command would be ignored.

    Args:
        joint_pos (list[float]): The end effector width commands in meters.

    Returns:
        result (bool): True if the command was sent successfully, False otherwise.
    """
    # For backward compatibility
    if not isinstance(joint_pos, list):
        joint_pos = [joint_pos]

    def _generate_joint_pos(
        joint_pos: List[float],
    ) -> Iterator[arm_pb2.SetStateRequest]:
        request = arm_pb2.SetStateRequest()
        joint_states = types_pb2.JointStates()
        now = time.time_ns()
        request.stamp.sec = int(now / 1e9)
        request.stamp.nanosec = int(now % 1e9)
        request.request_id = self.client_id
        request.component_names[:] = ["eef"]
        joint_states.name[:] = ["eef"]
        joint_states.position[:] = joint_pos
        request.commands.add(joint_states=joint_states)
        yield request

    result_iter = self._control_stub.SetState(_generate_joint_pos(joint_pos))

    first_result = next(result_iter, None)
    if first_result is not None and first_result.status != arm_pb2.Status.ACCEPTED:
        self.logger.error("Failed to move to joint position")
        return False

    return True

@grpc_safe()
def move_eff_pos(
    self,
    joint_pos: Union[List[float], float],
    blocking: bool = True,
) -> bool:
    """
    Previously name with typo for sending the command to the end effector.
    Renamed to `move_eef_pos`, but kept for backward compatibility.
    """
    return self.move_eef_pos(joint_pos, blocking)

@grpc_safe()
def move_to_cart_pose(
    self,
    cart_pose: List[List[float]],
    blocking: bool = True,
) -> bool:
    """
    Send one set of cartesian pose command to the robot.

    The cartesian pose command is sent to the driver server, and the robot
    would plan a path to reach the target cartesian pose then execute it.

    The zero point of the reference frame for the end pose is the intersection point
    of the rotating axis of first joint and installing plane.
    The axes of reference frame for the end are x axis pointing forward, y axis
    pointing leftward and z axis pointing upward.

    Args:
        cart_pose (list[list[float]]): The cartesian pose commands. \
            The first list is the cartesian translation of the pose in meters, \
            and the second list is the orientation quaternion (x, y, z, w) of the pose.
        blocking (bool): Whether to block until the command is finished. \
            Default: True

    Returns:
        result (bool): True if the command was sent successfully, False otherwise.
    """

    def _generate_cart_pose(
        cart_pose: List[List[float]],
    ) -> Iterator[arm_pb2.SetStateRequest]:
        transform = types_pb2.Transform()
        request = arm_pb2.SetStateRequest()
        now = time.time_ns()
        request.stamp.sec = int(now / 1e9)
        request.stamp.nanosec = int(now % 1e9)
        request.request_id = self.client_id
        request.component_names[:] = ["arm"]
        transform.translation.x = cart_pose[0][0]
        transform.translation.y = cart_pose[0][1]
        transform.translation.z = cart_pose[0][2]
        transform.orientation.x = cart_pose[1][0]
        transform.orientation.y = cart_pose[1][1]
        transform.orientation.z = cart_pose[1][2]
        transform.orientation.w = cart_pose[1][3]
        request.commands.add(transform=transform)
        yield request

    result_iter = self._control_stub.SetState(_generate_cart_pose(cart_pose))

    first_result = next(result_iter)
    if first_result.status != arm_pb2.Status.ACCEPTED:
        self.logger.error("Failed to move to joint position")
        return False

    if blocking:
        for result in result_iter:
            if result.status == arm_pb2.Status.FINISHED:
                self.logger.info("Moving to joint position...")
                return True
        return False
    else:
        return True

@grpc_safe()
def move_to_joint_pos(
    self,
    joint_pos: List[float],
    blocking: bool = True,
) -> bool:
    """
    Send one set of joint position command to the robot.

    The joint position command is sent to the driver server, and the robot
    would plan a path to reach the target joint position then execute it.

    Before calling this method, the robot should be in `PLANNING_POS` mode,
    otherwise the command would be ignored.

    Args:
        joint_pos (list[float]): The joint position commands in radian.
        blocking (bool): Whether to block until the command is finished.
            Default: True

    Returns:
        result (bool): True if the command was sent successfully, False otherwise.
    """

    def _generate_joint_pos(
        joint_pos: List[float],
    ) -> Iterator[arm_pb2.SetStateRequest]:
        request = arm_pb2.SetStateRequest()
        joint_states = types_pb2.JointStates()
        now = time.time_ns()
        request.stamp.sec = int(now / 1e9)
        request.stamp.nanosec = int(now % 1e9)
        request.request_id = self.client_id
        request.component_names[:] = ["arm"]
        joint_states.name[:] = [
            "joint1",
            "joint2",
            "joint3",
            "joint4",
            "joint5",
            "joint6",
        ]
        joint_states.position[:] = joint_pos
        request.commands.add(joint_states=joint_states)
        yield request

    result_iter = self._control_stub.SetState(_generate_joint_pos(joint_pos))

    first_result = next(result_iter)
    if first_result.status != arm_pb2.Status.ACCEPTED:
        self.logger.error("Failed to move to joint position")
        return False

    if blocking:
        for result in result_iter:
            if result.status == arm_pb2.Status.FINISHED:
                self.logger.info("Moving to joint position...")
                return True
        return False
    else:
        return True

@grpc_safe()
def move_with_cart_waypoints(
    self,
    waypoints: list[list[list[float]]],
    blocking: bool = True,
) -> bool:
    """
    Move the robot arm along a series of cartesian waypoints.

    The robot will interpolate through the given list of poses.

    Args:
        waypoints (list[list[list[float]]]): A list of poses, where each pose is a
            2-element list: [translation, orientation]
            - translation: list of 3 floats (x, y, z) in meters
            - orientation: list of 4 floats (x, y, z, w) as quaternion
        blocking (bool): Whether to wait until execution is finished.

    Returns:
        bool: True if the command was accepted and (optionally) completed.
    """

    def _generate_waypoints(
        cart_pose: list[list[list[float]]],
    ) -> Iterator[arm_pb2.SetStateRequest]:
        waypoints_obj = types_pb2.WayPoints()
        request = arm_pb2.SetStateRequest()
        now = time.time_ns()
        request.stamp.sec = int(now / 1e9)
        request.stamp.nanosec = int(now % 1e9)
        request.request_id = self.client_id
        request.component_names[:] = ["arm"]
        for e in cart_pose:
            transform = types_pb2.Transform()
            transform.translation.x = e[0][0]
            transform.translation.y = e[0][1]
            transform.translation.z = e[0][2]
            transform.orientation.x = e[1][0]
            transform.orientation.y = e[1][1]
            transform.orientation.z = e[1][2]
            transform.orientation.w = e[1][3]
            waypoints_obj.transform_array.extend([transform])

        command = types_pb2.Command()
        command.waypoints.CopyFrom(waypoints_obj)
        request.commands.append(command)
        yield request

    result_iter = self._control_stub.SetState(_generate_waypoints(waypoints))
    first_result = next(result_iter)
    if first_result.status != arm_pb2.Status.ACCEPTED:
        self.logger.error("Failed to move to joint position")
        return False
    if blocking:
        print("Waiting for arm to finish moving...")
        for result in result_iter:
            if result.status == arm_pb2.Status.FINISHED:
                self.logger.info("Arm movement finished")
                return True
        return False
    else:
        return True

@grpc_safe()
def move_with_joint_waypoints(
    self,
    waypoints: list[list[list[float]]],
    blocking: bool = True,
) -> bool:
    """
    Move the robot arm along a series of joint space waypoints.

    Args:
        waypoints (list[list[list[float]]]): A list of joint angle sets. Each element is a list of
            joint positions (floats) for the 6 joints [joint1, ..., joint6].
        blocking (bool): Whether to wait until execution is finished.

    Returns:
        bool: True if the command was accepted and (optionally) completed.
    """

    def _generate_waypoints(
        joint_pose: list[list[list[float]]],
    ) -> Iterator[arm_pb2.SetStateRequest]:
        waypoints_obj = types_pb2.WayPoints()
        request = arm_pb2.SetStateRequest()
        now = time.time_ns()
        request.stamp.sec = int(now / 1e9)
        request.stamp.nanosec = int(now % 1e9)
        request.request_id = self.client_id
        request.component_names[:] = ["arm"]

        for e in joint_pose:
            joint_states = types_pb2.JointStates()
            joint_states.name[:] = [
                "joint1",
                "joint2",
                "joint3",
                "joint4",
                "joint5",
                "joint6",
            ]
            joint_states.position[:] = e
            waypoints_obj.joint_states_array.extend([joint_states])

        command = types_pb2.Command()
        command.waypoints.CopyFrom(waypoints_obj)
        request.commands.append(command)
        yield request

    result_iter = self._control_stub.SetState(_generate_waypoints(waypoints))
    first_result = next(result_iter)
    if first_result.status != arm_pb2.Status.ACCEPTED:
        self.logger.error("Failed to move to joint position")
        return False
    if blocking:
        print("Waiting for arm to finish moving...")
        for result in result_iter:
            if result.status == arm_pb2.Status.FINISHED:
                self.logger.info("Arm movement finished")
                return True
        return False
    else:
        return True

@grpc_safe()
def servo_cart_pose(self, cart_pose: List[List[float]]) -> None:
    """
    Send continuous cartesian pose commands to the robot.

    The robot end point (end flange if no end effector installed, otherwise
    the effecting point of the end effector) would follow the cartesian pose
    commands, each of which representing the expected cartesian pose of the end
    flange or the effecting point of the end effector.

    Before calling this method, the robot should be in `SERVO_CART_POSE`
    mode, otherwise the command would be ignored.

    Args:
        cart_pose (list[list[float]]): The cartesian pose commands. \
            The first list is the cartesian translation of the pose in meters, \
            and the second list is the orientation (x, y, z, w) quaternion of the pose.
    """
    if self._servo_thread is None:
        self._servo_thread = threading.Thread(target=self._servo_cmd_gen)
        self._servo_thread.start()
    servo_cmd = types_pb2.Transform()
    servo_cmd.translation.x = cart_pose[0][0]
    servo_cmd.translation.y = cart_pose[0][1]
    servo_cmd.translation.z = cart_pose[0][2]
    servo_cmd.orientation.x = cart_pose[1][0]
    servo_cmd.orientation.y = cart_pose[1][1]
    servo_cmd.orientation.z = cart_pose[1][2]
    servo_cmd.orientation.w = cart_pose[1][3]
    self._servo_cmd_queue.append(servo_cmd)
    self._servo_condition.release()

@grpc_safe()
def servo_cart_twist(self, cart_twist: List[List[float]]) -> None:
    """
    Send continuous cartesian twist commands to the robot.

    The robot end point (end flange if no end effector installed, otherwise
    the effecting point of the end effector) would follow the cartesian twist
    commands, each of which representing the expected translational and
    rotational velocity of the end flange or the effecting point of the end
    effector.

    Before calling this method, the robot should be in `SERVO_CART_TWIST`
    mode, otherwise the command would be ignored.

    Args:
        cart_twist (list[list[float]]): The cartesian twist commands. \
            The first list is the translational velocity of the end flange \
            or the effecting point of the end effector, and the second list \
            is the rotational velocity of the end flange or the effecting point \
            of the end effector.
    """
    if self._servo_thread is None:
        self._servo_thread = threading.Thread(target=self._servo_cmd_gen)
        self._servo_thread.start()
    servo_cmd = types_pb2.Twist()
    servo_cmd.linear.x = cart_twist[0][0]
    servo_cmd.linear.y = cart_twist[0][1]
    servo_cmd.linear.z = cart_twist[0][2]
    servo_cmd.angular.x = cart_twist[1][0]
    servo_cmd.angular.y = cart_twist[1][1]
    servo_cmd.angular.z = cart_twist[1][2]
    self._servo_cmd_queue.append(servo_cmd)
    self._servo_condition.release()