How to plan a path using code.

[Akumar201]

Hello, I am trying to write my own pick and place python script using xarm_ros/xarm_vision/camera_demo/scripts https://github.com/xArm-Developer/xarm_ros/blob/master/xarm_vision/camera_demo/scripts/color_recognition.py color_recognition.py file as a reference. However I feel like the XArmCtrl class does not plan a path and move to near an object using predefined path , I would like to just confirm my doubt, please correct me if I am wrong. How can I use it to move to the point in 3D coordinate.

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

    def _init(self):
        # self._commander.set_max_acceleration_scaling_factor(0.5)
        # self._commander.set_max_velocity_scaling_factor(0.5)
        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

[Akumar201]

For future users.

1. Use the xarm_move_gropu.launch https://github.com/xArm-Developer/xarm_ros/tree/master/xarm_vision/camera_demo/launch to establish connection between arm and the computer.
2. Then write the code using the tutorial at http://docs.ros.org/en/kinetic/api/moveit_tutorials/html/doc/move_group_python_interface/move_group_python_interface_tutorial.htmll and you can test it.

I am closing this issue as it has been solved. If the problem persists, please comment and the issue will be reopened if appropriate.