ControlPlatform.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2014-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 RobotSkillTemplates::PlatformGroup
19  * @author Mirko Waechter ( waechter at kit dot edu )
20  * @date 2015
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 
25 #include "ControlPlatform.h"
26 
27 #include <RobotAPI/libraries/core/PIDController.h>
28 #include <ArmarXCore/core/time/TimeUtil.h>
29 #include <ArmarXCore/observers/variant/DatafieldRef.h>
30 
31 #include <RobotSkillTemplates/statecharts/PlatformGroup/PlatformContext.h>
32 
33 using namespace armarx;
34 using namespace PlatformGroup;
35 
36 // DO NOT EDIT NEXT LINE
37 ControlPlatform::SubClassRegistry ControlPlatform::Registry(ControlPlatform::GetName(), &ControlPlatform::CreateInstance);
38 
39 float signedMin(float newValue, float minAbsValue)
40 {
41  return std::copysign(std::min<float>(fabs(newValue), minAbsValue), newValue);
42 }
43 
44 
45 ControlPlatform::ControlPlatform(const XMLStateConstructorParams& stateData) :
46  XMLStateTemplate<ControlPlatform>(stateData), ControlPlatformGeneratedBase<ControlPlatform>(stateData)
47 {
48 }
49 
50 void ControlPlatform::onEnter()
51 {
52 
53  // put your user code for the enter-point here
54  // execution time should be short (<100ms)
55  PlatformContext* context = getContext<PlatformContext>();
56 
57  setTimeoutEvent(in.getTimeout(), createEventTimeout());
58 
59  if (in.getUsePlatformNativeController())
60  {
61  ARMARX_INFO << "Using native Platform Controller";
62  Vector3Ptr currentTarget = in.getTargetPose();
63  Literal checkX(context->platformUnitObserverName + ".platformPose.positionX", checks::approx, {currentTarget->x, in.getPositionalAccuracy()});
64  Literal checkY(context->platformUnitObserverName + ".platformPose.positionY", checks::approx, {currentTarget->y, in.getPositionalAccuracy()});
65 
66  Literal checkAngle(context->platformUnitObserverName + ".platformPose.rotation", checks::approx, {currentTarget->z, in.getOrientationalAccuracy()});
67  // for special case of orientations around 2*PI
68  float checkAngleValueOffset;
69 
70  if (currentTarget->z > M_PI)
71  {
72  checkAngleValueOffset = - 2 * M_PI;
73  }
74  else
75  {
76  checkAngleValueOffset = 2 * M_PI;
77  }
78 
79  Literal checkAngle2(context->platformUnitObserverName + ".platformPose.rotation", checks::approx, { (float)(currentTarget->z + checkAngleValueOffset), in.getOrientationalAccuracy()});
80 
81  installConditionForPoseReached(checkX && checkY && (checkAngle || checkAngle2));
82  ARMARX_INFO << "Moving to " << in.getTargetPose()->x << ", " <<
83  in.getTargetPose()->y << ", " <<
84  in.getTargetPose()->z << ", ";
85  context->platformUnitPrx->moveTo(in.getTargetPose()->x,
86  in.getTargetPose()->y,
87  in.getTargetPose()->z, in.getPositionalAccuracy(), in.getOrientationalAccuracy());
88  usleep(1000000);
89  context->platformUnitPrx->moveTo(in.getTargetPose()->x,
90  in.getTargetPose()->y,
91  in.getTargetPose()->z, in.getPositionalAccuracy(), in.getOrientationalAccuracy());
92 
93  this->setUseRunFunction(false);
94  }
95  else
96  {
97  this->setUseRunFunction(true);
98  }
99 
100 }
101 
102 void ControlPlatform::run()
103 {
104 
105  MultiDimPIDController pidTrans(in.getPTranslation(), in.getITranslation(), in.getDTranslation());
106  // PIDController pidY(in.getPTranslation(), in.getITranslation(), in.getDTranslation());
107  PIDController pidRot(in.getPOrientation(), in.getIOrientation(), in.getDOrientation());
108  // put your user code for the execution-phase here
109  // runs in seperate thread, thus can do complex operations
110  // should check constantly whether isRunningTaskStopped() returns true
111  PlatformContext* c = getContext<PlatformContext>();
112  DatafieldRefPtr posXRef = new DatafieldRef(c->platformUnitObserverPrx, "platformPose", "positionX");
113  DatafieldRefPtr posYRef = new DatafieldRef(c->platformUnitObserverPrx, "platformPose", "positionY");
114  DatafieldRefPtr oriRef = new DatafieldRef(c->platformUnitObserverPrx, "platformPose", "rotation");
115 
116 
117 
118  float maxTransVel = in.getMaxTranslationVelocity();
119  ARMARX_INFO << "orientation accuracy: " << in.getOrientationalAccuracy();
120  ARMARX_INFO << "translational accuracy: " << in.getPositionalAccuracy();
121 
122 
123  float targetX = in.getTargetPose()->x;
124  float targetY = in.getTargetPose()->y;
125  Eigen::Vector3f target {targetX, targetY, 0};
126  float targetOri = in.getTargetPose()->z;
127  // float curOri = oriRef->getDataField()->getFloat();
128 
129  // bool reverseZeroCrossing = (targetOri - curOri > M_PI);
130  // bool ZeroCrossing= (targetOri - curOri < -M_PI);
131 
132  // if(reverseZeroCrossing)
133  // {
134  // targetOri -= 2 * M_PI;
135  // }
136  // if(ZeroCrossing)
137  // {
138  // targetOri += 2 * M_PI;
139  // }
140 
141  float startOri = oriRef->getDataField()->getFloat();
142 
143 
144 
145  ARMARX_INFO << "Platform current pose: " << posXRef->getDataField()->getFloat() << ", " << posYRef->getDataField()->getFloat() << ", " << oriRef->getDataField()->getFloat();;
146  ARMARX_INFO << "Platform target pose: " << *in.getTargetPose();
147  ARMARX_INFO << "AdjustPlatform target angle: " << targetOri;
148  ARMARX_INFO << VAROUT(maxTransVel);
149 
150  if (targetOri > M_PI)
151  {
152  targetOri = - 2 * M_PI + targetOri;
153  }
154 
155  if (startOri > M_PI)
156  {
157  startOri = - 2 * M_PI + startOri;
158  }
159 
160  float angleAbsDelta = targetOri - startOri;
161 
162  // transform alpha to [-pi, pi)
163  while (angleAbsDelta < -M_PI)
164  {
165  angleAbsDelta += 2 * M_PI;
166  }
167 
168  while (angleAbsDelta >= M_PI)
169  {
170  angleAbsDelta -= 2 * M_PI;
171  }
172 
173 
174  Eigen::Vector2f targetOriVec(sin(targetOri), cos(targetOri));
175  bool verify = in.getVerifyTargetPose();
176  IceUtil::Time targetReachedTime = IceUtil::Time::seconds(0);
177  float cycleTime = in.getCycleTime();
178 
179  while (!isRunningTaskStopped()) // stop run function if returning true
180  {
181  // do your calculations
182  float posx = posXRef->getDataField()->getFloat();
183  float posy = posYRef->getDataField()->getFloat();
184  float ori = oriRef->getDataField()->getFloat();
185  // if(ZeroCrossing && ori < M_PI)
186  // ori += 2 * M_PI;
187 
188  // if(reverseZeroCrossing && ori > M_PI)
189  // ori -= 2 * M_PI;
190 
191  Eigen::Vector3f pos {posx, posy, 0.0f};
192 
193  if (ori > M_PI)
194  {
195  ori = - 2 * M_PI + ori;
196  }
197  // put your user code for the enter-point here
198  // execution time should be short (<100ms)
199  //PlatformContext* context = getContext<PlatformContext>();
200 
201  float angleDelta = targetOri - ori;
202  ARMARX_INFO << deactivateSpam(1) << VAROUT(angleDelta);
203 
204 
205  // transform alpha to [-pi, pi)
206 
207  while (angleDelta < -M_PI)
208  {
209  angleDelta += 2 * M_PI;
210  }
211 
212  while (angleDelta >= M_PI)
213  {
214  angleDelta -= 2 * M_PI;
215  }
216 
217 
218 
219  pidTrans.update(pos, target);
220  pidRot.update(angleDelta, 0);
221 
222  Eigen::Vector2f newVel(pidTrans.getControlValue()[0], pidTrans.getControlValue()[1]);
223 
224 
225  float newVelocityRot = -signedMin(pidRot.getControlValue(), in.getMaxRotationalVelocity());
226  Eigen::Vector3f globalVel {newVel[0], newVel[1], 0};
227  Eigen::Matrix3f m;
228  m = Eigen::AngleAxisf(ori, Eigen::Vector3f::UnitZ());
229  Eigen::Vector3f localVel = m.inverse() * globalVel;
230  if (localVel.head(2).norm() > maxTransVel)
231  {
232  localVel.head(2) *= maxTransVel / localVel.head(2).norm();
233  }
234 
235  // Eigen::Vector3f currentVelocity {velXRef->get<float>(), velXRef->get<float>(), 0};
236  // if((localVel-currentVelocity).norm()/(cycleTime*0.001) > maxAcc)
237  // localVel = localVel.norm()*(cycleTime*0.001*maxAcc);
238  if (std::isnan(localVel[0]) || std::isnan(localVel[1]) || std::isnan(newVelocityRot))
239  {
240  throw LocalException("A target velocity is NaN!");
241  }
242 
243 
244  // ARMARX_INFO << deactivateSpam(0.1) << VAROUT(pidRot.getControlValue());
245  // ARMARX_INFO << deactivateSpam(0.1) << VAROUT(newVelocityRot);
246 
247  c->platformUnitPrx->move(localVel[0],
248  localVel[1],
249  newVelocityRot);
250  //ARMARX_IMPORTANT << "local x velocity: " << localVel[0] << " local y velocity: " << localVel[1] << " rotation vel: " << newVelocityRot;
251 
252  if (fabs(pidTrans.previousError) < in.getPositionalAccuracy() && fabs(pidRot.previousError) < in.getOrientationalAccuracy())
253  {
254  if (!verify)
255  {
256  emitPoseReached();
257  }
258  else
259  {
260  if (targetReachedTime.toSeconds() == 0)
261  {
262  targetReachedTime = TimeUtil::GetTime();
263  }
264 
265  ARMARX_INFO << deactivateSpam(0.1) << "error: " << pidTrans.previousError << " x velocity: " << newVel[0] << " y velocity: " << newVel[1];
266  ARMARX_INFO << deactivateSpam(0.1) << "rot error: " << pidRot.previousError << " rot velocity: " << newVelocityRot;
267 
268 
269  if ((TimeUtil::GetTime() - targetReachedTime).toMilliSecondsDouble() > 500)
270  {
271  emitPoseReached();
272  }
273  }
274  }
275  else
276  {
277  ARMARX_INFO << deactivateSpam(1) << "error: " << pidTrans.previousError << " x velocity: " << newVel[0] << " y velocity: " << newVel[1];
278  ARMARX_INFO << deactivateSpam(1) << "rot error: " << pidRot.previousError << " rot velocity: " << newVelocityRot;
279  }
280 
281  usleep(cycleTime * 1000);
282  }
283 
284 
285  // Sync trans and rot:
286 
287  // float orientationMultiplyer = in.getOrientationMultiplyerForPID();
288  // float maxTransVel = in.getMaxTranslationVelocity();
289 
290  // MultiDimPIDController pidTransRot(in.getPTransRot(), in.getITransRot(), in.getDTransRot(), maxTransVel);
291 
292  // PlatformContext* c = getContext<PlatformContext>();
293  // DatafieldRefPtr posXRef = new DatafieldRef(c->platformUnitObserverPrx, "platformPose", "positionX");
294  // DatafieldRefPtr posYRef = new DatafieldRef(c->platformUnitObserverPrx, "platformPose", "positionY");
295  // DatafieldRefPtr oriRef = new DatafieldRef(c->platformUnitObserverPrx, "platformPose", "rotation");
296 
297 
298 
299  // float targetX = in.getTargetPose()->x;
300  // float targetY = in.getTargetPose()->y;
301  // float targetOri = in.getTargetPose()->z;
302 
303 
304  // float px = posXRef->getDataField()->getFloat();
305  // float py = posYRef->getDataField()->getFloat();
306 
307  // float wholeDistance = std::sqrt(px * px + py * py);
308 
309 
310 
311 
312 
313  // ARMARX_INFO << "Platform current pose: " << posXRef->getDataField()->getFloat() << ", " << posYRef->getDataField()->getFloat() << ", " << oriRef->getDataField()->getFloat();;
314  // ARMARX_INFO << "Platform target pose: " << *in.getTargetPose();
315  // ARMARX_INFO << "AdjustPlatform target angle: " << targetOri;
316  // ARMARX_INFO << VAROUT(maxTransVel);
317 
318 
319  // if (targetOri > M_PI)
320  // {
321  // targetOri = - 2 * M_PI + targetOri;
322  // }
323 
324  // bool verify = in.getVerifyTargetPose();
325  // IceUtil::Time targetReachedTime = IceUtil::Time::seconds(0);
326  // float cycleTime = in.getCycleTime();
327 
328  // while (!isRunningTaskStopped()) // stop run function if returning true
329  // {
330  // // do your calculations
331  // float posx = posXRef->getDataField()->getFloat();
332  // float posy = posYRef->getDataField()->getFloat();
333  // float ori = oriRef->getDataField()->getFloat();
334 
335  // float currentDistance = std::sqrt(posx * posx + posy * posy);
336  // float percentageDone = currentDistance <= in.getPositionalAccuracy() ? 0 : currentDistance / wholeDistance;
337 
338 
339  // if (ori > M_PI)
340  // {
341  // ori = - 2 * M_PI + ori;
342  // }
343  // float angleDelta = targetOri - ori;
344 
345  // // transform alpha to [-pi, pi)
346  // while (angleDelta < -M_PI)
347  // {
348  // angleDelta += 2 * M_PI;
349  // }
350 
351  // while (angleDelta >= M_PI)
352  // {
353  // angleDelta -= 2 * M_PI;
354  // }
355 
356 
357  // float posAngle = angleDelta * (-1) * orientationMultiplyer;
358  // float targetAngle = posAngle * percentageDone;
359 
360 
361  // Eigen::Vector3f pos {posx, posy, posAngle};
362  // ARMARX_INFO << "CurrentPIDValues: " << pos;
363  // Eigen::Vector3f target {targetX, targetY, targetAngle};
364  // ARMARX_INFO << "targetPIDValues: " << target;
365  // pidTransRot.update(pos, target);
366 
367  // Eigen::Vector3f newVel(pidTransRot.getControlValue()[0], pidTransRot.getControlValue()[1], pidTransRot.getControlValue()[2]);
368 
369  // float newVelRot = signedMin(newVel[2] / (in.getPTransRot() * orientationMultiplyer), in.getMaxRotationalVelocity());
370 
371  // Eigen::Vector3f globalVel {newVel[0], newVel[1], 0};
372  // Eigen::Matrix3f m;
373  // m = Eigen::AngleAxisf(ori, Eigen::Vector3f::UnitZ());
374  // Eigen::Vector3f localVel = m.inverse() * globalVel;
375  // if (localVel.norm() > maxTransVel)
376  // {
377  // localVel *= maxTransVel / localVel.norm();
378  // }
379 
380  // if (std::isnan(localVel[0]) || std::isnan(localVel[1]) || std::isnan(newVelRot))
381  // {
382  // throw LocalException("A target velocity is NaN!");
383  // }
384 
385  // c->platformUnitPrx->move(localVel[0],
386  // localVel[1],
387  // newVelRot);
388 
389 
390  // ARMARX_INFO << deactivateSpam(1) << "local x velocity: " << localVel[0] << " local y velocity: " << localVel[1] << " rotation vel: " << newVelRot;
391 
392  // if (fabs(pidTransRot.previousError) < in.getPositionalAccuracy() && fabs(pidTransRot.previousError) < in.getOrientationalAccuracy() * orientationMultiplyer)
393  // {
394  // if (!verify)
395  // {
396  // emitPoseReached();
397  // }
398  // else
399  // {
400  // if (targetReachedTime.toSeconds() == 0)
401  // {
402  // targetReachedTime = TimeUtil::GetTime();
403  // }
404 
405  // ARMARX_INFO << deactivateSpam(0.1) << "error: " << pidTransRot.previousError << " x velocity: " << localVel[0] << " y velocity: " << localVel[1] << " rot velocity: " << newVelRot;
406 
407  // if ((TimeUtil::GetTime() - targetReachedTime).toMilliSecondsDouble() > 500)
408  // {
409  // emitPoseReached();
410  // }
411  // }
412  // }
413  // else
414  // {
415  // ARMARX_INFO << deactivateSpam(0.1) << "error: " << pidTransRot.previousError << " x velocity: " << localVel[0] << " y velocity: " << localVel[1] << " rot velocity: " << newVelRot;
416  // }
417 
418  // usleep(cycleTime * 1000);
419  // }
420 
421 }
422 
423 void ControlPlatform::cleanUp()
424 {
425  // put your user code for the exit point here
426  // execution time should be short (<100ms)
427  PlatformContext* c = getContext<PlatformContext>();
428  DatafieldRefPtr posXRef = new DatafieldRef(c->platformUnitObserverPrx, "platformPose", "positionX");
429  DatafieldRefPtr posYRef = new DatafieldRef(c->platformUnitObserverPrx, "platformPose", "positionY");
430  DatafieldRefPtr oriRef = new DatafieldRef(c->platformUnitObserverPrx, "platformPose", "rotation");
431  float posX = posXRef->getDataField()->getFloat();
432  float posY = posYRef->getDataField()->getFloat();
433  float ori = oriRef->getDataField()->getFloat();
434  Vector3Ptr error = new Vector3(
435  fabs(in.getTargetPose()->x - posX),
436  fabs(in.getTargetPose()->y - posY),
437  fabs(in.getTargetPose()->z - ori)
438  );
439  ARMARX_INFO << "Remaining platform error: " << *error;
440  out.setRemainingError(error);
441 }
442 
443 void ControlPlatform::onBreak()
444 {
445  while (!isRunningTaskFinished())
446  {
447  ARMARX_INFO << deactivateSpam(1) << "Waiting for runfunction";
448  usleep(1000);
449  }
450  ARMARX_INFO << "Breaking state";
451  PlatformContext* c = getContext<PlatformContext>();
452  cleanUp();
453  if (in.getStopPlatformOnBreak())
454  {
455  c->platformUnitPrx->stopPlatform();
456  }
457 
458 }
459 
460 
461 
462 void ControlPlatform::onExit()
463 {
464  while (!isRunningTaskFinished())
465  {
466  ARMARX_INFO << deactivateSpam(1) << "Waiting for runfunction";
467  usleep(1000);
468  }
469  ARMARX_INFO << "Exiting state";
470 
471  cleanUp();
472  PlatformContext* c = getContext<PlatformContext>();
473  if (in.getStopPlatformOnExit())
474  {
475  c->platformUnitPrx->stopPlatform();
476  }
477 }
478 
479 
480 // DO NOT EDIT NEXT FUNCTION
481 XMLStateFactoryBasePtr ControlPlatform::CreateInstance(XMLStateConstructorParams stateData)
482 {
483  return XMLStateFactoryBasePtr(new ControlPlatform(stateData));
484 }
485 
armarx::PlatformGroup::ControlPlatform::ControlPlatform
ControlPlatform(const XMLStateConstructorParams &stateData)
Definition: ControlPlatform.cpp:45
armarx::MultiDimPIDControllerTemplate::update
void update(const double deltaSec, const PIDVectorX &measuredValue, const PIDVectorX &targetValue)
Definition: MultiDimPIDController.h:75
armarx::MultiDimPIDControllerTemplate<>
armarx::XMLStateConstructorParams
Definition: XMLState.h:50
boost::target
Vertex target(const detail::edge_base< Directed, Vertex > &e, const PCG &)
Definition: point_cloud_graph.h:688
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
armarx::PlatformGroup::ControlPlatform::onBreak
void onBreak() override
Definition: ControlPlatform.cpp:443
armarx::PIDController::getControlValue
double getControlValue() const
Definition: PIDController.cpp:273
armarx::PlatformGroup::ControlPlatform::onEnter
void onEnter() override
Definition: ControlPlatform.cpp:50
armarx::PIDController::update
void update(double deltaSec, double measuredValue, double targetValue)
Definition: PIDController.cpp:211
armarx::MultiDimPIDControllerTemplate::previousError
double previousError
Definition: MultiDimPIDController.h:215
IceInternal::Handle< Vector3 >
deactivateSpam
SpamFilterDataPtr deactivateSpam(SpamFilterDataPtr const &spamFilter, float deactivationDurationSec, const std::string &identifier, bool deactivate)
Definition: Logging.cpp:72
DatafieldRef.h
armarx::PlatformGroup::ControlPlatform::onExit
void onExit() override
Definition: ControlPlatform.cpp:462
armarx::XMLStateTemplate
Class for legacy to stay compatible with old statecharts.
Definition: XMLState.h:137
M_PI
#define M_PI
Definition: MathTools.h:17
armarx::PlatformGroup::ControlPlatform::run
void run() override
Definition: ControlPlatform.cpp:102
armarx::PIDController
Definition: PIDController.h:43
armarx::PlatformGroup::ControlPlatform::Registry
static SubClassRegistry Registry
Definition: ControlPlatform.h:47
armarx::PlatformGroup::ControlPlatform::CreateInstance
static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData)
Definition: ControlPlatform.cpp:481
armarx::PlatformContext::platformUnitObserverName
std::string platformUnitObserverName
Definition: PlatformContext.h:124
armarx::PIDController::previousError
double previousError
Definition: PIDController.h:73
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::PlatformGroup::ControlPlatform::cleanUp
void cleanUp()
Definition: ControlPlatform.cpp:423
armarx::TimeUtil::GetTime
static IceUtil::Time GetTime(TimeMode timeMode=TimeMode::VirtualTime)
Get the current time.
Definition: TimeUtil.cpp:42
armarx::VariantType::DatafieldRef
const VariantTypeId DatafieldRef
Definition: DatafieldRef.h:169
PIDController.h
armarx::XMLStateFactoryBasePtr
IceInternal::Handle< XMLStateFactoryBase > XMLStateFactoryBasePtr
Definition: XMLState.h:65
PlatformContext.h
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:600
TimeUtil.h
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
armarx::MultiDimPIDControllerTemplate::getControlValue
const PIDVectorX & getControlValue() const
Definition: MultiDimPIDController.h:186
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:182
armarx::PlatformGroup::ControlPlatform
Definition: ControlPlatform.h:31
memoryx::KBM::Vector3
Eigen::Vector3d Vector3
Definition: kbm.h:41
armarx::PlatformContext::platformUnitPrx
PlatformUnitInterfacePrx platformUnitPrx
Definition: PlatformContext.h:120
armarx::Literal
Definition: Term.h:208
armarx::PlatformContext
Definition: PlatformContext.h:64
signedMin
float signedMin(float newValue, float minAbsValue)
Definition: ControlPlatform.cpp:39
ControlPlatform.h
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28