24 #include "motion_kick_task.h" 26 #include <core/exceptions/system.h> 27 #include <utils/math/angle.h> 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);
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.