RobotService Interface

This section introduces Adapter interfaces related to RobotService, as listed below.

Simulate RobotServer Service

Simulate RobotServer Service is usually used in scenarios where the robot is master-controlled by Mech-Mind, but the robot is not directly controlled via RobotServer. Instead, the RobotService in Adapter simulates RobotServer and therefore is used to control the robot. After RobotService is registered in Mech-Center, Mech-Viz will interact with RobotService as it interacts with the real robot.

Simulated RobotServer service can be used to:

  • Obtain JPs of the robot for computation in Mech-Vision (in Eye In Hand mode)

  • Obtain the pose from the real robot and send it RobotService

Example

The RobotService object in a child class in Adapter is called as follows:

def _register_service(self):
       """
       register_service
       :return:
       """
       if self.robot_service:
           return

       self.robot_service = RobotService(self)
       other_info = {'robot_type': self.robot_service.service_name}
       self.server, _ = register_service(self.hub_caller, self.robot_service, other_info)

       self.robot_service.setJ([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])

The RobotService class is shown below.

class RobotService(JsonService):
    service_type = "robot"
    service_name = "robot"
    jps = [0, 0, 0, 0, 0, 0]
    pose = [0, 0, 0, 1, 0, 0, 0]

    def getJ(self, *_):
        return {"joint_positions": self.jps}

    def setJ(self, jps):
        logging.info("setJ:{}".format(jps))
        self.jps = jps

    def getL(self, *_):
        return {"tcp_pose": self.pose}

    def getFL(self, *_):
        return {"flange_pose": self.pose}

    def setL(self, pose):
        logging.info("setL:{}".format(pose))
        self.pose = pose

    def moveXs(self, params, _):
        pass

    def stop(self, *_):
        pass

    def setTcp(self, *_):
        pass

    def setDigitalOut(self, params, _):
        pass

    def getDigitalIn(self, *_):
        pass

    def switchPauseContinue(self, *_):
        pass

getJ

getJ() is used to obtain the current JPs in Mech-Viz and Mech-Vision. Usually, the current JPs are set in setJ() first and then getJ() will be called.

Example

def getJ(self, *_):
     pose = {"joint_positions": self._jps}
     return pose
  1. Eye In Hand: write the JPs sent by the robot into setJ().

    def setJ(self, jps):
      assert len(jps) == 6
      for i in range(6):
          jps[i] = deg2rad(float(jps[i]))
      self._jps = jps
      logging.info("SetJ:{}".format(self._jps))
    

    jps is the JPs sent by the robot, its value will be assigned to self._jps, which will be called by the getJ() function. Since the getJ() requires data in radians, please note unit conversions here.

  2. Eye To Hand: It is not necessary to set the current JPs according to the pose of the real robot. However, a Home position that will not lead to collision needs to be set to simulate the real robot, or else the value will be assigned randomly and an error may occur.

    def getJ(self, *_):
         return {"joint_positions": [1.246689,-0.525868,-0.789761,-1.330814, 0.922581, 4.364021]}
    

getFL

getFL() provides the flange pose when the robot captures images. Since in Eye In Hand mode, the calibrated positional relationship is between the flange and camera, and the data finally used is in the robot base reference frame, the flange pose when the robot captures images is needed.

def getFL(self, *_):
    return {"flange_pose": self.pose}

Please note the following details when using Eye In Hand.

  1. If the robot returns image capture pose in JPs, you can directly call setJ() in RobotService (the unit is radians) and the function will return to [].

  2. If the robot returns image capture pose in flange pose, then:

  • make sure that is_eye_in_hand in the external parameter file “extri_param.json” in the Mech-Vision project is set to True

  • call setFL() of RobotService (the pose is represented by quaternions and the unit is meters)

moveXs

RobotService uses moveXs() to receive targets of the path planned by Mech-Viz.

Function Definition

def moveXs(self, params, _):
     with self.lock:
         for move in params["moves"]:
             self.targets.append(move)
     return {"finished": True}

params passes all the parameters in the Mech-Viz project. All target poses, including targets from Vision Move, Relative Move, etc., can be obtained via params[“moves”]. The returned poses are in JPs by default, and will be passed into self.targets for subsequent calls.

Example

Usually, this function is used together with notify function. When Adapter receives a message, it will pass self.targets into the function and send it to the robot after converting and packing.

def notify(self, request, _):
    msg = request["notify_message"]
    logging.info("{} notify message:{}".format(self.service_name, msg))
    if msg == "started":
        with self.lock:
            self.move_targets.clear()
    elif msg == "finished":
        with self.lock:
            targets = self.move_targets[:]
            self.move_targets.clear()
        self.send_moves(targets)

When the notify message “started” is received, the targets in the list will be cleared, which aims to avoid duplicate targets caused by unexpected disconnection and restart of Mech-Viz. When the notify message “finished” is received, the target list will be passed into the pack_move function, which will consolidate the data and then send out the data.

def pack_move(self, move_param):
     move_list = []
     for i, move in enumerate(move_param):
         target = move["target"]
         move_list.append(target)
     logging.info("move list num:{}".format(len(move_list)))
     logging.info("move list:{}".format(*move_list))
     motion_cmd = pack('>24f', *move_list)
     self.send(motion_cmd)

According to actual needs of the project, you can choose to send all targets planned by Mech-Viz or select several targets from the list to send according to the index. pack_move() usually consolidates the data according to the specific format required by different robot brands, and the required data format is usually specified in the communication protocol in advance.