PlatformGlobalTrajectoryController.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 <VirtualRobot/Robot.h>
25 #include <VirtualRobot/VirtualRobot.h>
26 
31 
32 #include <armarx/control/interface/ConfigurableNJointControllerInterface.h>
36 
37 namespace armarx
38 {
39  class ControlTargetHolonomicPlatformVelocity;
40  class SensorValueHolonomicPlatformWithAbsolutePosition;
41 } // namespace armarx
42 
44 {
45  struct Targets
46  {
48  };
49 
50  struct Config
51  {
53 
56  };
57 
58 
59  struct Twist2D
60  {
61  Eigen::Vector2f linear = Eigen::Vector2f::Zero();
62  float angular = 0;
63 
64  void
66  {
67  linear = Eigen::Vector2f::Zero();
68  angular = 0;
69  }
70  };
71 
72  using Target = Twist2D;
73 
74  struct Debug
75  {
77 
78  double dropPointVelocity = 0;
79  double currentOrientation = 0;
80  double desiredOrientation = 0;
81  double orientationError = 0;
82  bool isFinalSegment = false;
83 
84  double positionError = 0;
85  };
86 
87 
88  struct Devices
89  {
91  };
92 
93  using NameValueMap = std::map<std::string, float>;
94 
95  class Controller :
96  virtual public NJointControllerWithTripleBuffer<Target>,
97  virtual public armarx::control::ConfigurableNJointControllerInterface
98  {
99  public:
100  using ConfigPtrT = control::ConfigurableNJointControllerConfigPtr;
101 
102  Controller(const RobotUnitPtr& robotUnit,
103  const NJointControllerConfigPtr& config,
104  const VirtualRobot::RobotPtr&);
105 
106  ~Controller() override;
107 
108  std::string getClassName(const Ice::Current& iceCurrent = Ice::emptyCurrent) const override;
109 
110  void rtRun(const IceUtil::Time& sensorValuesTimestamp,
111  const IceUtil::Time& timeSinceLastIteration) override;
112 
114  const Ice::Current& iceCurrent = Ice::emptyCurrent) override;
115 ::armarx::aron::data::dto::DictPtr getConfig(const ::Ice::Current& = ::Ice::emptyCurrent) override
116  {
117  ARMARX_ERROR << "NYI";
118  return nullptr;
119  }
120  protected:
121  void additionalTask();
122  void onPublish(const SensorAndControl& sac,
123  const DebugDrawerInterfacePrx& debugDrawer,
124  const DebugObserverInterfacePrx& debugObservers) override;
125 
126 
127  void onInitNJointController() override;
128  void rtPreActivateController() override;
129  void rtPostDeactivateController() override;
130 
131  private:
132  struct RobotState
133  {
134  Eigen::Isometry3f global_T_robot;
135  };
136 
137  TripleBuffer<Config> configBuffer_updateConfigToAdditionalTask;
138  TripleBuffer<Config> configBuffer_updateConfigToOnPublish;
139  TripleBuffer<Debug> targetBuffer_additionalTaskToOnPublish;
140  TripleBuffer<RobotState> robotStateBuffer_rtToAdditionalTask;
141 
142 
143  // internal
144  std::atomic_bool rtFirstRun = true;
145  std::atomic_bool rtReady = false;
146 
148 
149  std::optional<traj_ctrl::global::TrajectoryFollowingController>
150  trajectoryFollowingController;
151 
152 
153  Devices getDevices(const VirtualRobot::RobotNodeSet& rns);
154 
155  // additional task has exclusive access to this variable
156  Twist2D filteredTwist;
157  };
158 
159 } // namespace armarx::navigation::platform_controller::platform_global_trajectory
armarx::navigation::platform_controller::platform_global_trajectory::Debug::isFinalSegment
bool isFinalSegment
Definition: PlatformGlobalTrajectoryController.h:82
armarx::navigation::core::GlobalTrajectory
Definition: Trajectory.h:68
armarx::ControlTargetHolonomicPlatformVelocity
Brief description of class ControlTargetHolonomicPlatformVelocity.
Definition: ControlTargetHolonomicPlatformVelocity.h:38
armarx::navigation::platform_controller::platform_global_trajectory::Twist2D::reset
void reset()
Definition: PlatformGlobalTrajectoryController.h:65
NJointControllerWithTripleBuffer.h
SensorValueHolonomicPlatform.h
armarx::NJointControllerWithTripleBuffer
Definition: NJointControllerWithTripleBuffer.h:10
RobotUnit.h
armarx::navigation::platform_controller::platform_global_trajectory::Debug::dropPointVelocity
double dropPointVelocity
Definition: PlatformGlobalTrajectoryController.h:78
armarx::navigation::platform_controller::platform_global_trajectory::Targets
Definition: PlatformGlobalTrajectoryController.h:45
armarx::navigation::platform_controller::platform_global_trajectory::Controller::updateConfig
void updateConfig(const ::armarx::aron::data::dto::DictPtr &dto, const Ice::Current &iceCurrent=Ice::emptyCurrent) override
Definition: PlatformGlobalTrajectoryController.cpp:91
armarx::navigation::platform_controller::platform_global_trajectory::Debug::currentOrientation
double currentOrientation
Definition: PlatformGlobalTrajectoryController.h:79
armarx::NJointControllerBase::ConfigPtrT
NJointControllerConfigPtr ConfigPtrT
Definition: NJointControllerBase.h:586
TrajectoryFollowingController.h
armarx::navigation::platform_controller::platform_global_trajectory::Controller::onInitNJointController
void onInitNJointController() override
Definition: PlatformGlobalTrajectoryController.cpp:205
armarx::navigation::platform_controller::platform_global_trajectory::NameValueMap
std::map< std::string, float > NameValueMap
Definition: PlatformGlobalTrajectoryController.h:93
armarx::navigation::platform_controller::platform_global_trajectory::Config
Definition: PlatformGlobalTrajectoryController.h:50
armarx::navigation::platform_controller::platform_global_trajectory::Controller::additionalTask
void additionalTask()
Definition: PlatformGlobalTrajectoryController.cpp:112
armarx::detail::ControlThreadOutputBufferEntry
Definition: ControlThreadOutputBuffer.h:177
armarx::navigation::platform_controller::platform_global_trajectory::Twist2D
Definition: PlatformGlobalTrajectoryController.h:59
armarx::navigation::platform_controller::platform_global_trajectory::Controller::Controller
Controller(const RobotUnitPtr &robotUnit, const NJointControllerConfigPtr &config, const VirtualRobot::RobotPtr &)
Definition: PlatformGlobalTrajectoryController.cpp:21
armarx::navigation::platform_controller::platform_global_trajectory::Targets::trajectory
core::GlobalTrajectory trajectory
Definition: PlatformGlobalTrajectoryController.h:47
armarx::navigation::platform_controller::platform_global_trajectory::Controller::getClassName
std::string getClassName(const Ice::Current &iceCurrent=Ice::emptyCurrent) const override
Definition: PlatformGlobalTrajectoryController.cpp:67
armarx::navigation::platform_controller::platform_global_trajectory::Config::params
Params params
Definition: PlatformGlobalTrajectoryController.h:54
armarx::navigation::platform_controller::platform_global_trajectory::Controller::rtPostDeactivateController
void rtPostDeactivateController() override
This function is called after the controller is deactivated.
Definition: PlatformGlobalTrajectoryController.cpp:246
armarx::navigation::platform_controller::platform_global_trajectory::Twist2D::angular
float angular
Definition: PlatformGlobalTrajectoryController.h:62
armarx::navigation::platform_controller::platform_global_trajectory::Debug::orientationError
double orientationError
Definition: PlatformGlobalTrajectoryController.h:81
TrajectoryFollowingController.h
ControlTarget1DoFActuator.h
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::navigation::platform_controller::platform_global_trajectory::Devices
Definition: PlatformGlobalTrajectoryController.h:88
armarx::aron::data::DictPtr
std::shared_ptr< Dict > DictPtr
Definition: Dict.h:41
armarx::navigation::traj_ctrl::global::TrajectoryFollowingControllerParams
Definition: TrajectoryFollowingController.h:34
armarx::navigation::platform_controller::platform_global_trajectory::Controller::rtRun
void rtRun(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration) override
TODO make protected and use attorneys.
Definition: PlatformGlobalTrajectoryController.cpp:74
armarx::navigation::platform_controller::platform_global_trajectory::Controller::rtPreActivateController
void rtPreActivateController() override
This function is called before the controller is activated.
Definition: PlatformGlobalTrajectoryController.cpp:233
armarx::navigation::platform_controller::platform_global_trajectory::Debug::desiredOrientation
double desiredOrientation
Definition: PlatformGlobalTrajectoryController.h:80
IceInternal::ProxyHandle<::IceProxy::armarx::DebugDrawerInterface >
armarx::navigation::platform_controller::platform_global_trajectory::Twist2D::linear
Eigen::Vector2f linear
Definition: PlatformGlobalTrajectoryController.h:61
armarx::navigation::platform_controller::platform_global_trajectory::Debug::positionError
double positionError
Definition: PlatformGlobalTrajectoryController.h:84
armarx::navigation::platform_controller::platform_global_trajectory::Controller::~Controller
~Controller() override
armarx::navigation::platform_controller::platform_global_trajectory::Debug::target
Target target
Definition: PlatformGlobalTrajectoryController.h:76
armarx::navigation::platform_controller::platform_global_trajectory::Config::targets
Targets targets
Definition: PlatformGlobalTrajectoryController.h:55
armarx::navigation::platform_controller::platform_global_trajectory::Debug
Definition: PlatformGlobalTrajectoryController.h:74
armarx::navigation::platform_controller::platform_global_trajectory
Definition: aron_conversions.cpp:38
types.h
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::navigation::platform_controller::platform_global_trajectory::Controller::onPublish
void onPublish(const SensorAndControl &sac, const DebugDrawerInterfacePrx &debugDrawer, const DebugObserverInterfacePrx &debugObservers) override
Definition: PlatformGlobalTrajectoryController.cpp:173
armarx::navigation::platform_controller::platform_global_trajectory::Devices::platformTarget
ControlTargetHolonomicPlatformVelocity * platformTarget
Definition: PlatformGlobalTrajectoryController.h:90
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:18
armarx::navigation::platform_controller::platform_global_trajectory::Controller::getConfig
::armarx::aron::data::dto::DictPtr getConfig(const ::Ice::Current &=::Ice::emptyCurrent) override
Definition: PlatformGlobalTrajectoryController.h:115
armarx::navigation::platform_controller::platform_global_trajectory::Controller
Definition: PlatformGlobalTrajectoryController.h:95
armarx::TripleBuffer< Config >