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 #if 0 /* just to make Emacs auto-indent happy */
27 }
28 #endif
29 
30 /** Fix ALMotion's belief of body angles.
31  * If body angles have been set via the DCM, ALMotions model is out of
32  * date which can cause a quick snapping back to the last posture known
33  * to ALMotion causing very fast and potentially dangerous movements.
34  *
35  * Seems not to work as expected atm.
36  */
37 void
38 fix_angles(AL::ALPtr<AL::ALMotionProxy> &almotion)
39 {
40 
41  almotion->setAngles("Body", almotion->getAngles("Body", true), 1.);
42  //almotion->setStiffnesses("Body", 0.0);
43  //almotion->setStiffnesses("Body", 1.0);
44 }
45 
46 void
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,
59  float speed)
60 {
61  int num_joints = almotion->getJointNames("Body").size();
62 
63  std::vector<float> angles;
64  angles.push_back(head_yaw);
65  angles.push_back(head_pitch);
66 
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) { //academic version
72  angles.push_back(l_wrist_yaw);
73  angles.push_back(l_hand);
74  }
75 
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);
82 
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);
89 
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);
97  }
98 
99  std::vector<std::string> joint_names = almotion->getJointNames("Body");
100  almotion->killTasksUsingResources(joint_names);
101 
102  fix_angles(almotion);
103 
104  almotion->setAngles(joint_names, angles, speed);
105 }
106 
107 
108 int
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,
121  float time_sec)
122 {
123  int num_joints = almotion->getJointNames("Body").size();
124 
125  std::vector<float> angles;
126  angles.push_back(head_yaw);
127  angles.push_back(head_pitch);
128 
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) { //academic version
134  angles.push_back(l_wrist_yaw);
135  angles.push_back(l_hand);
136  }
137 
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);
144 
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);
151 
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);
159  }
160 
161  std::vector<std::string> joint_names = almotion->getJointNames("Body");
162  almotion->killTasksUsingResources(joint_names);
163 
164  fix_angles(almotion);
165 
166  return almotion->post.angleInterpolation("Body", angles, time_sec, true);
167 }
168 
169 
170 } // end of namespace motion