diff --git a/VirtualRobot/Manipulability/AbstractManipulability.h b/VirtualRobot/Manipulability/AbstractManipulability.h
index 7b3470473a53684ad01a0566e78a4f18062b179a..5603f989688759b5dac176fc4049701b66374521 100644
--- a/VirtualRobot/Manipulability/AbstractManipulability.h
+++ b/VirtualRobot/Manipulability/AbstractManipulability.h
@@ -84,6 +84,12 @@ public:
virtual std::vector<std::string> getJointNames() = 0;
+ virtual Eigen::VectorXd getJointAngles() = 0;
+
+ virtual Eigen::VectorXd getJointLimitsHigh() = 0;
+
+ virtual Eigen::VectorXd getJointLimitsLow() = 0;
+
static Eigen::MatrixXd GetJacobianSubMatrix(const Eigen::Matrix<double, 6, Eigen::Dynamic> &jacobian, IKSolver::CartesianSelection mode);
static Eigen::MatrixXd GetJacobianSubMatrix(const Eigen::Matrix<double, 6, Eigen::Dynamic> &jacobian, Mode mode);
diff --git a/VirtualRobot/Manipulability/AbstractManipulabilityTracking.cpp b/VirtualRobot/Manipulability/AbstractManipulabilityTracking.cpp
index 0c8126653fb3f9412d69beadaa56bff5eb8c433e..7f9ba6d0cdfc52d4d175b19bbe1a29c5222f0d61 100644
--- a/VirtualRobot/Manipulability/AbstractManipulabilityTracking.cpp
+++ b/VirtualRobot/Manipulability/AbstractManipulabilityTracking.cpp
@@ -164,6 +164,35 @@ double AbstractManipulabilityTracking::getDamping(const Eigen::MatrixXd &matrix)
return damping;
}
+Eigen::MatrixXd AbstractManipulabilityTracking::getJointsLimitsWeightMatrix(const Eigen::VectorXd jointAngles, const Eigen::VectorXd jointLimitsLow, const Eigen::VectorXd jointLimitsHigh) {
+ int nbJoints = jointAngles.size();
+ Eigen::VectorXd weights(nbJoints);
+ Eigen::VectorXd gradient(nbJoints);
+
+ //std::cout << "Saved gradient:\n" << jointAngleLimitGradient << std::endl;
+
+ for (int i = 0; i < nbJoints; i++) {
+ // TODO add condition on velocities
+ gradient(i) = std::pow(jointLimitsHigh(i) - jointLimitsLow(i), 2) * (2*jointAngles(i) - jointLimitsHigh(i) - jointLimitsLow(i)) /
+ ( 4 * std::pow(jointLimitsHigh(i) - jointAngles(i), 2) * std::pow(jointAngles(i) - jointLimitsLow(i), 2) );
+
+ if((std::abs(gradient(i)) - std::abs(jointAngleLimitGradient(i))) >= 0.) {
+ // Joint going towards limits
+ weights(i) = 1. / std::sqrt((1. + std::abs(gradient(i))));
+ }
+ else {
+ // Joint going towards the center
+ weights(i) = 1.;
+ }
+ }
+ //std::cout << "Gradient:\n" << gradient << std::endl;
+ setjointAngleLimitGradient(gradient);
+
+ Eigen::MatrixXd weightsMatrix = weights.asDiagonal();
+ return weightsMatrix;
+}
+
+
Eigen::MatrixXd AbstractManipulabilityTracking::computeManipulabilityJacobianMandelNotation(const Eigen::Tensor<double, 3> &manipulabilityJacobian) {
int num_task_vars = getTaskVars();
@@ -197,4 +226,8 @@ Eigen::VectorXd AbstractManipulabilityTracking::symMatrixToVector(const Eigen::M
return v;
}
+void AbstractManipulabilityTracking::setjointAngleLimitGradient(const Eigen::VectorXd &gradient) {
+ this->jointAngleLimitGradient = gradient;
+}
+
}
diff --git a/VirtualRobot/Manipulability/AbstractManipulabilityTracking.h b/VirtualRobot/Manipulability/AbstractManipulabilityTracking.h
index 772cd34c47bba1b8c83ea4c4b0a4e2fbe5a5a9a8..4fe51f9822ddbb62f1aabd7b24ec6feefeeeace6 100644
--- a/VirtualRobot/Manipulability/AbstractManipulabilityTracking.h
+++ b/VirtualRobot/Manipulability/AbstractManipulabilityTracking.h
@@ -81,6 +81,9 @@ public:
/*! Calculates damping factor for singularity avoidance */
double getDamping(const Eigen::MatrixXd &matrix);
+ /* Calculate weight matrix for joint limits avoidance */
+ Eigen::MatrixXd getJointsLimitsWeightMatrix(const Eigen::VectorXd jointAngles, const Eigen::VectorXd jointLimitsLow, const Eigen::VectorXd jointLimitsHigh);
+
Eigen::MatrixXd computeManipulabilityJacobianMandelNotation(const Eigen::Tensor<double, 3> &manipulabilityJacobian);
Eigen::VectorXd symMatrixToVector(const Eigen::MatrixXd &sym_matrix);
@@ -96,6 +99,10 @@ public:
virtual VisualizationNodePtr getManipulabilityVis(const Eigen::MatrixXd &manipulability, const std::string &visualizationType = "", double scaling = 1000.0) = 0;
virtual void setConvertMMtoM(bool value) = 0;
+
+ void setjointAngleLimitGradient(const Eigen::VectorXd &gradient);
+
+ Eigen::Matrix<double, Eigen::Dynamic, 1> jointAngleLimitGradient; // TODO initialize
};
typedef std::shared_ptr<AbstractManipulabilityTracking> AbstractManipulabilityTrackingPtr;
diff --git a/VirtualRobot/Manipulability/BimanualManipulability.cpp b/VirtualRobot/Manipulability/BimanualManipulability.cpp
index c7c187cfe2e3f284caa90bb90b00f0f1ab800a1e..9c7290f2385ec7f180679fa9df87d4d50c0b58c6 100644
--- a/VirtualRobot/Manipulability/BimanualManipulability.cpp
+++ b/VirtualRobot/Manipulability/BimanualManipulability.cpp
@@ -119,6 +119,63 @@ std::vector<std::string> BimanualManipulability::getJointNames() {
return robotNodeNames;
}
+Eigen::VectorXd BimanualManipulability::getJointAngles() {
+ Eigen::VectorXd jointAngles(rnsLeft->getSize() + rnsRight->getSize());
+ jointAngles.head(rnsLeft->getSize()) = rnsLeft->getJointValuesEigen().cast<double>();
+ jointAngles.tail(rnsRight->getSize()) = rnsRight->getJointValuesEigen().cast<double>();
+ return jointAngles;
+}
+
+Eigen::VectorXd BimanualManipulability::getJointLimitsHigh() {
+ Eigen::VectorXd jointLimitHi(rnsLeft->getSize() + rnsRight->getSize());
+ for (size_t i = 0; i < rnsLeft->getSize(); i++)
+ {
+ RobotNodePtr rn = rnsLeft->getNode(i);
+ /*if (rn->isLimitless())
+ {
+ jointLimitHi(i) = ?infinity;
+ }
+ else */
+ jointLimitHi(i) = rn->getJointLimitHi();
+ }
+ for (size_t i = 0; i < rnsRight->getSize(); i++)
+ {
+ RobotNodePtr rn = rnsRight->getNode(i);
+ /*if (rn->isLimitless())
+ {
+ jointLimitHi(i) = ?infinity;
+ }
+ else */
+ jointLimitHi(i + rnsLeft->getSize()) = rn->getJointLimitHi();
+ }
+ return jointLimitHi;
+}
+
+Eigen::VectorXd BimanualManipulability::getJointLimitsLow() {
+ Eigen::VectorXd jointLimitLo(rnsLeft->getSize() + rnsRight->getSize());
+ for (size_t i = 0; i < rnsLeft->getSize(); i++)
+ {
+ RobotNodePtr rn = rnsLeft->getNode(i);
+ /*if (rn->isLimitless())
+ {
+ jointLimitLo(i) = ?infinity;
+ }
+ else */
+ jointLimitLo(i) = rn->getJointLimitLo();
+ }
+ for (size_t i = 0; i < rnsRight->getSize(); i++)
+ {
+ RobotNodePtr rn = rnsRight->getNode(i);
+ /*if (rn->isLimitless())
+ {
+ jointLimitLo(i) = ?infinity;
+ }
+ else */
+ jointLimitLo(i + rnsLeft->getSize()) = rn->getJointLimitLo();
+ }
+ return jointLimitLo;
+}
+
RobotPtr BimanualManipulability::getRobot() {
return rnsLeft->getRobot();
}
diff --git a/VirtualRobot/Manipulability/BimanualManipulability.h b/VirtualRobot/Manipulability/BimanualManipulability.h
index 3d8ac22f8bc146be619148ab92843e7e1c766eb4..5697dd91552961e09c778362e873b068880029f1 100644
--- a/VirtualRobot/Manipulability/BimanualManipulability.h
+++ b/VirtualRobot/Manipulability/BimanualManipulability.h
@@ -74,6 +74,12 @@ public:
virtual std::vector<std::string> getJointNames() override;
+ virtual Eigen::VectorXd getJointAngles() override;
+
+ virtual Eigen::VectorXd getJointLimitsHigh() override;
+
+ virtual Eigen::VectorXd getJointLimitsLow() override;
+
RobotPtr getRobot();
RobotNodeSetPtr createRobotNodeSet(const std::string &name = "BimanualManipulabilityTracking");
diff --git a/VirtualRobot/Manipulability/SingleChainManipulability.cpp b/VirtualRobot/Manipulability/SingleChainManipulability.cpp
index e2e93fe45a89716777a470c3330f382c55de6538..e50b897a8d3d5258800391a6077a2aca02f19adc 100644
--- a/VirtualRobot/Manipulability/SingleChainManipulability.cpp
+++ b/VirtualRobot/Manipulability/SingleChainManipulability.cpp
@@ -88,6 +88,40 @@ std::vector<std::string> SingleRobotNodeSetManipulability::getJointNames() {
return rns->getNodeNames();
}
+Eigen::VectorXd SingleRobotNodeSetManipulability::getJointAngles() {
+ return rns->getJointValuesEigen().cast<double>();
+}
+
+Eigen::VectorXd SingleRobotNodeSetManipulability::getJointLimitsHigh() {
+ Eigen::VectorXd jointLimitHi(rns->getSize());
+ for (size_t i = 0; i < rns->getSize(); i++)
+ {
+ RobotNodePtr rn = rns->getNode(i);
+ /*if (rn->isLimitless())
+ {
+ jointLimitHi(i) = ?infinity;
+ }
+ else */
+ jointLimitHi(i) = rn->getJointLimitHi();
+ }
+ return jointLimitHi;
+}
+
+Eigen::VectorXd SingleRobotNodeSetManipulability::getJointLimitsLow() {
+ Eigen::VectorXd jointLimitLo(rns->getSize());
+ for (size_t i = 0; i < rns->getSize(); i++)
+ {
+ RobotNodePtr rn = rns->getNode(i);
+ /*if (rn->isLimitless())
+ {
+ jointLimitLo(i) = ?infinity;
+ }
+ else */
+ jointLimitLo(i) = rn->getJointLimitLo();
+ }
+ return jointLimitLo;
+}
+
Eigen::MatrixXd SingleRobotNodeSetManipulability::computeJacobian(IKSolver::CartesianSelection mode) {
return ik->getJacobianMatrix(node, mode).cast<double>();
}
@@ -97,13 +131,17 @@ Eigen::MatrixXd SingleRobotNodeSetManipulability::computeJacobian(IKSolver::Cart
SingleChainManipulability::SingleChainManipulability(const Eigen::Matrix<double, 6, Eigen::Dynamic> &jacobian,
AbstractManipulability::Mode mode, AbstractManipulability::Type type,
+ const Eigen::Matrix<double, Eigen::Dynamic, 1> &jointAngles, const Eigen::VectorXd &jointLimitsHigh, const Eigen::VectorXd &jointLimitsLow,
const std::vector<std::string> &jointNames,
const Eigen::Vector3f &globalPosition, const Eigen::Vector3f &localPosition) :
AbstractSingleChainManipulability(mode, type),
jacobian(jacobian),
jointNames(jointNames),
globalPosition(globalPosition),
- localPosition(localPosition)
+ localPosition(localPosition),
+ jointAngles(jointAngles),
+ jointLimitsHigh(jointLimitsHigh),
+ jointLimitsLow(jointLimitsLow)
{
}
@@ -119,6 +157,18 @@ std::vector<std::string> SingleChainManipulability::getJointNames() {
return jointNames;
}
+Eigen::VectorXd SingleChainManipulability::getJointAngles() {
+ return jointAngles;
+}
+
+Eigen::VectorXd SingleChainManipulability::getJointLimitsHigh() {
+ return jointLimitsHigh;
+}
+
+Eigen::VectorXd SingleChainManipulability::getJointLimitsLow() {
+ return jointLimitsLow;
+}
+
void SingleChainManipulability::setJacobian(const Eigen::Matrix<double, 6, Eigen::Dynamic> &jacobian) {
this->jacobian = jacobian;
}
diff --git a/VirtualRobot/Manipulability/SingleChainManipulability.h b/VirtualRobot/Manipulability/SingleChainManipulability.h
index 871e61d64d7bdcb1fe4a9c283838dab760545101..b96a658c86599af19c5adecb8420b4e4c25ca968 100644
--- a/VirtualRobot/Manipulability/SingleChainManipulability.h
+++ b/VirtualRobot/Manipulability/SingleChainManipulability.h
@@ -67,6 +67,12 @@ public:
virtual std::vector<std::string> getJointNames() override;
+ virtual Eigen::VectorXd getJointAngles() override;
+
+ virtual Eigen::VectorXd getJointLimitsHigh() override;
+
+ virtual Eigen::VectorXd getJointLimitsLow() override;
+
protected:
virtual Eigen::MatrixXd computeJacobian(IKSolver::CartesianSelection mode) override;
@@ -85,6 +91,7 @@ class SingleChainManipulability : public AbstractSingleChainManipulability
{
public:
SingleChainManipulability(const Eigen::Matrix<double, 6, Eigen::Dynamic> &jacobian, Mode mode, Type type,
+ const Eigen::Matrix<double, Eigen::Dynamic, 1> &jointAngles = Eigen::MatrixXd(), const Eigen::VectorXd &jointLimitsHigh = Eigen::MatrixXd(), const Eigen::VectorXd &jointLimitsLow = Eigen::MatrixXd(),
const std::vector<std::string> &jointNames = std::vector<std::string>(),
const Eigen::Vector3f &globalPosition = Eigen::Vector3f::Zero(), const Eigen::Vector3f &localPosition = Eigen::Vector3f::Zero());
@@ -94,6 +101,12 @@ public:
virtual std::vector<std::string> getJointNames() override;
+ virtual Eigen::VectorXd getJointAngles() override;
+
+ virtual Eigen::VectorXd getJointLimitsHigh() override;
+
+ virtual Eigen::VectorXd getJointLimitsLow() override;
+
void setJacobian(const Eigen::Matrix<double, 6, Eigen::Dynamic> &jacobian);
void setLocalPosition(const Eigen::Vector3f &localPosition);
@@ -112,6 +125,9 @@ private:
std::vector<std::string> jointNames;
Eigen::Vector3f globalPosition;
Eigen::Vector3f localPosition;
+ Eigen::VectorXd jointAngles;
+ Eigen::VectorXd jointLimitsHigh;
+ Eigen::VectorXd jointLimitsLow;
};
typedef std::shared_ptr<SingleChainManipulability> SingleChainManipulabilityPtr;
diff --git a/VirtualRobot/Manipulability/SingleChainManipulabilityTracking.cpp b/VirtualRobot/Manipulability/SingleChainManipulabilityTracking.cpp
index 9edd01a3299971e747abbcfa53badd04d3ad3e0c..cecf95ee3b26da7dcbaa0886e7b002fbbf30693c 100644
--- a/VirtualRobot/Manipulability/SingleChainManipulabilityTracking.cpp
+++ b/VirtualRobot/Manipulability/SingleChainManipulabilityTracking.cpp
@@ -30,6 +30,12 @@ namespace VirtualRobot
SingleChainManipulabilityTracking::SingleChainManipulabilityTracking(AbstractSingleChainManipulabilityPtr manipulability) : manipulability(manipulability)
{
+ // Initialize the jointAngleLimitGradient vector
+ Eigen::MatrixXd jacobian = manipulability->computeJacobian();
+ int nbJoints = jacobian.cols();
+ Eigen::VectorXd gradient(nbJoints);
+ gradient.setZero();
+ jointAngleLimitGradient = gradient;
}
Eigen::Tensor<double, 3> SingleChainManipulabilityTracking::computeManipulabilityJacobian(const Eigen::MatrixXd &jacobian) {
@@ -88,6 +94,34 @@ Eigen::VectorXf SingleChainManipulabilityTracking::calculateVelocity(const Eigen
return dq.cast<float>();
}
+Eigen::VectorXf SingleChainManipulabilityTracking::calculateVelocityWithJointLimitsAvoidance(const Eigen::MatrixXd &manipulabilityDesired, Eigen::MatrixXd gainMatrix) {
+ Eigen::MatrixXd jacobian = manipulability->computeFullJacobian(); // full jacobian is required for derivative
+ Eigen::MatrixXd jacobian_sub = manipulability->getJacobianSubMatrix(jacobian);
+ Eigen::MatrixXd manipulabilityCurrent = manipulability->computeManipulability(jacobian_sub);
+ Eigen::MatrixXd manipulability_velocity = logMap(manipulabilityDesired, manipulabilityCurrent);
+ Eigen::MatrixXd manipulability_mandel = symMatrixToVector(manipulability_velocity);
+
+ // Compute manipulability Jacobian
+ Eigen::Tensor<double, 3> manipJ = computeManipulabilityJacobian(jacobian);
+ Eigen::Tensor<double, 3> manipJ_sub = subCube(manipJ);
+ Eigen::MatrixXd matManipJ = computeManipulabilityJacobianMandelNotation(manipJ_sub);
+
+ // Compute weighted manipulability Jacobian
+ Eigen::MatrixXd jointsWeightMatrix = getJointsLimitsWeightMatrix(manipulability->getJointAngles(), manipulability->getJointLimitsHigh(), manipulability->getJointLimitsLow());
+ matManipJ = matManipJ * jointsWeightMatrix;
+ // Damping factor, and pseudo inverse
+ double dampingFactor = getDamping(matManipJ);
+ Eigen::MatrixXd dampedLSI = jointsWeightMatrix * dampedLeastSquaresInverse(matManipJ, dampingFactor);
+
+ if (gainMatrix.rows() == 0)
+ {
+ gainMatrix = getDefaultGainMatrix();
+ }
+
+ Eigen::VectorXd dq = dampedLSI * gainMatrix * manipulability_mandel;
+ return dq.cast<float>();
+}
+
int SingleChainManipulabilityTracking::getTaskVars() {
return manipulability->getTaskVars();
}
diff --git a/VirtualRobot/Manipulability/SingleChainManipulabilityTracking.h b/VirtualRobot/Manipulability/SingleChainManipulabilityTracking.h
index dedb6647d505ca1931a0090257a2946589828126..d655f2c6768bcf8ce27aa5de6101a40f7fe3a6e3 100644
--- a/VirtualRobot/Manipulability/SingleChainManipulabilityTracking.h
+++ b/VirtualRobot/Manipulability/SingleChainManipulabilityTracking.h
@@ -45,6 +45,8 @@ public:
Eigen::VectorXf calculateVelocity(const Eigen::MatrixXd &manipulabilityDesired, Eigen::MatrixXd gainMatrix=Eigen::MatrixXd()) override;
+ Eigen::VectorXf calculateVelocityWithJointLimitsAvoidance(const Eigen::MatrixXd &manipulabilityDesired, Eigen::MatrixXd gainMatrix=Eigen::MatrixXd());
+
virtual int getTaskVars() override;
virtual AbstractManipulability::Mode getMode() override;