From f5598823dfc46e4c91b0cc9e714e66bd45080852 Mon Sep 17 00:00:00 2001
From: Simon Ottenhaus <simon.ottenhaus@kit.edu>
Date: Thu, 23 Apr 2020 12:10:28 +0200
Subject: [PATCH] added joint costs to fix cla1 movement
---
...ointCartesianNaturalPositionController.cpp | 1 +
...uralPositionControllerWidgetController.cpp | 4 +
...rtesianNaturalPositionControllerConfig.ice | 5 +-
.../CartesianNaturalPositionController.cpp | 4 +
.../core/CartesianVelocityController.cpp | 131 ++++++++++--------
.../core/CartesianVelocityController.h | 19 ++-
.../CartesianVelocityControllerWithRamp.cpp | 4 +-
.../test/CartesianVelocityControllerTest.cpp | 6 +-
8 files changed, 106 insertions(+), 68 deletions(-)
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
index 6bbc17af4..106b2fbf9 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
@@ -38,6 +38,7 @@ namespace armarx
<< "maxJointVelocity " << vec2str(cfg.maxJointVelocity) << '\n'
<< "maxNullspaceVelocity " << vec2str(cfg.maxNullspaceVelocity) << '\n'
+ << "jointCosts " << vec2str(cfg.jointCosts) << '\n'
<< "jointLimitAvoidanceKp " << cfg.jointLimitAvoidanceKp << '\n'
<< "elbowKp " << cfg.elbowKp << '\n'
diff --git a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.cpp b/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.cpp
index 06d753341..f14d3a669 100644
--- a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.cpp
@@ -185,6 +185,9 @@ namespace armarx
arm.elbow = elb;
arm.tcp = tcp;
+ std::vector<float> jointCosts = std::vector<float>(rns->getSize(), 1);
+ jointCosts.at(0) = 4;
+
armarx::RemoteRobot::synchronizeLocalClone(_robot, _robotStateComponent);
_tcpTarget = rns->getTCP()->getPoseInRootFrame();
@@ -196,6 +199,7 @@ namespace armarx
ik.setUpperArmLength(upper_arm_length);
ik.setLowerArmLength(lower_arm_length);
NJointCartesianNaturalPositionControllerConfigPtr config = CartesianNaturalPositionControllerProxy::MakeConfig(rns->getName(), elb->getName());
+ config->runCfg.jointCosts = jointCosts;
CartesianNaturalPositionControllerConfig runCfg = config->runCfg;
updateKpSliders(runCfg);
//config->runCfg = runCfg;
diff --git a/source/RobotAPI/interface/core/CartesianNaturalPositionControllerConfig.ice b/source/RobotAPI/interface/core/CartesianNaturalPositionControllerConfig.ice
index efd0de988..a876489d1 100644
--- a/source/RobotAPI/interface/core/CartesianNaturalPositionControllerConfig.ice
+++ b/source/RobotAPI/interface/core/CartesianNaturalPositionControllerConfig.ice
@@ -35,10 +35,11 @@ module armarx
Ice::FloatSeq maxJointVelocity;
Ice::FloatSeq maxNullspaceVelocity;
+ Ice::FloatSeq jointCosts;
float jointLimitAvoidanceKp = 0.1;
- float elbowKp = 2;
- float nullspaceTargetKp = 2;
+ float elbowKp = 1;
+ float nullspaceTargetKp = 1;
float thresholdOrientationNear = 0.1;
float thresholdOrientationReached = 0.05;
diff --git a/source/RobotAPI/libraries/core/CartesianNaturalPositionController.cpp b/source/RobotAPI/libraries/core/CartesianNaturalPositionController.cpp
index 4b0b4be7d..158aa743b 100644
--- a/source/RobotAPI/libraries/core/CartesianNaturalPositionController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianNaturalPositionController.cpp
@@ -291,6 +291,10 @@ namespace armarx
cfg.maxOrientationAcceleration,
cfg.maxNullspaceAcceleration
);
+ if (cfg.jointCosts.size() > 0)
+ {
+ vcTcp.controller.setJointCosts(cfg.jointCosts);
+ }
}
Eigen::VectorXf CartesianNaturalPositionController::actualTcpVel(const Eigen::VectorXf& jointVel)
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
index 228df7d26..f140f9512 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
@@ -29,40 +29,60 @@
#include <RobotAPI/libraries/core/math/MathUtils.h>
#include <VirtualRobot/math/Helpers.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
using namespace armarx;
CartesianVelocityController::CartesianVelocityController(const VirtualRobot::RobotNodeSetPtr& rns, const VirtualRobot::RobotNodePtr& tcp, const VirtualRobot::JacobiProvider::InverseJacobiMethod invJacMethod, bool considerJointLimits)
: rns(rns),
- considerJointLimits(considerJointLimits)
+ _considerJointLimits(considerJointLimits)
{
//ARMARX_IMPORTANT << VAROUT(rns->getRobot()->getRootNode());
ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), invJacMethod));
- this->tcp = tcp ? tcp : rns->getTCP();
+ _tcp = tcp ? tcp : rns->getTCP();
- this->cartesianMMRegularization = 100;
- this->cartesianRadianRegularization = 1;
+ _cartesianMMRegularization = 100;
+ _cartesianRadianRegularization = 1;
+ _jointCosts = Eigen::VectorXf::Constant(rns->getSize(), 1);
}
-Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& cartesianVel, VirtualRobot::IKSolver::CartesianSelection mode)
+void CartesianVelocityController::calculateJacobis(VirtualRobot::IKSolver::CartesianSelection mode)
{
- jacobi = ik->getJacobianMatrix(tcp, mode);
+ jacobi = ik->getJacobianMatrix(_tcp, mode);
+ _jacobiWithCosts = Eigen::MatrixXf(jacobi.rows(), jacobi.cols());
+ for (int r = 0; r < jacobi.rows(); r++)
+ {
+ for (int c = 0; c < jacobi.cols(); c++)
+ {
+ _jacobiWithCosts(r, c) = jacobi(r, c) / _jointCosts(c);
+ }
+ }
+ _inv = ik->computePseudoInverseJacobianMatrix(_jacobiWithCosts, ik->getJacobiRegularization(mode));
+}
- inv = ik->computePseudoInverseJacobianMatrix(jacobi, ik->getJacobiRegularization(mode));
+Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& cartesianVel, VirtualRobot::IKSolver::CartesianSelection mode)
+{
+ return calculate(cartesianVel, Eigen::VectorXf::Zero(0), mode);
+ /*calculateJacobis(mode);
- if (considerJointLimits)
+ if (_considerJointLimits)
{
- adjustJacobiForJointsAtJointLimits(mode, cartesianVel);
+ clampJacobiAtJointLimits(mode, cartesianVel, _jacobiWithCosts, _inv);
}
- Eigen::VectorXf jointVel = inv * cartesianVel;
+ Eigen::VectorXf jointVel = _inv * cartesianVel;
+ jointVel += nsv;
+ for (int r = 0; r < jointVel.rows(); r++)
+ {
+ jointVel(r) = jointVel(r) / _jointCosts(r);
+ }
if (maximumJointVelocities.rows() > 0)
{
jointVel = ::math::Helpers::LimitVectorLength(jointVel, maximumJointVelocities);
}
- return jointVel;
+ return jointVel;*/
}
Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& cartesianVel, float KpJointLimitAvoidanceScale, VirtualRobot::IKSolver::CartesianSelection mode)
@@ -73,50 +93,44 @@ Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& ca
Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel, VirtualRobot::IKSolver::CartesianSelection mode)
{
- //ARMARX_IMPORTANT << VAROUT(rns.get());
- //ARMARX_IMPORTANT << VAROUT(tcp.get());
- //ARMARX_IMPORTANT << VAROUT(ik.get());
- jacobi = ik->getJacobianMatrix(tcp, mode);
- // ARMARX_IMPORTANT << "nullspaceVel " << nullspaceVel.transpose();
- Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(jacobi);
- //ARMARX_IMPORTANT << "The rank of the jacobi is " << lu_decomp.rank();
- //ARMARX_IMPORTANT << "Null space: " << lu_decomp.kernel();
- Eigen::MatrixXf nullspace = lu_decomp.kernel();
+ calculateJacobis(mode);
- // Eigen::MatrixXf nullspace = Eigen::MatrixXf::Identity(jacobi.cols(), jacobi.cols()) - inv * jacobi;
+ Eigen::VectorXf nsv = Eigen::VectorXf::Zero(rns->getSize());
- Eigen::VectorXf nsv = Eigen::VectorXf::Zero(nullspace.rows());
- for (int i = 0; i < nullspace.cols(); i++)
+ if (nullspaceVel.rows() > 0)
{
- float squaredNorm = nullspace.col(i).squaredNorm();
- // Prevent division by zero
- if (squaredNorm > 1.0e-32f)
+ // ARMARX_IMPORTANT << "nullspaceVel " << nullspaceVel.transpose();
+ Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(_jacobiWithCosts);
+ //ARMARX_IMPORTANT << "The rank of the _jacobiWithCosts is " << lu_decomp.rank();
+ //ARMARX_IMPORTANT << "Null space: " << lu_decomp.kernel();
+ Eigen::MatrixXf nullspace = lu_decomp.kernel();
+
+ for (int i = 0; i < nullspace.cols(); i++)
{
- nsv += nullspace.col(i) * nullspace.col(i).dot(nullspaceVel) / nullspace.col(i).squaredNorm();
+ float squaredNorm = nullspace.col(i).squaredNorm();
+ // Prevent division by zero
+ if (squaredNorm > 1.0e-32f)
+ {
+ nsv += nullspace.col(i) * nullspace.col(i).dot(nullspaceVel) / nullspace.col(i).squaredNorm();
+ }
}
}
- // Eigen::VectorXf nsv = nullspace * nullspaceVel;
-
- //nsv /= kernel.cols();
-
-
-
- inv = ik->computePseudoInverseJacobianMatrix(jacobi, ik->getJacobiRegularization(mode));
-
-
- if (considerJointLimits)
+ if (_considerJointLimits)
{
- adjustJacobiForJointsAtJointLimits(mode, cartesianVel);
+ clampJacobiAtJointLimits(mode, cartesianVel, _jacobiWithCosts, _inv);
}
- Eigen::VectorXf jointVel = inv * cartesianVel;
+ Eigen::VectorXf jointVel = _inv * cartesianVel;
jointVel += nsv;
-
+ for (int r = 0; r < jointVel.rows(); r++)
+ {
+ jointVel(r) = jointVel(r) / _jointCosts(r);
+ }
if (maximumJointVelocities.rows() > 0)
{
@@ -189,8 +203,8 @@ Eigen::VectorXf CartesianVelocityController::calculateNullspaceVelocity(const Ei
void CartesianVelocityController::setCartesianRegularization(float cartesianMMRegularization, float cartesianRadianRegularization)
{
- this->cartesianMMRegularization = cartesianMMRegularization;
- this->cartesianRadianRegularization = cartesianRadianRegularization;
+ _cartesianMMRegularization = cartesianMMRegularization;
+ _cartesianRadianRegularization = cartesianRadianRegularization;
}
Eigen::VectorXf CartesianVelocityController::calculateRegularization(VirtualRobot::IKSolver::CartesianSelection mode)
@@ -201,31 +215,30 @@ Eigen::VectorXf CartesianVelocityController::calculateRegularization(VirtualRobo
if (mode & VirtualRobot::IKSolver::X)
{
- regularization(i++) = 1 / cartesianMMRegularization;
+ regularization(i++) = 1 / _cartesianMMRegularization;
}
if (mode & VirtualRobot::IKSolver::Y)
{
- regularization(i++) = 1 / cartesianMMRegularization;
+ regularization(i++) = 1 / _cartesianMMRegularization;
}
if (mode & VirtualRobot::IKSolver::Z)
{
- regularization(i++) = 1 / cartesianMMRegularization;
+ regularization(i++) = 1 / _cartesianMMRegularization;
}
if (mode & VirtualRobot::IKSolver::Orientation)
{
- regularization(i++) = 1 / cartesianRadianRegularization;
- regularization(i++) = 1 / cartesianRadianRegularization;
- regularization(i++) = 1 / cartesianRadianRegularization;
+ regularization(i++) = 1 / _cartesianRadianRegularization;
+ regularization(i++) = 1 / _cartesianRadianRegularization;
+ regularization(i++) = 1 / _cartesianRadianRegularization;
}
return regularization.topRows(i);
}
-bool CartesianVelocityController::adjustJacobiForJointsAtJointLimits(VirtualRobot::IKSolver::CartesianSelection mode, const Eigen::VectorXf& cartesianVel, float jointLimitCheckAccuracy)
+bool CartesianVelocityController::clampJacobiAtJointLimits(VirtualRobot::IKSolver::CartesianSelection mode, const Eigen::VectorXf& cartesianVel, Eigen::MatrixXf& jacobi, Eigen::MatrixXf& inv, float jointLimitCheckAccuracy)
{
-
bool modifiedJacobi = false;
Eigen::VectorXf jointVel = inv * cartesianVel;
@@ -252,22 +265,32 @@ bool CartesianVelocityController::adjustJacobiForJointsAtJointLimits(VirtualRobo
modifiedJacobi = true;
}
}
-
if (modifiedJacobi)
{
- // calculate inverse jacobi again since atleast one joint would be driven into the limit
inv = ik->computePseudoInverseJacobianMatrix(jacobi, ik->getJacobiRegularization(mode));
}
+
return modifiedJacobi;
}
bool CartesianVelocityController::getConsiderJointLimits() const
{
- return considerJointLimits;
+ return _considerJointLimits;
}
void CartesianVelocityController::setConsiderJointLimits(bool value)
{
- considerJointLimits = value;
+ _considerJointLimits = value;
}
+
+void CartesianVelocityController::setJointCosts(const std::vector<float>& jointCosts)
+{
+ ARMARX_CHECK((int)jointCosts.size() == _jointCosts.rows());
+ for (size_t i = 0; i < jointCosts.size(); i++)
+ {
+ _jointCosts(i) = jointCosts.at(i);
+ }
+}
+
+
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.h b/source/RobotAPI/libraries/core/CartesianVelocityController.h
index 1639e82f6..a3a577198 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.h
+++ b/source/RobotAPI/libraries/core/CartesianVelocityController.h
@@ -39,7 +39,7 @@ namespace armarx
public:
CartesianVelocityController(const VirtualRobot::RobotNodeSetPtr& rns, const VirtualRobot::RobotNodePtr& tcp = nullptr,
const VirtualRobot::JacobiProvider::InverseJacobiMethod invJacMethod = VirtualRobot::JacobiProvider::eSVDDamped,
- bool considerJointLimits = true);
+ bool _considerJointLimits = true);
CartesianVelocityController(CartesianVelocityController&&) = default;
CartesianVelocityController& operator=(CartesianVelocityController&&) = default;
@@ -57,18 +57,23 @@ namespace armarx
void setConsiderJointLimits(bool value);
Eigen::MatrixXf jacobi;
- Eigen::MatrixXf inv;
VirtualRobot::RobotNodeSetPtr rns;
VirtualRobot::DifferentialIKPtr ik;
- VirtualRobot::RobotNodePtr tcp;
+ VirtualRobot::RobotNodePtr _tcp;
Eigen::VectorXf maximumJointVelocities;
+ void setJointCosts(const std::vector<float>& _jointCosts);
+
private:
+ void calculateJacobis(VirtualRobot::IKSolver::CartesianSelection mode);
+ Eigen::MatrixXf _jacobiWithCosts;
+ Eigen::MatrixXf _inv;
Eigen::VectorXf calculateRegularization(VirtualRobot::IKSolver::CartesianSelection mode);
- bool adjustJacobiForJointsAtJointLimits(VirtualRobot::IKSolver::CartesianSelection mode, const Eigen::VectorXf& cartesianVel, float jointLimitCheckAccuracy = 0.001f);
- bool considerJointLimits = true;
- float cartesianMMRegularization;
- float cartesianRadianRegularization;
+ bool clampJacobiAtJointLimits(VirtualRobot::IKSolver::CartesianSelection mode, const Eigen::VectorXf& cartesianVel, Eigen::MatrixXf& jacobi, Eigen::MatrixXf& _inv, float jointLimitCheckAccuracy = 0.001f);
+ bool _considerJointLimits = true;
+ float _cartesianMMRegularization;
+ float _cartesianRadianRegularization;
+ Eigen::VectorXf _jointCosts;
};
}
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
index 228f765b8..a72fe97e5 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
@@ -40,7 +40,7 @@ namespace armarx
void CartesianVelocityControllerWithRamp::setCurrentJointVelocity(const Eigen::Ref<const Eigen::VectorXf>& currentJointVelocity)
{
- Eigen::MatrixXf jacobi = controller.ik->getJacobianMatrix(controller.tcp, mode);
+ Eigen::MatrixXf jacobi = controller.ik->getJacobianMatrix(controller._tcp, mode);
//Eigen::MatrixXf inv = controller.ik->computePseudoInverseJacobianMatrix(jacobi, controller.ik->getJacobiRegularization(mode));
@@ -59,7 +59,7 @@ namespace armarx
void CartesianVelocityControllerWithRamp::switchMode(const Eigen::VectorXf& currentJointVelocity, VirtualRobot::IKSolver::CartesianSelection mode)
{
- Eigen::MatrixXf jacobi = controller.ik->getJacobianMatrix(controller.tcp, mode);
+ Eigen::MatrixXf jacobi = controller.ik->getJacobianMatrix(controller._tcp, mode);
cartesianVelocityRamp.setState(jacobi * currentJointVelocity, mode);
this->mode = mode;
}
diff --git a/source/RobotAPI/libraries/core/test/CartesianVelocityControllerTest.cpp b/source/RobotAPI/libraries/core/test/CartesianVelocityControllerTest.cpp
index 216c48d90..ce26e1c14 100644
--- a/source/RobotAPI/libraries/core/test/CartesianVelocityControllerTest.cpp
+++ b/source/RobotAPI/libraries/core/test/CartesianVelocityControllerTest.cpp
@@ -116,19 +116,19 @@ BOOST_AUTO_TEST_CASE(testJointLimitAwareness)
// ARMARX_INFO << "diff\n" << (inv-inv2);
rns->setJointValues(oldJV);
- Eigen::Vector3f posBefore = h->tcp->getPositionInRootFrame();
+ Eigen::Vector3f posBefore = h->_tcp->getPositionInRootFrame();
float accuracy = 0.001f;
Eigen::VectorXf jointVel = inv * cartesianVel;
rns->setJointValues(oldJV + jointVel * accuracy);
- Eigen::VectorXf resultCartesianVel = ((h->tcp->getPositionInRootFrame() - posBefore) / accuracy);
+ Eigen::VectorXf resultCartesianVel = ((h->_tcp->getPositionInRootFrame() - posBefore) / accuracy);
Eigen::VectorXf jointVel2 = inv2 * cartesianVel;
rns->setJointValues(oldJV + jointVel2 * accuracy);
- Eigen::VectorXf resultCartesianVel2 = ((h->tcp->getPositionInRootFrame() - posBefore) / accuracy);
+ Eigen::VectorXf resultCartesianVel2 = ((h->_tcp->getPositionInRootFrame() - posBefore) / accuracy);
Eigen::Vector3f diff = (resultCartesianVel - cartesianVel);
Eigen::Vector3f diff2 = (resultCartesianVel2 - cartesianVel);
--
GitLab