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