23 #include "motion_utils.h" 38 fix_angles(AL::ALPtr<AL::ALMotionProxy> &almotion)
41 almotion->setAngles(
"Body", almotion->getAngles(
"Body",
true), 1.);
47 move_joints(AL::ALPtr<AL::ALMotionProxy> &almotion,
48 float head_yaw,
float head_pitch,
49 float l_shoulder_pitch,
float l_shoulder_roll,
50 float l_elbow_yaw,
float l_elbow_roll,
51 float l_wrist_yaw,
float l_hand,
52 float l_hip_yaw_pitch,
float l_hip_roll,
float l_hip_pitch,
53 float l_knee_pitch,
float l_ankle_pitch,
float l_ankle_roll,
54 float r_shoulder_pitch,
float r_shoulder_roll,
55 float r_elbow_yaw,
float r_elbow_roll,
56 float r_wrist_yaw,
float r_hand,
57 float r_hip_yaw_pitch,
float r_hip_roll,
float r_hip_pitch,
58 float r_knee_pitch,
float r_ankle_pitch,
float r_ankle_roll,
61 int num_joints = almotion->getJointNames(
"Body").size();
63 std::vector<float> angles;
64 angles.push_back(head_yaw);
65 angles.push_back(head_pitch);
67 angles.push_back(l_shoulder_pitch);
68 angles.push_back(l_shoulder_roll);
69 angles.push_back(l_elbow_yaw);
70 angles.push_back(l_elbow_roll);
71 if (num_joints == 26) {
72 angles.push_back(l_wrist_yaw);
73 angles.push_back(l_hand);
76 angles.push_back(l_hip_yaw_pitch);
77 angles.push_back(l_hip_roll);
78 angles.push_back(l_hip_pitch);
79 angles.push_back(l_knee_pitch);
80 angles.push_back(l_ankle_pitch);
81 angles.push_back(l_ankle_roll);
83 angles.push_back(r_hip_yaw_pitch);
84 angles.push_back(r_hip_roll);
85 angles.push_back(r_hip_pitch);
86 angles.push_back(r_knee_pitch);
87 angles.push_back(r_ankle_pitch);
88 angles.push_back(r_ankle_roll);
90 angles.push_back(r_shoulder_pitch);
91 angles.push_back(r_shoulder_roll);
92 angles.push_back(r_elbow_yaw);
93 angles.push_back(r_elbow_roll);
94 if (num_joints == 26) {
95 angles.push_back(r_wrist_yaw);
96 angles.push_back(r_hand);
99 std::vector<std::string> joint_names = almotion->getJointNames(
"Body");
100 almotion->killTasksUsingResources(joint_names);
102 fix_angles(almotion);
104 almotion->setAngles(joint_names, angles, speed);
109 timed_move_joints(AL::ALPtr<AL::ALMotionProxy> &almotion,
110 float head_yaw,
float head_pitch,
111 float l_shoulder_pitch,
float l_shoulder_roll,
112 float l_elbow_yaw,
float l_elbow_roll,
113 float l_wrist_yaw,
float l_hand,
114 float l_hip_yaw_pitch,
float l_hip_roll,
float l_hip_pitch,
115 float l_knee_pitch,
float l_ankle_pitch,
float l_ankle_roll,
116 float r_shoulder_pitch,
float r_shoulder_roll,
117 float r_elbow_yaw,
float r_elbow_roll,
118 float r_wrist_yaw,
float r_hand,
119 float r_hip_yaw_pitch,
float r_hip_roll,
float r_hip_pitch,
120 float r_knee_pitch,
float r_ankle_pitch,
float r_ankle_roll,
123 int num_joints = almotion->getJointNames(
"Body").size();
125 std::vector<float> angles;
126 angles.push_back(head_yaw);
127 angles.push_back(head_pitch);
129 angles.push_back(l_shoulder_pitch);
130 angles.push_back(l_shoulder_roll);
131 angles.push_back(l_elbow_yaw);
132 angles.push_back(l_elbow_roll);
133 if (num_joints == 26) {
134 angles.push_back(l_wrist_yaw);
135 angles.push_back(l_hand);
138 angles.push_back(l_hip_yaw_pitch);
139 angles.push_back(l_hip_roll);
140 angles.push_back(l_hip_pitch);
141 angles.push_back(l_knee_pitch);
142 angles.push_back(l_ankle_pitch);
143 angles.push_back(l_ankle_roll);
145 angles.push_back(r_hip_yaw_pitch);
146 angles.push_back(r_hip_roll);
147 angles.push_back(r_hip_pitch);
148 angles.push_back(r_knee_pitch);
149 angles.push_back(r_ankle_pitch);
150 angles.push_back(r_ankle_roll);
152 angles.push_back(r_shoulder_pitch);
153 angles.push_back(r_shoulder_roll);
154 angles.push_back(r_elbow_yaw);
155 angles.push_back(r_elbow_roll);
156 if (num_joints == 26) {
157 angles.push_back(r_wrist_yaw);
158 angles.push_back(r_hand);
161 std::vector<std::string> joint_names = almotion->getJointNames(
"Body");
162 almotion->killTasksUsingResources(joint_names);
164 fix_angles(almotion);
166 return almotion->post.angleInterpolation(
"Body", angles, time_sec,
true);