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);
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.