23#include "motion_utils.h"
35fix_angles(AL::ALPtr<AL::ALMotionProxy> &almotion)
37 almotion->setAngles(
"Body", almotion->getAngles(
"Body",
true), 1.);
43move_joints(AL::ALPtr<AL::ALMotionProxy> &almotion,
46 float l_shoulder_pitch,
47 float l_shoulder_roll,
52 float l_hip_yaw_pitch,
58 float r_shoulder_pitch,
59 float r_shoulder_roll,
64 float r_hip_yaw_pitch,
72 int num_joints = almotion->getJointNames(
"Body").size();
74 std::vector<float> angles;
75 angles.push_back(head_yaw);
76 angles.push_back(head_pitch);
78 angles.push_back(l_shoulder_pitch);
79 angles.push_back(l_shoulder_roll);
80 angles.push_back(l_elbow_yaw);
81 angles.push_back(l_elbow_roll);
82 if (num_joints == 26) {
83 angles.push_back(l_wrist_yaw);
84 angles.push_back(l_hand);
87 angles.push_back(l_hip_yaw_pitch);
88 angles.push_back(l_hip_roll);
89 angles.push_back(l_hip_pitch);
90 angles.push_back(l_knee_pitch);
91 angles.push_back(l_ankle_pitch);
92 angles.push_back(l_ankle_roll);
94 angles.push_back(r_hip_yaw_pitch);
95 angles.push_back(r_hip_roll);
96 angles.push_back(r_hip_pitch);
97 angles.push_back(r_knee_pitch);
98 angles.push_back(r_ankle_pitch);
99 angles.push_back(r_ankle_roll);
101 angles.push_back(r_shoulder_pitch);
102 angles.push_back(r_shoulder_roll);
103 angles.push_back(r_elbow_yaw);
104 angles.push_back(r_elbow_roll);
105 if (num_joints == 26) {
106 angles.push_back(r_wrist_yaw);
107 angles.push_back(r_hand);
110 std::vector<std::string> joint_names = almotion->getJointNames(
"Body");
111 almotion->killTasksUsingResources(joint_names);
113 fix_angles(almotion);
115 almotion->setAngles(joint_names, angles, speed);
119timed_move_joints(AL::ALPtr<AL::ALMotionProxy> &almotion,
122 float l_shoulder_pitch,
123 float l_shoulder_roll,
128 float l_hip_yaw_pitch,
134 float r_shoulder_pitch,
135 float r_shoulder_roll,
140 float r_hip_yaw_pitch,
148 int num_joints = almotion->getJointNames(
"Body").size();
150 std::vector<float> angles;
151 angles.push_back(head_yaw);
152 angles.push_back(head_pitch);
154 angles.push_back(l_shoulder_pitch);
155 angles.push_back(l_shoulder_roll);
156 angles.push_back(l_elbow_yaw);
157 angles.push_back(l_elbow_roll);
158 if (num_joints == 26) {
159 angles.push_back(l_wrist_yaw);
160 angles.push_back(l_hand);
163 angles.push_back(l_hip_yaw_pitch);
164 angles.push_back(l_hip_roll);
165 angles.push_back(l_hip_pitch);
166 angles.push_back(l_knee_pitch);
167 angles.push_back(l_ankle_pitch);
168 angles.push_back(l_ankle_roll);
170 angles.push_back(r_hip_yaw_pitch);
171 angles.push_back(r_hip_roll);
172 angles.push_back(r_hip_pitch);
173 angles.push_back(r_knee_pitch);
174 angles.push_back(r_ankle_pitch);
175 angles.push_back(r_ankle_roll);
177 angles.push_back(r_shoulder_pitch);
178 angles.push_back(r_shoulder_roll);
179 angles.push_back(r_elbow_yaw);
180 angles.push_back(r_elbow_roll);
181 if (num_joints == 26) {
182 angles.push_back(r_wrist_yaw);
183 angles.push_back(r_hand);
186 std::vector<std::string> joint_names = almotion->getJointNames(
"Body");
187 almotion->killTasksUsingResources(joint_names);
189 fix_angles(almotion);
191 return almotion->post.angleInterpolation(
"Body", angles, time_sec,
true);