diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
index 6bbc17af491c5943ce91b819cb784386246646ac..106b2fbf9468339ab11e6e4d5fe4650f07a8cebe 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 06d7533416623326622ef427a9e8c342781d75e5..f14d3a669f9433a4400b3a7fdcf5193708404c07 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 efd0de9883cabfae9b5f86e45c7036588072e6e0..a876489d1f7f4042817f61c8b2fe751b97f06690 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 4b0b4be7dd251a2c8ceb9c19a1acfd50fb21436f..158aa743b09f1779a8c26382129957b94753f3c1 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 228df7d268204bb6d3578d49d158afd9164f5ddb..f140f9512bcb990a82e50ed3eb271536817d4d88 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 1639e82f6c98b52b56bb6965afe58ec28f150417..a3a577198b9a6a80534013ef3aea1218aec7da12 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 228f765b87f5b935b26917dce8d7aa05089a5a18..a72fe97e50b3249595840b6fe18e1aca8be36542 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 216c48d906af3f8ea6e14f3a2a89424a1ce13c22..ce26e1c141573e7033a5dd3272f63d5dad44578a 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);