From b6e953fd443f54bc279ad23b12dc6450db046c3a Mon Sep 17 00:00:00 2001
From: ArmarX User <armarx@kit.edu>
Date: Tue, 18 Feb 2020 09:59:49 +0100
Subject: [PATCH] some code cleanup in NJointTSDMPController
---
.../DMPController/NJointTSDMPController.cpp | 58 +++++--------------
.../DMPController/NJointTSDMPController.h | 15 +++--
2 files changed, 22 insertions(+), 51 deletions(-)
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
index eb262048b..4c8aced57 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
@@ -47,7 +47,6 @@ namespace armarx
// set tcp controller
tcpController.reset(new CartesianVelocityController(rns, tcp));
nodeSetName = cfg->nodeSetName;
- torquePIDs.resize(tcpController->rns->getSize(), pidController());
ik.reset(new VirtualRobot::DifferentialIK(rns, refFrame, VirtualRobot::JacobiProvider::eSVDDamped));
@@ -82,29 +81,16 @@ namespace armarx
rt2CtrlData.reinitAllBuffers(initSensorData);
// targetVels.resize(6);
- // NJointTSDMPControllerControlData initData;
+ targetVels.setZero(6);
+ NJointTSDMPControllerControlData initData;
// initData.targetTSVel.resize(6);
+ initData.targetTSVel.setZero(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.targetPose = refFrame->toLocalCoordinateSystem(tcp->getGlobalPose());
initData.nullspaceJointVelocities.resize(tcpController->rns->getSize(), 0);
@@ -123,7 +109,7 @@ namespace armarx
filtered_qvel.setZero(targets.size());
vel_filter_factor = cfg->vel_filter;
- filtered_position.setZero();
+ filtered_position.setZero(3);
pos_filter_factor = cfg->pos_filter;
// jlhigh = rns->getNode("..")->getJointLimitHi();
@@ -242,8 +228,6 @@ namespace armarx
void NJointTSDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
{
- // Eigen::Matrix4f currentPose = tcp->getPoseInRootFrame();
- //
Eigen::Matrix4f currentPose = refFrame->toLocalCoordinateSystem(tcp->getGlobalPose());
rt2UserData.getWriteBuffer().currentTcpPose = currentPose;
rt2UserData.commitWrite();
@@ -268,14 +252,10 @@ namespace armarx
Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
- Eigen::VectorXf qvel;
- qvel.resize(velocitySensors.size());
- Eigen::VectorXf qpos;
- qpos.resize(positionSensors.size());
+ Eigen::VectorXf qvel(velocitySensors.size());
for (size_t i = 0; i < velocitySensors.size(); ++i)
{
qvel(i) = velocitySensors[i]->velocity;
- qpos(i) = positionSensors[i]->position;
}
filtered_qvel = (1 - vel_filter_factor) * filtered_qvel + vel_filter_factor * qvel;
@@ -293,18 +273,16 @@ namespace armarx
Eigen::VectorXf targetVel = rtGetControlStruct().targetTSVel;
Eigen::Matrix4f targetPose = rtGetControlStruct().targetPose;
- Eigen::VectorXf jointTargetVelocities;
- jointTargetVelocities.setZero(targets.size());
+ Eigen::VectorXf jointTargetVelocities = Eigen::VectorXf::Zero(targets.size());
if (started)
{
- targetVel = rtGetControlStruct().targetTSVel;
- targetPose = rtGetControlStruct().targetPose;
+ // targetVel = rtGetControlStruct().targetTSVel;
+ // targetPose = rtGetControlStruct().targetPose;
Eigen::Matrix3f diffMat = targetPose.block<3, 3>(0, 0) * currentPose.block<3, 3>(0, 0).inverse();
Eigen::Vector3f errorRPY = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
- Eigen::VectorXf rtTargetVel;
- rtTargetVel.resize(6);
+ Eigen::Vector6f rtTargetVel;
rtTargetVel.block<3, 1>(0, 0) = KpF * (targetPose.block<3, 1>(0, 3) - currentPose.block<3, 1>(0, 3)) + DpF * (- tcptwist.block<3, 1>(0, 0));
// 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));
@@ -326,12 +304,11 @@ namespace armarx
// cartesian vel controller
- Eigen::VectorXf x;
- x.resize(6);
- for (size_t i = 0; i < 6; i++)
- {
- x(i) = rtTargetVel(i);
- }
+ // Eigen::Vector6f x;
+ // for (size_t i = 0; i < 6; i++)
+ // {
+ // x(i) = rtTargetVel(i);
+ // }
Eigen::VectorXf jnv = Eigen::VectorXf::Zero(tcpController->rns->getSize());
float jointLimitAvoidanceKp = rtGetControlStruct().avoidJointLimitsKp;
@@ -345,7 +322,7 @@ namespace armarx
}
// jointTargetVelocities = tcpController->calculate(x, jnv, VirtualRobot::IKSolver::CartesianSelection::All);
- jointTargetVelocities = calcIK(x, jnv, VirtualRobot::IKSolver::CartesianSelection::All);
+ jointTargetVelocities = calcIK(rtTargetVel, jnv, VirtualRobot::IKSolver::CartesianSelection::All);
ARMARX_CHECK_EXPRESSION(!targets.empty());
ARMARX_CHECK_LESS(targets.size(), 1000);
@@ -519,7 +496,6 @@ namespace armarx
void NJointTSDMPController::runDMPWithTime(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&)
{
- // ARMARX_INFO << "------dmp controller: " << VAROUT(goals);
firstRun = true;
while (firstRun)
{
@@ -537,14 +513,10 @@ namespace armarx
dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
finished = false;
-
- // ARMARX_INFO << "run DMP";
started = true;
}
-
-
void NJointTSDMPController::rtPreActivateController()
{
}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
index 95d31adcc..e0671966c 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
@@ -28,7 +28,7 @@ namespace armarx
class NJointTSDMPControllerControlData
{
public:
- Eigen::VectorXf targetTSVel;
+ Eigen::Vector6f targetTSVel;
Eigen::Matrix4f targetPose;
// cartesian velocity control data
std::vector<float> nullspaceJointVelocities;
@@ -69,7 +69,7 @@ namespace armarx
// NJointController interface
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
// NJointTSDMPControllerInterface interface
void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&) override;
@@ -108,13 +108,13 @@ namespace armarx
VirtualRobot::IKSolver::CartesianSelection ModeFromIce(const NJointTaskSpaceDMPControllerMode::CartesianSelection mode);
Eigen::VectorXf calcIK(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspace, VirtualRobot::IKSolver::CartesianSelection mode);
protected:
- void rtPreActivateController() override;
- void rtPostDeactivateController() override;
+ void rtPreActivateController();
+ void rtPostDeactivateController();
// VirtualRobot::IKSolver::CartesianSelection ModeFromIce(const NJointTaskSpaceDMPControllerMode::CartesianSelection mode);
virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
- void onInitNJointController() override;
- void onDisconnectNJointController() override;
+ void onInitNJointController();
+ void onDisconnectNJointController();
void controllerRun();
private:
@@ -174,7 +174,6 @@ namespace armarx
// velocity ik controller parameters
- std::vector<pidController> torquePIDs;
CartesianVelocityControllerPtr tcpController;
std::string nodeSetName;
@@ -185,7 +184,7 @@ namespace armarx
VirtualRobot::DifferentialIKPtr ik;
VirtualRobot::RobotNodePtr tcp;
VirtualRobot::RobotNodePtr refFrame;
- Eigen::VectorXf targetVels;
+ Eigen::Vector6f targetVels;
Eigen::Matrix4f targetPose;
NJointTaskSpaceDMPControllerConfigPtr cfg;
--
GitLab