PlatformFollowerController.h
Go to the documentation of this file.
1 /**
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @author Fabian Reister ( fabian dot reister at kit dot edu )
17  * @date 2022
18  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
19  * GNU General Public License
20  */
21 
22 #pragma once
23 
24 #include <qstatemachine.h>
25 
26 #include <VirtualRobot/Robot.h>
27 #include <VirtualRobot/VirtualRobot.h>
28 
30 
35 
36 #include <armarx/control/interface/ConfigurableNJointControllerInterface.h>
37 #include <armarx/control/njoint_controller/platform/platform_follower_controller/aron/PlatformFollowerControllerConfig.aron.generated.h>
38 
39 namespace armarx
40 {
41  class ControlTargetHolonomicPlatformVelocity;
42  class SensorValueHolonomicPlatformWithAbsolutePosition;
43 } // namespace armarx
44 
46 {
47  struct RobotState
48  {
49  Eigen::Vector3f handPositionRight;
50  Eigen::Vector3f handPositionLeft;
51  Eigen::Vector3f currentVel = Eigen::Vector3f::Zero();
53 
54  Eigen::Vector3f
55  direction() const
56  {
57  return (handPositionRight - handPositionLeft).normalized();
58  }
59  };
60 
61  struct Config
62  {
63  using Params = platform_follower_controller::arondto::Params;
64 
66  };
67 
68  struct Target
69  {
70  struct
71  {
72  double x = 0;
73  double y = 0;
74  double yaw = 0;
76 
77  void
79  {
83  }
84  };
85 
86  struct Devices
87  {
89  };
90 
91  using NameValueMap = std::map<std::string, float>;
92 
93  class Controller :
94  virtual public armarx::NJointControllerWithTripleBuffer<Target>,
95  virtual public armarx::control::ConfigurableNJointControllerInterface
96  {
97  public:
98  using ConfigPtrT = armarx::control::ConfigurableNJointControllerConfigPtr;
99 
100  Controller(const armarx::RobotUnitPtr& robotUnit,
101  const armarx::NJointControllerConfigPtr& config,
102  const VirtualRobot::RobotPtr&);
103 
104  ~Controller() override;
105 
106  std::string getClassName(const Ice::Current& iceCurrent = Ice::emptyCurrent) const override;
107 
108  void rtRun(const IceUtil::Time& sensorValuesTimestamp,
109  const IceUtil::Time& timeSinceLastIteration) override;
110 
112  const Ice::Current& iceCurrent = Ice::emptyCurrent) override;
113 
115  getConfig(const Ice::Current& iceCurrent = Ice::emptyCurrent) override
116  {
117  ARMARX_WARNING << "NYI";
118  return nullptr;
119  }
120 
121  protected:
122  void additionalTask();
123  void onPublish(const armarx::SensorAndControl& sac,
124  const armarx::DebugDrawerInterfacePrx& debugDrawer,
125  const armarx::DebugObserverInterfacePrx& debugObservers) override;
126 
127 
128  void onInitNJointController() override;
129  void rtPreActivateController() override;
130 
131  private:
132  // updateConfig -> additionalTask
133  armarx::TripleBuffer<Config> configBuffer;
134 
135  // rt -> additionalTask
137 
138  // internal
139  std::atomic_bool rtFirstRun = true;
140  std::atomic_bool rtReady = false;
141 
143 
144  // FIXME add platform sensors here
145  std::vector<const armarx::SensorValue1DoFActuatorVelocity*> velocitySensors;
146 
147  Devices getDevices(const VirtualRobot::RobotNodeSet& rns);
148 
149  RobotState initialState;
150 
151  VirtualRobot::RobotNodePtr tcpRight;
152  VirtualRobot::RobotNodePtr tcpLeft;
153 
154  struct
155  {
156  Eigen::Vector2f filteredLinearVel = Eigen::Vector2f::Zero();
159  } additionalTaskData;
160 
161  struct OnPublishData
162  {
163  Target target;
164 
165  Eigen::Vector3f position_error_left;
166  Eigen::Vector3f position_error_right;
167  };
168 
169  armarx::TripleBuffer<OnPublishData> onPublishBuffer;
170  };
171 
172 } // namespace armarx::control::njoint_controller::platform::platform_follower_controller
armarx::control::njoint_controller::platform::platform_follower_controller::Controller
Definition: PlatformFollowerController.h:93
armarx::control::njoint_controller::platform::platform_follower_controller::Target
Definition: PlatformFollowerController.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::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::njoint_controller::platform::platform_follower_controller::Controller::lastTime
armarx::core::time::DateTime lastTime
Definition: PlatformFollowerController.h:158
NJointControllerWithTripleBuffer.h
armarx::control::njoint_controller::platform::platform_follower_controller::Config::params
Params params
Definition: PlatformFollowerController.h:65
armarx::control::njoint_controller::platform::platform_follower_controller::Controller::onInitNJointController
void onInitNJointController() override
Definition: PlatformFollowerController.cpp:248
SensorValueHolonomicPlatform.h
armarx::NJointControllerWithTripleBuffer
Definition: NJointControllerWithTripleBuffer.h:10
RobotUnit.h
armarx::core::time::DateTime::Now
static DateTime Now()
Definition: DateTime.cpp:55
DateTime.h
boost::target
Vertex target(const detail::edge_base< Directed, Vertex > &e, const PCG &)
Definition: point_cloud_graph.h:688
armarx::NJointControllerBase::ConfigPtrT
NJointControllerConfigPtr ConfigPtrT
Definition: NJointControllerBase.h:586
armarx::control::njoint_controller::platform::platform_follower_controller::RobotState::time
armarx::core::time::DateTime time
Definition: PlatformFollowerController.h:52
armarx::control::njoint_controller::platform::platform_follower_controller::Config::Params
platform_follower_controller::arondto::Params Params
Definition: PlatformFollowerController.h:63
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
armarx::control::njoint_controller::platform::platform_follower_controller::Controller::filteredAngularVel
float filteredAngularVel
Definition: PlatformFollowerController.h:157
armarx::detail::ControlThreadOutputBufferEntry
Definition: ControlThreadOutputBuffer.h:177
armarx::control::njoint_controller::platform::platform_follower_controller::Controller::filteredLinearVel
Eigen::Vector2f filteredLinearVel
Definition: PlatformFollowerController.h:156
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
armarx::control::njoint_controller::platform::platform_follower_controller::Target::x
double x
Definition: PlatformFollowerController.h:72
armarx::control::njoint_controller::platform::platform_follower_controller::Devices
Definition: PlatformFollowerController.h:86
armarx::control::njoint_controller::platform::platform_follower_controller::RobotState
Definition: PlatformFollowerController.h:47
ControlTarget1DoFActuator.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::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::control::njoint_controller::platform::platform_follower_controller::Target::y
double y
Definition: PlatformFollowerController.h:73
armarx::control::njoint_controller::platform::platform_follower_controller::Controller::getConfig
::armarx::aron::data::dto::DictPtr getConfig(const Ice::Current &iceCurrent=Ice::emptyCurrent) override
Definition: PlatformFollowerController.h:115
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
armarx::control::njoint_controller::platform::platform_follower_controller::RobotState::currentVel
Eigen::Vector3f currentVel
Definition: PlatformFollowerController.h:51
armarx::control::njoint_controller::platform::platform_follower_controller::NameValueMap
std::map< std::string, float > NameValueMap
Definition: PlatformFollowerController.h:91
armarx::core::time::DateTime
Represents a point in time.
Definition: DateTime.h:24
armarx::aron::data::DictPtr
std::shared_ptr< Dict > DictPtr
Definition: Dict.h:41
armarx::control::njoint_controller::platform::platform_follower_controller::Devices::platformTarget
armarx::ControlTargetHolonomicPlatformVelocity * platformTarget
Definition: PlatformFollowerController.h:88
armarx::control::njoint_controller::platform::platform_follower_controller::Target::yaw
double yaw
Definition: PlatformFollowerController.h:74
armarx::control::njoint_controller::platform::platform_follower_controller::Target::reset
void reset()
Definition: PlatformFollowerController.h:78
armarx::control::njoint_controller::platform::platform_follower_controller::RobotState::handPositionLeft
Eigen::Vector3f handPositionLeft
Definition: PlatformFollowerController.h:50
IceInternal::ProxyHandle<::IceProxy::armarx::DebugDrawerInterface >
armarx::control::njoint_controller::platform::platform_follower_controller::RobotState::direction
Eigen::Vector3f direction() const
Definition: PlatformFollowerController.h:55
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
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
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:18
armarx::control::njoint_controller::platform::platform_follower_controller::Target::platformVelocityTargets
struct armarx::control::njoint_controller::platform::platform_follower_controller::Target::@56 platformVelocityTargets
armarx::TripleBuffer< Config >