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 <cstdlib>
30 #include <cstring>
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 /** Destructor. */
62 {
63 }
64 
65 /*
66 static void
67 print_angles(std::vector<float> angles)
68 {
69  for (std::vector<float>::iterator i = angles.begin(); i != angles.end(); ++i) {
70  printf("%f, ", *i);
71  }
72  printf("\n");
73  for (std::vector<float>::iterator i = angles.begin(); i != angles.end(); ++i) {
74  printf("%f, ", rad2deg(*i));
75  }
76  printf("\n\n\n");
77 }
78 */
79 
80 void
81 NaoQiMotionKickTask::goto_start_pos(AL::ALValue speed, bool concurrent)
82 {
83  float shoulder_pitch = 120;
84  float shoulder_roll = 15;
85  float elbow_yaw = -90;
86  float elbow_roll = -80;
87  float hip_yaw_pitch = 0;
88  float hip_roll = 0;
89  float hip_pitch = -25;
90  float knee_pitch = 40;
91  float ankle_pitch = -20;
92  float ankle_roll = 0;
93 
94  ALValue target_angles;
95  target_angles.arraySetSize(20);
96 
97  // left arm
98  target_angles[0] = deg2rad(shoulder_pitch);
99  target_angles[1] = deg2rad(shoulder_roll);
100  target_angles[2] = deg2rad(elbow_yaw);
101  target_angles[3] = deg2rad(elbow_roll);
102  // right arm
103  target_angles[4] = deg2rad(shoulder_pitch);
104  target_angles[5] = -deg2rad(shoulder_roll);
105  target_angles[6] = -deg2rad(elbow_yaw);
106  target_angles[7] = -deg2rad(elbow_roll);
107  // left leg
108  target_angles[8] = deg2rad(hip_yaw_pitch);
109  target_angles[9] = deg2rad(hip_roll);
110  target_angles[10] = deg2rad(hip_pitch);
111  target_angles[11] = deg2rad(knee_pitch);
112  target_angles[12] = deg2rad(ankle_pitch);
113  target_angles[13] = deg2rad(ankle_roll);
114  // right leg
115  target_angles[14] = deg2rad(hip_yaw_pitch);
116  target_angles[15] = deg2rad(hip_roll);
117  target_angles[16] = deg2rad(hip_pitch);
118  target_angles[17] = deg2rad(knee_pitch);
119  target_angles[18] = deg2rad(ankle_pitch);
120  target_angles[19] = -deg2rad(ankle_roll);
121 
122  ALValue names = ALValue::array("LArm", "RArm", "LLeg", "RLeg");
123 
124  if (concurrent) {
125  almotion_->post.angleInterpolationWithSpeed(names, target_angles, speed);
126  } else {
127  almotion_->angleInterpolationWithSpeed(names, target_angles, speed);
128  }
129 }
130 
131 /** Stop the current kick task.
132  * Stops the current motion and posts a goto for the start position.
133  * This is not stable from all configurations but seems to suffices
134  * most of the time.
135  */
136 void
138 {
139  quit_ = true;
140  std::vector<std::string> joint_names = almotion_->getJointNames("Body");
141  almotion_->killTasksUsingResources(joint_names);
142  goto_start_pos(0.2, true);
143 }
144 
145 /** Run the kick. */
146 void
148 {
149  const char *shoot_hip_roll_name = NULL;
150  const char *support_hip_roll_name = NULL;
151  const char *shoot_hip_pitch_name = NULL;
152  const char *support_hip_pitch_name = NULL;
153  const char *shoot_knee_pitch_name = NULL;
154  const char *shoot_ankle_pitch_name = NULL;
155  const char *shoot_ankle_roll_name = NULL;
156  const char *support_ankle_roll_name = NULL;
157 
158  float shoot_hip_roll = 0;
159  float support_hip_roll = 0;
160  float shoot_hip_pitch = 0;
161  float support_hip_pitch = 0;
162  float shoot_knee_pitch = 0;
163  float shoot_ankle_pitch = 0;
164  float shoot_ankle_roll = 0;
165  float support_ankle_roll = 0;
166 
167  float BALANCE_HIP_ROLL = 0;
168  float BALANCE_ANKLE_ROLL = 0;
169  float STRIKE_OUT_HIP_ROLL = 0;
170 
172  shoot_hip_roll_name = "LHipRoll";
173  support_hip_roll_name = "RHipRoll";
174  shoot_hip_pitch_name = "LHipPitch";
175  support_hip_pitch_name = "RHipPitch";
176  shoot_knee_pitch_name = "LKneePitch";
177  shoot_ankle_pitch_name = "LAnklePitch";
178  shoot_ankle_roll_name = "LAnkleRoll";
179  support_ankle_roll_name = "RAnkleRoll";
180 
181  BALANCE_HIP_ROLL = 20;
182  BALANCE_ANKLE_ROLL = -25;
183  STRIKE_OUT_HIP_ROLL = 30;
184  } else if (leg_ == fawkes::HumanoidMotionInterface::LEG_RIGHT) {
185  shoot_hip_roll_name = "RHipRoll";
186  support_hip_roll_name = "LHipRoll";
187  shoot_hip_pitch_name = "RHipPitch";
188  support_hip_pitch_name = "LHipPitch";
189  shoot_knee_pitch_name = "RKneePitch";
190  shoot_ankle_pitch_name = "RAnklePitch";
191  shoot_ankle_roll_name = "RAnkleRoll";
192  support_ankle_roll_name = "LAnkleRoll";
193 
194  BALANCE_HIP_ROLL = -20;
195  BALANCE_ANKLE_ROLL = 25;
196  STRIKE_OUT_HIP_ROLL = -30;
197  }
198 
199  if (quit_)
200  return;
201  goto_start_pos(0.2);
202 
203  ALValue names;
204  ALValue target_angles;
205  float speed = 0;
206 
207  // Balance on supporting leg
208  names.arraySetSize(4);
209  target_angles.arraySetSize(4);
210 
211  support_hip_roll = BALANCE_HIP_ROLL;
212  shoot_hip_roll = BALANCE_HIP_ROLL;
213  shoot_ankle_roll = BALANCE_ANKLE_ROLL;
214  support_ankle_roll = BALANCE_ANKLE_ROLL;
215 
216  names = ALValue::array(support_hip_roll_name,
217  shoot_hip_roll_name,
218  support_ankle_roll_name,
219  shoot_ankle_roll_name);
220  target_angles = ALValue::array(deg2rad(support_hip_roll),
221  deg2rad(shoot_hip_roll),
222  deg2rad(support_ankle_roll),
223  deg2rad(shoot_ankle_roll));
224  speed = 0.15;
225 
226  //if (quit_) return;
227  almotion_->angleInterpolationWithSpeed(names, target_angles, speed);
228 
229  names.clear();
230  target_angles.clear();
231 
232  // Raise shooting leg
233  names.arraySetSize(3);
234  target_angles.arraySetSize(3);
235 
236  shoot_hip_roll = STRIKE_OUT_HIP_ROLL;
237  shoot_knee_pitch = 90;
238  shoot_ankle_pitch = -50;
239 
240  names = ALValue::array(shoot_hip_roll_name, shoot_knee_pitch_name, shoot_ankle_pitch_name);
241  target_angles =
242  ALValue::array(deg2rad(shoot_hip_roll), deg2rad(shoot_knee_pitch), deg2rad(shoot_ankle_pitch));
243  speed = 0.2;
244 
245  if (quit_)
246  return;
247  almotion_->angleInterpolationWithSpeed(names, target_angles, speed);
248 
249  names.clear();
250  target_angles.clear();
251 
252  // Strike out
253  names.arraySetSize(2);
254  target_angles.arraySetSize(2);
255 
256  shoot_hip_pitch = 20;
257  support_hip_pitch = -50;
258 
259  names = ALValue::array(shoot_hip_pitch_name, support_hip_pitch_name);
260  target_angles = ALValue::array(deg2rad(shoot_hip_pitch), deg2rad(support_hip_pitch));
261  speed = 0.1;
262 
263  if (quit_)
264  return;
265  almotion_->angleInterpolationWithSpeed(names, target_angles, speed);
266 
267  names.clear();
268  target_angles.clear();
269 
270  // Shoot
271  names.arraySetSize(4);
272  target_angles.arraySetSize(4);
273 
274  shoot_hip_pitch = -80;
275  support_hip_pitch = -40;
276  shoot_knee_pitch = 50;
277  shoot_ankle_pitch = -30;
278 
279  names = ALValue::array(shoot_hip_pitch_name,
280  support_hip_pitch_name,
281  shoot_knee_pitch_name,
282  shoot_ankle_pitch_name);
283  target_angles = ALValue::array(deg2rad(shoot_hip_pitch),
284  deg2rad(support_hip_pitch),
285  deg2rad(shoot_knee_pitch),
286  deg2rad(shoot_ankle_pitch));
287  speed = 1.0;
288  if (quit_)
289  return;
290  almotion_->angleInterpolationWithSpeed(names, target_angles, speed);
291 
292  names.clear();
293  target_angles.clear();
294 
295  // Move to upright position
296  names.arraySetSize(2);
297  target_angles.arraySetSize(2);
298 
299  shoot_hip_pitch = -25;
300  support_hip_pitch = -25;
301 
302  names = ALValue::array(shoot_hip_pitch_name, support_hip_pitch_name);
303  target_angles = ALValue::array(deg2rad(shoot_hip_pitch), deg2rad(support_hip_pitch));
304  speed = 0.1;
305 
306  if (quit_)
307  return;
308  almotion_->angleInterpolationWithSpeed(names, target_angles, speed);
309 
310  //names.clear();
311  //target_angles.clear();
312 
313  if (quit_)
314  return;
315  goto_start_pos(0.1);
316 }
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:36