From 8544e04e56f8cec4f6bf1477effd5604ee8f7811 Mon Sep 17 00:00:00 2001
From: Armar6 <armar6@h2t.com>
Date: Sat, 2 Mar 2019 12:02:07 +0100
Subject: [PATCH] added workspace for controller
---
.../NJointTaskSpaceDMPController.ice | 6 ++++
...NJointPeriodicTSDMPCompliantController.cpp | 31 +++++++++++++++++--
.../NJointPeriodicTSDMPCompliantController.h | 2 ++
3 files changed, 36 insertions(+), 3 deletions(-)
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index ee185d0bd..3c303ee6d 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -356,6 +356,12 @@ module armarx
float maxLinearVel;
float maxAngularVel;
+
+ Ice::FloatSeq ws_x;
+ Ice::FloatSeq ws_y;
+ Ice::FloatSeq ws_z;
+
+
};
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
index ba0d95adb..e7948a9fc 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
@@ -100,6 +100,22 @@ namespace armarx
qvel_filtered.setZero(targets.size());
+ ARMARX_CHECK_EQUAL(cfg->ws_x.size(), 2);
+ ARMARX_CHECK_EQUAL(cfg->ws_y.size(), 2);
+ ARMARX_CHECK_EQUAL(cfg->ws_z.size(), 2);
+ // only for ARMAR-6 (safe-guard)
+ ARMARX_CHECK_LESS(cfg->ws_x[0], cfg->ws_x[1]);
+ ARMARX_CHECK_LESS(cfg->ws_x[0], 1000);
+ ARMARX_CHECK_LESS(-200, cfg->ws_x[1]);
+
+ ARMARX_CHECK_LESS(cfg->ws_y[0], cfg->ws_y[1]);
+ ARMARX_CHECK_LESS(cfg->ws_y[0], 1200);
+ ARMARX_CHECK_LESS(0, cfg->ws_y[1]);
+
+ ARMARX_CHECK_LESS(cfg->ws_z[0], cfg->ws_z[1]);
+ ARMARX_CHECK_LESS(cfg->ws_z[0], 1800);
+ ARMARX_CHECK_LESS(300, cfg->ws_z[1]);
+
}
void NJointPeriodicTSDMPCompliantController::onInitNJointController()
@@ -292,8 +308,6 @@ namespace armarx
targetVel(i) = 0;
}
-
-
// rotation changes
// if (filteredForceInRoot.norm() > fabs(cfg->minimumReactForce))
@@ -356,6 +370,17 @@ namespace armarx
float dTf = (float)deltaT;
targetPose.block<3, 1>(0, 3) = targetPose.block<3, 1>(0, 3) + dTf * targetVel.block<3, 1>(0, 0);
+ targetPose(0, 3) = targetPose(0, 3) > cfg->ws_x[0] ? targetPose(0, 3) : cfg->ws_x[0];
+ targetPose(0, 3) = targetPose(0, 3) < cfg->ws_x[1] ? targetPose(0, 3) : cfg->ws_x[1];
+
+ targetPose(1, 3) = targetPose(1, 3) > cfg->ws_y[0] ? targetPose(1, 3) : cfg->ws_y[0];
+ targetPose(1, 3) = targetPose(1, 3) < cfg->ws_y[1] ? targetPose(1, 3) : cfg->ws_y[1];
+
+ targetPose(2, 3) = targetPose(2, 3) > cfg->ws_z[0] ? targetPose(2, 3) : cfg->ws_z[0];
+ targetPose(2, 3) = targetPose(2, 3) < cfg->ws_z[1] ? targetPose(2, 3) : cfg->ws_z[1];
+
+
+
// Eigen::Matrix3f rotVel = VirtualRobot::MathTools::rpy2eigen3f(targetVel(3) * dTf, targetVel(4) * dTf, targetVel(5) * dTf);
// targetPose.block<3, 3>(0, 0) = rotVel * targetPose.block<3, 3>(0, 0);
@@ -384,7 +409,7 @@ namespace armarx
{
linearVel = kpos.cwiseProduct(desiredPosition - currentTCPPosition);
}
- Eigen::Vector3f tcpDesiredForce = 0.001 * linearVel + dpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
+ Eigen::Vector3f tcpDesiredForce = 0.001 * linearVel + dpos.cwiseProduct(- currentTCPLinearVelocity);
Eigen::Vector3f currentTCPAngularVelocity;
currentTCPAngularVelocity << currentTwist(3), currentTwist(4), currentTwist(5);
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h
index 91aa9f460..5ca91b2bc 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h
@@ -174,6 +174,8 @@ namespace armarx
Eigen::Matrix3f origHandOri;
Eigen::VectorXf qvel_filtered;
+
+
};
} // namespace armarx
--
GitLab