Go to the documentation of this file.
25 #include <VirtualRobot/MathTools.h>
31 #include <armarx/control/common/control_law/aron/TaskspaceAdmittanceControllerConfig.aron.generated.h>
39 "NJointTaskspaceAdmittanceController");
42 const RobotUnitPtr& robotUnit,
43 const NJointControllerConfigPtr& config,
47 ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
54 VirtualRobot::RobotNodeSetPtr rtRns =
rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
57 VirtualRobot::RobotNodeSetPtr nonRtRns =
nonRtRobot->getRobotNodeSet(cfg->nodeSetName);
65 for (
size_t i = 0; i < rtRns->getSize(); ++i)
67 std::string jointName = rtRns->getNode(i)->getName();
73 auto* casted_ct = ct->
asA<ControlTarget1DoFActuatorTorque>();
77 const auto* torqueSensor = sv->
asA<SensorValue1DoFActuatorTorque>();
78 const auto* velocitySensor = sv->
asA<SensorValue1DoFActuatorVelocity>();
79 const auto* positionSensor = sv->
asA<SensorValue1DoFActuatorPosition>();
81 if (torqueSensor ==
nullptr)
85 if (velocitySensor ==
nullptr)
87 ARMARX_WARNING <<
"No velocity sensor available for " << jointName;
89 if (positionSensor ==
nullptr)
91 ARMARX_WARNING <<
"No position sensor available for " << jointName;
100 auto configData = ::armarx::fromAron<AronDTO, BO>(cfg->config);
115 return "NJointTaskspaceAdmittanceController";
126 runTask(
"NJointTaskspaceAdmittanceAdditionalTask",
132 <<
"Create a new thread alone NJointTaskspaceAdmittanceController";
133 while (
getState() == eManagedIceObjectStarted)
139 c.waitForCycleDuration();
159 double deltaT = timeSinceLastIteration.toSecondsDouble();
169 controller.updateFT(userConfig.ftConfig, deltaT);
174 for (
size_t i = 0; i < nDoF; ++i)
197 const Ice::Current& iceCurrent)
199 auto configData = ::armarx::fromAron<AronDTO, BO>(dto);
216 const auto checkNonNegative = [](
const auto&
v) {
ARMARX_CHECK((
v.array() >= 0).all()); };
220 ARMARX_INFO <<
"No user defined nullspace target, reset to zero with " <<
VAROUT(nDoF);
221 configData.desiredNullspaceJointAngles = Eigen::VectorXf::Zero(nDoF);
224 checkSize(configData.desiredNullspaceJointAngles.value());
225 checkSize(configData.kdNullspace);
226 checkSize(configData.kpNullspace);
228 checkNonNegative(configData.kdNullspace);
229 checkNonNegative(configData.kpNullspace);
230 checkNonNegative(configData.kdImpedance);
231 checkNonNegative(configData.kpImpedance);
241 auto nonRtData =
controller.bufferNonRtToOnPublish.getUpToDateReadBuffer();
242 auto rtData =
controller.bufferRtToOnPublish.getUpToDateReadBuffer();
259 debugObs->setDebugChannel(
"NJointTaskspaceAdmittanceController", datafields);
280 ARMARX_IMPORTANT <<
"rt preactivate controller with target pose\n\n" << currentPose;
285 c.desiredNullspaceJointAngles.value() = rns->getJointValuesEigen();
286 c.desiredPose = currentPose;
294 const auto nDoF = rns->getSize();
301 for (
size_t i = 0; i < nDoF; ++i)
common::control_law::ControlTarget controlTargets
std::atomic_bool reInitPreActivate
TripleBuffer< law::RobotStatus > bufferRtToAdditionalTask
void debugEigenPose(StringVariantBaseMap &datafields, const std::string &name, Eigen::Matrix4f pose)
std::vector< float > targets
void toggleGravityCompensation(const bool toggle, const Ice::Current &) override
ft sensor
const VirtualRobot::RobotPtr & useSynchronizedRtRobot(bool updateCollisionModel=false)
Requests a VirtualRobot for use in rtRun *.
std::map< std::string, VariantBasePtr > StringVariantBaseMap
The SensorValueBase class.
TripleBuffer< BO > bufferUserToRt
std::vector< const SensorValue1DoFActuatorVelocity * > velocitySensors
void rtPostDeactivateController() override
This function is called after the controller is deactivated.
Brief description of class JointControlTargetBase.
std::vector< const SensorValue1DoFActuatorTorque * > torqueSensors
std::vector< float > jointTorque
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...
void onPublish(const SensorAndControl &, const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &) override
This util class helps with keeping a cycle time during a control cycle.
int getState() const
Retrieve current state of the ManagedIceObject.
VirtualRobot::RobotPtr nonRtRobot
NJointControllerConfigPtr ConfigPtrT
std::vector< float > jointVelocity
void rtPreActivateController() override
This function is called before the controller is activated.
NJointControllerRegistration< NJointTaskspaceAdmittanceController > registrationControllerNJointTaskspaceAdmittanceController("NJointTaskspaceAdmittanceController")
void rtRun(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration) override
TODO make protected and use attorneys.
#define ARMARX_CHECK(expression)
Shortcut for ARMARX_CHECK_EXPRESSION.
std::vector< ControlTarget1DoFActuatorTorque * > targets
const VirtualRobot::RobotPtr & rtGetRobot()
TODO make protected and use attorneys.
virtual void additionalTask()
std::string getClassName(const Ice::Current &) const override
law::TaskspaceAdmittanceController::Config BO
bool isControllerActive(const Ice::Current &=Ice::emptyCurrent) const final override
void runTask(const std::string &taskName, Task &&task)
Executes a given task in a separate thread from the Application ThreadPool.
TripleBuffer< BO > bufferUserToAdditionalTask
set buffers
armarx::core::time::DateTime Time
std::vector< float > jointPosition
void debugEigenVec(StringVariantBaseMap &datafields, const std::string &name, Eigen::VectorXf vec)
std::shared_ptr< Dict > DictPtr
double v(double t, double v0, double a0, double j)
MatrixXX< 4, 4, float > Matrix4f
#define ARMARX_CHECK_EXPRESSION(expression)
This macro evaluates the expression and if it turns out to be false it will throw an ExpressionExcept...
void validateConfigData(BO &config)
SensorDevices sensorDevices
void updateConfig(const ::armarx::aron::data::dto::DictPtr &dto, const Ice::Current &iceCurrent=Ice::emptyCurrent) override
NJointController interface.
std::string kinematicChainName
variables
const T & getUpToDateReadBuffer() const
#define ARMARX_CHECK_EQUAL(lhs, rhs)
This macro evaluates whether lhs is equal (==) rhs and if it turns out to be false it will throw an E...
std::atomic_bool rtFirstRun
std::vector< const SensorValue1DoFActuatorPosition * > positionSensors
void onInitNJointController() override
NJointControllerBase interface.
std::vector< std::string > jointNames
std::enable_if< std::is_copy_constructible< U >::value >::type reinitAllBuffers(const T &init)
double s(double t, double s0, double v0, double a0, double j)
NJointTaskspaceAdmittanceController(const RobotUnitPtr &robotUnit, const NJointControllerConfigPtr &config, const VirtualRobot::RobotPtr &)
ArmarXObjectSchedulerPtr getObjectScheduler() const
const SensorValueBase * useSensorValue(const std::string &sensorDeviceName) const
Get a const ptr to the given SensorDevice's SensorValue.
std::shared_ptr< class Robot > RobotPtr