24 #include "motion_kick_task.h"
26 #include <core/exceptions/system.h>
27 #include <utils/math/angle.h>
81 NaoQiMotionKickTask::goto_start_pos(AL::ALValue speed,
bool concurrent)
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;
89 float hip_pitch = -25;
90 float knee_pitch = 40;
91 float ankle_pitch = -20;
94 ALValue target_angles;
95 target_angles.arraySetSize(20);
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);
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);
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);
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);
122 ALValue names = ALValue::array(
"LArm",
"RArm",
"LLeg",
"RLeg");
125 almotion_->post.angleInterpolationWithSpeed(names, target_angles, speed);
127 almotion_->angleInterpolationWithSpeed(names, target_angles, speed);
140 std::vector<std::string> joint_names = almotion_->getJointNames(
"Body");
141 almotion_->killTasksUsingResources(joint_names);
142 goto_start_pos(0.2,
true);
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;
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;
167 float BALANCE_HIP_ROLL = 0;
168 float BALANCE_ANKLE_ROLL = 0;
169 float STRIKE_OUT_HIP_ROLL = 0;
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";
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";
194 BALANCE_HIP_ROLL = -20;
195 BALANCE_ANKLE_ROLL = 25;
196 STRIKE_OUT_HIP_ROLL = -30;
204 ALValue target_angles;
208 names.arraySetSize(4);
209 target_angles.arraySetSize(4);
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;
216 names = ALValue::array(support_hip_roll_name,
218 support_ankle_roll_name,
219 shoot_ankle_roll_name);
220 target_angles = ALValue::array(
deg2rad(support_hip_roll),
227 almotion_->angleInterpolationWithSpeed(names, target_angles, speed);
230 target_angles.clear();
233 names.arraySetSize(3);
234 target_angles.arraySetSize(3);
236 shoot_hip_roll = STRIKE_OUT_HIP_ROLL;
237 shoot_knee_pitch = 90;
238 shoot_ankle_pitch = -50;
240 names = ALValue::array(shoot_hip_roll_name, shoot_knee_pitch_name, shoot_ankle_pitch_name);
247 almotion_->angleInterpolationWithSpeed(names, target_angles, speed);
250 target_angles.clear();
253 names.arraySetSize(2);
254 target_angles.arraySetSize(2);
256 shoot_hip_pitch = 20;
257 support_hip_pitch = -50;
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));
265 almotion_->angleInterpolationWithSpeed(names, target_angles, speed);
268 target_angles.clear();
271 names.arraySetSize(4);
272 target_angles.arraySetSize(4);
274 shoot_hip_pitch = -80;
275 support_hip_pitch = -40;
276 shoot_knee_pitch = 50;
277 shoot_ankle_pitch = -30;
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),
290 almotion_->angleInterpolationWithSpeed(names, target_angles, speed);
293 target_angles.clear();
296 names.arraySetSize(2);
297 target_angles.arraySetSize(2);
299 shoot_hip_pitch = -25;
300 support_hip_pitch = -25;
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));
308 almotion_->angleInterpolationWithSpeed(names, target_angles, speed);