diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5ae7be96ac49e4e245d16d978a2bdace25c0d911..6a8913998ec10b4d98a2536c4aca3dc3eb06efc5 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -20,7 +20,7 @@ set(ARMARX_ENABLE_AUTO_CODE_FORMATTING TRUE)
armarx_project("RobotAPI")
depends_on_armarx_package(ArmarXGui)
-set(ArmarX_Simox_VERSION 2.3.59)
+set(ArmarX_Simox_VERSION 2.3.64)
option(REQUIRE_SIMOX "If enabled the Simox dependency is a required dependency" TRUE)
if(REQUIRE_SIMOX)
diff --git a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
index f2d1ec328e91bd315d5f74bf0ab8879b85e112c0..d14b838fae5204870608e981e451eedc17f324f3 100644
--- a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
+++ b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
@@ -75,6 +75,7 @@ set(LIB_HEADERS
util/introspection/ClassMemberInfoEntry.h
util/introspection/ClassMemberInfo.h
util/RtLogging.h
+ util/RtTiming.h
#robot unit modules need to be added to the list below (but not here)
RobotUnitModules/RobotUnitModuleBase.h
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.cpp
index 7d2264e65314ffe5023cc98d9855936596d268eb..ed5405e802c16addc84980dd510345774ed63a98 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.cpp
@@ -312,6 +312,13 @@ namespace armarx
writeControlStruct();
}
+ void NJointCartesianVelocityController::setAvoidJointLimitsKp(float kp)
+ {
+ LockGuardType guard {controlDataMutex};
+ getWriterControlStruct().avoidJointLimitsKp = kp;
+ writeControlStruct();
+ }
+
std::string NJointCartesianVelocityController::getNodeSetName() const
{
return nodeSetName;
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h
index 7c792c309119b66a07e9c13983e75c3e177eac4d..995d84fe733411d2f101f1968325f814b8a595cb 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h
@@ -108,6 +108,7 @@ namespace armarx
// for TCPControlUnit
void setVelocities(float xVel, float yVel, float zVel, float rollVel, float pitchVel, float yawVel, VirtualRobot::IKSolver::CartesianSelection mode);
+ void setAvoidJointLimitsKp(float kp);
std::string getNodeSetName() const;
static ::armarx::WidgetDescription::WidgetSeq createSliders();
WidgetDescription::HBoxLayoutPtr createJointSlidersLayout(float min, float max, float defaultValue) const;
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
index f69ae7ff8721f315772428ace9ff89a3447e1abe..d02cbfc4e7720d21f1798c2896e5d43adaecd839 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
@@ -59,10 +59,12 @@ namespace armarx
ARMARX_CHECK_EXPRESSION(filteredVelocitySensor || velocitySensor);
if (filteredVelocitySensor)
{
+ ARMARX_IMPORTANT << "Using filtered velocity for joint " << jointName;
velocitySensors.push_back(&filteredVelocitySensor->filteredvelocity);
}
else if (velocitySensor)
{
+ ARMARX_IMPORTANT << "Using raw velocity for joint " << jointName;
velocitySensors.push_back(&velocitySensor->velocity);
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
index e641b108bf9f034d7e37a7b0e0a007e8d1d7ce9a..227e603433e7ee4e1b9ab193dfd208050294a2e9 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
@@ -275,6 +275,7 @@ namespace armarx
{
if (!rtGetActivatedJointControllers().at(i)->rtIsTargetVaild())
{
+ ARMARX_ERROR << "INVALID TARGET: " << rtGetControlDevices().at(i)->getDeviceName();
rtDeactivateAssignedNJointControllerBecauseOfError(i, false);
oneIsInvalid = true;
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp
index af602a752cb92a8c306585598287a7c06868d9c7..cae190c728f98344cac595f9ef79f9f5483ee652 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp
@@ -131,7 +131,7 @@ namespace armarx
void ControllerManagement::deleteNJointController(const NJointControllerPtr& ctrl)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- deleteNJointControllers({ctrl});
+ deleteNJointControllers(std::vector<NJointControllerPtr> {ctrl});
}
StringNJointControllerPrxDictionary ControllerManagement::getAllNJointControllers(const Ice::Current&) const
@@ -289,7 +289,7 @@ namespace armarx
void ControllerManagement::activateNJointController(const NJointControllerPtr& ctrl)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- activateNJointControllers({ctrl});
+ activateNJointControllers(std::vector<NJointControllerPtr> {ctrl});
}
void ControllerManagement::activateNJointControllers(const std::vector<NJointControllerPtr>& ctrlsToActVec)
{
@@ -388,7 +388,7 @@ namespace armarx
void ControllerManagement::deactivateNJointController(const NJointControllerPtr& ctrl)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- deactivateNJointControllers({ctrl});
+ deactivateNJointControllers(std::vector<NJointControllerPtr> {ctrl});
}
void ControllerManagement::deactivateNJointControllers(const std::vector<NJointControllerPtr>& ctrlsDeacVec)
{
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.h
index 08d197d027d251d6df530cf2ba1d44f8c3367d5f..6530e53c19e35a6b9258eb8b12063ddae6ecc8a7 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.h
@@ -69,13 +69,13 @@ namespace armarx
* @return A proxy to the \ref NJointController.
* @see getAllNJointControllers
*/
- NJointControllerInterfacePrx getNJointController(const std::string& name, const Ice::Current&) const override;
+ NJointControllerInterfacePrx getNJointController(const std::string& name, const Ice::Current& = GlobalIceCurrent) const override;
/**
* @brief Returns proxies to all \ref NJointController "NJointControllers"
* @return Proxies to all \ref NJointController "NJointControllers"
* @see getNJointController
*/
- StringNJointControllerPrxDictionary getAllNJointControllers(const Ice::Current&) const override;
+ StringNJointControllerPrxDictionary getAllNJointControllers(const Ice::Current& = GlobalIceCurrent) const override;
/**
* @brief Returns the status of the \ref NJointController.
@@ -95,7 +95,7 @@ namespace armarx
* @see getNJointControllerDescriptionWithStatus
* @see getNJointControllerDescriptionsWithStatuses
*/
- NJointControllerStatusSeq getNJointControllerStatuses(const Ice::Current&) const override;
+ NJointControllerStatusSeq getNJointControllerStatuses(const Ice::Current& = GlobalIceCurrent) const override;
/**
* @brief Returns the description of the \ref NJointController.
@@ -116,7 +116,7 @@ namespace armarx
* @see getNJointControllerDescriptionsWithStatuses
*/
- NJointControllerDescriptionSeq getNJointControllerDescriptions(const Ice::Current&) const override;
+ NJointControllerDescriptionSeq getNJointControllerDescriptions(const Ice::Current& = GlobalIceCurrent) const override;
/**
* @brief Returns the status and description of the \ref NJointController.
@@ -143,7 +143,7 @@ namespace armarx
* @see getNJointControllerDescription
* @see getNJointControllerDescriptions
*/
- NJointControllerDescriptionWithStatusSeq getNJointControllerDescriptionsWithStatuses(const Ice::Current&) const override;
+ NJointControllerDescriptionWithStatusSeq getNJointControllerDescriptionsWithStatuses(const Ice::Current& = GlobalIceCurrent) const override;
/**
* @brief getNJointControllerClassDescription
@@ -156,7 +156,7 @@ namespace armarx
* @brief getNJointControllerClassDescriptions
* @return
*/
- NJointControllerClassDescriptionSeq getNJointControllerClassDescriptions(const Ice::Current&) const override;
+ NJointControllerClassDescriptionSeq getNJointControllerClassDescriptions(const Ice::Current& = GlobalIceCurrent) const override;
/**
* @brief Loads the given lib. (calls `getArmarXManager()->loadLibFromPath(path)`)
* @param path Path to the lib to load.
@@ -200,7 +200,7 @@ namespace armarx
* @see nJointControllersToBeDeleted
* @see deleteNJointControllers
*/
- void deleteNJointController(const std::string& name, const Ice::Current&) override;
+ void deleteNJointController(const std::string& name, const Ice::Current& = GlobalIceCurrent) override;
/**
* @brief Queues the given \ref NJointController "NJointControllers" for deletion.
* @param names The \ref NJointController "NJointControllers" to delete.
@@ -208,7 +208,7 @@ namespace armarx
* @see nJointControllersToBeDeleted
* @see deleteNJointController
*/
- void deleteNJointControllers(const Ice::StringSeq& names, const Ice::Current&) override;
+ void deleteNJointControllers(const Ice::StringSeq& names, const Ice::Current& = GlobalIceCurrent) override;
/**
* @brief Queues the given \ref NJointController for deletion and deactivates it if necessary.
* @param name The \ref NJointController to delete.
@@ -218,7 +218,7 @@ namespace armarx
* @see deleteNJointControllers
* @see deactivateAnddeleteNJointControllers
*/
- void deactivateAndDeleteNJointController(const std::string& name, const Ice::Current&) override;
+ void deactivateAndDeleteNJointController(const std::string& name, const Ice::Current& = GlobalIceCurrent) override;
/**
* @brief Queues the given \ref NJointController "NJointControllers" for deletion and deactivates them if necessary.
* @param names The \ref NJointController "NJointControllers" to delete.
@@ -235,26 +235,26 @@ namespace armarx
* @param name The requested \ref NJointController.
* @see activateNJointControllers
*/
- void activateNJointController(const std::string& name, const Ice::Current&) override;
+ void activateNJointController(const std::string& name, const Ice::Current& = GlobalIceCurrent) override;
/**
* @brief Requests activation for the given \ref NJointController "NJointControllers".
* @param names The requested \ref NJointController "NJointControllers".
* @see activateNJointController
*/
- void activateNJointControllers(const Ice::StringSeq& names, const Ice::Current&) override;
+ void activateNJointControllers(const Ice::StringSeq& names, const Ice::Current& = GlobalIceCurrent) override;
/**
* @brief Requests deactivation for the given \ref NJointController.
* @param name The \ref NJointController to be deactivated.
* @see deactivateNJointControllers
*/
- void deactivateNJointController(const std::string& name, const Ice::Current&) override;
+ void deactivateNJointController(const std::string& name, const Ice::Current& = GlobalIceCurrent) override;
/**
* @brief Requests deactivation for the given \ref NJointController "NJointControllers".
* @param names The \ref NJointController "NJointControllers" to be deactivated.
* @see deactivateNJointController
*/
- void deactivateNJointControllers(const Ice::StringSeq& names, const Ice::Current&) override;
+ void deactivateNJointControllers(const Ice::StringSeq& names, const Ice::Current& = GlobalIceCurrent) override;
/**
* @brief Cretes a \ref NJointController.
@@ -265,7 +265,7 @@ namespace armarx
*/
NJointControllerInterfacePrx createNJointController(
const std::string& className, const std::string& instanceName,
- const NJointControllerConfigPtr& config, const Ice::Current&) override;
+ const NJointControllerConfigPtr& config, const Ice::Current& = GlobalIceCurrent) override;
/**
* @brief Cretes a \ref NJointController.
* @param className The \ref NJointController's class.
@@ -275,7 +275,7 @@ namespace armarx
*/
NJointControllerInterfacePrx createNJointControllerFromVariantConfig(
const std::string& className, const std::string& instanceName,
- const StringVariantBaseMap& variants, const Ice::Current&) override;
+ const StringVariantBaseMap& variants, const Ice::Current& = GlobalIceCurrent) override;
/**
* @brief Changes the set of requested \ref NJointController "NJointControllers" to the given set.
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
index 992fc75721a477248d2c47496e8ea27e3ce3185b..3d71420100273fb9843222dbadc07a3ed154a9f9 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
@@ -444,7 +444,7 @@ namespace armarx
auto guard = getGuard();
throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
- if (!getProperty<bool>("CreateTrajectoryPlayer").getValue())
+ if (!getProperty<bool>("CreateTCPControlUnit").getValue())
{
return;
}
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp
index 572a2e77c8433f3d20fbe7a5c8bde1dde71260d3..7ac788f054c7e51a56f1ffa8a030231bf582a311 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp
@@ -24,6 +24,8 @@
#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h>
+
using namespace armarx;
void TCPControllerSubUnit::update(const SensorAndControl& sc, const JointAndNJointControllers& c)
@@ -31,6 +33,11 @@ void TCPControllerSubUnit::update(const SensorAndControl& sc, const JointAndNJoi
}
+PropertyDefinitionsPtr TCPControllerSubUnit::createPropertyDefinitions()
+{
+ return PropertyDefinitionsPtr(new TCPControllerSubUnitPropertyDefinitions(getConfigIdentifier()));
+}
+
void TCPControllerSubUnit::setup(RobotUnit* rUnit, VirtualRobot::RobotPtr robot)
{
ARMARX_CHECK_EXPRESSION(robot);
@@ -62,11 +69,11 @@ void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const
}
auto activeNJointControllers = robotUnit->getNJointControllersNotNull(robotUnit->getNJointControllerNames());
- NJointTCPControllerPtr tcpController;
+ NJointCartesianVelocityControllerPtr tcpController;
bool nodeSetAlreadyControlled = false;
for (NJointControllerPtr controller : activeNJointControllers)
{
- tcpController = NJointTCPControllerPtr::dynamicCast(controller);
+ tcpController = NJointCartesianVelocityControllerPtr::dynamicCast(controller);
if (!tcpController)
{
continue;
@@ -77,19 +84,6 @@ void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const
break;
}
}
-
- if (!nodeSetAlreadyControlled)
- {
- NJointTCPControllerConfigPtr config = new NJointTCPControllerConfig(nodeSetName, tcp);
- NJointTCPControllerPtr ctrl = NJointTCPControllerPtr::dynamicCast(
- robotUnit->createNJointController(
- "NJointTCPController", this->getName() + "_" + tcp + "_" + nodeSetName,
- config, true, true
- )
- );
- tcpController = ctrl;
- }
-
robotUnit->updateVirtualRobot(coordinateTransformationRobot);
float xVel = 0.f;
@@ -98,7 +92,7 @@ void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const
float rollVel = 0.f;
float pitchVel = 0.f;
float yawVel = 0.f;
- VirtualRobot::IKSolver::CartesianSelection mode = VirtualRobot::IKSolver::All;
+
FramedDirectionPtr globalTransVel = FramedDirectionPtr::dynamicCast(translationVelocity);
if (globalTransVel)
@@ -119,39 +113,90 @@ void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const
}
+ NJointCartesianVelocityControllerMode::CartesianSelection mode = NJointCartesianVelocityControllerMode::eAll;
bool noMode = false;
if (globalTransVel && globalOriVel)
{
- mode = VirtualRobot::IKSolver::All;
+ mode = NJointCartesianVelocityControllerMode::eAll;
}
else if (globalTransVel && !globalOriVel)
{
- mode = VirtualRobot::IKSolver::Position;
+ mode = NJointCartesianVelocityControllerMode::ePosition;
}
else if (!globalTransVel && globalOriVel)
{
- mode = VirtualRobot::IKSolver::Orientation;
+ mode = NJointCartesianVelocityControllerMode::eOrientation;
}
else
{
noMode = true;
}
+ ARMARX_DEBUG << "CartesianSelection-Mode: " << (int)mode;
+
+ auto controllerName = this->getName() + "_" + tcp + "_" + nodeSetName;
+ if (!nodeSetAlreadyControlled)
+ {
+ NJointCartesianVelocityControllerConfigPtr config = new NJointCartesianVelocityControllerConfig(nodeSetName, tcp, mode);
+ NJointCartesianVelocityControllerPtr ctrl = NJointCartesianVelocityControllerPtr::dynamicCast(
+ robotUnit->createNJointController(
+ "NJointCartesianVelocityController", controllerName,
+ config, true, true
+ )
+ );
+
+ tcpController = ctrl;
+ tcpController->setAvoidJointLimitsKp(getProperty<float>("AvoidJointLimitsKp").getValue());
+ }
+
- ARMARX_DEBUG << "CartesianSelection-Mode: " << mode;
// Only activate controller if at least either translationVelocity or orientaionVelocity is set
if (!noMode)
{
ARMARX_CHECK_EXPRESSION(tcpController);
- tcpController->setVelocities(xVel, yVel, zVel, rollVel, pitchVel, yawVel, mode);
+ // ARMARX_CHECK_EXPRESSION(tcpController->getObjectScheduler());
+ tcpController->setVelocities(xVel, yVel, zVel, rollVel, pitchVel, yawVel, NJointCartesianVelocityController::ModeFromIce(mode));
+ // if (!tcpController->getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted, 5000))
+ // {
+ // ARMARX_ERROR << "NJointController was not initialized after 5000ms - bailing out!";
+ // return;
+ // }
if (!tcpController->isControllerActive())
{
- robotUnit->activateNJointController(tcpController);
+ robotUnit->activateNJointController(controllerName);
}
}
}
bool TCPControllerSubUnit::isRequested(const Ice::Current&)
{
- return true;
+ auto activeNJointControllers = robotUnit->getNJointControllersNotNull(robotUnit->getNJointControllerNames());
+ for (NJointControllerPtr controller : activeNJointControllers)
+ {
+ auto tcpController = NJointCartesianVelocityControllerPtr::dynamicCast(controller);
+ if (!tcpController)
+ {
+ return true;
+ }
+ }
+
+ return false;
+}
+
+
+void armarx::TCPControllerSubUnit::icePropertiesUpdated(const std::set<std::string>& changedProperties)
+{
+ if (changedProperties.count("AvoidJointLimitsKp") && robotUnit)
+ {
+ float avoidJointLimitsKp = getProperty<float>("AvoidJointLimitsKp").getValue();
+ auto activeNJointControllers = robotUnit->getNJointControllersNotNull(robotUnit->getNJointControllerNames());
+ for (NJointControllerPtr controller : activeNJointControllers)
+ {
+ auto tcpController = NJointCartesianVelocityControllerPtr::dynamicCast(controller);
+ if (tcpController)
+ {
+ tcpController->setAvoidJointLimitsKp(avoidJointLimitsKp);
+ }
+ }
+ }
}
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h
index 2dd3c3c6f3a5a6922fe47dcd99777b40bcb08797..cd87b0fdcfb6f68f08b25098cd72536f83299c4b 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h
@@ -31,6 +31,15 @@
namespace armarx
{
TYPEDEF_PTRS_HANDLE(RobotUnit);
+ class TCPControllerSubUnitPropertyDefinitions : public armarx::ComponentPropertyDefinitions
+ {
+ public:
+ TCPControllerSubUnitPropertyDefinitions(std::string prefix):
+ armarx::ComponentPropertyDefinitions(prefix)
+ {
+ defineOptionalProperty<float>("AvoidJointLimitsKp", 1.f, "Proportional gain value of P-Controller for joint limit avoidance", PropertyDefinitionBase::eModifiable);
+ }
+ };
TYPEDEF_PTRS_HANDLE(TCPControllerSubUnit);
class TCPControllerSubUnit :
@@ -50,7 +59,7 @@ namespace armarx
void update(const SensorAndControl& sc, const JointAndNJointControllers& c) override;
private:
// mutable std::mutex dataMutex;
- RobotUnit* robotUnit = NULL;
+ RobotUnit* robotUnit = nullptr;
VirtualRobot::RobotPtr coordinateTransformationRobot;
// ManagedIceObject interface
@@ -62,6 +71,9 @@ namespace armarx
return "TCPControlUnit";
}
+
+ PropertyDefinitionsPtr createPropertyDefinitions() override;
+
// UnitResourceManagementInterface interface
public:
void request(const Ice::Current&) override
@@ -91,5 +103,9 @@ namespace armarx
void reportJointCurrents(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override {}
void reportJointMotorTemperatures(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override {}
void reportJointStatuses(const NameStatusMap&, Ice::Long, bool, const Ice::Current&) override {}
+
+ // PropertyUser interface
+ public:
+ void icePropertiesUpdated(const std::set<std::string>& changedProperties) override;
};
}
diff --git a/source/RobotAPI/components/units/RobotUnit/util/RtTiming.h b/source/RobotAPI/components/units/RobotUnit/util/RtTiming.h
new file mode 100644
index 0000000000000000000000000000000000000000..65cf13a5717de73e70769603d6188011a8ceaa57
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/util/RtTiming.h
@@ -0,0 +1,54 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ArmarX
+ * @author Mirko Waechter( mirko.waechter at kit dot edu)
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
+
+#include <chrono>
+//#include <ArmarXCore/core/time/TimeUtil.h>
+#include "ControlThreadOutputBuffer.h"
+
+namespace armarx
+{
+ inline IceUtil::Time rtNow()
+ {
+ using namespace std::chrono;
+ auto epoch = time_point_cast<microseconds>(high_resolution_clock::now()).time_since_epoch();
+ return IceUtil::Time::microSeconds(duration_cast<milliseconds>(epoch).count());
+ }
+}
+
+#define RT_TIMING_START(name) auto name = armarx::rtNow();
+//! \ingroup VirtualTime
+//! Prints duration with comment in front of it, yet only once per second.
+#define RT_TIMING_END_COMMENT(name, comment) ARMARX_RT_LOGF_INFO("%s - duration: %.3f ms", comment, IceUtil::Time(armarx::rtNow()-name).toMilliSecondsDouble()).deactivateSpam(1);
+//! \ingroup VirtualTime
+//! Prints duration
+#define RT_TIMING_END(name) RT_TIMING_END_COMMENT(name,#name)
+//! \ingroup VirtualTime
+//! Prints duration with comment in front of it if it took longer than threshold
+#define RT_TIMING_CEND_COMMENT(name, comment, thresholdMs) if((armarx::rtNow()-name).toMilliSecondsDouble() >= thresholdMs) RT_TIMING_END_COMMENT(name, comment)
+//! \ingroup VirtualTime
+//! Prints duration if it took longer than thresholdMs
+#define RT_TIMING_CEND(name, thresholdMs) RT_TIMING_CEND_COMMENT(name, #name, thresholdMs)
+
+
diff --git a/source/RobotAPI/interface/core/FramedPoseBase.ice b/source/RobotAPI/interface/core/FramedPoseBase.ice
index 41f3d684ae1329c62a3d6ab28a774674cedc7c84..b4947429a1e360c808a896b6cf38bd8008259f87 100644
--- a/source/RobotAPI/interface/core/FramedPoseBase.ice
+++ b/source/RobotAPI/interface/core/FramedPoseBase.ice
@@ -116,5 +116,14 @@ module armarx
};
+ /**
+ * PoseAverageFilterBase filters poses with an average filter.
+ */
+ ["cpp:virtual"]
+ class PoseAverageFilterBase extends AverageFilterBase
+ {
+
+ };
+
};
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index ac949e0e7eb4aa0898ad8a0b3c5613dfcca9b962..d3d62e88d8e4bb28a2b3da115ed570abaa308c71 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -185,6 +185,8 @@ module armarx
float torqueLimit;
+ float startReduceTorque;
+
// Ice::FloatSeq Kpf;
// Ice::FloatSeq Kif;
// Ice::FloatSeq DesiredForce;
@@ -212,5 +214,55 @@ module armarx
};
+
+ class NJointTaskSpaceImpedanceDMPControllerConfig extends NJointControllerConfig
+ {
+
+ // dmp configuration
+ int kernelSize = 100;
+ string dmpMode = "MinimumJerk";
+ string dmpType = "Discrete";
+ double timeDuration;
+ string nodeSetName;
+
+ // phaseStop technique
+ double phaseL = 10;
+ double phaseK = 10;
+ double phaseDist0 = 50;
+ double phaseDist1 = 10;
+ double posToOriRatio = 100;
+
+ Ice::FloatSeq Kpos;
+ Ice::FloatSeq Dpos;
+ Ice::FloatSeq Kori;
+ Ice::FloatSeq Dori;
+
+ float Knull;
+ float Dnull;
+
+
+
+ bool useNullSpaceJointDMP;
+ Ice::FloatSeq defaultNullSpaceJointValues;
+
+ float torqueLimit;
+
+ };
+
+ interface NJointTaskSpaceImpedanceDMPControllerInterface extends NJointControllerInterface
+ {
+ void learnDMPFromFiles(Ice::StringSeq trajfiles);
+ void learnJointDMPFromFiles(string jointTrajFile);
+
+ bool isFinished();
+ void runDMP(Ice::DoubleSeq goals);
+
+ void setViaPoints(double canVal, Ice::DoubleSeq point);
+ void setGoals(Ice::DoubleSeq goals);
+
+ double getVirtualTime();
+
+ };
+
};
diff --git a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp
index 462493dfb858a6b15d643f44da4ddf34a5da3ed8..425a9abf8412506f0c598b4167cdce30720baee1 100644
--- a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp
+++ b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp
@@ -88,10 +88,10 @@ void TaskSpaceDMPController::flow(double deltaT, const Eigen::Matrix4f& currentP
currentState = dmpPtr->calculateDirectlyVelocity(currentState, canVal / timeDuration, deltaT / timeDuration, targetPoseVec);
-// for (size_t i = 0; i < 7; ++i)
-// {
-// targetPoseVec[i] = currentState[i].pos;
-// }
+ // for (size_t i = 0; i < 7; ++i)
+ // {
+ // targetPoseVec[i] = currentState[i].pos;
+ // }
if (isPhaseStopControl)
@@ -136,7 +136,11 @@ void TaskSpaceDMPController::flow(double deltaT, const Eigen::Matrix4f& currentP
double angularChange = angularVel0.angularDistance(oldDMPAngularVelocity);
- if (angularVel0.w() > 0 && oldDMPAngularVelocity.w() < 0 && angularChange < 1e-2)
+ if (angularVel0.w() * oldDMPAngularVelocity.w() < 0 &&
+ angularVel0.x() * oldDMPAngularVelocity.x() < 0 &&
+ angularVel0.y() * oldDMPAngularVelocity.y() < 0 &&
+ angularVel0.z() * oldDMPAngularVelocity.z() < 0 &&
+ angularChange < 1e-2)
{
angularVel0.w() = - angularVel0.w();
angularVel0.x() = - angularVel0.x();
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt b/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
index bbef6320f875fb4b58c9980cbb209a4cd1c9884a..a8b163ccb60447b1d687988c9d3d365ffa3858f9 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
@@ -46,6 +46,7 @@ if (DMP_FOUND )
DMPController/NJointTSDMPController.h
DMPController/NJointCCDMPController.h
DMPController/NJointBimanualCCDMPController.h
+ DMPController/NJointTaskSpaceImpedanceDMPController.h
)
list(APPEND LIB_FILES
@@ -54,7 +55,7 @@ if (DMP_FOUND )
DMPController/NJointTSDMPController.cpp
DMPController/NJointCCDMPController.cpp
DMPController/NJointBimanualCCDMPController.cpp
-
+ DMPController/NJointTaskSpaceImpedanceDMPController.cpp
)
list(APPEND LIBS ${DMP_LIBRARIES} DMPController)
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp
index 2563140cbc25077ea7a71edf91f193adf40e5e54..bd4aabafd75ee1b243147281b3f692836bf7dc34 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp
@@ -13,10 +13,18 @@ namespace armarx
useSynchronizedRtRobot();
leftRNS = rtGetRobot()->getRobotNodeSet("LeftArm");
+ leftNullSpaceCoefs.resize(leftRNS->getSize());
+ leftNullSpaceCoefs.setOnes();
+
for (size_t i = 0; i < leftRNS->getSize(); ++i)
{
std::string jointName = leftRNS->getNode(i)->getName();
+ if (leftRNS->getNode(i)->isLimitless())
+ {
+ leftNullSpaceCoefs(i) = 0;
+ }
+
leftJointNames.push_back(jointName);
ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
const SensorValueBase* sv = useSensorValue(jointName);
@@ -46,9 +54,18 @@ namespace armarx
};
rightRNS = rtGetRobot()->getRobotNodeSet("RightArm");
+
+ rightNullSpaceCoefs.resize(rightRNS->getSize());
+ rightNullSpaceCoefs.setOnes();
for (size_t i = 0; i < rightRNS->getSize(); ++i)
{
std::string jointName = rightRNS->getNode(i)->getName();
+
+ if (rightRNS->getNode(i)->isLimitless())
+ {
+ rightNullSpaceCoefs(i) = 0;
+ }
+
rightJointNames.push_back(jointName);
ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
const SensorValueBase* sv = useSensorValue(jointName);
@@ -135,6 +152,9 @@ namespace armarx
initData.leftTargetVel.setZero();
initData.rightTargetVel.resize(6);
initData.rightTargetVel.setZero();
+ initData.leftTargetPose = tcpLeft->getPoseInRootFrame();
+ initData.rightTargetPose = tcpRight->getPoseInRootFrame();
+ initData.virtualTime = cfg->timeDuration;
reinitTripleBuffer(initData);
leftDesiredJointValues.resize(leftTargets.size());
@@ -159,6 +179,10 @@ namespace armarx
leftKori << cfg->leftKori[0], cfg->leftKori[1], cfg->leftKori[2];
leftDori << cfg->leftDori[0], cfg->leftDori[1], cfg->leftDori[2];
+ rightKpos << cfg->rightKpos[0], cfg->rightKpos[1], cfg->rightKpos[2];
+ rightDpos << cfg->rightDpos[0], cfg->rightDpos[1], cfg->rightDpos[2];
+ rightKori << cfg->rightKori[0], cfg->rightKori[1], cfg->rightKori[2];
+ rightDori << cfg->rightDori[0], cfg->rightDori[1], cfg->rightDori[2];
knull = cfg->knull;
dnull = cfg->dnull;
@@ -170,7 +194,8 @@ namespace armarx
maxLinearVel = cfg->maxLinearVel;
maxAngularVel = cfg->maxAngularVel;
-
+ torqueFactor = 1.0;
+ startReduceTorque = cfg->startReduceTorque;
}
std::string NJointBimanualCCDMPController::getClassName(const Ice::Current&) const
@@ -245,7 +270,7 @@ namespace armarx
TaskSpaceDMPControllerPtr followerDMP = currentControlGroup[1];
virtualtimer = leaderDMP->canVal;
- if(virtualtimer < 1e-8)
+ if (virtualtimer < 1e-8)
{
finished = true;
}
@@ -482,36 +507,36 @@ namespace armarx
-// Eigen::VectorXf leftJointLimitAvoidance(leftRNS->getSize());
-// for (size_t i = 0; i < leftRNS->getSize(); i++)
-// {
-// VirtualRobot::RobotNodePtr rn = leftRNS->getNode(i);
-// if (rn->isLimitless())
-// {
-// leftJointLimitAvoidance(i) = 0;
-// }
-// else
-// {
-// float f = math::MathUtils::ILerp(rn->getJointLimitLo(), rn->getJointLimitHi(), rn->getJointValue());
-// leftJointLimitAvoidance(i) = cos(f * M_PI);
-// }
-// }
-// Eigen::VectorXf leftNullspaceTorque = knull * leftJointLimitAvoidance - dnull * leftqvel;
-// Eigen::VectorXf rightJointLimitAvoidance(rightRNS->getSize());
-// for (size_t i = 0; i < rightRNS->getSize(); i++)
-// {
-// VirtualRobot::RobotNodePtr rn = rightRNS->getNode(i);
-// if (rn->isLimitless())
-// {
-// rightJointLimitAvoidance(i) = 0;
-// }
-// else
-// {
-// float f = math::MathUtils::ILerp(rn->getJointLimitLo(), rn->getJointLimitHi(), rn->getJointValue());
-// rightJointLimitAvoidance(i) = cos(f * M_PI);
-// }
-// }
-// Eigen::VectorXf rightNullspaceTorque = knull * rightJointLimitAvoidance - dnull * rightqvel;
+ // Eigen::VectorXf leftJointLimitAvoidance(leftRNS->getSize());
+ // for (size_t i = 0; i < leftRNS->getSize(); i++)
+ // {
+ // VirtualRobot::RobotNodePtr rn = leftRNS->getNode(i);
+ // if (rn->isLimitless())
+ // {
+ // leftJointLimitAvoidance(i) = 0;
+ // }
+ // else
+ // {
+ // float f = math::MathUtils::ILerp(rn->getJointLimitLo(), rn->getJointLimitHi(), rn->getJointValue());
+ // leftJointLimitAvoidance(i) = cos(f * M_PI);
+ // }
+ // }
+ // Eigen::VectorXf leftNullspaceTorque = knull * leftJointLimitAvoidance - dnull * leftqvel;
+ // Eigen::VectorXf rightJointLimitAvoidance(rightRNS->getSize());
+ // for (size_t i = 0; i < rightRNS->getSize(); i++)
+ // {
+ // VirtualRobot::RobotNodePtr rn = rightRNS->getNode(i);
+ // if (rn->isLimitless())
+ // {
+ // rightJointLimitAvoidance(i) = 0;
+ // }
+ // else
+ // {
+ // float f = math::MathUtils::ILerp(rn->getJointLimitLo(), rn->getJointLimitHi(), rn->getJointValue());
+ // rightJointLimitAvoidance(i) = cos(f * M_PI);
+ // }
+ // }
+ // Eigen::VectorXf rightNullspaceTorque = knull * rightJointLimitAvoidance - dnull * rightqvel;
Eigen::VectorXf leftNullspaceTorque = knull * (leftDesiredJointValues - leftqpos) - dnull * leftqvel;
@@ -521,13 +546,20 @@ namespace armarx
Eigen::MatrixXf jtpinvL = leftIK->computePseudoInverseJacobianMatrix(jacobiL.transpose(), lambda);
Eigen::MatrixXf jtpinvR = rightIK->computePseudoInverseJacobianMatrix(jacobiR.transpose(), lambda);
- Eigen::VectorXf leftJointDesiredTorques = jacobiL.transpose() * leftJointControlWrench + (I - jacobiL.transpose() * jtpinvL) * leftNullspaceTorque;
- Eigen::VectorXf rightJointDesiredTorques = jacobiR.transpose() * rightJointControlWrench + (I - jacobiR.transpose() * jtpinvR) * rightNullspaceTorque;
+ Eigen::VectorXf leftJointDesiredTorques = jacobiL.transpose() * leftJointControlWrench + (I - jacobiL.transpose() * jtpinvL) * leftNullSpaceCoefs.cwiseProduct(leftNullspaceTorque);
+ Eigen::VectorXf rightJointDesiredTorques = jacobiR.transpose() * rightJointControlWrench + (I - jacobiR.transpose() * jtpinvR) * rightNullSpaceCoefs.cwiseProduct(rightNullspaceTorque);
+
+
+
+ if (virtualtime < startReduceTorque && startReduceTorque != 0)
+ {
+ torqueFactor -= deltaT / startReduceTorque;
+ }
// torque limit
for (size_t i = 0; i < leftTargets.size(); ++i)
{
- float desiredTorque = leftJointDesiredTorques(i);
+ float desiredTorque = torqueFactor * leftJointDesiredTorques(i);
if (isnan(desiredTorque))
{
@@ -537,7 +569,7 @@ namespace armarx
desiredTorque = (desiredTorque > torqueLimit) ? torqueLimit : desiredTorque;
desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
- debugDataInfo.getWriteBuffer().desired_torques[leftJointNames[i]] = leftJointDesiredTorques(i);
+ debugDataInfo.getWriteBuffer().desired_torques[leftJointNames[i]] = desiredTorque;
leftTargets.at(i)->torque = desiredTorque;
}
@@ -546,7 +578,7 @@ namespace armarx
for (size_t i = 0; i < rightTargets.size(); ++i)
{
- float desiredTorque = rightJointDesiredTorques(i);
+ float desiredTorque = torqueFactor * rightJointDesiredTorques(i);
if (isnan(desiredTorque))
{
@@ -556,7 +588,7 @@ namespace armarx
desiredTorque = (desiredTorque > torqueLimit) ? torqueLimit : desiredTorque;
desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
- debugDataInfo.getWriteBuffer().desired_torques[rightJointNames[i]] = rightJointDesiredTorques(i);
+ debugDataInfo.getWriteBuffer().desired_torques[rightJointNames[i]] = desiredTorque;
rightTargets.at(i)->torque = desiredTorque;
}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.h
index 9b62d80f6a10a21d917143993c881df08b658c70..3402d5cc0b2426350916353f8a316fa266944166 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.h
@@ -214,14 +214,6 @@ namespace armarx
Eigen::VectorXf leftDesiredJointValues;
Eigen::VectorXf rightDesiredJointValues;
- float KoriFollower;
- float KposFollower;
- float DposFollower;
- float DoriFollower;
-
- Eigen::VectorXf forceC_des;
- float boxWidth;
-
Eigen::Vector3f leftKpos;
Eigen::Vector3f leftKori;
Eigen::Vector3f leftDpos;
@@ -243,10 +235,17 @@ namespace armarx
VirtualRobot::RobotNodeSetPtr leftRNS;
VirtualRobot::RobotNodeSetPtr rightRNS;
+ Eigen::VectorXf leftNullSpaceCoefs;
+ Eigen::VectorXf rightNullSpaceCoefs;
+
+ float torqueFactor;
+ float startReduceTorque;
float maxLinearVel;
float maxAngularVel;
+
+
// NJointBimanualCCDMPControllerInterface interface
};
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
index 4ce687e3eb4680d31dcdcda379534ade1020e833..4b0e26b6c2a55346b9619b28ca6b0bf27c11a0b0 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
@@ -66,7 +66,7 @@ namespace armarx
taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
- taskSpaceDMPController.reset(new TaskSpaceDMPController("default", taskSpaceDMPConfig));
+ taskSpaceDMPController.reset(new TaskSpaceDMPController("default", taskSpaceDMPConfig, false));
// initialize tcp position and orientation
@@ -126,35 +126,35 @@ namespace armarx
targetPose = taskSpaceDMPController->getTargetPoseMat();
- // std::vector<double> targetState = taskSpaceDMPController->getTargetPose();
- // debugOutputData.getWriteBuffer().latestTargetVelocities["x_vel"] = targetVels(0);
- // debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = targetVels(1);
- // debugOutputData.getWriteBuffer().latestTargetVelocities["z_vel"] = targetVels(2);
- // debugOutputData.getWriteBuffer().latestTargetVelocities["roll_vel"] = targetVels(3);
- // debugOutputData.getWriteBuffer().latestTargetVelocities["pitch_vel"] = targetVels(4);
- // debugOutputData.getWriteBuffer().latestTargetVelocities["yaw_vel"] = targetVels(5);
- // debugOutputData.getWriteBuffer().dmpTargets["dmp_x"] = targetState[0];
- // debugOutputData.getWriteBuffer().dmpTargets["dmp_y"] = targetState[1];
- // debugOutputData.getWriteBuffer().dmpTargets["dmp_z"] = targetState[2];
- // debugOutputData.getWriteBuffer().dmpTargets["dmp_qw"] = targetState[3];
- // debugOutputData.getWriteBuffer().dmpTargets["dmp_qx"] = targetState[4];
- // debugOutputData.getWriteBuffer().dmpTargets["dmp_qy"] = targetState[5];
- // debugOutputData.getWriteBuffer().dmpTargets["dmp_qz"] = targetState[6];
- // debugOutputData.getWriteBuffer().currentPose["currentPose_x"] = currentPose(0, 3);
- // debugOutputData.getWriteBuffer().currentPose["currentPose_y"] = currentPose(1, 3);
- // debugOutputData.getWriteBuffer().currentPose["currentPose_z"] = currentPose(2, 3);
-
- // VirtualRobot::MathTools::Quaternion currentQ = VirtualRobot::MathTools::eigen4f2quat(currentPose);
- // debugOutputData.getWriteBuffer().currentPose["currentPose_qw"] = currentQ.w;
- // debugOutputData.getWriteBuffer().currentPose["currentPose_qx"] = currentQ.x;
- // debugOutputData.getWriteBuffer().currentPose["currentPose_qy"] = currentQ.y;
- // debugOutputData.getWriteBuffer().currentPose["currentPose_qz"] = currentQ.z;
+ std::vector<double> targetState = taskSpaceDMPController->getTargetPose();
+ debugOutputData.getWriteBuffer().latestTargetVelocities["x_vel"] = targetVels(0);
+ debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = targetVels(1);
+ debugOutputData.getWriteBuffer().latestTargetVelocities["z_vel"] = targetVels(2);
+ debugOutputData.getWriteBuffer().latestTargetVelocities["roll_vel"] = targetVels(3);
+ debugOutputData.getWriteBuffer().latestTargetVelocities["pitch_vel"] = targetVels(4);
+ debugOutputData.getWriteBuffer().latestTargetVelocities["yaw_vel"] = targetVels(5);
+ debugOutputData.getWriteBuffer().dmpTargets["dmp_x"] = targetState[0];
+ debugOutputData.getWriteBuffer().dmpTargets["dmp_y"] = targetState[1];
+ debugOutputData.getWriteBuffer().dmpTargets["dmp_z"] = targetState[2];
+ debugOutputData.getWriteBuffer().dmpTargets["dmp_qw"] = targetState[3];
+ debugOutputData.getWriteBuffer().dmpTargets["dmp_qx"] = targetState[4];
+ debugOutputData.getWriteBuffer().dmpTargets["dmp_qy"] = targetState[5];
+ debugOutputData.getWriteBuffer().dmpTargets["dmp_qz"] = targetState[6];
+ debugOutputData.getWriteBuffer().currentPose["currentPose_x"] = currentPose(0, 3);
+ debugOutputData.getWriteBuffer().currentPose["currentPose_y"] = currentPose(1, 3);
+ debugOutputData.getWriteBuffer().currentPose["currentPose_z"] = currentPose(2, 3);
+
+ VirtualRobot::MathTools::Quaternion currentQ = VirtualRobot::MathTools::eigen4f2quat(currentPose);
+ debugOutputData.getWriteBuffer().currentPose["currentPose_qw"] = currentQ.w;
+ debugOutputData.getWriteBuffer().currentPose["currentPose_qx"] = currentQ.x;
+ debugOutputData.getWriteBuffer().currentPose["currentPose_qy"] = currentQ.y;
+ debugOutputData.getWriteBuffer().currentPose["currentPose_qz"] = currentQ.z;
debugOutputData.getWriteBuffer().currentCanVal = taskSpaceDMPController->debugData.canVal;
- // debugOutputData.getWriteBuffer().mpcFactor = taskSpaceDMPController->debugData.mpcFactor;
- // debugOutputData.getWriteBuffer().error = taskSpaceDMPController->debugData.poseError;
- // debugOutputData.getWriteBuffer().posError = taskSpaceDMPController->debugData.posiError;
- // debugOutputData.getWriteBuffer().oriError = taskSpaceDMPController->debugData.oriError;
- // debugOutputData.getWriteBuffer().deltaT = deltaT;
+ debugOutputData.getWriteBuffer().mpcFactor = taskSpaceDMPController->debugData.mpcFactor;
+ debugOutputData.getWriteBuffer().error = taskSpaceDMPController->debugData.poseError;
+ debugOutputData.getWriteBuffer().posError = taskSpaceDMPController->debugData.posiError;
+ debugOutputData.getWriteBuffer().oriError = taskSpaceDMPController->debugData.oriError;
+ debugOutputData.getWriteBuffer().deltaT = deltaT;
debugOutputData.commitWrite();
@@ -200,6 +200,7 @@ namespace armarx
rtTargetVel.resize(6);
rtTargetVel.block<3, 1>(0, 0) = KpF * (targetPose.block<3, 1>(0, 3) - currentPose.block<3, 1>(0, 3)) + DpF * (targetVel.block<3, 1>(0, 0) - tcptwist.block<3, 1>(0, 0));
rtTargetVel.block<3, 1>(3, 0) = KoF * errorRPY + DoF * (targetVel.block<3, 1>(3, 0) - tcptwist.block<3, 1>(3, 0));
+ // rtTargetVel = targetVel;
float normLinearVelocity = rtTargetVel.block<3, 1>(0, 0).norm();
@@ -380,40 +381,40 @@ namespace armarx
{
std::string datafieldName = debugName;
StringVariantBaseMap datafields;
- // auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
- // for (auto& pair : values)
- // {
- // datafieldName = pair.first + "_" + debugName;
- // datafields[datafieldName] = new Variant(pair.second);
- // }
-
- // auto dmpTargets = debugOutputData.getUpToDateReadBuffer().dmpTargets;
- // for (auto& pair : dmpTargets)
- // {
- // datafieldName = pair.first + "_" + debugName;
- // datafields[datafieldName] = new Variant(pair.second);
- // }
-
- // auto currentPose = debugOutputData.getUpToDateReadBuffer().currentPose;
- // for (auto& pair : currentPose)
- // {
- // datafieldName = pair.first + "_" + debugName;
- // datafields[datafieldName] = new Variant(pair.second);
- // }
+ auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
+ for (auto& pair : values)
+ {
+ datafieldName = pair.first + "_" + debugName;
+ datafields[datafieldName] = new Variant(pair.second);
+ }
+
+ auto dmpTargets = debugOutputData.getUpToDateReadBuffer().dmpTargets;
+ for (auto& pair : dmpTargets)
+ {
+ datafieldName = pair.first + "_" + debugName;
+ datafields[datafieldName] = new Variant(pair.second);
+ }
+
+ auto currentPose = debugOutputData.getUpToDateReadBuffer().currentPose;
+ for (auto& pair : currentPose)
+ {
+ datafieldName = pair.first + "_" + debugName;
+ datafields[datafieldName] = new Variant(pair.second);
+ }
datafieldName = "canVal_" + debugName;
datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
- // datafieldName = "mpcFactor_" + debugName;
- // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
- // datafieldName = "error_" + debugName;
- // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().error);
- // datafieldName = "posError_" + debugName;
- // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().posError);
- // datafieldName = "oriError_" + debugName;
- // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().oriError);
- // datafieldName = "deltaT_" + debugName;
- // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
- // datafieldName = "DMPController_" + debugName;
+ datafieldName = "mpcFactor_" + debugName;
+ datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
+ datafieldName = "error_" + debugName;
+ datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().error);
+ datafieldName = "posError_" + debugName;
+ datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().posError);
+ datafieldName = "oriError_" + debugName;
+ datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().oriError);
+ datafieldName = "deltaT_" + debugName;
+ datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
+ datafieldName = "DMPController_" + debugName;
debugObs->setDebugChannel(datafieldName, datafields);
}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceDMPController.cpp
deleted file mode 100644
index 0b3858033b0d6b34ae858d1379b34cdc7ccebef8..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceDMPController.cpp
+++ /dev/null
@@ -1,453 +0,0 @@
-#include "NJointTaskSpaceDMPController.h"
-
-namespace armarx
-{
- NJointControllerRegistration<NJointTaskSpaceDMPController> registrationControllerNJointTaskSpaceDMPController("NJointTaskSpaceDMPController");
-
- NJointTaskSpaceDMPController::NJointTaskSpaceDMPController(const RobotUnitPtr& robotUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
- {
- useSynchronizedRtRobot();
- cfg = NJointTaskSpaceDMPControllerConfigPtr::dynamicCast(config);
- ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
- VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
-
- ARMARX_CHECK_EXPRESSION_W_HINT(rns, cfg->nodeSetName);
- for (size_t i = 0; i < rns->getSize(); ++i)
- {
- std::string jointName = rns->getNode(i)->getName();
- ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
- const SensorValueBase* sv = useSensorValue(jointName);
- targets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
-
- const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
- const SensorValue1DoFGravityTorque* gravityTorqueSensor = sv->asA<SensorValue1DoFGravityTorque>();
- if (!torqueSensor)
- {
- ARMARX_WARNING << "No Torque sensor available for " << jointName;
- }
- if (!gravityTorqueSensor)
- {
- ARMARX_WARNING << "No Gravity Torque sensor available for " << jointName;
- }
-
- torqueSensors.push_back(torqueSensor);
- gravityTorqueSensors.push_back(gravityTorqueSensor);
- };
-
- tcp = (cfg->tcpName.empty()) ? rns->getTCP() : rtGetRobot()->getRobotNode(cfg->tcpName);
- ARMARX_CHECK_EXPRESSION_W_HINT(tcp, cfg->tcpName);
-
- // set tcp controller
- tcpController.reset(new CartesianVelocityController(rns, tcp));
- nodeSetName = cfg->nodeSetName;
- torquePIDs.resize(tcpController->rns->getSize(), pidController());
-
-
- NJointTaskSpaceDMPControllerControlData initData;
- initData.nullspaceJointVelocities.resize(tcpController->rns->getSize(), 0);
- initData.torqueKp.resize(tcpController->rns->getSize(), 0);
- initData.torqueKd.resize(tcpController->rns->getSize(), 0);
- initData.mode = ModeFromIce(cfg->mode);
-
- // set DMP
- dmpPtr.reset(new UMIDMP(cfg->kernelSize, cfg->DMPKd, cfg->baseMode, cfg->tau));
- timeDuration = cfg->timeDuration;
- canVal = timeDuration;
- finished = false;
-
- initData.dmpPtr = dmpPtr;
- initData.isStart = false;
- reinitTripleBuffer(initData);
-
-
- isDisturbance = false;
- phaseL = cfg->phaseL;
- phaseK = cfg->phaseK;
- phaseDist0 = cfg->phaseDist0;
- phaseDist1 = cfg->phaseDist1;
- phaseKpPos = cfg->phaseKpPos;
- phaseKpOri = cfg->phaseKpOri;
-
- // initialize tcp position and orientation
- Eigen::Matrix4f pose = tcp->getPoseInRootFrame();
- tcpPosition(0) = pose(0, 3);
- tcpPosition(1) = pose(1, 3);
- tcpPosition(2) = pose(2, 3);
-
- tcpOrientation = VirtualRobot::MathTools::eigen4f2rpy(pose);
-
- posToOriRatio = cfg->posToOriRatio;
- }
-
- std::string NJointTaskSpaceDMPController::getClassName(const Ice::Current&) const
- {
- return "NJointTaskSpaceDMPController";
- }
-
- void NJointTaskSpaceDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
- {
- if (rtGetControlStruct().isStart && !finished)
- {
- UMIDMPPtr dmpPtr = rtGetControlStruct().dmpPtr;
-
- // DMP controller
- Eigen::Matrix4f pose = tcp->getPoseInRootFrame();
- Eigen::Vector3f currPosition;
- Eigen::Vector3f currOrientation = VirtualRobot::MathTools::eigen4f2rpy(pose);
- currPosition(0) = pose(0, 3);
- currPosition(1) = pose(1, 3);
- currPosition(2) = pose(2, 3);
-
- double deltaT = timeSinceLastIteration.toSecondsDouble();
- double dmpDeltaT = deltaT / timeDuration;
-
- Eigen::Vector3f posVel;
- Eigen::Vector3f oriVel;
-
- if (deltaT == 0)
- {
- posVel << 0, 0 , 0;
- oriVel << 0, 0 , 0;
- }
- else
- {
- posVel = (currPosition - tcpPosition) / deltaT;
- oriVel = (currOrientation - tcpOrientation) / deltaT;
- }
-
- tcpPosition = currPosition;
- tcpOrientation = currOrientation;
-
- double phaseStop = 0;
- double error = 0;
- currentState.clear();
-
- double posError = 0;
- double oriError = 0;
- for (size_t i = 0; i < 3; i++)
- {
- DMP::DMPState currentPos;
- currentPos.pos = tcpPosition(i);
- currentPos.vel = posVel(i) * timeDuration;
- currentState.push_back(currentPos);
-
- posError += pow(currentPos.pos - targetState[i], 2);
- }
-
- for (size_t i = 0; i < 3; i++)
- {
- DMP::DMPState currentPos;
- currentPos.pos = tcpOrientation(i);
- currentPos.vel = oriVel(i) * timeDuration;
- currentState.push_back(currentPos);
-
- oriError += pow(currentPos.pos - targetState[i + 3], 2);
- }
-
-
- error = sqrt(posError + posToOriRatio * oriError);
-
- double phaseDist;
- if (isDisturbance)
- {
- phaseDist = phaseDist1;
- }
- else
- {
- phaseDist = phaseDist0;
- }
-
- phaseStop = phaseL / (1 + exp(-phaseK * (error - phaseDist)));
- double mpcFactor = 1 - (phaseStop / phaseL);
-
- if (mpcFactor < 0.1)
- {
- isDisturbance = true;
- }
-
- if (mpcFactor > 0.9)
- {
- isDisturbance = false;
- }
-
-
- double tau = dmpPtr->getTemporalFactor();
- canVal -= 1 / tau * deltaT * 1 / (1 + phaseStop) ;
- currentState = dmpPtr->calculateDirectlyVelocity(currentState, canVal / timeDuration, dmpDeltaT, targetState);
-
- if (canVal < 1e-8)
- {
- finished = true;
- }
-
- // cartesian velocity controller
- Eigen::VectorXf x;
- auto mode = rtGetControlStruct().mode;
-
- double vel0, vel1;
- if (mode == VirtualRobot::IKSolver::All)
- {
- x.resize(6);
-
- for (size_t i = 0; i < 3; i++)
- {
- vel0 = currentState[i].vel / timeDuration;
- vel1 = phaseKpPos * (targetState[i] - tcpPosition[i]);
- x(i) = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
-
- }
- for (size_t i = 0; i < 3; i++)
- {
- vel0 = currentState[i + 3].vel / timeDuration;
- vel1 = phaseKpOri * (targetState[i + 3] - tcpOrientation[i]);
- x(i + 3) = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
- }
-
- debugOutputData.getWriteBuffer().latestTargetVelocities["x_vel"] = x(0);
- debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = x(1);
- debugOutputData.getWriteBuffer().latestTargetVelocities["z_vel"] = x(2);
- debugOutputData.getWriteBuffer().latestTargetVelocities["r_vel"] = x(3);
- debugOutputData.getWriteBuffer().latestTargetVelocities["p_vel"] = x(4);
- debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = x(5);
- debugOutputData.getWriteBuffer().currentCanVal = canVal;
- debugOutputData.getWriteBuffer().mpcFactor = mpcFactor;
- debugOutputData.commitWrite();
-
- }
- else if (mode == VirtualRobot::IKSolver::Position)
- {
- x.resize(3);
-
- for (size_t i = 0; i < 3; i++)
- {
- vel0 = currentState[i].vel / timeDuration;
- vel1 = phaseKpPos * (targetState[i] - tcpPosition[i]);
- x(i) = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
- }
-
- debugOutputData.getWriteBuffer().latestTargetVelocities["x_vel"] = x(0);
- debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = x(1);
- debugOutputData.getWriteBuffer().latestTargetVelocities["z_vel"] = x(2);
- debugOutputData.getWriteBuffer().currentCanVal = canVal;
- debugOutputData.getWriteBuffer().mpcFactor = mpcFactor;
- debugOutputData.commitWrite();
- }
- else if (mode == VirtualRobot::IKSolver::Orientation)
- {
- x.resize(3);
-
- for (size_t i = 0; i < 3; i++)
- {
- vel0 = currentState[i + 3].vel / timeDuration;
- vel1 = phaseKpOri * (targetState[i + 3] - tcpOrientation[i]);
- x(i) = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
- }
-
- debugOutputData.getWriteBuffer().latestTargetVelocities["r_vel"] = x(3);
- debugOutputData.getWriteBuffer().latestTargetVelocities["p_vel"] = x(4);
- debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = x(5);
- debugOutputData.getWriteBuffer().currentCanVal = canVal;
- debugOutputData.getWriteBuffer().mpcFactor = mpcFactor;
- debugOutputData.commitWrite();
- }
- else
- {
- // No mode has been set
- return;
- }
-
-
- Eigen::VectorXf jnv = Eigen::VectorXf::Zero(tcpController->rns->getSize());
- float jointLimitAvoidanceKp = rtGetControlStruct().avoidJointLimitsKp;
- if (jointLimitAvoidanceKp > 0)
- {
- jnv += jointLimitAvoidanceKp * tcpController->calculateJointLimitAvoidance();
- }
- for (size_t i = 0; i < tcpController->rns->getSize(); i++)
- {
- jnv(i) += rtGetControlStruct().nullspaceJointVelocities.at(i);
- if (torqueSensors.at(i) && gravityTorqueSensors.at(i) && rtGetControlStruct().torqueKp.at(i) != 0)
- {
- torquePIDs.at(i).Kp = rtGetControlStruct().torqueKp.at(i);
- torquePIDs.at(i).Kd = rtGetControlStruct().torqueKd.at(i);
- jnv(i) += torquePIDs.at(i).update(timeSinceLastIteration.toSecondsDouble(), torqueSensors.at(i)->torque - gravityTorqueSensors.at(i)->gravityTorque);
- //jnv(i) += rtGetControlStruct().torqueKp.at(i) * (torqueSensors.at(i)->torque - gravityTorqueSensors.at(i)->gravityTorque);
- }
- else
- {
- torquePIDs.at(i).lastError = 0;
- }
- }
-
- Eigen::VectorXf jointTargetVelocities = tcpController->calculate(x, jnv, mode);
-
- for (size_t i = 0; i < targets.size(); ++i)
- {
- targets.at(i)->velocity = jointTargetVelocities(i);
- }
- }
- else
- {
- for (size_t i = 0; i < targets.size(); ++i)
- {
- targets.at(i)->velocity = 0;
- }
- }
- }
-
-
- void NJointTaskSpaceDMPController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
- {
- DMP::Vec<DMP::SampledTrajectoryV2 > trajs;
-
- DMP::DVec ratios;
- for (size_t i = 0; i < fileNames.size(); ++i)
- {
- DMP::SampledTrajectoryV2 traj;
- traj.readFromCSVFile(fileNames.at(i));
- traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
- trajs.push_back(traj);
-
- if (i == 0)
- {
- ratios.push_back(1.0);
- }
- else
- {
- ratios.push_back(0.0);
- }
- }
-
- dmpPtr->learnFromTrajectories(trajs);
- dmpPtr->setOneStepMPC(true);
- dmpPtr->styleParas = dmpPtr->getStyleParasWithRatio(ratios);
-
- ARMARX_INFO << "Learned DMP ... ";
- }
-
- void NJointTaskSpaceDMPController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
- {
- ViaPoint viaPoint(u, viapoint);
- dmpPtr->setViaPoint(viaPoint);
-
- LockGuardType guard {controlDataMutex};
- getWriterControlStruct().dmpPtr = dmpPtr;
- writeControlStruct();
- }
-
-
- void NJointTaskSpaceDMPController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
- {
- setViaPoints(dmpPtr->getUMin(), goals, ice);
- }
-
- void NJointTaskSpaceDMPController::setControllerTarget(Ice::Float avoidJointLimitsKp, NJointTaskSpaceDMPControllerMode::CartesianSelection mode, const Ice::Current&)
- {
- LockGuardType guard {controlDataMutex};
- getWriterControlStruct().avoidJointLimitsKp = avoidJointLimitsKp;
- getWriterControlStruct().mode = ModeFromIce(mode);
- writeControlStruct();
- }
-
- VirtualRobot::IKSolver::CartesianSelection NJointTaskSpaceDMPController::ModeFromIce(const NJointTaskSpaceDMPControllerMode::CartesianSelection mode)
- {
- if (mode == NJointTaskSpaceDMPControllerMode::CartesianSelection::ePosition)
- {
- return VirtualRobot::IKSolver::CartesianSelection::Position;
- }
- if (mode == NJointTaskSpaceDMPControllerMode::CartesianSelection::eOrientation)
- {
- return VirtualRobot::IKSolver::CartesianSelection::Orientation;
- }
- if (mode == NJointTaskSpaceDMPControllerMode::CartesianSelection::eAll)
- {
- return VirtualRobot::IKSolver::CartesianSelection::All;
- }
- ARMARX_ERROR_S << "invalid mode " << mode;
- return (VirtualRobot::IKSolver::CartesianSelection)0;
- }
-
- void NJointTaskSpaceDMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
- {
- StringVariantBaseMap datafields;
- auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
- for (auto& pair : values)
- {
- datafields[pair.first] = new Variant(pair.second);
- }
-
- datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
- datafields["mpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
- debugObs->setDebugChannel("latestDMPTargetVelocities", datafields);
- }
-
-
- void NJointTaskSpaceDMPController::setTorqueKp(const StringFloatDictionary& torqueKp, const Ice::Current&)
- {
- LockGuardType guard {controlDataMutex};
- for (size_t i = 0; i < tcpController->rns->getSize(); i++)
- {
- getWriterControlStruct().torqueKp.at(i) = torqueKp.at(tcpController->rns->getNode(i)->getName());
- }
- writeControlStruct();
- }
-
- void NJointTaskSpaceDMPController::setNullspaceJointVelocities(const StringFloatDictionary& nullspaceJointVelocities, const Ice::Current&)
- {
- LockGuardType guard {controlDataMutex};
- for (size_t i = 0; i < tcpController->rns->getSize(); i++)
- {
- getWriterControlStruct().nullspaceJointVelocities.at(i) = nullspaceJointVelocities.at(tcpController->rns->getNode(i)->getName());
- }
- writeControlStruct();
- }
-
- void NJointTaskSpaceDMPController::runDMP(const Ice::DoubleSeq& goals, double tau, const Ice::Current&)
- {
- currentState.clear();
- targetState.clear();
- for (size_t i = 0; i < 3; i++)
- {
- DMP::DMPState currentPos;
- currentPos.pos = tcpPosition(i);
- currentPos.vel = 0;
- currentState.push_back(currentPos);
- targetState.push_back(currentPos.pos);
- }
- for (size_t i = 0; i < 3; i++)
- {
- DMP::DMPState currentPos;
- currentPos.pos = tcpOrientation(i);
- currentPos.vel = 0;
- currentState.push_back(currentPos);
- targetState.push_back(currentPos.pos);
- }
- dmpPtr->prepareExecution(goals, currentState, 1, tau);
- finished = false;
- dmpPtr->setTemporalFactor(tau);
-
- LockGuardType guard {controlDataMutex};
- getWriterControlStruct().dmpPtr = dmpPtr;
- getWriterControlStruct().isStart = true;
- writeControlStruct();
-
- }
-
- void NJointTaskSpaceDMPController::setTemporalFactor(double tau, const Ice::Current&)
- {
- dmpPtr->setTemporalFactor(tau);
- LockGuardType guard {controlDataMutex};
- getWriterControlStruct().dmpPtr = dmpPtr;
- writeControlStruct();
- }
-
- void NJointTaskSpaceDMPController::rtPreActivateController()
- {
- }
-
- void NJointTaskSpaceDMPController::rtPostDeactivateController()
- {
-
- }
-
-}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceDMPController.h
deleted file mode 100644
index e3bb4412951d30c4d2e22209cf67cdb5140863f0..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceDMPController.h
+++ /dev/null
@@ -1,141 +0,0 @@
-
-#pragma once
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <VirtualRobot/Robot.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <dmp/representation/dmp/umidmp.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
-#include <RobotAPI/libraries/core/CartesianVelocityController.h>
-
-
-using namespace DMP;
-namespace armarx
-{
-
-
- TYPEDEF_PTRS_HANDLE(NJointTaskSpaceDMPController);
- TYPEDEF_PTRS_HANDLE(NJointTaskSpaceDMPControllerControlData);
-
- typedef std::pair<double, DVec > ViaPoint;
- typedef std::vector<ViaPoint > ViaPointsSet;
- class NJointTaskSpaceDMPControllerControlData
- {
- public:
- // dmp control data
- UMIDMPPtr dmpPtr;
- bool isStart;
-
- // cartesian velocity control data
- std::vector<float> nullspaceJointVelocities;
- float avoidJointLimitsKp = 0;
- std::vector<float> torqueKp;
- std::vector<float> torqueKd;
- VirtualRobot::IKSolver::CartesianSelection mode = VirtualRobot::IKSolver::All;
- };
-
- class pidController
- {
- public:
- float Kp = 0, Kd = 0;
- float lastError = 0;
- float update(float dt, float error)
- {
- float derivative = (error - lastError) / dt;
- float retVal = Kp * error + Kd * derivative;
- lastError = error;
- return retVal;
- }
- };
-
- class NJointTaskSpaceDMPController :
- public NJointControllerWithTripleBuffer<NJointTaskSpaceDMPControllerControlData>,
- public NJointTaskSpaceDMPControllerInterface
- {
- public:
- using ConfigPtrT = NJointTaskSpaceDMPControllerConfigPtr;
- NJointTaskSpaceDMPController(const RobotUnitPtr& robotUnit, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
- // NJointControllerInterface interface
- std::string getClassName(const Ice::Current&) const;
-
- // NJointController interface
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
-
- // NJointTaskSpaceDMPControllerInterface interface
- void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&);
- bool isFinished(const Ice::Current&)
- {
- return finished;
- }
-
- void runDMP(const Ice::DoubleSeq& goals, Ice::Double tau, const Ice::Current&);
- void setTemporalFactor(Ice::Double tau, const Ice::Current&);
- void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&);
-
- void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
-
- void setControllerTarget(Ice::Float avoidJointLimitsKp, NJointTaskSpaceDMPControllerMode::CartesianSelection mode, const Ice::Current&);
- void setTorqueKp(const StringFloatDictionary& torqueKp, const Ice::Current&);
- void setNullspaceJointVelocities(const StringFloatDictionary& nullspaceJointVelocities, const Ice::Current&);
- protected:
- void rtPreActivateController() override;
- void rtPostDeactivateController() override;
- VirtualRobot::IKSolver::CartesianSelection ModeFromIce(const NJointTaskSpaceDMPControllerMode::CartesianSelection mode);
- virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
-
- private:
- struct DebugBufferData
- {
- StringFloatDictionary latestTargetVelocities;
- double currentCanVal;
- double mpcFactor;
- };
-
- TripleBuffer<DebugBufferData> debugOutputData;
-
-
- std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors;
- std::vector<const SensorValue1DoFGravityTorque*> gravityTorqueSensors;
- std::vector<ControlTarget1DoFActuatorVelocity*> targets;
-
- // velocity ik controller parameters
- std::vector<pidController> torquePIDs;
- CartesianVelocityControllerPtr tcpController;
- std::string nodeSetName;
-
- // dmp parameters
- UMIDMPPtr dmpPtr;
- double timeDuration;
- double canVal;
- bool finished;
- double tau;
- ViaPointsSet viaPoints;
-
- // phaseStop parameters
- double phaseL;
- double phaseK;
- double phaseDist0;
- double phaseDist1;
- double phaseKpPos;
- double phaseKpOri;
- double posToOriRatio;
-
-
- bool isDisturbance;
- NJointTaskSpaceDMPControllerConfigPtr cfg;
- VirtualRobot::RobotNodePtr tcp;
- Eigen::Vector3f tcpPosition;
- Eigen::Vector3f tcpOrientation;
- DMP::Vec<DMP::DMPState> currentState;
- DMP::DVec targetState;
-
-
- };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6c773caadf6d5bbb951a8dcfb1226aa212544b94
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
@@ -0,0 +1,368 @@
+#include "NJointTaskSpaceImpedanceDMPController.h"
+
+namespace armarx
+{
+ NJointControllerRegistration<NJointTaskSpaceImpedanceDMPController> registrationControllerNJointTaskSpaceImpedanceDMPController("NJointTaskSpaceImpedanceDMPController");
+
+ NJointTaskSpaceImpedanceDMPController::NJointTaskSpaceImpedanceDMPController(armarx::NJointControllerDescriptionProviderInterfacePtr prov, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
+ {
+ cfg = NJointTaskSpaceImpedanceDMPControllerConfigPtr::dynamicCast(config);
+ RobotUnitPtr robotUnit = RobotUnitPtr::dynamicCast(prov);
+ VirtualRobot::RobotNodeSetPtr rns = robotUnit->getRtRobot()->getRobotNodeSet(cfg->nodeSetName);
+ ARMARX_CHECK_EXPRESSION_W_HINT(rns, cfg->nodeSetName);
+
+ for (size_t i = 0; i < rns->getSize(); ++i)
+ {
+ std::string jointName = rns->getNode(i)->getName();
+ jointNames.push_back(jointName);
+ ControlTargetBase* ct = prov->getControlTarget(jointName, ControlModes::Torque1DoF);
+ const SensorValueBase* sv = prov->getSensorValue(jointName);
+ targets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
+ const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
+ const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
+
+ if (!velocitySensor)
+ {
+ ARMARX_WARNING << "No velocitySensor available for " << jointName;
+ }
+ if (!positionSensor)
+ {
+ ARMARX_WARNING << "No positionSensor available for " << jointName;
+ }
+
+ velocitySensors.push_back(velocitySensor);
+ positionSensors.push_back(positionSensor);
+ };
+
+ tcp = rns->getTCP();
+ ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+
+ // set DMP
+ TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
+ taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
+ taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
+ taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
+ taskSpaceDMPConfig.DMPStyle = cfg->dmpType;
+ taskSpaceDMPConfig.DMPAmplitude = 1.0;
+ taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
+ taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
+ taskSpaceDMPConfig.phaseStopParas.Kpos = 0;
+ taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
+ taskSpaceDMPConfig.phaseStopParas.Kori = 0;
+ taskSpaceDMPConfig.phaseStopParas.Dori = 0;
+ taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
+ taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
+ taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
+
+ dmpCtrl.reset(new TaskSpaceDMPController("DMPController", taskSpaceDMPConfig, false));
+ finished = false;
+
+ useNullSpaceJointDMP = cfg->useNullSpaceJointDMP;
+ nullSpaceJointDMPPtr.reset(new DMP::UMIDMP(100));
+
+ isNullSpaceJointDMPLearned = false;
+
+ defaultNullSpaceJointValues.resize(targets.size());
+ ARMARX_CHECK_EQUAL(cfg->defaultNullSpaceJointValues.size(), targets.size());
+
+ for (size_t i = 0; i < targets.size(); ++i)
+ {
+ defaultNullSpaceJointValues(i) = cfg->defaultNullSpaceJointValues.at(i);
+ }
+
+
+ kpos << cfg->Kpos[0], cfg->Kpos[1], cfg->Kpos[2];
+ dpos << cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2];
+ kori << cfg->Kori[0], cfg->Kori[1], cfg->Kori[2];
+ dori << cfg->Dori[0], cfg->Dori[1], cfg->Dori[2];
+ knull = cfg->Knull;
+ dnull = cfg->Dnull;
+
+ torqueLimit = cfg->torqueLimit;
+ timeDuration = cfg->timeDuration;
+ NJointTaskSpaceImpedanceDMPControllerControlData initData;
+ initData.targetPose = tcp->getPoseInRootFrame();
+ initData.targetVel.resize(6);
+ initData.targetVel.setZero();
+ initData.desiredNullSpaceJointValues = defaultNullSpaceJointValues;
+ reinitTripleBuffer(initData);
+
+ }
+
+ std::string NJointTaskSpaceImpedanceDMPController::getClassName(const Ice::Current&) const
+ {
+ return "NJointTaskSpaceImpedanceDMPController";
+ }
+
+ void NJointTaskSpaceImpedanceDMPController::controllerRun()
+ {
+ if (!controllerSensorData.updateReadBuffer() || !dmpCtrl)
+ {
+ return;
+ }
+
+ if (dmpCtrl->canVal < 1e-8)
+ {
+ finished = true;
+ LockGuardType guard {controlDataMutex};
+ getWriterControlStruct().targetVel.setZero();
+ writeControlStruct();
+ return;
+ }
+
+ double deltaT = controllerSensorData.getReadBuffer().deltaT;
+ Eigen::Matrix4f currentPose = controllerSensorData.getReadBuffer().currentPose;
+ Eigen::VectorXf currentTwist = controllerSensorData.getReadBuffer().currentTwist;
+
+ dmpCtrl->flow(deltaT, currentPose, currentTwist);
+
+ Eigen::VectorXf desiredNullSpaceJointValues(jointNames.size());
+ if (useNullSpaceJointDMP && isNullSpaceJointDMPLearned)
+ {
+ DMP::DVec targetJointState;
+ currentJointState = nullSpaceJointDMPPtr->calculateDirectlyVelocity(currentJointState, dmpCtrl->canVal / timeDuration, deltaT / timeDuration, targetJointState);
+
+ if (targetJointState.size() == jointNames.size())
+ {
+ for (size_t i = 0; i < targetJointState.size(); ++i)
+ {
+ desiredNullSpaceJointValues(i) = targetJointState[i];
+ }
+ }
+ else
+ {
+ desiredNullSpaceJointValues = defaultNullSpaceJointValues;
+ }
+ }
+ else
+ {
+ desiredNullSpaceJointValues = defaultNullSpaceJointValues;
+ }
+
+ LockGuardType guard {controlDataMutex};
+ getWriterControlStruct().desiredNullSpaceJointValues = desiredNullSpaceJointValues;
+ getWriterControlStruct().targetVel = dmpCtrl->getTargetVelocity();
+ getWriterControlStruct().targetPose = dmpCtrl->getTargetPoseMat();
+ writeControlStruct();
+ }
+
+ void NJointTaskSpaceImpedanceDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ double deltaT = timeSinceLastIteration.toSecondsDouble();
+
+ Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
+
+ Eigen::VectorXf qpos(positionSensors.size());
+ Eigen::VectorXf qvel(velocitySensors.size());
+ for (size_t i = 0; i < positionSensors.size(); ++i)
+ {
+ qpos(i) = positionSensors[i]->position;
+ qvel(i) = velocitySensors[i]->velocity;
+ }
+
+ Eigen::Matrix4f currentPose = tcp->getPoseInRootFrame();
+ Eigen::VectorXf currentTwist = jacobi * qvel;
+
+ controllerSensorData.getWriteBuffer().currentPose = currentPose;
+ controllerSensorData.getWriteBuffer().currentTwist = currentTwist;
+ controllerSensorData.getWriteBuffer().deltaT = deltaT;
+ controllerSensorData.getWriteBuffer().currentTime += deltaT;
+ controllerSensorData.commitWrite();
+
+ jacobi.block<3, 8>(0, 0) = 0.001 * jacobi.block<3, 8>(0, 0); // convert mm to m
+
+ Eigen::Matrix4f targetPose = rtGetControlStruct().targetPose;
+ Eigen::VectorXf targetVel = rtGetControlStruct().targetVel;
+
+ Eigen::Vector6f jointControlWrench;
+ {
+ Eigen::Vector3f targetTCPLinearVelocity;
+ targetTCPLinearVelocity << 0.001 * targetVel(0), 0.001 * targetVel(1), 0.001 * targetVel(2);
+ Eigen::Vector3f currentTCPLinearVelocity;
+ currentTCPLinearVelocity << 0.001 * currentTwist(0), 0.001 * currentTwist(1), 0.001 * currentTwist(2);
+ Eigen::Vector3f currentTCPPosition = currentPose.block<3, 1>(0, 3);
+ Eigen::Vector3f desiredPosition = targetPose.block<3, 1>(0, 3);
+ Eigen::Vector3f tcpDesiredForce = 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition) + dpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
+
+ Eigen::Vector3f currentTCPAngularVelocity;
+ currentTCPAngularVelocity << currentTwist(3), currentTwist(4), currentTwist(5);
+ Eigen::Matrix3f currentRotMat = currentPose.block<3, 3>(0, 0);
+ Eigen::Matrix3f diffMat = targetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
+ Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
+ Eigen::Vector3f tcpDesiredTorque = kori.cwiseProduct(rpy) - dori.cwiseProduct(currentTCPAngularVelocity);
+ jointControlWrench << tcpDesiredForce, tcpDesiredTorque;
+ }
+
+ Eigen::MatrixXf I = Eigen::MatrixXf::Identity(targets.size(), targets.size());
+
+ Eigen::VectorXf desiredNullSpaceJointValues = rtGetControlStruct().desiredNullSpaceJointValues;
+ Eigen::VectorXf nullspaceTorque = knull * (desiredNullSpaceJointValues - qpos) - dnull * qvel;
+ Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), 2.0);
+ Eigen::VectorXf jointDesiredTorques = jacobi.transpose() * jointControlWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
+
+ // torque limit
+ for (size_t i = 0; i < targets.size(); ++i)
+ {
+ float desiredTorque = jointDesiredTorques(i);
+
+ if (isnan(desiredTorque))
+ {
+ desiredTorque = 0;
+ }
+
+ desiredTorque = (desiredTorque > torqueLimit) ? torqueLimit : desiredTorque;
+ desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
+
+ debugOutputData.getWriteBuffer().desired_torques[jointNames[i]] = jointDesiredTorques(i);
+ debugOutputData.getWriteBuffer().desired_nullspaceJoint[jointNames[i]] = desiredNullSpaceJointValues(i);
+
+ targets.at(i)->torque = desiredTorque;
+ }
+
+ debugOutputData.getWriteBuffer().currentCanVal = dmpCtrl->debugData.canVal;
+ debugOutputData.getWriteBuffer().targetPose_x = targetPose(0, 3);
+ debugOutputData.getWriteBuffer().targetPose_y = targetPose(1, 3);
+ debugOutputData.getWriteBuffer().targetPose_z = targetPose(2, 3);
+ VirtualRobot::MathTools::Quaternion targetQuat = VirtualRobot::MathTools::eigen4f2quat(targetPose);
+ debugOutputData.getWriteBuffer().targetPose_qw = targetQuat.w;
+ debugOutputData.getWriteBuffer().targetPose_qx = targetQuat.x;
+ debugOutputData.getWriteBuffer().targetPose_qy = targetQuat.y;
+ debugOutputData.getWriteBuffer().targetPose_qz = targetQuat.z;
+
+
+
+
+
+ debugOutputData.getWriteBuffer().currentPose_x = currentPose(0, 3);
+ debugOutputData.getWriteBuffer().currentPose_y = currentPose(1, 3);
+ debugOutputData.getWriteBuffer().currentPose_z = currentPose(2, 3);
+ VirtualRobot::MathTools::Quaternion currentQuat = VirtualRobot::MathTools::eigen4f2quat(currentPose);
+ debugOutputData.getWriteBuffer().currentPose_qw = currentQuat.w;
+ debugOutputData.getWriteBuffer().currentPose_qx = currentQuat.x;
+ debugOutputData.getWriteBuffer().currentPose_qy = currentQuat.y;
+ debugOutputData.getWriteBuffer().currentPose_qz = currentQuat.z;
+
+ debugOutputData.commitWrite();
+
+ }
+
+
+ void NJointTaskSpaceImpedanceDMPController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
+ {
+ dmpCtrl->learnDMPFromFiles(fileNames);
+ ARMARX_INFO << "Learned DMP ... ";
+ }
+
+ void NJointTaskSpaceImpedanceDMPController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
+ {
+ LockGuardType guard(controllerMutex);
+ ARMARX_INFO << "setting via points ";
+ dmpCtrl->setViaPose(u, viapoint);
+ }
+
+
+ void NJointTaskSpaceImpedanceDMPController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
+ {
+ dmpCtrl->setGoalPoseVec(goals);
+ }
+
+ void NJointTaskSpaceImpedanceDMPController::learnJointDMPFromFiles(const std::string& fileName, const Ice::Current&)
+ {
+ DMP::Vec<DMP::SampledTrajectoryV2 > trajs;
+ DMP::DVec ratios;
+ DMP::SampledTrajectoryV2 traj;
+ traj.readFromCSVFile(fileName);
+ traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
+ if (traj.dim() != jointNames.size())
+ {
+ isNullSpaceJointDMPLearned = false;
+ return;
+ }
+
+ DMP::DVec goal;
+ goal.resize(traj.dim());
+ currentJointState.resize(traj.dim());
+
+ for (size_t i = 0; i < goal.size(); ++i)
+ {
+ goal.at(i) = traj.rbegin()->getPosition(i);
+ currentJointState.at(i).pos = traj.begin()->getPosition(i);
+ currentJointState.at(i).vel = 0;
+ }
+
+ trajs.push_back(traj);
+ nullSpaceJointDMPPtr->learnFromTrajectories(trajs);
+
+ // prepare exeuction of joint dmp
+ nullSpaceJointDMPPtr->prepareExecution(goal, currentJointState, 1.0, 1.0);
+ ARMARX_INFO << "prepared nullspace joint dmp";
+ isNullSpaceJointDMPLearned = true;
+ }
+
+ void NJointTaskSpaceImpedanceDMPController::runDMP(const Ice::DoubleSeq& goals, const Ice::Current&)
+ {
+ Eigen::Matrix4f pose = tcp->getPoseInRootFrame();
+ dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
+ finished = false;
+
+ if (isNullSpaceJointDMPLearned && useNullSpaceJointDMP)
+ {
+ ARMARX_INFO << "Using Null Space Joint DMP";
+ }
+
+ controllerTask->start();
+ }
+
+
+
+ void NJointTaskSpaceImpedanceDMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
+ {
+ StringVariantBaseMap datafields;
+ auto values = debugOutputData.getUpToDateReadBuffer().desired_torques;
+ for (auto& pair : values)
+ {
+ datafields[pair.first] = new Variant(pair.second);
+ }
+
+ auto values_null = debugOutputData.getUpToDateReadBuffer().desired_nullspaceJoint;
+ for (auto& pair : values_null)
+ {
+ datafields[pair.first] = new Variant(pair.second);
+ }
+
+ datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
+
+ datafields["targetPose_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_x);
+ datafields["targetPose_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_y);
+ datafields["targetPose_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_z);
+ datafields["targetPose_qw"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_qw);
+ datafields["targetPose_qx"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_qx);
+ datafields["targetPose_qy"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_qy);
+ datafields["targetPose_qz"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_qz);
+
+ datafields["currentPose_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_x);
+ datafields["currentPose_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_y);
+ datafields["currentPose_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_z);
+ datafields["currentPose_qw"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qw);
+ datafields["currentPose_qx"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qx);
+ datafields["currentPose_qy"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qy);
+ datafields["currentPose_qz"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qz);
+
+ debugObs->setDebugChannel("TaskSpaceImpedanceDMP", datafields);
+ }
+
+ void NJointTaskSpaceImpedanceDMPController::onInitComponent()
+ {
+ ARMARX_INFO << "init ...";
+ controllerTask = new PeriodicTask<NJointTaskSpaceImpedanceDMPController>(this, &NJointTaskSpaceImpedanceDMPController::controllerRun, 0.3);
+ }
+
+ void NJointTaskSpaceImpedanceDMPController::onDisconnectComponent()
+ {
+ controllerTask->stop();
+ ARMARX_INFO << "stopped ...";
+ }
+
+
+
+}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
new file mode 100644
index 0000000000000000000000000000000000000000..843908dfeb6729802815be1d3cbb3a3e326e4433
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
@@ -0,0 +1,156 @@
+
+#pragma once
+
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
+#include <VirtualRobot/Robot.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
+#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
+#include <dmp/representation/dmp/umidmp.h>
+
+
+using namespace DMP;
+namespace armarx
+{
+
+
+ TYPEDEF_PTRS_HANDLE(NJointTaskSpaceImpedanceDMPController);
+ TYPEDEF_PTRS_HANDLE(NJointTaskSpaceImpedanceDMPControllerControlData);
+
+
+ class NJointTaskSpaceImpedanceDMPControllerControlData
+ {
+ public:
+ Eigen::VectorXf targetVel;
+ Eigen::Matrix4f targetPose;
+ Eigen::VectorXf desiredNullSpaceJointValues;
+ };
+
+
+
+ class NJointTaskSpaceImpedanceDMPController :
+ public NJointControllerWithTripleBuffer<NJointTaskSpaceImpedanceDMPControllerControlData>,
+ public NJointTaskSpaceImpedanceDMPControllerInterface
+ {
+ public:
+ using ConfigPtrT = NJointTaskSpaceImpedanceDMPControllerConfigPtr;
+ NJointTaskSpaceImpedanceDMPController(NJointControllerDescriptionProviderInterfacePtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+
+ // NJointControllerInterface interface
+ std::string getClassName(const Ice::Current&) const;
+
+ // NJointController interface
+
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+
+ // NJointTaskSpaceImpedanceDMPControllerInterface interface
+ void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&);
+ bool isFinished(const Ice::Current&)
+ {
+ return finished;
+ }
+
+ void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&);
+ void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
+
+ void learnJointDMPFromFiles(const std::string& fileName, const Ice::Current&);
+ void runDMP(const Ice::DoubleSeq& goals, const Ice::Current&);
+ Ice::Double getVirtualTime(const Ice::Current&)
+ {
+ return dmpCtrl->canVal;
+ }
+
+ protected:
+ virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
+
+ void onInitComponent();
+ void onDisconnectComponent();
+ void controllerRun();
+
+ private:
+ struct DebugBufferData
+ {
+ double currentCanVal;
+ float targetPose_x;
+ float targetPose_y;
+ float targetPose_z;
+ float targetPose_qw;
+ float targetPose_qx;
+ float targetPose_qy;
+ float targetPose_qz;
+
+ float currentPose_x;
+ float currentPose_y;
+ float currentPose_z;
+ float currentPose_qw;
+ float currentPose_qx;
+ float currentPose_qy;
+ float currentPose_qz;
+
+ StringFloatDictionary desired_torques;
+ StringFloatDictionary desired_nullspaceJoint;
+
+ };
+
+ TripleBuffer<DebugBufferData> debugOutputData;
+
+ struct NJointTaskSpaceImpedanceDMPControllerSensorData
+ {
+ double currentTime;
+ double deltaT;
+ Eigen::Matrix4f currentPose;
+ Eigen::VectorXf currentTwist;
+ };
+ TripleBuffer<NJointTaskSpaceImpedanceDMPControllerSensorData> controllerSensorData;
+
+ TaskSpaceDMPControllerPtr dmpCtrl;
+
+ std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors;
+ std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors;
+ std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
+ std::vector<ControlTarget1DoFActuatorTorque*> targets;
+
+ // velocity ik controller parameters
+ // dmp parameters
+ double timeDuration;
+ bool finished;
+
+ // phaseStop parameters
+ double phaseL;
+ double phaseK;
+ double phaseDist0;
+ double phaseDist1;
+ double posToOriRatio;
+
+
+ NJointTaskSpaceImpedanceDMPControllerConfigPtr cfg;
+ VirtualRobot::DifferentialIKPtr ik;
+ VirtualRobot::RobotNodePtr tcp;
+
+ float torqueLimit;
+
+ Eigen::Vector3f kpos;
+ Eigen::Vector3f kori;
+ Eigen::Vector3f dpos;
+ Eigen::Vector3f dori;
+ float knull;
+ float dnull;
+
+ DMP::UMIDMPPtr nullSpaceJointDMPPtr;
+ bool useNullSpaceJointDMP;
+ bool isNullSpaceJointDMPLearned;
+ DMP::Vec<DMP::DMPState> currentJointState;
+
+
+ Eigen::VectorXf defaultNullSpaceJointValues;
+ std::vector<std::string> jointNames;
+ mutable MutexType controllerMutex;
+ PeriodicTask<NJointTaskSpaceImpedanceDMPController>::pointer_type controllerTask;
+
+ };
+
+} // namespace armarx
+
diff --git a/source/RobotAPI/libraries/core/CMakeLists.txt b/source/RobotAPI/libraries/core/CMakeLists.txt
index 76beb836204971595ff2b84c4b2f973ea488bb09..9e863f65c7c570d36a34a4c5933c27d0a82253ac 100644
--- a/source/RobotAPI/libraries/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/core/CMakeLists.txt
@@ -31,6 +31,7 @@ set(LIB_FILES
FramedOrientedPoint.cpp
RobotStatechartContext.cpp
checks/ConditionCheckMagnitudeChecks.cpp
+ observerfilters/PoseAverageFilter.cpp
observerfilters/PoseMedianOffsetFilter.cpp
observerfilters/MedianDerivativeFilterV3.cpp
RobotAPIObjectFactories.cpp
@@ -59,6 +60,7 @@ set(LIB_HEADERS
FramedOrientedPoint.h
RobotStatechartContext.h
observerfilters/PoseMedianFilter.h
+ observerfilters/PoseAverageFilter.h
observerfilters/PoseMedianOffsetFilter.h
observerfilters/OffsetFilter.h
observerfilters/MatrixFilters.h
diff --git a/source/RobotAPI/libraries/core/RobotAPIObjectFactories.cpp b/source/RobotAPI/libraries/core/RobotAPIObjectFactories.cpp
index 0a85c9d8b064d34f62f2cec5aee27daf675a8d7c..e2d22fa0e321fc0ce6c3699466fc24b5002755c0 100644
--- a/source/RobotAPI/libraries/core/RobotAPIObjectFactories.cpp
+++ b/source/RobotAPI/libraries/core/RobotAPIObjectFactories.cpp
@@ -32,6 +32,7 @@
#include <RobotAPI/libraries/core/observerfilters/OffsetFilter.h>
#include <RobotAPI/libraries/core/observerfilters/MatrixFilters.h>
#include <RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.h>
+#include <RobotAPI/libraries/core/observerfilters/PoseAverageFilter.h>
#include <RobotAPI/libraries/core/observerfilters/MedianDerivativeFilterV3.h>
#include <RobotAPI/libraries/core/OrientedPoint.h>
#include <RobotAPI/libraries/core/FramedOrientedPoint.h>
@@ -59,6 +60,7 @@ ObjectFactoryMap RobotAPIObjectFactories::getFactories()
add<armarx::FramedOrientedPointBase, armarx::FramedOrientedPoint>(map);
add<armarx::PoseMedianFilterBase, armarx::filters::PoseMedianFilter>(map);
+ add<armarx::PoseAverageFilterBase, armarx::filters::PoseAverageFilter>(map);
add<armarx::PoseMedianOffsetFilterBase, armarx::filters::PoseMedianOffsetFilter>(map);
add<armarx::MedianDerivativeFilterV3Base, armarx::filters::MedianDerivativeFilterV3>(map);
add<armarx::OffsetFilterBase, armarx::filters::OffsetFilter>(map);
@@ -69,5 +71,6 @@ ObjectFactoryMap RobotAPIObjectFactories::getFactories()
add<armarx::MatrixPercentilesFilterBase, armarx::filters::MatrixPercentilesFilter>(map);
add<armarx::MatrixCumulativeFrequencyFilterBase, armarx::filters::MatrixCumulativeFrequencyFilter>(map);
add<armarx::TrajectoryBase, armarx::Trajectory>(map);
+
return map;
}
diff --git a/source/RobotAPI/libraries/core/observerfilters/PoseAverageFilter.cpp b/source/RobotAPI/libraries/core/observerfilters/PoseAverageFilter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e7a3eb89f12d234c31a406b1b54f9b927c937252
--- /dev/null
+++ b/source/RobotAPI/libraries/core/observerfilters/PoseAverageFilter.cpp
@@ -0,0 +1,116 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ArmarX
+ * @author Mirko Waechter( mirko.waechter at kit dot edu)
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#include "PoseAverageFilter.h"
+
+namespace armarx
+{
+ namespace filters
+ {
+
+ VariantBasePtr PoseAverageFilter::calculate(const Ice::Current& c) const
+ {
+ ScopedLock lock(historyMutex);
+
+ if (dataHistory.size() == 0)
+ {
+ return NULL;
+ }
+
+ VariantTypeId type = dataHistory.begin()->second->getType();
+
+ if ((type == VariantType::Vector3) || (type == VariantType::FramedDirection) || (type == VariantType::FramedPosition))
+ {
+
+ Eigen::Vector3f vec;
+ vec.setZero();
+ std::string frame = "";
+ std::string agent = "";
+ VariantPtr var = VariantPtr::dynamicCast(dataHistory.begin()->second);
+
+ if (type == VariantType::FramedDirection)
+ {
+ FramedDirectionPtr p = var->get<FramedDirection>();
+ frame = p->frame;
+ agent = p->agent;
+ }
+ else if (type == VariantType::FramedPosition)
+ {
+ FramedPositionPtr p = var->get<FramedPosition>();
+ frame = p->frame;
+ agent = p->agent;
+ }
+ Eigen::MatrixXf keypointPositions(dataHistory.size(), 3);
+ int i = 0;
+ for (auto& v : dataHistory)
+ {
+ VariantPtr v2 = VariantPtr::dynamicCast(v.second);
+ Eigen::Vector3f value = v2->get<Vector3>()->toEigen();
+ keypointPositions(i, 0) = value(0);
+ keypointPositions(i, 1) = value(1);
+ keypointPositions(i, 2) = value(2);
+ i++;
+ }
+ vec = keypointPositions.colwise().mean();
+
+
+ if (type == VariantType::Vector3)
+ {
+ Vector3Ptr vecVar = new Vector3(vec);
+ return new Variant(vecVar);
+ }
+ else if (type == VariantType::FramedDirection)
+ {
+
+ FramedDirectionPtr vecVar = new FramedDirection(vec, frame, agent);
+ return new Variant(vecVar);
+ }
+ else if (type == VariantType::FramedPosition)
+ {
+ FramedPositionPtr vecVar = new FramedPosition(vec, frame, agent);
+ return new Variant(vecVar);
+ }
+ else
+ {
+ ARMARX_WARNING_S << "Implementation missing here";
+ return NULL;
+ }
+ }
+ else if (type == VariantType::Double)
+ {
+ // auto values = SortVariants<double>(dataHistory);
+ // return new Variant(values.at(values.size()/2));
+ }
+ else if (type == VariantType::Int)
+ {
+ // auto values = SortVariants<int>(dataHistory);
+ // return new Variant(values.at(values.size()/2));
+ }
+
+ return AverageFilter::calculate(c);
+ }
+
+
+
+ } // namespace filters
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/observerfilters/PoseAverageFilter.h b/source/RobotAPI/libraries/core/observerfilters/PoseAverageFilter.h
new file mode 100644
index 0000000000000000000000000000000000000000..34dae88a939f94111f1507dd8d1aee5132b1fa87
--- /dev/null
+++ b/source/RobotAPI/libraries/core/observerfilters/PoseAverageFilter.h
@@ -0,0 +1,68 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ArmarX
+ * @author Mirko Waechter( mirko.waechter at kit dot edu)
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
+
+#include <ArmarXCore/observers/filters/AverageFilter.h>
+
+#include <RobotAPI/libraries/core/FramedPose.h>
+
+
+
+namespace armarx
+{
+ namespace filters
+ {
+
+ class PoseAverageFilter :
+ public ::armarx::PoseAverageFilterBase,
+ public AverageFilter
+ {
+ public:
+ PoseAverageFilter(int windowSize = 11)
+ {
+ this->windowFilterSize = windowSize;
+ }
+
+ // DatafieldFilterBase interface
+ public:
+ VariantBasePtr calculate(const Ice::Current& c) const override;
+
+ /**
+ * @brief This filter supports: Vector3, FramedDirection, FramedPosition
+ * @return List of VariantTypes
+ */
+ ParameterTypeList getSupportedTypes(const Ice::Current& c) const override
+ {
+ ParameterTypeList result = AverageFilter::getSupportedTypes(c);
+ result.push_back(VariantType::Vector3);
+ result.push_back(VariantType::FramedDirection);
+ result.push_back(VariantType::FramedPosition);
+ return result;
+ }
+ };
+
+ } // namespace
+} // namespace
+
+
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
index b1399079c9f1b84f8e323ff41967cfd3480d88c3..cfa6f00629a5d027130a5f0cb76bb8a488a49e09 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
@@ -477,7 +477,7 @@ namespace armarx
if (!c->setConfig(jointName, jointAngle))
{
- ARMARX_WARNING_S << "Joint not known in local copy:" << jointName << ". Skipping..." << endl;
+ ARMARX_WARNING << deactivateSpam(10, jointName) << "Joint not known in local copy:" << jointName << ". Skipping..." << endl;
}
}
@@ -510,7 +510,7 @@ namespace armarx
if (!c->setConfig(jointName, jointAngle))
{
- ARMARX_WARNING_S << "Joint not known in local copy:" << jointName << ". Skipping..." << endl;
+ ARMARX_WARNING << deactivateSpam(10, jointName) << "Joint not known in local copy:" << jointName << ". Skipping..." << endl;
}
}