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
34using namespace AL;
35using 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 */
49NaoQiMotionKickTask::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/*
66static void
67print_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
80void
81NaoQiMotionKickTask::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 */
136void
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. */
146void
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;
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 void exitTask()
Stop the current kick task.
virtual void run()
Run the kick.
virtual ~NaoQiMotionKickTask()
Destructor.
LegEnum
Type to determinate leg side.
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:36