NJointHolonomicPlatformVelocityControllerWithRamp.cpp
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  * @package RobotAPI::ArmarXObjects::NJointHolonomicPlatformUnitVelocityPassThroughController
17  * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
18  * @date 2017
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
24 
25 #include <VirtualRobot/Robot.h>
26 
28 
33 
34 #include "../ControlTargets/ControlTargetHolonomicPlatformVelocity.h"
35 
36 namespace armarx
37 {
40  ConfigPtrT cfg,
41  const VirtualRobot::RobotPtr&) :
42  ramp(cfg->maxPositionAcceleration, cfg->maxOrientationAcceleration)
43  {
44  target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)
47  << "The actuator " << cfg->platformName << " has no control mode "
48  << ControlModes::HolonomicPlatformVelocity;
49 
50  const auto sensor = useSensorValue(cfg->platformName);
51  ARMARX_CHECK_EXPRESSION(sensor) << "No sensor value for " << cfg->platformName;
54  << "Sensor value for " << cfg->platformName << " has invalid type";
55  }
56 
57  void
59  {
63  activationTime = IceUtil::Time::now();
64 
65  // init velocity ramp
67  }
68 
69  void
71  const IceUtil::Time& sensorValuesTimestamp,
72  const IceUtil::Time& timeSinceLastIteration)
73  {
74  auto commandAge = sensorValuesTimestamp - rtGetControlStruct().commandTimestamp;
75 
76  if (commandAge > maxCommandDelay && // command must be recent
77  (rtGetControlStruct().velocityX != 0.0f || rtGetControlStruct().velocityY != 0.0f ||
78  rtGetControlStruct().velocityRotation !=
79  0.0f)) // only throw error if any command is not zero
80  {
81  ARMARX_RT_LOGF_WARNING("Platform target velocity was not set for a too long time: "
82  "delay: %f, s, max allowed delay: %f s",
83  commandAge.toSecondsDouble(),
84  maxCommandDelay.toSecondsDouble())
85  .deactivateSpam(0.5);
86  // << deactivateSpam(0.5)
87  // << "Platform target velocity was not set for a too long time: delay: "
88  // << commandAge.toSecondsDouble()
89  // << " s, max allowed delay: " << maxCommandDelay.toSecondsDouble() << " s";
90  const Eigen::Vector3f targetVelocity = Eigen::Vector3f::Zero();
91  Eigen::Vector3f result =
92  ramp.update(targetVelocity, timeSinceLastIteration.toSecondsDouble());
93  target->velocityX = result(0);
94  target->velocityY = result(1);
95  target->velocityRotation = result(2);
96  }
97  else
98  {
99  Eigen::Vector3f result;
100  if (activationTime > rtGetControlStruct().commandTimestamp)
101  {
102  // No valid command has been specified. Sane default: come to rest.
103  const Eigen::Vector3f targetVelocity = Eigen::Vector3f::Zero();
104  result = ramp.update(targetVelocity, timeSinceLastIteration.toSecondsDouble());
105  }
106  else
107  {
108  Eigen::Vector3f x(rtGetControlStruct().velocityX,
109  rtGetControlStruct().velocityY,
110  rtGetControlStruct().velocityRotation);
111  result = ramp.update(x, timeSinceLastIteration.toSecondsDouble());
112  }
113 
114  target->velocityX = result(0);
115  target->velocityY = result(1);
116  target->velocityRotation = result(2);
117  }
118  }
119 
120  void
122  float maxPositionAcceleration,
123  float maxOrientationAcceleration)
124  {
125  ramp.setMaxPositionAcceleration(maxPositionAcceleration);
126  ramp.setMaxOrientationAcceleration(maxOrientationAcceleration);
127  }
128 
131  "NJointHolonomicPlatformVelocityControllerWithRamp");
132 
134  float maxOrientationAcceleration) :
135  maxPositionAcceleration(maxPositionAcceleration),
136  maxOrientationAcceleration(maxOrientationAcceleration)
137  {
138  }
139 
140  void
141  Cartesian2DimVelocityRamp::init(const Eigen::Vector3f& state)
142  {
143  this->state = state;
144  }
145 
146  Eigen::Vector3f
147  Cartesian2DimVelocityRamp::update(const Eigen::Vector3f& target, float dt)
148  {
149  if (dt <= 0)
150  {
151  return state;
152  }
153  Eigen::Vector3f delta = target - state;
154  float factor = 1;
155 
156  Eigen::Vector2f posDelta = delta.block<2, 1>(0, 0);
157  float posFactor = posDelta.norm() / maxPositionAcceleration / dt;
158  factor = std::max(factor, posFactor);
159 
160  float oriFactor = std::abs(delta(2)) / maxOrientationAcceleration / dt;
161  factor = std::max(factor, oriFactor);
162 
163  state += delta / factor;
164  return state;
165  }
166 
167  void
169  {
170  this->maxPositionAcceleration = maxPositionAcceleration;
171  }
172 
173  void
175  {
176  this->maxOrientationAcceleration = maxOrientationAcceleration;
177  }
178 
179 } // namespace armarx
armarx::ControlTargetHolonomicPlatformVelocity
Brief description of class ControlTargetHolonomicPlatformVelocity.
Definition: ControlTargetHolonomicPlatformVelocity.h:38
armarx::ControlTargetBase::asA
const T * asA() const
Definition: ControlTargetBase.h:76
armarx::NJointHolonomicPlatformVelocityControllerWithRamp::rtRun
void rtRun(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration) override
TODO make protected and use attorneys.
Definition: NJointHolonomicPlatformVelocityControllerWithRamp.cpp:70
armarx::SensorValueHolonomicPlatformVelocity::velocityX
DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION float velocityX
Definition: SensorValueHolonomicPlatform.h:33
armarx::NJointControllerRegistration
Definition: NJointControllerRegistry.h:74
armarx::NJointControllerWithTripleBuffer< NJointHolonomicPlatformVelocityControllerControlData >::rtGetControlStruct
const NJointHolonomicPlatformVelocityControllerControlData & rtGetControlStruct() const
Definition: NJointControllerWithTripleBuffer.h:32
ARMARX_RT_LOGF_WARNING
#define ARMARX_RT_LOGF_WARNING(...)
Definition: ControlThreadOutputBuffer.h:348
armarx::SensorValueBase::asA
const T * asA() const
Definition: SensorValueBase.h:82
SensorValueHolonomicPlatform.h
armarx::Cartesian2DimVelocityRamp::update
Eigen::Vector3f update(const Eigen::Vector3f &target, float dt)
Definition: NJointHolonomicPlatformVelocityControllerWithRamp.cpp:147
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::registrationNJointHolonomicPlatformVelocityControllerWithRamp
NJointControllerRegistration< NJointHolonomicPlatformVelocityControllerWithRamp > registrationNJointHolonomicPlatformVelocityControllerWithRamp("NJointHolonomicPlatformVelocityControllerWithRamp")
boost::target
Vertex target(const detail::edge_base< Directed, Vertex > &e, const PCG &)
Definition: point_cloud_graph.h:668
armarx::NJointHolonomicPlatformVelocityControllerWithRamp::NJointHolonomicPlatformVelocityControllerWithRamp
NJointHolonomicPlatformVelocityControllerWithRamp(RobotUnit *prov, ConfigPtrT config, const VirtualRobot::RobotPtr &)
Definition: NJointHolonomicPlatformVelocityControllerWithRamp.cpp:39
armarx::NJointHolonomicPlatformVelocityControllerWithRamp::ramp
Cartesian2DimVelocityRamp ramp
Definition: NJointHolonomicPlatformVelocityControllerWithRamp.h:95
armarx::NJointControllerBase::ConfigPtrT
NJointControllerConfigPtr ConfigPtrT
Definition: NJointControllerBase.h:587
armarx::ControlTargetHolonomicPlatformVelocity::velocityRotation
float velocityRotation
Definition: ControlTargetHolonomicPlatformVelocity.h:43
armarx::SensorValueHolonomicPlatformVelocity::velocityRotation
float velocityRotation
Definition: SensorValueHolonomicPlatform.h:35
armarx::NJointHolonomicPlatformVelocityControllerInterface::maxCommandDelay
IceUtil::Time maxCommandDelay
Definition: NJointHolonomicPlatformVelocityControllerInterface.h:57
armarx::NJointHolonomicPlatformVelocityControllerWithRamp::velocitySensor
const SensorValueHolonomicPlatformVelocity * velocitySensor
Definition: NJointHolonomicPlatformVelocityControllerWithRamp.h:101
SensorValueBase.h
armarx::SensorValueHolonomicPlatformVelocity::velocityY
float velocityY
Definition: SensorValueHolonomicPlatform.h:34
armarx::abs
std::vector< T > abs(const std::vector< T > &v)
Definition: VectorHelpers.h:281
armarx::NJointHolonomicPlatformVelocityControllerWithRamp::setMaxAccelerations
void setMaxAccelerations(float maxPositionAcceleration, float maxOrientationAcceleration)
Definition: NJointHolonomicPlatformVelocityControllerWithRamp.cpp:121
armarx::SensorValueHolonomicPlatformVelocity
Definition: SensorValueHolonomicPlatform.h:30
armarx::Cartesian2DimVelocityRamp::init
void init(const Eigen::Vector3f &state)
Definition: NJointHolonomicPlatformVelocityControllerWithRamp.cpp:141
armarx::NJointHolonomicPlatformVelocityControllerWithRamp::target
ControlTargetHolonomicPlatformVelocity * target
Definition: NJointHolonomicPlatformVelocityControllerWithRamp.h:100
max
T max(T t1, T t2)
Definition: gdiam.h:51
armarx::ControlTargetHolonomicPlatformVelocity::velocityX
float velocityX
Definition: ControlTargetHolonomicPlatformVelocity.h:41
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::ControlTargetHolonomicPlatformVelocity::velocityY
float velocityY
Definition: ControlTargetHolonomicPlatformVelocity.h:42
ExpressionException.h
armarx::NJointHolonomicPlatformVelocityControllerWithRamp::activationTime
IceUtil::Time activationTime
Definition: NJointHolonomicPlatformVelocityControllerWithRamp.h:98
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
NJointHolonomicPlatformVelocityControllerWithRamp.h
armarx::NJointHolonomicPlatformVelocityControllerControlData::commandTimestamp
IceUtil::Time commandTimestamp
Definition: NJointHolonomicPlatformVelocityControllerInterface.h:38
armarx::RobotUnit
The RobotUnit class manages a robot and its controllers.
Definition: RobotUnit.h:180
armarx::Cartesian2DimVelocityRamp::setMaxOrientationAcceleration
void setMaxOrientationAcceleration(float maxOrientationAcceleration)
Definition: NJointHolonomicPlatformVelocityControllerWithRamp.cpp:174
ControlThreadOutputBuffer.h
armarx::Cartesian2DimVelocityRamp::setMaxPositionAcceleration
void setMaxPositionAcceleration(float maxPositionAcceleration)
Definition: NJointHolonomicPlatformVelocityControllerWithRamp.cpp:168
armarx::NJointHolonomicPlatformVelocityControllerWithRamp::rtPreActivateController
void rtPreActivateController() override
This function is called before the controller is activated.
Definition: NJointHolonomicPlatformVelocityControllerWithRamp.cpp:58
NJointControllerRegistry.h
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:27
armarx::Cartesian2DimVelocityRamp::Cartesian2DimVelocityRamp
Cartesian2DimVelocityRamp(float maxPositionAcceleration, float maxOrientationAcceleration)
Definition: NJointHolonomicPlatformVelocityControllerWithRamp.cpp:133
armarx::NJointHolonomicPlatformVelocityControllerWithRamp::activationVelocity
Eigen::Vector3f activationVelocity
Definition: NJointHolonomicPlatformVelocityControllerWithRamp.h:97
dt
constexpr T dt
Definition: UnscentedKalmanFilterTest.cpp:45
armarx::NJointControllerBase::useSensorValue
const SensorValueBase * useSensorValue(const std::string &sensorDeviceName) const
Get a const ptr to the given SensorDevice's SensorValue.
Definition: NJointController.cpp:383
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:19