Project

General

Profile

Planning API functions description » History » Version 4

Kévin Desormeaux, 2021-06-11 12:01

1 1 Kévin Desormeaux
h1. Planning API functions description
2
3
h2. Master branch
4
5 3 Kévin Desormeaux
h3. Main functions
6
7 1 Kévin Desormeaux
The whole CM2P stack can be launched with a simple launch file. The launch files are available in this package (dual_arm_planning_api/launch).
8
There is two of them. One is for launching CM2P in simulation mode. 
9
10
<pre>
11
roslaunch dual_arm_planning_api dual_arm_planning_api_simu.launch
12
</pre>
13
14
The other is for launching CM2P with the real robots. *The two instances of CM2C must be running beforehand so that the controllers are spawned.* 
15
16
<pre>
17
roslaunch dual_arm_planning_api dual_arm_planning_api.launch
18
</pre>
19
20
Now the dual_arm_planning_api node is running. It is necessary to run the following rosservice in another terminal to finish the initialization of the node:
21
22
<pre>
23
rosservice call /dual_arm_planning_api/init "{}"
24
</pre>
25
26
This takes a few seconds to load the environment, the frames etc. You should see all of this pop in Rviz.
27
After that the node is ready to take commands.
28
29
30
We can start with a left pick or a right pick.
31
32
<pre>
33
rostopic pub /dual_arm_planning_api/PickLeft/goal dual_arm_planning_api/PickActionGoal "header:
34
  seq: 0
35
  stamp:
36
    secs: 0
37
    nsecs: 0
38
  frame_id: ''
39
goal_id:
40
  stamp:
41
    secs: 0
42
    nsecs: 0
43
  id: ''
44
goal:
45
  planning_time: 10.0
46
  pre_grasp_approach_direction:
47
    x: 0.0
48
    y: 0.0
49
    z: 1.0
50
  pre_grasp_approach_distance: 0.1"
51 2 Kévin Desormeaux
52
53
rostopic pub /dual_arm_planning_api/PickRight/goal dual_arm_planning_api/PickActionGoal "header:
54
  seq: 0
55
  stamp:
56
    secs: 0
57
    nsecs: 0
58
  frame_id: ''
59
goal_id:
60
  stamp:
61
    secs: 0
62
    nsecs: 0
63
  id: ''
64
goal:
65
  planning_time: 10.0
66
  pre_grasp_approach_direction:
67
    x: 0.0
68
    y: 0.0
69
    z: 1.0
70
  pre_grasp_approach_distance: 0.1"
71 1 Kévin Desormeaux
</pre>
72
73 2 Kévin Desormeaux
First you can notice that i use rostopic pub instead of rosaction call. The reason is that with rostopic pub we can have auto-completion in the terminal (with tab) which is very useful when we have a lot of arguments. You will have it with rosservices as well, but not with rosactions.
74 1 Kévin Desormeaux
But most importantly you will notice that there is no pose defined for the grasp frame. This is because there is one defined during the initialization for each arm. 
75
But you can change it whenever you want with the following service:
76
77
<pre>
78
rosservice call /dual_arm_planning_api/SetLeftGrasp  "position:
79
  x: 0.0
80
  y: 0.0
81
  z: 0.0
82
orientation:
83
  x: 0.0
84
  y: 0.0
85
  z: 0.0"
86 2 Kévin Desormeaux
</pre>
87
88
You can notice that the frames (left_arm_desired_Grasp or right_arm_desired_Grasp) are updated in Rviz after calling the service. 
89
The SetLeftGrasp (respectively SetRightGrasp) service must be called however before the PickLeft (respectively PickRight) action. 
90
You can use PickLeft or PickRight in the order you want. The first pick will be a default moveit! pick and the second one a dual-arm pick. 
91
You don't have to worry about that as you will use PickLeft (respectively PickRight) for both.
92
93
Now that the object is picked by both arms, we can start to think about the place.
94
First we have to define the new location of the object after the place. For that we call the following service:
95
96
<pre>
97
rosservice call  /dual_arm_planning_api/SetObjectPose "position:
98
  x: 0.0
99
  y: 0.0
100
  z: 0.4
101
orientation:
102
  x: 0.0
103
  y: 0.0
104
  z: 0.0"
105
</pre>
106
107
With this service we define the pose of the dynamic object (after place) in the frame of the static object.
108
You can notice that the frames are updated in Rviz after calling the service. This is really useful to check the validity of the new pose before planning.
109
Once this is done we can plan a place().
110
111
This is done by calling the following action:
112
113
<pre>
114
rostopic pub /dual_arm_planning_api/PlanPlace/goal dual_arm_planning_api/PlanPlaceActionGoal "header:
115
  seq: 0
116
  stamp:
117
    secs: 0
118
    nsecs: 0
119
  frame_id: ''
120
goal_id:
121
  stamp:
122
    secs: 0
123
    nsecs: 0
124
  id: ''
125
goal:
126
  planning_time: 400.0
127
  start_translation_direction:
128
    x: 0.0
129
    y: 0.0
130
    z: 1.0
131
  start_translation_distance: 0.1"
132
</pre>
133
134
It only takes the start translation (translation at the beginning of the place) and the planning time as arguments. 
135
In the example above we add a translation of 10cm in the Z direction. This is defined in the frame of the object.
136
We use a planning time of 400seconds. Once a solution is found, the remaining time will be used for path simplification.
137
138
Once the solution is found and the path simplified, you should be able to see in Rviz the planned trajectories displayed in purple. 
139
After checking the validity of the trajectories you can execute the place() by calling the following action:
140
141
<pre>
142
rosaction call /dual_arm_planning_api/Place "{}"
143 1 Kévin Desormeaux
</pre>
144 3 Kévin Desormeaux
145
h3. Utilities, untested functions
146
147
I am currently adding various utilities to the API. For examples functions to release the object, homing functions, or move functions (without pick). 
148
Not sure if this will be finished before i leave the lab but i will list some here:
149
150
<pre>
151
rosservice call /dual_arm_planning_api/ReleaseLeft "{}"
152
rosservice call /dual_arm_planning_api/ReleaseRight "{}"
153
rosservice call /dual_arm_planning_api/HomingLeft "{}"
154
rosservice call /dual_arm_planning_api/HomingRight "{}"
155
</pre>
156
157
May be not everything will be listed here, and you can check what is available with the following commands:
158
159
<pre>
160
rosservice list
161
rossaction list
162
</pre>
163
164
165 4 Kévin Desormeaux
h2. pro_act_demo branch
166 3 Kévin Desormeaux
167
There is another branch, the pro_act_demo branch. This is the code used for the pro-act demonstration. It is a linear sequence of pick/place hardcoded functions etc. You can replay the demo by using the launch file and pressing enter in the terminal to execute each step of the demo. The code can be interesting to look at as i reused it a lot to create the master branch. This code should still run as it is without any modification to CM2C.