Fawkes API Fawkes Development Version
motion_utils.cpp
1
2/***************************************************************************
3 * motion_utils.cpp - Motion utility functions
4 *
5 * Created: Wed Aug 17 21:54:59 2011
6 * Copyright 2011 Tim Niemueller [www.niemueller.de]
7 *
8 ****************************************************************************/
9
10/* This program is free software; you can redistribute it and/or modify
11 * it under the terms of the GNU General Public License as published by
12 * the Free Software Foundation; either version 2 of the License, or
13 * (at your option) any later version.
14 *
15 * This program is distributed in the hope that it will be useful,
16 * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 * GNU Library General Public License for more details.
19 *
20 * Read the full text in the LICENSE.GPL file in the doc directory.
21 */
22
23#include "motion_utils.h"
24
25namespace motion {
26
27/** Fix ALMotion's belief of body angles.
28 * If body angles have been set via the DCM, ALMotions model is out of
29 * date which can cause a quick snapping back to the last posture known
30 * to ALMotion causing very fast and potentially dangerous movements.
31 *
32 * Seems not to work as expected atm.
33 */
34void
35fix_angles(AL::ALPtr<AL::ALMotionProxy> &almotion)
36{
37 almotion->setAngles("Body", almotion->getAngles("Body", true), 1.);
38 //almotion->setStiffnesses("Body", 0.0);
39 //almotion->setStiffnesses("Body", 1.0);
40}
41
42void
43move_joints(AL::ALPtr<AL::ALMotionProxy> &almotion,
44 float head_yaw,
45 float head_pitch,
46 float l_shoulder_pitch,
47 float l_shoulder_roll,
48 float l_elbow_yaw,
49 float l_elbow_roll,
50 float l_wrist_yaw,
51 float l_hand,
52 float l_hip_yaw_pitch,
53 float l_hip_roll,
54 float l_hip_pitch,
55 float l_knee_pitch,
56 float l_ankle_pitch,
57 float l_ankle_roll,
58 float r_shoulder_pitch,
59 float r_shoulder_roll,
60 float r_elbow_yaw,
61 float r_elbow_roll,
62 float r_wrist_yaw,
63 float r_hand,
64 float r_hip_yaw_pitch,
65 float r_hip_roll,
66 float r_hip_pitch,
67 float r_knee_pitch,
68 float r_ankle_pitch,
69 float r_ankle_roll,
70 float speed)
71{
72 int num_joints = almotion->getJointNames("Body").size();
73
74 std::vector<float> angles;
75 angles.push_back(head_yaw);
76 angles.push_back(head_pitch);
77
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) { //academic version
83 angles.push_back(l_wrist_yaw);
84 angles.push_back(l_hand);
85 }
86
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);
93
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);
100
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);
108 }
109
110 std::vector<std::string> joint_names = almotion->getJointNames("Body");
111 almotion->killTasksUsingResources(joint_names);
112
113 fix_angles(almotion);
114
115 almotion->setAngles(joint_names, angles, speed);
116}
117
118int
119timed_move_joints(AL::ALPtr<AL::ALMotionProxy> &almotion,
120 float head_yaw,
121 float head_pitch,
122 float l_shoulder_pitch,
123 float l_shoulder_roll,
124 float l_elbow_yaw,
125 float l_elbow_roll,
126 float l_wrist_yaw,
127 float l_hand,
128 float l_hip_yaw_pitch,
129 float l_hip_roll,
130 float l_hip_pitch,
131 float l_knee_pitch,
132 float l_ankle_pitch,
133 float l_ankle_roll,
134 float r_shoulder_pitch,
135 float r_shoulder_roll,
136 float r_elbow_yaw,
137 float r_elbow_roll,
138 float r_wrist_yaw,
139 float r_hand,
140 float r_hip_yaw_pitch,
141 float r_hip_roll,
142 float r_hip_pitch,
143 float r_knee_pitch,
144 float r_ankle_pitch,
145 float r_ankle_roll,
146 float time_sec)
147{
148 int num_joints = almotion->getJointNames("Body").size();
149
150 std::vector<float> angles;
151 angles.push_back(head_yaw);
152 angles.push_back(head_pitch);
153
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) { //academic version
159 angles.push_back(l_wrist_yaw);
160 angles.push_back(l_hand);
161 }
162
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);
169
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);
176
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);
184 }
185
186 std::vector<std::string> joint_names = almotion->getJointNames("Body");
187 almotion->killTasksUsingResources(joint_names);
188
189 fix_angles(almotion);
190
191 return almotion->post.angleInterpolation("Body", angles, time_sec, true);
192}
193
194} // end of namespace motion