From 0e87721ac5f7a11ddc78e08d7267c7184ccd5a86 Mon Sep 17 00:00:00 2001
From: Mirko Waechter <mirko.waechter@kit.edu>
Date: Thu, 12 Apr 2018 11:15:40 +0200
Subject: [PATCH] made velocity mode for kinematicunit configurable
---
.../ControlTarget1DoFActuator.h | 14 +--
...ointKinematicUnitPassThroughController.cpp | 18 +---
...NJointKinematicUnitPassThroughController.h | 5 -
.../components/units/RobotUnit/RobotUnit.cpp | 91 +++++++++++--------
.../components/units/RobotUnit/RobotUnit.h | 5 +
5 files changed, 69 insertions(+), 64 deletions(-)
diff --git a/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h
index db15a4bd3..bd87e5658 100644
--- a/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h
+++ b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h
@@ -66,29 +66,29 @@ namespace armarx
make_ControlTarget1DoFActuator(ControlTarget1DoFActuatorTorque, torque, ControlModes::Torque1DoF);
make_ControlTarget1DoFActuator(ControlTarget1DoFActuatorCurrent, current, ControlModes::Current1DoF);
- class VelocityTorqueControlTarget: public ControlTarget1DoFActuatorVelocity
+ class ControlTarget1DoFActuatorTorqueVelocity: public ControlTarget1DoFActuatorVelocity
{
public:
float maxTorque;
const std::string& getControlMode() const override
{
- return ControlModes::Velocity1DoF;
+ return ControlModes::VelocityTorque;
}
void reset() override
{
ControlTarget1DoFActuatorVelocity::reset();
- maxTorque = 10.f;
+ maxTorque = -1.0f; // if < 0, default value for joint is to be used
}
bool isValid() const override
{
- return ControlTarget1DoFActuatorVelocity::isValid() && maxTorque >= 0;
+ return ControlTarget1DoFActuatorVelocity::isValid() && std::isfinite(maxTorque);
}
- static ControlTargetInfo<VelocityTorqueControlTarget> GetClassMemberInfo()
+ static ControlTargetInfo<ControlTarget1DoFActuatorTorqueVelocity> GetClassMemberInfo()
{
- ControlTargetInfo<VelocityTorqueControlTarget> cti;
+ ControlTargetInfo<ControlTarget1DoFActuatorTorqueVelocity> cti;
cti.addBaseClass<ControlTarget1DoFActuatorVelocity>();
- cti.addMemberVariable(&VelocityTorqueControlTarget::maxTorque, "maxTorque");
+ cti.addMemberVariable(&ControlTarget1DoFActuatorTorqueVelocity::maxTorque, "maxTorque");
return cti;
}
DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.cpp
index 02da648c7..f5e9ee2c8 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.cpp
@@ -43,7 +43,7 @@ NJointKinematicUnitPassThroughController::NJointKinematicUnitPassThroughControll
ControlTargetBase* ct = prov->getControlTarget(cfg->deviceName, controlMode);
//get sensor
- if (controlMode == ControlModes::Position1DoF)
+ if (ct->isA<ControlTarget1DoFActuatorPosition>())
{
ARMARX_CHECK_EXPRESSION(sv->asA<SensorValue1DoFActuatorPosition>());
sensor = &(sv->asA<SensorValue1DoFActuatorPosition>()->position);
@@ -51,26 +51,16 @@ NJointKinematicUnitPassThroughController::NJointKinematicUnitPassThroughControll
target = &(ct->asA<ControlTarget1DoFActuatorPosition>()->position);
sensorToControlOnActivateFactor = 1;
}
- else if (controlMode == ControlModes::Velocity1DoF)
+ else if (ct->isA<ControlTarget1DoFActuatorVelocity>())
{
ARMARX_CHECK_EXPRESSION(sv->asA<SensorValue1DoFActuatorVelocity>());
sensor = &(sv->asA<SensorValue1DoFActuatorVelocity>()->velocity);
ARMARX_CHECK_EXPRESSION(ct->asA<ControlTarget1DoFActuatorVelocity>());
target = &(ct->asA<ControlTarget1DoFActuatorVelocity>()->velocity);
sensorToControlOnActivateFactor = 1;
- resetZeroThreshold = 0.1;
+ resetZeroThreshold = 0.1f; // TODO: way to big value?!
}
- else if (controlMode == ControlModes::VelocityTorque)
- {
- ARMARX_CHECK_EXPRESSION(sv->asA<SensorValue1DoFActuatorVelocity>());
- sensor = &(sv->asA<SensorValue1DoFActuatorVelocity>()->velocity);
- ARMARX_CHECK_EXPRESSION(ct->asA<VelocityTorqueControlTarget>());
- target = &(ct->asA<VelocityTorqueControlTarget>()->velocity);
- target2 = &(ct->asA<VelocityTorqueControlTarget>()->maxTorque);
- sensorToControlOnActivateFactor = 1;
- resetZeroThreshold = 0.1;
- }
- else if (controlMode == ControlModes::Torque1DoF)
+ else if (ct->isA<ControlTarget1DoFActuatorTorque>())
{
ARMARX_CHECK_EXPRESSION(sv->asA<SensorValue1DoFActuatorTorque>());
sensor = &(sv->asA<SensorValue1DoFActuatorTorque>()->torque);
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h
index 3a651d7f8..bea46eacc 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h
@@ -61,10 +61,6 @@ namespace armarx
inline void rtRun(const IceUtil::Time&, const IceUtil::Time&) override
{
*target = control;
- if (target2)
- {
- *target2 = 8;
- }
}
inline void rtPreActivateController() override
{
@@ -94,7 +90,6 @@ namespace armarx
protected:
std::atomic<float> control {0};
float* target {nullptr};
- float* target2 {nullptr};
const float* sensor
{
nullptr
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
index 5791b8bcf..f8fd2a54d 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
@@ -385,41 +385,13 @@ namespace armarx
ARMARX_INFO << "finishDeviceInitialization...done! " << (MicroNow() - beg).count() << " us";
}
- void armarx::RobotUnit::initializeDefaultUnits()
- {
- auto beg = MicroNow();
- {
- auto guard = getGuard();
- throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
- ARMARX_INFO << "initializing default units";
- initializeKinematicUnit();
- ARMARX_DEBUG << "KinematicUnit initialized";
- initializePlatformUnit();
- ARMARX_DEBUG << "PlatformUnit initialized";
- initializeForceTorqueUnit();
- ARMARX_DEBUG << "ForceTorqueUnit initialized";
- initializeInertialMeasurementUnit();
- ARMARX_DEBUG << "InertialMeasurementUnit initialized";
- initializeTrajectoryControllerUnit();
- ARMARX_DEBUG << "TrajectoryControllerUnit initialized";
- initializeTcpControllerUnit();
- ARMARX_DEBUG << "TcpControllerUnit initialized";
- }
- ARMARX_INFO << "initializing default units...done! " << (MicroNow() - beg).count() << " us";
- }
-
- void armarx::RobotUnit::initializeKinematicUnit()
+ ManagedIceObjectPtr RobotUnit::createKinematicSubUnit(const Ice::PropertiesPtr& properties, const std::string& positionControlMode,
+ const std::string& velocityControlMode, const std::string& torqueControlMode)
{
using UnitT = KinematicSubUnit;
- using IfaceT = KinematicUnitInterface;
- auto guard = getGuard();
- throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
- //check if unit is already added
- if (getUnit(IfaceT::ice_staticId()))
- {
- return;
- }
+
+
//add ctrl et al
std::map<std::string, UnitT::ActuatorData> devs;
for (const ControlDevicePtr& controlDevice : controlDevices.values())
@@ -454,9 +426,9 @@ namespace armarx
ARMARX_CHECK_EXPRESSION(pt); \
} \
}
- initializeKinematicUnit_tmp_helper(ControlModes::Position1DoF, ControlTarget1DoFActuatorPosition, ad.ctrlPos)
- initializeKinematicUnit_tmp_helper(ControlModes::Velocity1DoF, ControlTarget1DoFActuatorVelocity, ad.ctrlVel)
- initializeKinematicUnit_tmp_helper(ControlModes::Torque1DoF, ControlTarget1DoFActuatorTorque, ad.ctrlTor)
+ initializeKinematicUnit_tmp_helper(positionControlMode, ControlTarget1DoFActuatorPosition, ad.ctrlPos)
+ initializeKinematicUnit_tmp_helper(velocityControlMode, ControlTarget1DoFActuatorVelocity, ad.ctrlVel)
+ initializeKinematicUnit_tmp_helper(torqueControlMode, ControlTarget1DoFActuatorTorque, ad.ctrlTor)
#undef initializeKinematicUnit_tmp_helper
@@ -469,13 +441,13 @@ namespace armarx
if (devs.empty())
{
ARMARX_IMPORTANT << "No joint devices found - skipping adding of KinematicUnit";
- return;
+ return NULL;
}
ARMARX_CHECK_EXPRESSION_W_HINT(!devs.empty(), "no 1DoF ControlDevice found with matching SensorDevice");
//add it
+
const std::string configName = getProperty<std::string>("KinematicUnitName");
const std::string confPre = getConfigDomain() + "." + configName + ".";
- Ice::PropertiesPtr properties = getIceProperties()->clone();
//properties->setProperty(confPre + "MinimumLoggingLevel", getProperty<std::string>("MinimumLoggingLevel").getValue());
//fill properties
properties->setProperty(confPre + "RobotNodeSetName", robotNodeSetName);
@@ -493,8 +465,50 @@ namespace armarx
ctrlModeGroups.groupsMerged,
this
);
+ return unit;
+ }
+
+ void armarx::RobotUnit::initializeDefaultUnits()
+ {
+ auto beg = MicroNow();
+ {
+ auto guard = getGuard();
+ throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
+ ARMARX_INFO << "initializing default units";
+ initializeKinematicUnit();
+ ARMARX_DEBUG << "KinematicUnit initialized";
+ initializePlatformUnit();
+ ARMARX_DEBUG << "PlatformUnit initialized";
+ initializeForceTorqueUnit();
+ ARMARX_DEBUG << "ForceTorqueUnit initialized";
+ initializeInertialMeasurementUnit();
+ ARMARX_DEBUG << "InertialMeasurementUnit initialized";
+ initializeTrajectoryControllerUnit();
+ ARMARX_DEBUG << "TrajectoryControllerUnit initialized";
+ initializeTcpControllerUnit();
+ ARMARX_DEBUG << "TcpControllerUnit initialized";
+ }
+ ARMARX_INFO << "initializing default units...done! " << (MicroNow() - beg).count() << " us";
+ }
+
+ void armarx::RobotUnit::initializeKinematicUnit()
+ {
+ using IfaceT = KinematicUnitInterface;
+
+ auto guard = getGuard();
+ throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
+ //check if unit is already added
+ if (getUnit(IfaceT::ice_staticId()))
+ {
+ return;
+ }
+
+ auto unit = createKinematicSubUnit(getIceProperties()->clone());
//add
- addUnit(std::move(unit));
+ if (unit)
+ {
+ addUnit(std::move(unit));
+ }
}
void armarx::RobotUnit::initializePlatformUnit()
@@ -690,6 +704,7 @@ namespace armarx
void armarx::RobotUnit::addUnit(ManagedIceObjectPtr unit)
{
+ ARMARX_CHECK_EXPRESSION(unit);
auto guard = getGuard();
throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
getArmarXManager()->addObjectAsync(unit, "", true, false);
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnit.h b/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
index 3bdaee759..706aaced3 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
@@ -690,6 +690,11 @@ namespace armarx
// State: InitializingUnits
// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
protected:
+ ManagedIceObjectPtr createKinematicSubUnit(const Ice::PropertiesPtr& properties, const std::string& positionControlMode = ControlModes::Position1DoF,
+ const std::string& velocityControlMode = ControlModes::Velocity1DoF, const std::string& torqueControlMode = ControlModes::Torque1DoF
+ );
+
+
virtual void initializeDefaultUnits();
virtual void initializeKinematicUnit();
virtual void initializePlatformUnit();
--
GitLab