How can I move the robot using MoveGroupCommander

edit

I am trying to write my pick-and-place python script using the color_recognition.py file as a reference. However, I feel like the XArmCtrl class does not plan a path and move near an object using a predefined path. I want to confirm my doubt. Therefore, please correct me if I am wrong. How can I use it to move to the point in the 3D coordinate?

Below is my code:

class XArmCtrl(object):
    def __init__(self, dof):
        self._commander = moveit_commander.move_group.MoveGroupCommander(
            "xarm{}".format(dof)
        )
        self._init()

    def _init(self):
        pass

    def set_joints(self, angles, wait=True):
        try:
            joint_target = self._commander.get_current_joint_values()
            print(joint_target)
            for i in range(joint_target):
                if i >= len(angles):
                    break
                if angles[i] is not None:
                    joint_target[i] = math.radians(angles[i])
            print("set_joints, joints={}".format(joint_target))
            self._commander.set_joint_value_target(joint_target)
            ret = self._commander.go(wait=wait)
            print("move to finish, ret={}".format(ret))
            return ret
        except Exception as e:
            print("[Ex] set_joints exception, ex={}".format(e))

    def set_joint(self, angle, inx=-1, wait=True):
        try:
            joint_target = self._commander.get_current_joint_values()
            joint_target[inx] = math.radians(angle)
            print("set_joints, joints={}".format(joint_target))
            self._commander.set_joint_value_target(joint_target)
            ret = self._commander.go(wait=wait)
            print("move to finish, ret={}".format(ret))
            return ret
        except Exception as e:
            print("[Ex] set_joint exception, ex={}".format(e))
        return False

    def moveto(
        self,
        x=None,
        y=None,
        z=None,
        ox=None,
        oy=None,
        oz=None,
        relative=False,
        wait=True,
    ):
        if (
            x == 0
            and y == 0
            and z == 0
            and ox == 0
            and oy == 0
            and oz == 0
            and relative
        ):
            return True
        try:
            pose_target = self._commander.get_current_pose().pose
            if relative:
                pose_target.position.x += x / 1000.0 if x is not None else 0
                pose_target.position.y += y / 1000.0 if y is not None else 0
                pose_target.position.z += z / 1000.0 if z is not None else 0
                pose_target.orientation.x += ox if ox is not None else 0
                pose_target.orientation.y += oy if oy is not None else 0
                pose_target.orientation.z += oz if oz is not None else 0
            else:
                pose_target.position.x = x / 1000.0 if x is not None else pose_target.position.x
                pose_target.position.y = y / 1000.0 if y is not None else pose_target.position.y
                pose_target.position.z = z / 1000.0 if z is not None else pose_target.position.z
                pose_target.orientation.x = ox if ox is not None else pose_target.orientation.x
                pose_target.orientation.y = oy if oy is not None else pose_target.orientation.y
                pose_target.orientation.z = oz if oz is not None else pose_target.orientation.z
            print(
                "move to position=[{:.2f}, {:.2f}, {:.2f}], orientation=[{:.6f}, {:.6f}, {:.6f}]".format(
                    pose_target.position.x * 1000.0,
                    pose_target.position.y * 1000.0,
                    pose_target.position.z * 1000.0,
                    pose_target.orientation.x,
                    pose_target.orientation.y,
                    pose_target.orientation.z,
                )
            )
            self._commander.set_pose_target(pose_target)
            ret = self._commander.go(wait=wait)
            print("move to finish, ret={}".format(ret))
            return ret
        except Exception as e:
            print("[Ex] moveto exception: {}".format(e))
        return False