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::run
virtual void run()
Run the kick.
Definition: motion_kick_task.cpp:147
fawkes::HumanoidMotionInterface::LEG_RIGHT
@ LEG_RIGHT
Right leg.
Definition: HumanoidMotionInterface.h:62
fawkes::HumanoidMotionInterface::LEG_LEFT
@ LEG_LEFT
Left leg.
Definition: HumanoidMotionInterface.h:61
fawkes::HumanoidMotionInterface::LegEnum
LegEnum
Type to determinate leg side.
Definition: HumanoidMotionInterface.h:54
NaoQiMotionKickTask::NaoQiMotionKickTask
NaoQiMotionKickTask(AL::ALPtr< AL::ALMotionProxy > almotion, fawkes::HumanoidMotionInterface::LegEnum leg)
Constructor.
Definition: motion_kick_task.cpp:49
fawkes
fawkes::deg2rad
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:42
NaoQiMotionKickTask::exitTask
virtual void exitTask()
Stop the current kick task.
Definition: motion_kick_task.cpp:137
NaoQiMotionKickTask::~NaoQiMotionKickTask
virtual ~NaoQiMotionKickTask()
Destructor.
Definition: motion_kick_task.cpp:61