Go to the documentation of this file.
25 #include <VirtualRobot/Robot.h>
34 #include "../ControlTargets/ControlTargetHolonomicPlatformVelocity.h"
42 ramp(cfg->maxPositionAcceleration, cfg->maxOrientationAcceleration)
47 <<
"The actuator " << cfg->platformName <<
" has no control mode "
48 << ControlModes::HolonomicPlatformVelocity;
54 <<
"Sensor value for " << cfg->platformName <<
" has invalid type";
82 "delay: %f, s, max allowed delay: %f s",
83 commandAge.toSecondsDouble(),
90 const Eigen::Vector3f targetVelocity = Eigen::Vector3f::Zero();
91 Eigen::Vector3f result =
92 ramp.
update(targetVelocity, timeSinceLastIteration.toSecondsDouble());
99 Eigen::Vector3f result;
103 const Eigen::Vector3f targetVelocity = Eigen::Vector3f::Zero();
104 result =
ramp.
update(targetVelocity, timeSinceLastIteration.toSecondsDouble());
111 result =
ramp.
update(
x, timeSinceLastIteration.toSecondsDouble());
122 float maxPositionAcceleration,
123 float maxOrientationAcceleration)
131 "NJointHolonomicPlatformVelocityControllerWithRamp");
134 float maxOrientationAcceleration) :
135 maxPositionAcceleration(maxPositionAcceleration),
136 maxOrientationAcceleration(maxOrientationAcceleration)
153 Eigen::Vector3f delta =
target - state;
156 Eigen::Vector2f posDelta = delta.block<2, 1>(0, 0);
157 float posFactor = posDelta.norm() / maxPositionAcceleration /
dt;
158 factor =
std::max(factor, posFactor);
160 float oriFactor =
std::abs(delta(2)) / maxOrientationAcceleration /
dt;
161 factor =
std::max(factor, oriFactor);
163 state += delta / factor;
170 this->maxPositionAcceleration = maxPositionAcceleration;
176 this->maxOrientationAcceleration = maxOrientationAcceleration;
const NJointHolonomicPlatformVelocityControllerControlData & rtGetControlStruct() const
#define ARMARX_RT_LOGF_WARNING(...)
Eigen::Vector3f update(const Eigen::Vector3f &target, float dt)
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...
NJointControllerRegistration< NJointHolonomicPlatformVelocityControllerWithRamp > registrationNJointHolonomicPlatformVelocityControllerWithRamp("NJointHolonomicPlatformVelocityControllerWithRamp")
Vertex target(const detail::edge_base< Directed, Vertex > &e, const PCG &)
NJointControllerConfigPtr ConfigPtrT
std::vector< T > abs(const std::vector< T > &v)
void init(const Eigen::Vector3f &state)
armarx::core::time::DateTime Time
#define ARMARX_CHECK_EXPRESSION(expression)
This macro evaluates the expression and if it turns out to be false it will throw an ExpressionExcept...
The RobotUnit class manages a robot and its controllers.
void setMaxOrientationAcceleration(float maxOrientationAcceleration)
void setMaxPositionAcceleration(float maxPositionAcceleration)
This file offers overloads of toIce() and fromIce() functions for STL container types.
Cartesian2DimVelocityRamp(float maxPositionAcceleration, float maxOrientationAcceleration)
const SensorValueBase * useSensorValue(const std::string &sensorDeviceName) const
Get a const ptr to the given SensorDevice's SensorValue.
std::shared_ptr< class Robot > RobotPtr