From 254c8c758a2f1a0e4ad635bad3ca79f59f0c62d9 Mon Sep 17 00:00:00 2001
From: zhou <you.zhou@kit.edu>
Date: Mon, 8 Oct 2018 11:24:41 +0200
Subject: [PATCH] added setViaPoints for BimanualForceMPController
---
.../NJointBimanualForceMPController.ice | 2 ++
.../NJointBimanualForceMPController.cpp | 23 ++++++++++++++++---
.../NJointBimanualForceMPController.h | 2 ++
3 files changed, 24 insertions(+), 3 deletions(-)
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointBimanualForceMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointBimanualForceMPController.ice
index 2f1b3c366..49acfeefb 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointBimanualForceMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointBimanualForceMPController.ice
@@ -77,6 +77,8 @@ module armarx
bool isFinished();
void runDMP(Ice::DoubleSeq leftGoals, Ice::DoubleSeq rightGoals);
double getCanVal();
+
+ void setViaPoints(string whichDMP, double canVal, Ice::DoubleSeq viaPoint);
};
};
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.cpp
index 1aa129cb5..eeb9ebe8f 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.cpp
@@ -1,4 +1,6 @@
#include "NJointBimanualForceMPController.h"
+#include <random>
+
namespace armarx
{
@@ -158,18 +160,22 @@ namespace armarx
Eigen::Vector3f leftForce = controllerSensorData.getReadBuffer().leftForceInRootFrame;
Eigen::Vector3f rightForce = controllerSensorData.getReadBuffer().rightForceInRootFrame;
- float forceOnHands = (leftForce + rightForce)(2);
+ float forceOnHands = (leftForce + rightForce)(2) ;
+
+ xvel = cfg->forceP * (targetSupportForce + forceOnHands) + I_decay * cfg->forceI * forceIterm;
- xvel = cfg->forceP * (targetSupportForce - forceOnHands) + I_decay * cfg->forceI * forceIterm;
forceIterm += targetSupportForce - forceOnHands;
canVal = canVal + forceSign * xvel * deltaT;
+ // canVal = cfg->timeDuration + forceSign * xvel;
+
if (canVal > cfg->timeDuration)
{
canVal = cfg->timeDuration;
}
+
leftDMPController->canVal = canVal;
rightDMPController->canVal = canVal;
@@ -254,7 +260,6 @@ namespace armarx
}
Eigen::Vector6f rightTwist = rightJacobi * qvel;
-
leftFilteredValue = (1 - cfg->filterCoeff) * (leftForceTorque->force - leftForceOffset) + cfg->filterCoeff * leftFilteredValue;
rightFilteredValue = (1 - cfg->filterCoeff) * (rightForceTorque->force - rightForceOffset) + cfg->filterCoeff * rightFilteredValue;
@@ -432,6 +437,18 @@ namespace armarx
}
+ void NJointBimanualForceMPController::setViaPoints(const std::string& whichDMP, double u, const Ice::DoubleSeq& viaPoint, const Ice::Current&)
+ {
+ if (whichDMP == "Left")
+ {
+ leftDMPController->setViaPose(u, viaPoint);
+ }
+ if (whichDMP == "Right")
+ {
+ rightDMPController->setViaPose(u, viaPoint);
+ }
+ }
+
void NJointBimanualForceMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
{
std::string debugName = cfg->debugName;
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.h
index f276fc1f3..c59c12e2f 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.h
@@ -63,6 +63,8 @@ namespace armarx
return canVal;
}
+ void setViaPoints(const string& whichDMP, double canVal, const Ice::DoubleSeq& viaPoint, const Ice::Current&);
+
protected:
virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
--
GitLab