UR5 visual servoing

edit

Let me suggest 2 approaches.

1) Get the object pose with the visp_tracker. Use the look_at_pose package to calculate the pose of the robot arm that would look at the object (and keep the camera upright). Send the arm to that pose with MoveIt!, then move forward towards the object a bit more (i.e. +x in the end-effector frame). This approach will be a little bit slow and choppy while you wait for MoveIt! plans and the intermediate moves. You might also get large swings of the arm, aka reconfigurations.

2) Get the object pose with visp_tracker. Use the jog_arm package to send a small step towards the object. This would essentially be a 6-dimensional proportional controller: x_cmd = k*(object_x - robot_x), likewise for the other dimensions. jog_arm was developed on a UR5 so I'm sure it would work on your robot. Upside to this approach: smooth, fast motion and no reconfigurations. Downsides to this approach: you need to figure out the controller and the camera probably won't remain upright.

You could get creative and mix approach #1 and #2. Like, use look_at_pose to get a good arm pose, then use jog_arm to move towards it. That might be the best way to go.

Also, the UR5 tends to run into joint limits and singularities more often than we'd all like. C'est la vie

So, the tools you need are out there, but implementing it all is non-trivial, as @PeteBlackerThe3rd said.

This really is a non-trivial task you're trying to undertake. The challenge is reactive velocity control of the arm as opposed to deterministic goal based control. This is still an active research topic as far as I'm aware.

MoveIt has the ability to perform basic velocity control, but this will result in extreme behaviour near singularities which will throw your UR5 into safe mode. I have personal experience of doing exactly this.

Can you answer a couple of questions:

Are you using a wrist mounted camera or a statically mounted one?

Do you need the arm to follow the marker in real time?

Edit:

To implement real-time tracking you need to use velocity control of the arm. This is not too complicated to implement if you ignore the problem of singularities, I'll describe the theory below.

You'll need to create a control law that determines the desired angular and linear velocities of the end effector based upon the relative position of the marker. The velocity should be higher the further away the end effector is from its desired location, and you'll want a dead zone around the perfect position to avoid it oscillating around this.

Once you've got this desired velocity (twist message type), you'll need to get moveit to calculate the Jacobin matrix of the manipulator at its current location. This matrix can be used to translate the velocity of the end effector into a vector of joint velocities.

Now you simply pass these joint velocities to the UR driver to control the arm.

One limitation of this method is it will happily run straight into a singularity, solving that is a whole new problem. Hope this helps.