PlatformFollowerController.cpp
Go to the documentation of this file.
2 
3 #include <VirtualRobot/RobotNodeSet.h>
4 #include <VirtualRobot/math/Helpers.h>
5 
11 
14 
17 #include <armarx/control/njoint_controller/platform/platform_follower_controller/aron/PlatformFollowerControllerConfig.aron.generated.h>
19 
21 {
22  const std::string NAME = "PlatformFollowerController";
23 
25 
26  Controller::Controller(const armarx::RobotUnitPtr& robotUnit,
27  const armarx::NJointControllerConfigPtr& config,
29  {
30  // config
31  ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
33  // ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
34 
35  ARMARX_CHECK_EXPRESSION(robotUnit);
36 
37  const auto robot = useSynchronizedRtRobot();
38 
39  platformTarget = useControlTarget(robotUnit->getRobotPlatformName(),
40  armarx::ControlModes::HolonomicPlatformVelocity)
42  ARMARX_CHECK_EXPRESSION(platformTarget)
43  << "The actuator " << robotUnit->getRobotPlatformName() << " has no control mode "
44  << armarx::ControlModes::HolonomicPlatformVelocity;
45 
46  const Config configData = ::armarx::fromAron<arondto::Config, Config>(cfg->config);
47 
49  configBuffer.reinitAllBuffers(configData);
50 
51  // TODO init tcps
52  const VirtualRobot::RobotNodeSetPtr rnsRight =
53  robot->getRobotNodeSet(configData.params.nodeSetNameRight);
54  ARMARX_CHECK(rnsRight);
55  const VirtualRobot::RobotNodeSetPtr rnsLeft =
56  robot->getRobotNodeSet(configData.params.nodeSetNameLeft);
57  ARMARX_CHECK(rnsLeft);
58  tcpRight = rnsRight->getTCP();
59  tcpLeft = rnsLeft->getTCP();
60  }
61 
62  std::string
63  Controller::getClassName(const Ice::Current& iceCurrent) const
64  {
65  return NAME;
66  }
67 
68  void
69  Controller::rtRun(const IceUtil::Time& sensorValuesTimestamp,
70  const IceUtil::Time& timeSinceLastIteration)
71  {
72  // update state
73  stateBuffer.getWriteBuffer().handPositionRight = tcpRight->getPositionInRootFrame();
74  stateBuffer.getWriteBuffer().handPositionLeft = tcpLeft->getPositionInRootFrame();
75  stateBuffer.getWriteBuffer().time = armarx::Clock::Now();
76  stateBuffer.getWriteBuffer().currentVel.setZero();
77  stateBuffer.commitWrite();
78 
79  // write control targets
81 
82  // FIXME remove
83  //platformTarget->velocityX = 0;
84  //platformTarget->velocityY = 0;
85  //platformTarget->velocityRotation = 0;
86 
87  // FIXME uncomment
88  platformTarget->velocityX = rtGetControlStruct().platformVelocityTargets.x;
89  platformTarget->velocityY = rtGetControlStruct().platformVelocityTargets.y;
90  platformTarget->velocityRotation = rtGetControlStruct().platformVelocityTargets.yaw;
91  }
92 
93  void
95  const Ice::Current& iceCurrent)
96  {
97  ARMARX_IMPORTANT << "Controller::updateConfig";
98 
99  // TODO maybe update pid controller
100 
101  auto updateConfig = ::armarx::fromAron<arondto::Config, Config>(dto);
102  configBuffer.reinitAllBuffers(updateConfig);
103  }
104 
105  void
107  {
108  if (not rtReady.load())
109  {
110  // store result
111  getWriterControlStruct().platformVelocityTargets.x = 0;
112  getWriterControlStruct().platformVelocityTargets.y = 0;
113  getWriterControlStruct().platformVelocityTargets.yaw = 0;
114 
116  }
117 
118  configBuffer.updateReadBuffer();
119  stateBuffer.updateReadBuffer();
120 
121  const auto state = stateBuffer.getReadBuffer();
122  const auto params = configBuffer.getReadBuffer().params;
123 
124  const double deltaT = (state.time - additionalTaskData.lastTime).toSecondsDouble();
125  additionalTaskData.lastTime = state.time;
126 
127  if (deltaT == 0)
128  return;
129 
130  const Eigen::Vector3f currentVel = state.currentVel;
131  const Eigen::Vector3f currentPositionRight = state.handPositionRight;
132  const Eigen::Vector3f currentPositionLeft = state.handPositionLeft;
133  const Eigen::Vector3f positionDeltaRight =
134  currentPositionRight - initialState.handPositionRight;
135  const Eigen::Vector3f positionDeltaLeft =
136  currentPositionLeft - initialState.handPositionLeft;
137 
138  Eigen::Vector2f positionDelta = ((positionDeltaLeft + positionDeltaRight) / 2).head(2);
139  Eigen::Vector2f velocity = Eigen::Vector2f::Zero();
140  if (positionDelta.norm() >= params.minPosTolerance)
141  {
142  positionDelta =
143  positionDelta - params.minPosTolerance * positionDelta / positionDelta.norm();
144  velocity = positionDelta * params.kPos - currentVel.head(2) * params.dPos;
145  }
146  const float Tstar = 1 / ((params.filterTimeConstant / deltaT) + 1);
147  additionalTaskData.filteredLinearVel =
148  Tstar * (velocity - additionalTaskData.filteredLinearVel) +
149  additionalTaskData.filteredLinearVel;
150  if (!std::isfinite(additionalTaskData.filteredLinearVel(0)))
151  {
152  additionalTaskData.filteredLinearVel(0) = 0;
153  }
154  if (!std::isfinite(additionalTaskData.filteredLinearVel(1)))
155  {
156  additionalTaskData.filteredLinearVel(1) = 0;
157  }
158  velocity = additionalTaskData.filteredLinearVel.norm() > params.maxPosVel
159  ? additionalTaskData.filteredLinearVel.normalized() * params.maxPosVel
160  : additionalTaskData.filteredLinearVel;
161 
162 
163  const Eigen::Vector3f currentDirection = state.direction();
164  const float cosAlpha = currentDirection.dot(initialState.direction());
165  const Eigen::Vector3f directionDiff = currentDirection - initialState.direction();
166  float alpha = 0;
167  if (directionDiff(1) < 0)
168  {
169  alpha = -acos(cosAlpha);
170  }
171  else
172  {
173  alpha = acos(cosAlpha);
174  }
175  float angularVelocity;
176  if (fabs(alpha) < params.minAngleTolerance)
177  {
178  angularVelocity = 0;
179  }
180  else
181  {
182  if (alpha > 0)
183  {
184  alpha -= params.minAngleTolerance;
185  }
186  else
187  {
188  alpha += params.minAngleTolerance;
189  }
190  angularVelocity = params.kAngle * alpha - params.dAngle * currentVel.z();
191  }
192  additionalTaskData.filteredAngularVel =
193  Tstar * (angularVelocity - additionalTaskData.filteredAngularVel) +
194  additionalTaskData.filteredAngularVel;
195  if (!std::isfinite(additionalTaskData.filteredAngularVel))
196  {
197  additionalTaskData.filteredAngularVel = 0;
198  }
199  angularVelocity = std::min(additionalTaskData.filteredAngularVel, params.maxAngleVel);
200 
201 
202  {
203  auto& onPublishData = onPublishBuffer.getWriteBuffer();
204 
205  onPublishData.target.platformVelocityTargets.x = velocity.x();
206  onPublishData.target.platformVelocityTargets.y = velocity.y();
207  onPublishData.target.platformVelocityTargets.yaw = angularVelocity;
208 
209  onPublishData.position_error_left = positionDeltaLeft;
210  onPublishData.position_error_right = positionDeltaRight;
211  onPublishBuffer.commitWrite();
212  }
213 
214 
215  // store result
216  getWriterControlStruct().platformVelocityTargets.x = velocity.x();
217  getWriterControlStruct().platformVelocityTargets.y = velocity.y();
218  getWriterControlStruct().platformVelocityTargets.yaw = angularVelocity;
219 
221  }
222 
223  void
225  const armarx::DebugDrawerInterfacePrx& /*debugDrawer*/,
226  const armarx::DebugObserverInterfacePrx& debugObservers)
227  {
228  armarx::StringVariantBaseMap datafields;
229 
230  onPublishBuffer.updateReadBuffer();
231  const auto& data = onPublishBuffer.getReadBuffer();
232 
233  datafields["vx"] = new armarx::Variant(data.target.platformVelocityTargets.x);
234  datafields["vy"] = new armarx::Variant(data.target.platformVelocityTargets.y);
235  datafields["vyaw"] = new armarx::Variant(data.target.platformVelocityTargets.yaw);
236 
237  datafields["position_error_left_x"] = new armarx::Variant(data.position_error_left.x());
238  datafields["position_error_left_y"] = new armarx::Variant(data.position_error_left.y());
239  datafields["position_error_left_z"] = new armarx::Variant(data.position_error_left.z());
240  datafields["position_error_right_x"] = new armarx::Variant(data.position_error_right.x());
241  datafields["position_error_right_y"] = new armarx::Variant(data.position_error_right.y());
242  datafields["position_error_right_z"] = new armarx::Variant(data.position_error_right.z());
243 
244  debugObservers->setDebugChannel(NAME, datafields);
245  }
246 
247  void
249  {
250  runTask(NAME + "AdditionalTask",
251  [&]
252  {
254  4); // please don't set this to 0 as it is the rtRun/control thread
257 
258  armarx::CycleUtil c(10);
259  getObjectScheduler()->waitForObjectStateMinimum(
260  armarx::eManagedIceObjectStarted);
261  ARMARX_IMPORTANT << "Create a new thread alone PlatformTrajectory controller";
262  while (getState() == armarx::eManagedIceObjectStarted)
263  {
264  if (isControllerActive() and rtReady.load())
265  {
266  ARMARX_VERBOSE << "additional task";
267  additionalTask();
268  }
269  c.waitForCycleDuration();
270  }
271  });
272 
273  ARMARX_INFO << "PlatformTrajectoryVelocityController::onInitNJointController";
274  }
275 
276  void
278  {
279  initialState.handPositionRight = tcpRight->getPositionInRootFrame();
280  initialState.handPositionLeft = tcpLeft->getPositionInRootFrame();
281  initialState.time = armarx::core::time::DateTime::Now();
282 
283  ARMARX_INFO << "Init done.";
284 
285  rtReady.store(true);
286  }
287 
288  Controller::~Controller() = default;
289 
290 } // namespace armarx::control::njoint_controller::platform::platform_follower_controller
armarx::NJointControllerWithTripleBuffer< Target >::reinitTripleBuffer
void reinitTripleBuffer(const Target &initial)
Definition: NJointControllerWithTripleBuffer.h:68
armarx::ControlTargetHolonomicPlatformVelocity
Brief description of class ControlTargetHolonomicPlatformVelocity.
Definition: ControlTargetHolonomicPlatformVelocity.h:38
armarx::control::njoint_controller::platform::platform_follower_controller
This file is part of ArmarX.
Definition: aron_conversions.cpp:27
armarx::ControlTargetBase::asA
const T * asA() const
Definition: ControlTargetBase.h:76
armarx::Variant
The Variant class is described here: Variants.
Definition: Variant.h:224
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:180
armarx::control::njoint_controller::platform::platform_follower_controller::Controller::Controller
Controller(const armarx::RobotUnitPtr &robotUnit, const armarx::NJointControllerConfigPtr &config, const VirtualRobot::RobotPtr &)
Definition: PlatformFollowerController.cpp:26
armarx::control::ethercat::RTUtility::RT_THREAD_PRIORITY
static constexpr int RT_THREAD_PRIORITY
The thread priority for rt-threads.
Definition: RTUtility.h:24
armarx::control::njoint_controller::platform::platform_follower_controller::Config::params
Params params
Definition: PlatformFollowerController.h:65
armarx::TripleBuffer::getWriteBuffer
T & getWriteBuffer()
Definition: TripleBuffer.h:90
armarx::NJointControllerRegistration
Definition: NJointControllerRegistry.h:74
armarx::NJointControllerBase::useSynchronizedRtRobot
const VirtualRobot::RobotPtr & useSynchronizedRtRobot(bool updateCollisionModel=false)
Requests a VirtualRobot for use in rtRun *.
Definition: NJointController.cpp:293
armarx::control::njoint_controller::platform::platform_follower_controller::Controller::onInitNJointController
void onInitNJointController() override
Definition: PlatformFollowerController.cpp:248
armarx::NJointControllerWithTripleBuffer< Target >::rtGetControlStruct
const Target & rtGetControlStruct() const
Definition: NJointControllerWithTripleBuffer.h:32
armarx::StringVariantBaseMap
std::map< std::string, VariantBasePtr > StringVariantBaseMap
Definition: ManagedIceObject.h:111
armarx::control::njoint_controller::platform::platform_follower_controller::Registration
const armarx::NJointControllerRegistration< Controller > Registration(NAME)
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:183
PlatformFollowerController.h
armarx::TripleBuffer::commitWrite
void commitWrite()
Definition: TripleBuffer.h:146
armarx::core::time::DateTime::Now
static DateTime Now()
Definition: DateTime.cpp:55
armarx::NJointControllerBase::useControlTarget
ControlTargetBase * useControlTarget(const std::string &deviceName, const std::string &controlMode)
Declares to calculate the ControlTarget for the given ControlDevice in the given ControlMode when rtR...
Definition: NJointController.cpp:410
armarx::control::ethercat::RTUtility::pinThreadToCPU
static bool pinThreadToCPU(unsigned int cpu)
Pins the calling thread to the CPU with the given id.
Definition: RTUtility.cpp:52
armarx::CycleUtil
This util class helps with keeping a cycle time during a control cycle.
Definition: CycleUtil.h:40
armarx::ManagedIceObject::getState
int getState() const
Retrieve current state of the ManagedIceObject.
Definition: ManagedIceObject.cpp:725
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
armarx::NJointControllerBase::ConfigPtrT
NJointControllerConfigPtr ConfigPtrT
Definition: NJointControllerBase.h:586
armarx::NJointControllerWithTripleBuffer< Target >::getWriterControlStruct
Target & getWriterControlStruct()
Definition: NJointControllerWithTripleBuffer.h:54
armarx::control::njoint_controller::platform::platform_follower_controller::RobotState::time
armarx::core::time::DateTime time
Definition: PlatformFollowerController.h:52
armarx::ControlTargetHolonomicPlatformVelocity::velocityRotation
float velocityRotation
Definition: ControlTargetHolonomicPlatformVelocity.h:43
type.h
ARMARX_CHECK
#define ARMARX_CHECK(expression)
Shortcut for ARMARX_CHECK_EXPRESSION.
Definition: ExpressionException.h:82
Clock.h
armarx::control::njoint_controller::platform::platform_follower_controller::RobotState::handPositionRight
Eigen::Vector3f handPositionRight
Definition: PlatformFollowerController.h:49
armarx::control::njoint_controller::platform::platform_follower_controller::Controller::additionalTask
void additionalTask()
Definition: PlatformFollowerController.cpp:106
std::isfinite
bool isfinite(const std::vector< T, Ts... > &v)
Definition: algorithm.h:327
armarx::detail::ControlThreadOutputBufferEntry
Definition: ControlThreadOutputBuffer.h:177
armarx::control::njoint_controller::platform::platform_follower_controller::Controller::getClassName
std::string getClassName(const Ice::Current &iceCurrent=Ice::emptyCurrent) const override
Definition: PlatformFollowerController.cpp:63
if
if(!yyvaluep)
Definition: Grammar.cpp:724
armarx::NJointControllerBase::isControllerActive
bool isControllerActive(const Ice::Current &=Ice::emptyCurrent) const final override
Definition: NJointControllerBase.h:777
armarx::NJointControllerBase::runTask
void runTask(const std::string &taskName, Task &&task)
Executes a given task in a separate thread from the Application ThreadPool.
Definition: NJointControllerBase.h:753
data
uint8_t data[1]
Definition: EtherCATFrame.h:68
armarx::NJointControllerWithTripleBuffer< Target >::rtUpdateControlStruct
bool rtUpdateControlStruct()
Definition: NJointControllerWithTripleBuffer.h:38
armarx::NJointControllerWithTripleBuffer< Target >::writeControlStruct
void writeControlStruct()
Definition: NJointControllerWithTripleBuffer.h:44
armarx::TripleBuffer::getReadBuffer
const T & getReadBuffer() const
Definition: TripleBuffer.h:102
ArmarXObjectScheduler.h
armarx::control::njoint_controller::platform::platform_follower_controller::Controller::rtPreActivateController
void rtPreActivateController() override
This function is called before the controller is activated.
Definition: PlatformFollowerController.cpp:277
armarx::ControlTargetHolonomicPlatformVelocity::velocityX
float velocityX
Definition: ControlTargetHolonomicPlatformVelocity.h:41
armarx::control::ethercat::RTUtility::elevateThreadPriority
static bool elevateThreadPriority(int priority)
Elevate the thread priority of the calling thread to the given priority.
Definition: RTUtility.cpp:17
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::ControlTargetHolonomicPlatformVelocity::velocityY
float velocityY
Definition: ControlTargetHolonomicPlatformVelocity.h:42
armarx::control::njoint_controller::platform::platform_follower_controller::Controller::rtRun
void rtRun(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration) override
TODO make protected and use attorneys.
Definition: PlatformFollowerController.cpp:69
CycleUtil.h
ExpressionException.h
armarx::control::njoint_controller::platform::platform_follower_controller::NAME
const std::string NAME
Definition: PlatformFollowerController.cpp:22
armarx::aron::data::DictPtr
std::shared_ptr< Dict > DictPtr
Definition: Dict.h:41
ControlTargetHolonomicPlatformVelocity.h
ARMARX_CHECK_EXPRESSION
#define ARMARX_CHECK_EXPRESSION(expression)
This macro evaluates the expression and if it turns out to be false it will throw an ExpressionExcept...
Definition: ExpressionException.h:73
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
armarx::control::njoint_controller::platform::platform_follower_controller::RobotState::handPositionLeft
Eigen::Vector3f handPositionLeft
Definition: PlatformFollowerController.h:50
IceInternal::ProxyHandle<::IceProxy::armarx::DebugDrawerInterface >
armarx::core::time::Clock::Now
static DateTime Now()
Current time on the virtual clock.
Definition: Clock.cpp:97
aron_conversions.h
armarx::control::njoint_controller::platform::platform_follower_controller::RobotState::direction
Eigen::Vector3f direction() const
Definition: PlatformFollowerController.h:55
Logging.h
min
T min(T t1, T t2)
Definition: gdiam.h:42
armarx::control::njoint_controller::platform::platform_follower_controller::Controller::updateConfig
void updateConfig(const ::armarx::aron::data::dto::DictPtr &dto, const Ice::Current &iceCurrent=Ice::emptyCurrent) override
Definition: PlatformFollowerController.cpp:94
armarx::control::njoint_controller::platform::platform_follower_controller::Config
Definition: PlatformFollowerController.h:61
armarx::control::njoint_controller::platform::platform_follower_controller::Controller::onPublish
void onPublish(const armarx::SensorAndControl &sac, const armarx::DebugDrawerInterfacePrx &debugDrawer, const armarx::DebugObserverInterfacePrx &debugObservers) override
Definition: PlatformFollowerController.cpp:224
armarx::control::njoint_controller::platform::platform_follower_controller::Controller::~Controller
~Controller() override
armarx::TripleBuffer::reinitAllBuffers
std::enable_if< std::is_copy_constructible< U >::value >::type reinitAllBuffers(const T &init)
Definition: TripleBuffer.h:153
RTUtility.h
NJointControllerRegistry.h
armarx::ManagedIceObject::getObjectScheduler
ArmarXObjectSchedulerPtr getObjectScheduler() const
Definition: ManagedIceObject.cpp:731
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:18
armarx::TripleBuffer::updateReadBuffer
bool updateReadBuffer() const
Swaps in the hidden buffer if it contains new data.
Definition: TripleBuffer.h:131