# Give a hand-eye robot series of task

Hello everyone,

I want to give my robot a series of tasks. From this image, I want my robot to calculate the coordinates and then select first back left cow teat, back right teat, then front left and front right teat:

My main task is to grab a cup from a fixed coordinate and attach it to the cow teat based on the given sequence. I am getting the world(3D){link} coordinate for the detected object, and based on that, I want to move the hand-eye robot.

The sample result that I achieved up until now is shown in this screenshot:

The series of tasks described below:

1. Grab the first cups from a fixed location and go to the work location (under cow udder), detect the four teats and attach the first cup to the left-back teat.
2. Same with the remaining three teat in the above sequence.

Question:

1. What package should I use? Eg. MoveIt? How can I implement it?
2. How can I set the fixed location for the cup?
3. How can I get the sequence of the teat?
4. How can I achieve the series of goals for this use-case?

Originally posted by Ranjit Kathiriya on ROS Answers with karma: 1622 on 2021-02-04

Post score: 5

Comment by fvd on 2021-02-04:
I uploaded your images to the post because I enjoyed the cow teat detector. You should have enough karma to do it yourself in the future. I can't post a response right now, but I'll say that it's quite a broad set.

Comment by Ranjit Kathiriya on 2021-02-04:
Thanks a lot for reposting my question...

interesting usecase :).

What package should I use? Eg. MoveIt? How can I implement it?

I'd use moveit, there are a lot of tutorials that apply to your problem/question, devide your problem into smaller problems & read the documentation how to implement it.

How can I set the fixed location for the cup?

You can hard-code the cup position as cartesian coordinates in your /world or /robot_base or whatever you want.

How can I get the sequence of the teat?

That's a bit harder, cause your camera doesn't know distance. You can take a picture and then compare the teal size to see which one is front/back. (don't know if this will give reliable result). Moving to the teal untill the cup touch the cow brings also other challenges.

How can I achieve the series of goals for this use-case?

if you mean planning goals, then I'd recommend to look into the movit documentation

Originally posted by crnewton with karma: 71 on 2021-02-04

This answer was ACCEPTED on the original site

Post score: 2

Comment by fvd on 2021-02-04:
Actually it looks like he is using an RGB-D camera, so he could obtain the distance from the depth image or pointcloud.

Comment by Ranjit Kathiriya on 2021-02-05:
I am getting Z(Distance) from the point cloud.

def direction_teat(list_a):
if len(list_a) == 4:
list_a.sort(key=takeSecond,reverse=True)
list_a[0:2][0][4] = "back"
list_a[0:2][1][4] = "back"
list_a[2:][0][4] = "front"
list_a[2:][1][4] = "front"

print(list_a)

def takeSecond(elem):
return elem[3]

list_a = [['teat 1', -0.035606520698008404, -0.009521973548766684, 0.41100001335144043, 'OO'], ['teat 2', -0.08732340555748698, 0.013691530685289598, 0.445000022649765, 'OO'], ['teat 4', -0.01155150568601558, 0.029179580759476156, 0.47200003266334534, 'OO'], ['teat 3', -0.08205969353363421, 0.019158233769792383, 0.49200001


Comment by Ranjit Kathiriya on 2021-02-05:
In this above code, I am getting front and back position from the distance(Z), but for getting left and right I am confused should I take X or Y?

1. What should be my approach to solving this problem?
2. I am getting tf transformation of quaternion for every teat and I have to create a new node in the move-it. Then subscribe to tf topic and just give coordinate to the robot but I will I be able to manage the sequencing of that teat.

For example:

• what if the algorithm Yolo5 has detected tail as a teat?
• what if the cow can able milk from 3 teats?
• sometimes cow has 5 teats how to ignore 1 teat from that?
• If the cow kicks the robot hand and cannot able to attach the cup so for that what will be the scenario?

Comment by fvd on 2021-02-05:
Each of these questions could be a post of its own. I don't know that any of them can really be answered.

• You should know where "left" and "right" are relative to your camera.
• You don't have to create a "a new node in the moveit" (I don't even know what that means). It would make sense if you started one node that uses MoveIt and commands your robot to go to each position sequentially. You could use something like MoveIt Task Constructor to plan motions, or think up a procedure that will work for most or all of the cases you encounter. This is your engineering challenge.
• If there is a misdetection, you could either accept it and milk it anyway, or try to confirm each result from multiple angles or a series of images. You also get a confidence rating from Yolo which you can use.
• If a cow only has 3 teats then you can only milk 3. What you do with the last one depends on your machine.

Comment by fvd on 2021-02-05:
(cont)

• If a cow has 5 teats you can probably ignore any one of them. Or not - you should ask a farmer, not a robotics forum.
• If the robot cannot attach the cups, it cannot attach the cups. What comes next depends on your constraints, which you have not described and only you know about. Maybe you can alert the supervisor and have a human milk the problematic cow? Or you could tag the cow as uncooperative and send it off to the butcher?

Consider narrowing down your questions and making a new post. Comments are not really meant for extending the question.

Comment by Ranjit Kathiriya on 2021-02-05: