From a9cf7786c735a7db2acdfc6d062f4825cfae98c4 Mon Sep 17 00:00:00 2001
From: ArmarX User <armarx@kit.edu>
Date: Thu, 29 Aug 2019 12:32:43 +0200
Subject: [PATCH] fix merge
---
.../NJointTaskSpaceDMPController.ice | 5 ++
.../DMPController/TaskSpaceDMPController.cpp | 5 +-
.../DMPController/TaskSpaceDMPController.h | 5 +-
.../DMPController/NJointTSDMPController.cpp | 78 ++++++++++---------
.../DMPController/NJointTSDMPController.h | 1 -
source/RobotAPI/libraries/core/CMakeLists.txt | 6 ++
6 files changed, 59 insertions(+), 41 deletions(-)
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index 70defbba2..5d5a40944 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -264,11 +264,16 @@ module armarx
bool isFinished();
void runDMP(Ice::DoubleSeq goals);
+ void runDMPWithTime(Ice::DoubleSeq goals, double timeDuration);
+
void setViaPoints(double canVal, Ice::DoubleSeq point);
void setGoals(Ice::DoubleSeq goals);
double getVirtualTime();
+ void resetDMP();
+ void stopDMP();
+ void resumeDMP();
};
diff --git a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp
index 304fab026..7f84c71a0 100644
--- a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp
+++ b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp
@@ -43,7 +43,7 @@ double TaskSpaceDMPController::flow(double canVal, double deltaT, const Eigen::M
}
if (canVal < 0.1 && config.DMPStyle == "Periodic")
{
- canVal = config.motionTimeDuration;
+ canVal = config.motionTimeDuration; // prepareExecution(eigen4f2vec(currentPose), goalPoseVec);
}
if (canVal < 1e-8 && config.DMPStyle == "Discrete")
{
@@ -85,8 +85,9 @@ double TaskSpaceDMPController::flow(double canVal, double deltaT, const Eigen::M
isDisturbance = false;
}
+// double tau = dmpPtr->getTemporalFactor();
double timeDuration = config.motionTimeDuration;
- canVal -= tau * deltaT * 1;// / (1 + phaseStop) ;
+ canVal -= tau * deltaT * 1;// / (1 + phaseStop) ; //canVal -= deltaT * 1 / (1 + phaseStop) ;
DMP::Vec<DMP::DMPState > temporalState = dmpPtr->calculateDirectlyVelocity(currentState, canVal / timeDuration, deltaT / timeDuration, targetPoseVec);
diff --git a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h
index 81e87e887..aec4f1c0a 100644
--- a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h
+++ b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h
@@ -199,8 +199,6 @@ namespace armarx
double canVal;
bool isPhaseStopControl;
std::string dmpName;
- TaskSpaceDMPControllerConfig config;
- bool paused;
private:
@@ -212,7 +210,8 @@ namespace armarx
DMP::UMITSMPPtr dmpPtr;
DMP::Vec<DMP::DMPState > currentState;
-
+ TaskSpaceDMPControllerConfig config;
+ bool paused;
bool isDisturbance;
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
index 24ffb9039..9a13d4079 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
@@ -19,14 +19,21 @@ namespace armarx
const SensorValueBase* sv = useSensorValue(jointName);
targets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
- // const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
- // const SensorValue1DoFGravityTorque* gravityTorqueSensor = sv->asA<SensorValue1DoFGravityTorque>();
-
+ const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
+ const SensorValue1DoFGravityTorque* gravityTorqueSensor = sv->asA<SensorValue1DoFGravityTorque>();
const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
+ 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);
+ torqueSensors.push_back(torqueSensor);
+ gravityTorqueSensors.push_back(gravityTorqueSensor);
velocitySensors.push_back(velocitySensor);
positionSensors.push_back(positionSensor);
};
@@ -71,17 +78,20 @@ namespace armarx
initSensorData.currentTwist.setZero();
rt2CtrlData.reinitAllBuffers(initSensorData);
- targetVels.resize(6);
-
- NJointTSDMPControllerControlData initData;
- initData.targetTSVel.resize(6);
- for (size_t i = 0; i < 6; ++i)
- {
- initData.targetTSVel(i) = 0;
- targetVels(i) = 0;
- }
- initData.nullspaceJointVelocities.resize(tcpController->rns->getSize(), 0);
- reinitTripleBuffer(initData);
+ // targetVels.resize(6);
+ // NJointTSDMPControllerControlData initData;
+ // initData.targetTSVel.resize(6);
+ // for (size_t i = 0; i < 6; ++i)
+ // {
+ // initData.targetTSVel(i) = 0;
+ // targetVels(i) = 0;
+ // }
+ // initData.targetPose = tcp->getPoseInRootFrame();
+ // 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);
+ // reinitTripleBuffer(initData);
debugName = cfg->debugName;
@@ -115,9 +125,9 @@ namespace armarx
// NJointTSDMPControllerInterfaceData initInterfaceData;
// initInterfaceData.currentTcpPose = tcp->getPoseInRootFrame();
// interfaceData.reinitAllBuffers(initInterfaceData);
- RTToUserData initInterfaceData2;
- initInterfaceData2.currentTcpPose = Eigen::Matrix4f::Identity();
- rt2UserData.reinitAllBuffers(initInterfaceData2);
+ RTToUserData initInterfaceData;
+ initInterfaceData.currentTcpPose = tcp->getPoseInRootFrame();
+ rt2UserData.reinitAllBuffers(initInterfaceData);
}
std::string NJointTSDMPController::getClassName(const Ice::Current&) const
@@ -506,22 +516,20 @@ namespace armarx
void NJointTSDMPController::onInitNJointController()
{
ARMARX_INFO << "init ...";
- /*
- targetVels.resize(6);
- NJointTSDMPControllerControlData initData;
- initData.targetTSVel.resize(6);
- for (size_t i = 0; i < 6; ++i)
- {
- initData.targetTSVel(i) = 0;
- targetVels(i) = 0;
- initData.targetPose = tcp->getPoseInRootFrame();
- }
- 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);
- reinitTripleBuffer(initData);
- */
+ targetVels.resize(6);
+ NJointTSDMPControllerControlData initData;
+ initData.targetTSVel.resize(6);
+ for (size_t i = 0; i < 6; ++i)
+ {
+ initData.targetTSVel(i) = 0;
+ targetVels(i) = 0;
+ initData.targetPose = tcp->getPoseInRootFrame();
+ }
+ 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);
+ reinitTripleBuffer(initData);
started = false;
runTask("NJointTSDMPController", [&]
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
index 924393193..403bbaef1 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
@@ -36,7 +36,6 @@ namespace armarx
std::vector<float> torqueKp;
std::vector<float> torqueKd;
VirtualRobot::IKSolver::CartesianSelection mode = VirtualRobot::IKSolver::All;
-
};
class pidController
diff --git a/source/RobotAPI/libraries/core/CMakeLists.txt b/source/RobotAPI/libraries/core/CMakeLists.txt
index 249994acd..73a786619 100644
--- a/source/RobotAPI/libraries/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/core/CMakeLists.txt
@@ -3,8 +3,14 @@ armarx_set_target("RobotAPI Core Library: RobotAPICore")
find_package(Eigen3 QUIET)
find_package(Simox ${ArmarX_Simox_VERSION} QUIET)
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
+if (Eigen3_FOUND AND Simox_FOUND)
+ include_directories(${Simox_INCLUDE_DIRS})
+ include_directories(SYSTEM ${Eigen3_INCLUDE_DIR})
+endif()
+
set(LIB_NAME RobotAPICore)
set(LIBS RobotAPIInterfaces ArmarXCoreObservers ArmarXCoreStatechart ArmarXCoreEigen3Variants
--
GitLab