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 
25 namespace 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  */
34 void
35 fix_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 
42 void
43 move_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 
118 int
119 timed_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