24 #include "motion_kick_task.h"
26 #include <core/exceptions/system.h>
27 #include <utils/math/angle.h>
35 using namespace fawkes;
53 __almotion = almotion;
83 NaoQiMotionKickTask::goto_start_pos(AL::ALValue speed,
bool concurrent)
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;
91 float hip_pitch = -25;
92 float knee_pitch = 40;
93 float ankle_pitch = -20;
96 ALValue target_angles;
97 target_angles.arraySetSize(20);
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);
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);
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);
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);
124 ALValue names = ALValue::array(
"LArm",
"RArm",
"LLeg",
"RLeg");
127 __almotion->post.angleInterpolationWithSpeed(names, target_angles, speed);
129 __almotion->angleInterpolationWithSpeed(names, target_angles, speed);
143 std::vector<std::string> joint_names = __almotion->getJointNames(
"Body");
144 __almotion->killTasksUsingResources(joint_names);
145 goto_start_pos(0.2,
true);
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;
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;
170 float BALANCE_HIP_ROLL = 0;
171 float BALANCE_ANKLE_ROLL = 0;
172 float STRIKE_OUT_HIP_ROLL = 0;
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";
184 BALANCE_HIP_ROLL = 20;
185 BALANCE_ANKLE_ROLL = -25;
186 STRIKE_OUT_HIP_ROLL = 30;
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";
197 BALANCE_HIP_ROLL = -20;
198 BALANCE_ANKLE_ROLL = 25;
199 STRIKE_OUT_HIP_ROLL = -30;
206 ALValue target_angles;
210 names.arraySetSize(4);
211 target_angles.arraySetSize(4);
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;
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),
225 __almotion->angleInterpolationWithSpeed(names, target_angles, speed);
228 target_angles.clear();
231 names.arraySetSize(3);
232 target_angles.arraySetSize(3);
234 shoot_hip_roll = STRIKE_OUT_HIP_ROLL;
235 shoot_knee_pitch = 90;
236 shoot_ankle_pitch = -50;
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),
244 __almotion->angleInterpolationWithSpeed(names, target_angles, speed);
247 target_angles.clear();
250 names.arraySetSize(2);
251 target_angles.arraySetSize(2);
253 shoot_hip_pitch = 20;
254 support_hip_pitch = -50;
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));
261 __almotion->angleInterpolationWithSpeed(names, target_angles, speed);
264 target_angles.clear();
267 names.arraySetSize(4);
268 target_angles.arraySetSize(4);
270 shoot_hip_pitch = -80;
271 support_hip_pitch = -40;
272 shoot_knee_pitch = 50;
273 shoot_ankle_pitch = -30;
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),
281 __almotion->angleInterpolationWithSpeed(names, target_angles, speed);
284 target_angles.clear();
287 names.arraySetSize(2);
288 target_angles.arraySetSize(2);
290 shoot_hip_pitch = -25;
291 support_hip_pitch = -25;
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));
298 __almotion->angleInterpolationWithSpeed(names, target_angles, speed);