Fawkes API  Fawkes Development Version
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 }
NaoQiMotionKickTask(AL::ALPtr< AL::ALMotionProxy > almotion, fawkes::HumanoidMotionInterface::LegEnum leg)
Constructor.
virtual ~NaoQiMotionKickTask()
Destructor.
Fawkes library namespace.
virtual void exitTask()
Stop the current kick task.
LegEnum
Type to determinate leg side.
virtual void run()
Run the kick.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:37