okr.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
5  *
6  * ArmarX is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License version 2 as
8  * published by the Free Software Foundation.
9  *
10  * ArmarX is distributed in the hope that it will be useful, but
11  * WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program. If not, see <http://www.gnu.org/licenses/>.
17  *
18  * @package
19  * @author
20  * @date
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 #include "okr.h"
25 
26 using namespace armarx;
27 
28 void OKR::calc()
29 {
30  std::scoped_lock lock(dataMutex);
31 
32  if (!newFlow)
33  {
34  return;
35  }
36 
37  // if (velocityBased && !reportedJointVelocitiesBool)
38  // {
39  // return;
40  // }
41  else if (!velocityBased && !reportedJointAnglesBool)
42  {
43  return;
44  }
45 
46  newFlow = false;
47 
48  // float comp = 0.0;
49  // if (velocityBased)
50  // {
51  // comp = 3;
52  // }
53  // else
54  // {
55  // comp = 1;
56  // }
57 
58  /*if (flowX < comp && flowX > -comp)
59  {
60  flowX = 0.0;
61  }
62  if (flowY < comp && flowY > -comp)
63  {
64  flowY = 0.0;
65  }*/
66 
67  NameValueMap headJoints;
68  NameControlModeMap jointModes;
69 
70  if (velocityBased)
71  {
72  reportedJointVelocitiesBool = false;
73 
74  removeSelfInducedOpticalFlow();
75 
76  //headJoints[eye_pitch_left] = (flowY) - kp * errorY;
77  //headJoints[eye_yaw_right] = -1.0 * (flowX) + kp * errorX;
78 
79  // gain < 1 is to avoid unstability with the head
80  headJoints[eye_pitch_left] = 0.0; //0.8 * (flowY) - kp * errorY;
81  headJoints[eye_yaw_right] = -0.8 * (flowX);// + kp * errorX;
82 
83  headJoints[eye_yaw_left] = headJoints[eye_yaw_right];
84 
85  // uncomment for baseline
86  //headJoints[eye_pitch_left] = headJoints[eye_yaw_left] = headJoints[eye_yaw_right] = 0.;
87 
88  }
89  else
90  {
91  reportedJointAnglesBool = false;
92 
93  headJoints[eye_pitch_left] = (reportedJointAngles[eye_pitch_left] + flowY);
94  jointModes[eye_pitch_left] = ePositionControl;
95 
96  if (armar4)
97  {
98  headJoints[eye_pitch_right] = (reportedJointAngles[eye_pitch_left] + flowY);
99  jointModes[eye_pitch_right] = ePositionControl;
100  }
101 
102  headJoints[eye_yaw_left] = (reportedJointAngles[eye_yaw_left] - flowX);
103  jointModes[eye_yaw_left] = ePositionControl;
104 
105  headJoints[eye_yaw_right] = (reportedJointAngles[eye_yaw_left] - flowX);
106  jointModes[eye_yaw_right] = ePositionControl;
107 
108  }
109 
110  // mutexNodeSet.unlock();
111 
112  std::scoped_lock lock2(mutex);
113 
114  jointAngles.swap(headJoints);
115 }
116 
117 void OKR::removeSelfInducedOpticalFlow()
118 {
119  NameValueMap velocities;
120 
121  if (jointVelocities.size() == 0)
122  {
123  ARMARX_WARNING << deactivateSpam(1) << "No self-induced velocity found (0)";
124  return;
125  }
126 
127  // find the velocities closest to the timestamp of the 2nd image
128  for (auto& kv : jointVelocities)
129  {
130  //std::scoped_lock lock(dataMutex);
131  long timestamp = kv.first;
132 
133  if (timestamp <= flow_time + flowT)
134  {
135  velocities = kv.second;
136  jointVelocities.erase(kv.first); // erase the values belonging to the previous optical flow
137  }
138  else
139  {
140  velocities = kv.second;
141  break;
142  }
143  }
144 
145  if (velocities.find(eye_yaw_right) == velocities.end()) // no velocity found
146  {
147  velocities = jointVelocities.rbegin()->second;
148  ARMARX_WARNING << deactivateSpam(1) << "No self-induced velocity found";
149  }
150 
151  flowY = flowY + velocities[eye_pitch_left] ;
152  flowX = flowX - velocities[eye_yaw_right] ;
153 
154 }
155 
156 void OKR::setJointNames(std::string eye_pitch_left, std::string eye_pitch_right, std::string eye_yaw_left, std::string eye_yaw_right, std::string neck_roll)
157 {
158  std::scoped_lock lock(mutex);
159  this->eye_pitch_left = eye_pitch_left;
160  this->eye_pitch_right = eye_pitch_right;
161  this->eye_yaw_left = eye_yaw_left;
162  this->eye_yaw_right = eye_yaw_right;
163  this->neck_roll = neck_roll;
164 }
165 
166 void OKR::setPIDValues(float kp, float ki, float kd)
167 {
168  std::scoped_lock lock(mutex);
169 
170  this->kp = kp;
171  this->ki = ki;
172  this->kd = kd;
173 }
174 
175 void OKR::setBools(bool armar4, bool velocityBased)
176 {
177  std::scoped_lock lock(mutex);
178 
179  this->armar4 = armar4;
180  this->velocityBased = velocityBased;
181 }
182 
183 std::vector<float> OKR::pid(std::vector<float> error, std::vector<float> kp, std::vector<float> ki, std::vector<float> kd)
184 {
185  if (error.size() != kp.size() || kp.size() != ki.size() || ki.size() != kd.size())
186  {
187  ARMARX_WARNING_S << "Different arraysizes in HeadStabilization::pid";
188  return {0.0, 0.0, 0.0};
189  }
190 
191  std::vector<float> y;
192 
193  for (size_t i = 0; i < error.size(); i++)
194  {
195  err_sum[i] += error[i];
196 
197  y.push_back(kp[i] * error[i] + (ki[i] * err_sum[i]) + (kd[i] * (error[i] - err_old[i])));
198 
199  err_old[i] = error[i];
200  }
201 
202  return y;
203 }
204 
205 void OKR::reportJointAngles(const NameValueMap& values, bool valueChanged, const Ice::Current& c)
206 {
207  std::scoped_lock lock(dataMutex);
208 
209  reportedJointAnglesBool = true;
210  this->reportedJointAngles = values;
211 
212 }
213 
214 void OKR::reportJointVelocities(const NameValueMap& values, bool valueChanged, long timestamp, const Ice::Current& c)
215 {
216  std::scoped_lock lock(dataMutex);
217 
218  reportedJointVelocitiesBool = true;
219  this->reportedJointVelocities = values;
220 
221  NameValueMap test(values);
222  jointVelocities[timestamp] = test;
223 
224 
225 }
226 
227 void OKR::reportNewOpticalFlow(float x, float y, float deltaT, long timestamp)
228 {
229  std::scoped_lock lock(dataMutex);
230 
231  newFlow = true;
232  this->flowX = x;
233  this->flowY = y;
234  this->flowT = deltaT;
235 
236  flow_time = timestamp;
237 }
238 
240 {
241  std::scoped_lock lock(dataMutex);
242 
243  this->errorX = angleX;
244  this->errorY = angleY;
245 }
246 
247 
248 void OKR::onStop()
249 {
250  std::scoped_lock lock(dataMutex);
251 
252  newFlow = false;
253  reportedJointVelocitiesBool = false;
254  reportedJointAnglesBool = false;
255 }
armarx::VariantType::Float
const VariantTypeId Float
Definition: Variant.h:918
armarx::OKR::setJointNames
void setJointNames(std::string eye_pitch_left, std::string eye_pitch_right, std::string eye_yaw_left, std::string eye_yaw_right, std::string neck_roll)
Definition: okr.cpp:156
okr.h
armarx::OKR::reportNewTrackingError
void reportNewTrackingError(Ice::Float pixelX, Ice::Float pixelY, Ice::Float angleX, Ice::Float angleY)
Definition: okr.cpp:239
armarx::OKR::setPIDValues
void setPIDValues(float kp, float ki, float kd)
Definition: okr.cpp:166
ProsthesisInterface.values
values
Definition: ProsthesisInterface.py:190
armarx::OKR::reportJointVelocities
void reportJointVelocities(const NameValueMap &values, bool valueChanged, long timestamp, const Ice::Current &c)
Definition: okr.cpp:214
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
armarx::navigation::platform_controller::platform_global_trajectory::NameValueMap
std::map< std::string, float > NameValueMap
Definition: PlatformGlobalTrajectoryController.h:93
armarx::Reflex::jointAngles
std::map< std::string, float > jointAngles
Definition: reflex.h:132
deactivateSpam
SpamFilterDataPtr deactivateSpam(SpamFilterDataPtr const &spamFilter, float deactivationDurationSec, const std::string &identifier, bool deactivate)
Definition: Logging.cpp:72
armarx::OKR::reportNewOpticalFlow
void reportNewOpticalFlow(float x, float y, float deltaT, long timestamp)
Definition: okr.cpp:227
armarx::Reflex::mutex
std::mutex mutex
Definition: reflex.h:130
armarx::channels::KinematicUnitObserver::jointVelocities
const KinematicUnitDatafieldCreator jointVelocities("jointVelocities")
ARMARX_WARNING_S
#define ARMARX_WARNING_S
Definition: Logging.h:206
armarx::OKR::setBools
void setBools(bool armar4, bool velocityBased)
Definition: okr.cpp:175
armarx::OKR::reportJointAngles
void reportJointAngles(const NameValueMap &values, bool valueChanged, const Ice::Current &c)
Definition: okr.cpp:205
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28