23 #include "motion_standup_task.h"
25 #include <core/exceptions/system.h>
50 AL::ALPtr<AL::ALMotionProxy> almotion,
71 #define _t(x) times.arrayPush(x)
72 #define _tc() times.clear()
73 #define _tp() all_times.arrayPush(times)
75 #define _a(x) angles.arrayPush(x)
76 #define _ac() angles.clear()
77 #define _ap() all_angles.arrayPush(angles)
80 NaoQiMotionStandupTask::goto_start_pos()
82 ALValue joints, angles, times, all_angles, all_times;
84 joints.arrayPush(
"HeadPitch");
85 joints.arrayPush(
"HeadYaw");
86 joints.arrayPush(
"LAnklePitch");
87 joints.arrayPush(
"LAnkleRoll");
88 joints.arrayPush(
"LElbowRoll");
89 joints.arrayPush(
"LElbowYaw");
90 joints.arrayPush(
"LHipPitch");
91 joints.arrayPush(
"LHipRoll");
92 joints.arrayPush(
"LHipYawPitch");
93 joints.arrayPush(
"LKneePitch");
94 joints.arrayPush(
"LShoulderPitch");
95 joints.arrayPush(
"LShoulderRoll");
96 joints.arrayPush(
"RAnklePitch");
97 joints.arrayPush(
"RAnkleRoll");
98 joints.arrayPush(
"RElbowRoll");
99 joints.arrayPush(
"RElbowYaw");
100 joints.arrayPush(
"RHipPitch");
101 joints.arrayPush(
"RHipRoll");
102 joints.arrayPush(
"RKneePitch");
103 joints.arrayPush(
"RShoulderPitch");
104 joints.arrayPush(
"RShoulderRoll");
106 times = ALValue::array(1.9, 2.9);
108 for (
unsigned int i = 0; i < joints.getSize(); ++i) {
109 all_times.arrayPush(times);
197 bool is_absolute =
true;
198 almotion_->angleInterpolation(joints, all_angles, all_times, is_absolute);
202 NaoQiMotionStandupTask::standup_from_back()
204 ALValue joints, angles, times, all_angles, all_times;
208 joints.arrayPush(
"HeadPitch");
209 joints.arrayPush(
"HeadYaw");
210 joints.arrayPush(
"LAnklePitch");
211 joints.arrayPush(
"LAnkleRoll");
212 joints.arrayPush(
"LElbowRoll");
213 joints.arrayPush(
"LElbowYaw");
214 joints.arrayPush(
"LHipPitch");
215 joints.arrayPush(
"LHipRoll");
216 joints.arrayPush(
"LHipYawPitch");
217 joints.arrayPush(
"LKneePitch");
218 joints.arrayPush(
"LShoulderPitch");
219 joints.arrayPush(
"LShoulderRoll");
220 joints.arrayPush(
"RAnklePitch");
221 joints.arrayPush(
"RAnkleRoll");
222 joints.arrayPush(
"RElbowRoll");
223 joints.arrayPush(
"RElbowYaw");
224 joints.arrayPush(
"RHipPitch");
225 joints.arrayPush(
"RHipRoll");
226 joints.arrayPush(
"RHipYawPitch");
227 joints.arrayPush(
"RKneePitch");
228 joints.arrayPush(
"RShoulderPitch");
229 joints.arrayPush(
"RShoulderRoll");
789 bool is_absolute =
true;
790 almotion_->angleInterpolation(joints, all_angles, all_times, is_absolute);
794 NaoQiMotionStandupTask::standup_from_front()
796 ALValue joints, angles, times, all_angles, all_times;
800 joints.arrayPush(
"HeadPitch");
801 joints.arrayPush(
"LAnklePitch");
802 joints.arrayPush(
"LAnkleRoll");
803 joints.arrayPush(
"LElbowRoll");
804 joints.arrayPush(
"LElbowYaw");
805 joints.arrayPush(
"LHipPitch");
806 joints.arrayPush(
"LHipRoll");
807 joints.arrayPush(
"LHipYawPitch");
808 joints.arrayPush(
"LKneePitch");
809 joints.arrayPush(
"LShoulderPitch");
810 joints.arrayPush(
"LShoulderRoll");
811 joints.arrayPush(
"RAnklePitch");
812 joints.arrayPush(
"RAnkleRoll");
813 joints.arrayPush(
"RElbowRoll");
814 joints.arrayPush(
"RElbowYaw");
815 joints.arrayPush(
"RHipPitch");
816 joints.arrayPush(
"RHipRoll");
817 joints.arrayPush(
"RHipYawPitch");
818 joints.arrayPush(
"RKneePitch");
819 joints.arrayPush(
"RShoulderPitch");
820 joints.arrayPush(
"RShoulderRoll");
1230 bool is_absolute =
true;
1231 almotion_->angleInterpolation(joints, all_angles, all_times, is_absolute);
1240 standup_from_back();
1243 standup_from_front();
1245 if (accel_x_ > 0.8) {
1247 standup_from_front();
1248 }
else if (accel_x_ < -0.8) {
1250 standup_from_back();
NaoQiMotionStandupTask(AL::ALPtr< AL::ALMotionProxy > almotion, fawkes::HumanoidMotionInterface::StandupEnum from_pos, float accel_x, float accel_y, float accel_z)
Constructor.
virtual ~NaoQiMotionStandupTask()
Destructor.
virtual void run()
Run the standup.
StandupEnum
From which position to standup.
@ STANDUP_BACK
Standup from lying on the back.
@ STANDUP_FRONT
Standup from lying on the tummy.
Fawkes library namespace.