Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
motion_kick_task.cpp
1 
2 /***************************************************************************
3  * motion_kick_task.cpp - Make the robot kick asses... ehm soccer balls
4  *
5  * Created: Fri Jan 23 18:36:01 2009
6  * Copyright 2006-2011 Tim Niemueller [www.niemueller.de]
7  * 2010 Patrick Podbregar [www.podbregar.com]
8  *
9  ****************************************************************************/
10 
11 /* This program is free software; you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation; either version 2 of the License, or
14  * (at your option) any later version.
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL file in the doc directory.
22  */
23 
24 #include "motion_kick_task.h"
25 
26 #include <core/exceptions/system.h>
27 #include <utils/math/angle.h>
28 
29 #include <cstring>
30 #include <cstdlib>
31 #include <string>
32 #include <unistd.h>
33 
34 using namespace AL;
35 using namespace fawkes;
36 
37 /** @class NaoQiMotionKickTask "motion_kick_task.h"
38  * NaoQi kick task.
39  * This task can be used to make the robot kick in a non-blocking way. It will
40  * use (blocking) ALMotion calls to execute the move. Note that ALMotion should
41  * not be used otherwise while kicking.
42  * @author Tim Niemueller
43  */
44 
45 /** Constructor.
46  * @param almotion ALMotion proxy
47  * @param leg leg to kick with
48  */
49 NaoQiMotionKickTask::NaoQiMotionKickTask(AL::ALPtr<AL::ALMotionProxy> almotion,
51 {
52  __quit = false;
53  __almotion = almotion;
54  __leg = leg;
55 
56  // ALTask variable to cause auto-destruct when done
57  fAutoDelete = true;
58 }
59 
60 
61 /** Destructor. */
63 {
64 }
65 
66 
67 /*
68 static void
69 print_angles(std::vector<float> angles)
70 {
71  for (std::vector<float>::iterator i = angles.begin(); i != angles.end(); ++i) {
72  printf("%f, ", *i);
73  }
74  printf("\n");
75  for (std::vector<float>::iterator i = angles.begin(); i != angles.end(); ++i) {
76  printf("%f, ", rad2deg(*i));
77  }
78  printf("\n\n\n");
79 }
80 */
81 
82 void
83 NaoQiMotionKickTask::goto_start_pos(AL::ALValue speed, bool concurrent)
84 {
85  float shoulder_pitch = 120;
86  float shoulder_roll = 15;
87  float elbow_yaw = -90;
88  float elbow_roll = -80;
89  float hip_yaw_pitch = 0;
90  float hip_roll = 0;
91  float hip_pitch = -25;
92  float knee_pitch = 40;
93  float ankle_pitch = -20;
94  float ankle_roll = 0;
95 
96  ALValue target_angles;
97  target_angles.arraySetSize(20);
98 
99  // left arm
100  target_angles[0] = deg2rad(shoulder_pitch);
101  target_angles[1] = deg2rad(shoulder_roll);
102  target_angles[2] = deg2rad(elbow_yaw);
103  target_angles[3] = deg2rad(elbow_roll);
104  // right arm
105  target_angles[4] = deg2rad(shoulder_pitch);
106  target_angles[5] = -deg2rad(shoulder_roll);
107  target_angles[6] = -deg2rad(elbow_yaw);
108  target_angles[7] = -deg2rad(elbow_roll);
109  // left leg
110  target_angles[8] = deg2rad(hip_yaw_pitch);
111  target_angles[9] = deg2rad(hip_roll);
112  target_angles[10] = deg2rad(hip_pitch);
113  target_angles[11] = deg2rad(knee_pitch);
114  target_angles[12] = deg2rad(ankle_pitch);
115  target_angles[13] = deg2rad(ankle_roll);
116  // right leg
117  target_angles[14] = deg2rad(hip_yaw_pitch);
118  target_angles[15] = deg2rad(hip_roll);
119  target_angles[16] = deg2rad(hip_pitch);
120  target_angles[17] = deg2rad(knee_pitch);
121  target_angles[18] = deg2rad(ankle_pitch);
122  target_angles[19] = -deg2rad(ankle_roll);
123 
124  ALValue names = ALValue::array("LArm", "RArm", "LLeg", "RLeg");
125 
126  if (concurrent) {
127  __almotion->post.angleInterpolationWithSpeed(names, target_angles, speed);
128  } else {
129  __almotion->angleInterpolationWithSpeed(names, target_angles, speed);
130  }
131 }
132 
133 
134 /** Stop the current kick task.
135  * Stops the current motion and posts a goto for the start position.
136  * This is not stable from all configurations but seems to suffices
137  * most of the time.
138  */
139 void
141 {
142  __quit = true;
143  std::vector<std::string> joint_names = __almotion->getJointNames("Body");
144  __almotion->killTasksUsingResources(joint_names);
145  goto_start_pos(0.2, true);
146 }
147 
148 /** Run the kick. */
149 void
151 {
152  const char *shoot_hip_roll_name = NULL;
153  const char *support_hip_roll_name = NULL;
154  const char *shoot_hip_pitch_name = NULL;
155  const char *support_hip_pitch_name = NULL;
156  const char *shoot_knee_pitch_name = NULL;
157  const char *shoot_ankle_pitch_name = NULL;
158  const char *shoot_ankle_roll_name = NULL;
159  const char *support_ankle_roll_name = NULL;
160 
161  float shoot_hip_roll = 0;
162  float support_hip_roll = 0;
163  float shoot_hip_pitch = 0;
164  float support_hip_pitch = 0;
165  float shoot_knee_pitch = 0;
166  float shoot_ankle_pitch = 0;
167  float shoot_ankle_roll = 0;
168  float support_ankle_roll = 0;
169 
170  float BALANCE_HIP_ROLL = 0;
171  float BALANCE_ANKLE_ROLL = 0;
172  float STRIKE_OUT_HIP_ROLL = 0;
173 
175  shoot_hip_roll_name = "LHipRoll";
176  support_hip_roll_name = "RHipRoll";
177  shoot_hip_pitch_name = "LHipPitch";
178  support_hip_pitch_name = "RHipPitch";
179  shoot_knee_pitch_name = "LKneePitch";
180  shoot_ankle_pitch_name = "LAnklePitch";
181  shoot_ankle_roll_name = "LAnkleRoll";
182  support_ankle_roll_name = "RAnkleRoll";
183 
184  BALANCE_HIP_ROLL = 20;
185  BALANCE_ANKLE_ROLL = -25;
186  STRIKE_OUT_HIP_ROLL = 30;
187  } else if (__leg == fawkes::HumanoidMotionInterface::LEG_RIGHT ) {
188  shoot_hip_roll_name = "RHipRoll";
189  support_hip_roll_name = "LHipRoll";
190  shoot_hip_pitch_name = "RHipPitch";
191  support_hip_pitch_name = "LHipPitch";
192  shoot_knee_pitch_name = "RKneePitch";
193  shoot_ankle_pitch_name = "RAnklePitch";
194  shoot_ankle_roll_name = "RAnkleRoll";
195  support_ankle_roll_name = "LAnkleRoll";
196 
197  BALANCE_HIP_ROLL = -20;
198  BALANCE_ANKLE_ROLL = 25;
199  STRIKE_OUT_HIP_ROLL = -30;
200  }
201 
202  if (__quit) return;
203  goto_start_pos(0.2);
204 
205  ALValue names;
206  ALValue target_angles;
207  float speed = 0;
208 
209  // Balance on supporting leg
210  names.arraySetSize(4);
211  target_angles.arraySetSize(4);
212 
213  support_hip_roll = BALANCE_HIP_ROLL;
214  shoot_hip_roll = BALANCE_HIP_ROLL;
215  shoot_ankle_roll = BALANCE_ANKLE_ROLL;
216  support_ankle_roll = BALANCE_ANKLE_ROLL;
217 
218  names = ALValue::array(support_hip_roll_name, shoot_hip_roll_name,
219  support_ankle_roll_name, shoot_ankle_roll_name);
220  target_angles = ALValue::array(deg2rad(support_hip_roll), deg2rad(shoot_hip_roll),
221  deg2rad(support_ankle_roll), deg2rad(shoot_ankle_roll));
222  speed = 0.15;
223 
224  //if (__quit) return;
225  __almotion->angleInterpolationWithSpeed(names, target_angles, speed);
226 
227  names.clear();
228  target_angles.clear();
229 
230  // Raise shooting leg
231  names.arraySetSize(3);
232  target_angles.arraySetSize(3);
233 
234  shoot_hip_roll = STRIKE_OUT_HIP_ROLL;
235  shoot_knee_pitch = 90;
236  shoot_ankle_pitch = -50;
237 
238  names = ALValue::array(shoot_hip_roll_name, shoot_knee_pitch_name, shoot_ankle_pitch_name);
239  target_angles = ALValue::array(deg2rad(shoot_hip_roll), deg2rad(shoot_knee_pitch),
240  deg2rad(shoot_ankle_pitch));
241  speed = 0.2;
242 
243  if (__quit) return;
244  __almotion->angleInterpolationWithSpeed(names, target_angles, speed);
245 
246  names.clear();
247  target_angles.clear();
248 
249  // Strike out
250  names.arraySetSize(2);
251  target_angles.arraySetSize(2);
252 
253  shoot_hip_pitch = 20;
254  support_hip_pitch = -50;
255 
256  names = ALValue::array(shoot_hip_pitch_name, support_hip_pitch_name);
257  target_angles = ALValue::array(deg2rad(shoot_hip_pitch), deg2rad(support_hip_pitch));
258  speed = 0.1;
259 
260  if (__quit) return;
261  __almotion->angleInterpolationWithSpeed(names, target_angles, speed);
262 
263  names.clear();
264  target_angles.clear();
265 
266  // Shoot
267  names.arraySetSize(4);
268  target_angles.arraySetSize(4);
269 
270  shoot_hip_pitch = -80;
271  support_hip_pitch = -40;
272  shoot_knee_pitch = 50;
273  shoot_ankle_pitch = -30;
274 
275  names = ALValue::array(shoot_hip_pitch_name, support_hip_pitch_name,
276  shoot_knee_pitch_name, shoot_ankle_pitch_name);
277  target_angles = ALValue::array(deg2rad(shoot_hip_pitch), deg2rad(support_hip_pitch),
278  deg2rad(shoot_knee_pitch), deg2rad(shoot_ankle_pitch));
279  speed = 1.0;
280  if (__quit) return;
281  __almotion->angleInterpolationWithSpeed(names, target_angles, speed);
282 
283  names.clear();
284  target_angles.clear();
285 
286  // Move to upright position
287  names.arraySetSize(2);
288  target_angles.arraySetSize(2);
289 
290  shoot_hip_pitch = -25;
291  support_hip_pitch = -25;
292 
293  names = ALValue::array(shoot_hip_pitch_name, support_hip_pitch_name);
294  target_angles = ALValue::array(deg2rad(shoot_hip_pitch), deg2rad(support_hip_pitch));
295  speed = 0.1;
296 
297  if (__quit) return;
298  __almotion->angleInterpolationWithSpeed(names, target_angles, speed);
299 
300  //names.clear();
301  //target_angles.clear();
302 
303  if (__quit) return;
304  goto_start_pos(0.1);
305 }