From 1025359e7c6c310a6a3ad199055a27e2ffb48887 Mon Sep 17 00:00:00 2001
From: zhou <you.zhou@kit.edu>
Date: Thu, 4 Jun 2020 13:20:55 +0200
Subject: [PATCH] added interface func for changing control parameters in
NJointBimanualObjLevelController
---
.../NJointBimanualObjLevelController.ice | 9 +-
.../NJointBimanualObjLevelController.cpp | 134 +++++++++++++++++-
.../NJointBimanualObjLevelController.h | 25 ++++
3 files changed, 166 insertions(+), 2 deletions(-)
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointBimanualObjLevelController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointBimanualObjLevelController.ice
index 2707d344c..9053a1d8a 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointBimanualObjLevelController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointBimanualObjLevelController.ice
@@ -112,7 +112,14 @@ module armarx
void setViaPoints(double u, Ice::DoubleSeq viapoint);
double getVirtualTime();
-
+ void setKpImpedance(Ice::FloatSeq value);
+ void setKdImpedance(Ice::FloatSeq value);
+ void setKmAdmittance(Ice::FloatSeq value);
+ void setKpAdmittance(Ice::FloatSeq value);
+ void setKdAdmittance(Ice::FloatSeq value);
+
+ Ice::FloatSeq getCurrentObjVel();
+ Ice::FloatSeq getCurrentObjForce();
};
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp
index 02e92853f..1baf81f98 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp
@@ -104,6 +104,7 @@ namespace armarx
tcpLeft = leftRNS->getTCP();
tcpRight = rightRNS->getTCP();
+ //initialize control parameters
KpImpedance.setZero(cfg->KpImpedance.size());
ARMARX_CHECK_EQUAL(cfg->KpImpedance.size(), 12);
@@ -144,6 +145,15 @@ namespace armarx
KmAdmittance(i) = cfg->KmAdmittance.at(i);
}
+
+ Inferface2rtData initInt2rtData;
+ initInt2rtData.KpImpedance = KpImpedance;
+ initInt2rtData.KdImpedance = KdImpedance;
+ initInt2rtData.KmAdmittance = KmAdmittance;
+ initInt2rtData.KpAdmittance = KpAdmittance;
+ initInt2rtData.KdAdmittance = KdAdmittance;
+ interface2rtBuffer.reinitAllBuffers(initInt2rtData);
+
leftDesiredJointValues.resize(leftTargets.size());
ARMARX_CHECK_EQUAL(cfg->leftDesiredJointValues.size(), leftTargets.size());
@@ -191,6 +201,9 @@ namespace armarx
ControlInterfaceData initInterfaceData;
initInterfaceData.currentLeftPose = tcpLeft->getPoseInRootFrame();
initInterfaceData.currentRightPose = tcpRight->getPoseInRootFrame();
+ initInterfaceData.currentObjPose = boxInitialPose;
+ initInterfaceData.currentObjForce.setZero();
+ initInterfaceData.currentObjVel.setZero();
controlInterfaceBuffer.reinitAllBuffers(initInterfaceData);
@@ -254,6 +267,10 @@ namespace armarx
{
targetWrench(i) = cfg->targetWrench[i];
}
+
+
+
+
}
@@ -314,7 +331,6 @@ namespace armarx
controlInterfaceBuffer.getWriteBuffer().currentLeftPose = currentLeftPose;
controlInterfaceBuffer.getWriteBuffer().currentRightPose = currentRightPose;
- controlInterfaceBuffer.commitWrite();
double deltaT = timeSinceLastIteration.toSecondsDouble();
ftcalibrationTimer += deltaT;
@@ -342,6 +358,13 @@ namespace armarx
ARMARX_INFO << "modified right pose:\n " << rightPose;
}
+ // --------------------------------------------- get control parameters ---------------------------------------
+ KpImpedance = interface2rtBuffer.getUpToDateReadBuffer().KpImpedance;
+ KdImpedance = interface2rtBuffer.getUpToDateReadBuffer().KdImpedance;
+ KmAdmittance = interface2rtBuffer.getUpToDateReadBuffer().KmAdmittance;
+ KpAdmittance = interface2rtBuffer.getUpToDateReadBuffer().KpAdmittance;
+ KdAdmittance = interface2rtBuffer.getUpToDateReadBuffer().KdAdmittance;
+
if (ftcalibrationTimer < cfg->ftCalibrationTime)
{
ftOffset.block<3, 1>(0, 0) = 0.5 * ftOffset.block<3, 1>(0, 0) + 0.5 * leftForceTorque->force;
@@ -453,6 +476,8 @@ namespace armarx
rt2ControlBuffer.getWriteBuffer().currentTime += deltaT;
rt2ControlBuffer.commitWrite();
+
+
// --------------------------------------------- get ft sensor ---------------------------------------------
// static compensation
Eigen::Vector3f gravity;
@@ -503,11 +528,20 @@ namespace armarx
}
}
+ // pass sensor value to statechart
+ controlInterfaceBuffer.getWriteBuffer().currentObjPose = boxCurrentPose;
+ controlInterfaceBuffer.getWriteBuffer().currentObjVel = boxCurrentTwist.head(3);
+ controlInterfaceBuffer.getWriteBuffer().currentObjForce = objFTValue.head(3);
+ controlInterfaceBuffer.commitWrite();
+
// --------------------------------------------- get MP target ---------------------------------------------
Eigen::Matrix4f boxPose = rtGetControlStruct().boxPose;
Eigen::VectorXf boxTwist = rtGetControlStruct().boxTwist;
+
+
+
// --------------------------------------------- obj admittance control ---------------------------------------------
// admittance
Eigen::VectorXf objPoseError(6);
@@ -746,6 +780,102 @@ namespace armarx
}
+ void NJointBimanualObjLevelController::setKpImpedance(const Ice::FloatSeq& value, const Ice::Current&)
+ {
+ Eigen::VectorXf setpoint;
+ setpoint.setZero(value.size());
+ ARMARX_CHECK_EQUAL(value.size(), 12);
+
+ for (size_t i = 0; i < value.size(); ++i)
+ {
+ setpoint(i) = value.at(i);
+ }
+
+ LockGuardType guard {interfaceDataMutex};
+ interface2rtBuffer.getWriteBuffer().KpImpedance = setpoint;
+ interface2rtBuffer.commitWrite();
+
+ }
+
+ void NJointBimanualObjLevelController::setKdImpedance(const Ice::FloatSeq& value, const Ice::Current&)
+ {
+ Eigen::VectorXf setpoint;
+ setpoint.setZero(value.size());
+ ARMARX_CHECK_EQUAL(value.size(), 12);
+
+ for (size_t i = 0; i < value.size(); ++i)
+ {
+ setpoint(i) = value.at(i);
+ }
+
+ LockGuardType guard {interfaceDataMutex};
+ interface2rtBuffer.getWriteBuffer().KdImpedance = setpoint;
+ interface2rtBuffer.commitWrite();
+ }
+
+ void NJointBimanualObjLevelController::setKpAdmittance(const Ice::FloatSeq& value, const Ice::Current&)
+ {
+ Eigen::VectorXf setpoint;
+ setpoint.setZero(value.size());
+ ARMARX_CHECK_EQUAL(value.size(), 6);
+
+ for (size_t i = 0; i < value.size(); ++i)
+ {
+ setpoint(i) = value.at(i);
+ }
+
+ LockGuardType guard {interfaceDataMutex};
+ interface2rtBuffer.getWriteBuffer().KpAdmittance = setpoint;
+ interface2rtBuffer.commitWrite();
+ }
+
+ void NJointBimanualObjLevelController::setKdAdmittance(const Ice::FloatSeq& value, const Ice::Current&)
+ {
+ Eigen::VectorXf setpoint;
+ setpoint.setZero(value.size());
+ ARMARX_CHECK_EQUAL(value.size(), 6);
+
+ for (size_t i = 0; i < value.size(); ++i)
+ {
+ setpoint(i) = value.at(i);
+ }
+
+ LockGuardType guard {interfaceDataMutex};
+ interface2rtBuffer.getWriteBuffer().KdAdmittance = setpoint;
+ interface2rtBuffer.commitWrite();
+ }
+
+ std::vector<float> NJointBimanualObjLevelController::getCurrentObjVel(const Ice::Current&)
+ {
+ Eigen::Vector3f tvel = controlInterfaceBuffer.getUpToDateReadBuffer().currentObjVel;
+ std::vector<float> tvelvec = {tvel(0), tvel(1), tvel(2)};
+ return tvelvec;
+ }
+
+ std::vector<float> NJointBimanualObjLevelController::getCurrentObjForce(const Ice::Current&)
+ {
+ Eigen::Vector3f fvel = controlInterfaceBuffer.getUpToDateReadBuffer().currentObjForce;
+ std::vector<float> fvelvec = {fvel(0), fvel(1), fvel(2)};
+ return fvelvec;
+ }
+
+ void NJointBimanualObjLevelController::setKmAdmittance(const Ice::FloatSeq& value, const Ice::Current&)
+ {
+ Eigen::VectorXf setpoint;
+ setpoint.setZero(value.size());
+ ARMARX_CHECK_EQUAL(value.size(), 6);
+
+ for (size_t i = 0; i < value.size(); ++i)
+ {
+ setpoint(i) = value.at(i);
+ }
+
+ LockGuardType guard {interfaceDataMutex};
+ interface2rtBuffer.getWriteBuffer().KmAdmittance = setpoint;
+ interface2rtBuffer.commitWrite();
+ }
+
+
void NJointBimanualObjLevelController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
{
@@ -890,6 +1020,8 @@ namespace armarx
void NJointBimanualObjLevelController::onInitNJointController()
{
+
+
ARMARX_INFO << "init ...";
runTask("NJointBimanualObjLevelController", [&]
{
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.h
index 7f1c10d71..8cf7c3d2c 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.h
@@ -75,6 +75,15 @@ namespace armarx
return virtualtimer;
}
+ void setKpImpedance(const Ice::FloatSeq& value, const Ice::Current&);
+ void setKdImpedance(const Ice::FloatSeq& value, const Ice::Current&);
+ void setKmAdmittance(const Ice::FloatSeq& value, const Ice::Current&);
+ void setKpAdmittance(const Ice::FloatSeq& value, const Ice::Current&);
+ void setKdAdmittance(const Ice::FloatSeq& value, const Ice::Current&);
+
+ std::vector<float> getCurrentObjVel(const Ice::Current&);
+ std::vector<float> getCurrentObjForce(const Ice::Current&);
+
protected:
virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
@@ -175,10 +184,25 @@ namespace armarx
{
Eigen::Matrix4f currentLeftPose;
Eigen::Matrix4f currentRightPose;
+ Eigen::Matrix4f currentObjPose;
+ Eigen::Vector3f currentObjVel;
+ Eigen::Vector3f currentObjForce;
};
TripleBuffer<ControlInterfaceData> controlInterfaceBuffer;
+ struct Inferface2rtData
+ {
+ Eigen::VectorXf KpImpedance;
+ Eigen::VectorXf KdImpedance;
+ Eigen::VectorXf KmAdmittance;
+ Eigen::VectorXf KpAdmittance;
+ Eigen::VectorXf KdAdmittance;
+ };
+ TripleBuffer<Inferface2rtData> interface2rtBuffer;
+
+
+
std::vector<ControlTarget1DoFActuatorTorque*> leftTargets;
std::vector<const SensorValue1DoFActuatorAcceleration*> leftAccelerationSensors;
std::vector<const SensorValue1DoFActuatorVelocity*> leftVelocitySensors;
@@ -203,6 +227,7 @@ namespace armarx
double virtualtimer;
mutable MutexType controllerMutex;
+ mutable MutexType interfaceDataMutex;
Eigen::VectorXf leftDesiredJointValues;
Eigen::VectorXf rightDesiredJointValues;
--
GitLab