From f65cc9d072f82d51f35505fd9159a70d95b2d76c Mon Sep 17 00:00:00 2001
From: Raphael Grimm <raphael.grimm@kit.edu>
Date: Thu, 3 May 2018 07:31:19 +0200
Subject: [PATCH] Apply changes from 0e87721ac5f7a11ddc78e08d7267c7184ccd5a86
("made velocity mode for kinematicunit configurable")
---
.../RobotUnitModules/RobotUnitModuleUnits.cpp | 34 ++++++++++++++-----
.../RobotUnitModules/RobotUnitModuleUnits.h | 7 ++++
2 files changed, 32 insertions(+), 9 deletions(-)
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
index 8dd4ea34b..d2ac94760 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
@@ -151,10 +151,9 @@ namespace armarx
ARMARX_INFO << "initializing default units...done! " << (MicroNow() - beg).count() << " us";
}
+
void Units::initializeKinematicUnit()
{
- /// TODO move init code to Kinematic sub unit
- using UnitT = KinematicSubUnit;
using IfaceT = KinematicUnitInterface;
auto guard = getGuard();
@@ -164,6 +163,24 @@ namespace armarx
{
return;
}
+ auto unit = createKinematicSubUnit(getIceProperties()->clone());
+ //add
+ if (unit)
+ {
+ addUnit(std::move(unit));
+ }
+ }
+
+ ManagedIceObjectPtr Units::createKinematicSubUnit(
+ const Ice::PropertiesPtr& properties,
+ const std::string& positionControlMode,
+ const std::string& velocityControlMode,
+ const std::string& torqueControlMode
+ )
+ {
+ /// TODO move init code to Kinematic sub unit
+ using UnitT = KinematicSubUnit;
+
//add ctrl et al
std::map<std::string, UnitT::ActuatorData> devs;
for (const ControlDevicePtr& controlDevice : _module<Devices>().getControlDevices().values())
@@ -198,9 +215,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
@@ -213,13 +230,12 @@ namespace armarx
if (devs.empty())
{
ARMARX_IMPORTANT << "No joint devices found - skipping adding of KinematicUnit";
- return;
+ return nullptr;
}
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", _module<RobotData>().getRobotNodetSeName());
@@ -237,8 +253,7 @@ namespace armarx
_module<Devices>().getControlModeGroups().groupsMerged,
dynamic_cast<RobotUnit*>(this)
);
- //add
- addUnit(std::move(unit));
+ return unit;
}
void Units::initializePlatformUnit()
@@ -437,6 +452,7 @@ namespace armarx
void Units::addUnit(ManagedIceObjectPtr unit)
{
+ ARMARX_CHECK_NOT_NULL(unit);
auto guard = getGuard();
throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
getArmarXManager()->addObjectAsync(unit, "", true, false);
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h
index f6667acfd..1f75b820f 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h
@@ -248,6 +248,13 @@ namespace armarx
virtual void initializeTrajectoryControllerUnit();
/// @brief Initializes the TcpControllerUnit
virtual void initializeTcpControllerUnit();
+ /// @brief Create the \ref KinematicUnit (this function should be called in \ref initializeKinematicUnit)
+ ManagedIceObjectPtr createKinematicSubUnit(
+ const Ice::PropertiesPtr& properties,
+ const std::string& positionControlMode = ControlModes::Position1DoF,
+ const std::string& velocityControlMode = ControlModes::Velocity1DoF,
+ const std::string& torqueControlMode = ControlModes::Torque1DoF
+ );
/**
* @brief Registers a unit for management (the Unit is added and removed to the \ref ArmarXManager
* and updated with new values.
--
GitLab