Added weighted manipulability for abstract and single chain

VirtualRobot/Manipulability/AbstractManipulability.cpp

@@ -32,10 +32,11 @@
 namespace VirtualRobot
 {
 
-AbstractManipulability::AbstractManipulability(AbstractManipulability::Mode mode, AbstractManipulability::Type type) :
+AbstractManipulability::AbstractManipulability(AbstractManipulability::Mode mode, AbstractManipulability::Type type,  Eigen::MatrixXd weightMatrixInit) :
     mode(mode),
     type(type)
 {
+    weightMatrix = weightMatrixInit;
 }
 
 Eigen::MatrixXd AbstractManipulability::computeJacobian() {

VirtualRobot/Manipulability/AbstractManipulability.h

@@ -42,7 +42,7 @@ public:
         Whole = 1, Position = 2, Orientation = 3
     };
 
-    AbstractManipulability(Mode mode, Type type);
+    AbstractManipulability(Mode mode, Type type, Eigen::MatrixXd weightMatrixInit = Eigen::MatrixXd());
 
     Eigen::MatrixXd computeJacobian();
 
@@ -96,6 +96,8 @@ public:
 
     static IKSolver::CartesianSelection GetCartesianSelection(Mode mode);
 
+    Eigen::MatrixXd weightMatrix; 
+
 protected:
     virtual Eigen::MatrixXd computeJacobian(IKSolver::CartesianSelection mode) = 0;
 

VirtualRobot/Manipulability/SingleChainManipulability.cpp

@@ -37,7 +37,12 @@ namespace VirtualRobot
 {
 
 Eigen::MatrixXd AbstractSingleChainManipulability::computeManipulability(const Eigen::MatrixXd &jacobian, AbstractManipulability::Type type) {
-    Eigen::MatrixXd velocityManipulability = jacobian * jacobian.transpose();
+    if (weightMatrix.rows() == 0){
+        int nbJoints = jacobian.cols();
+        weightMatrix.setIdentity(nbJoints, nbJoints); 
+    }
+    std::cout << weightMatrix << std::endl;
+    Eigen::MatrixXd velocityManipulability = jacobian * weightMatrix * weightMatrix.transpose() * jacobian.transpose();
 
     switch (type) {
     case Velocity:
@@ -52,13 +57,14 @@ Eigen::MatrixXd AbstractSingleChainManipulability::computeManipulability(const E
 
 
 
-SingleRobotNodeSetManipulability::SingleRobotNodeSetManipulability(const RobotNodeSetPtr &rns, Mode mode, Type type, bool convertMMtoM)
-    : SingleRobotNodeSetManipulability(rns, rns->getTCP(), mode, type, RobotNodePtr(), convertMMtoM)
+
+SingleRobotNodeSetManipulability::SingleRobotNodeSetManipulability(const RobotNodeSetPtr &rns, Mode mode, Type type, Eigen::MatrixXd weightMatrixInit, bool convertMMtoM)
+    : SingleRobotNodeSetManipulability(rns, rns->getTCP(), mode, type, RobotNodePtr(), weightMatrixInit, convertMMtoM)
 {
 }
 
-SingleRobotNodeSetManipulability::SingleRobotNodeSetManipulability(const RobotNodeSetPtr &rns, const RobotNodePtr &node, Mode mode, Type type, const RobotNodePtr &coordSystem, bool convertMMtoM)
-    : AbstractSingleChainManipulability(mode, type), rns(rns), node(node), coordSystem(coordSystem)
+SingleRobotNodeSetManipulability::SingleRobotNodeSetManipulability(const RobotNodeSetPtr &rns, const RobotNodePtr &node, Mode mode, Type type, const RobotNodePtr &coordSystem, Eigen::MatrixXd weightMatrixInit, bool convertMMtoM)
+    : AbstractSingleChainManipulability(mode, type, weightMatrixInit), rns(rns), node(node), coordSystem(coordSystem)
 {
     ik.reset(new DifferentialIK(rns, coordSystem, JacobiProvider::eSVDDamped));
     ik->convertModelScalingtoM(convertMMtoM);

VirtualRobot/Manipulability/SingleChainManipulability.h

@@ -46,14 +46,14 @@ typedef std::shared_ptr<AbstractSingleChainManipulability> AbstractSingleChainMa
 class SingleRobotNodeSetManipulability : public AbstractSingleChainManipulability
 {
 public:
-    SingleRobotNodeSetManipulability(const RobotNodeSetPtr& rns, Mode mode, Type type, bool convertMMtoM = true);
+    SingleRobotNodeSetManipulability(const RobotNodeSetPtr& rns, Mode mode, Type type, Eigen::MatrixXd weightMatrixInit = Eigen::MatrixXd(), bool convertMMtoM = true);
 
     /**
      * @param rns The robotNodes (i.e., joints) for which the Jacobians should be calculated.
      * @param node
      * @param coordSystem The coordinate system in which the Jacobians are defined. By default the global coordinate system is used.
      */
-    SingleRobotNodeSetManipulability(const RobotNodeSetPtr& rns, const RobotNodePtr& node, Mode mode, Type type, const RobotNodePtr& coordSystem = RobotNodePtr(), bool convertMMtoM = true);
+    SingleRobotNodeSetManipulability(const RobotNodeSetPtr& rns, const RobotNodePtr& node, Mode mode, Type type, const RobotNodePtr& coordSystem = RobotNodePtr(), Eigen::MatrixXd weightMatrixInit = Eigen::MatrixXd(), bool convertMMtoM = true);
 
     RobotNodeSetPtr getRobotNodeSet();
 

VirtualRobot/Manipulability/SingleChainManipulabilityTracking.cpp

@@ -39,17 +39,23 @@ SingleChainManipulabilityTracking::SingleChainManipulabilityTracking(AbstractSin
 }
 
 Eigen::Tensor<double, 3> SingleChainManipulabilityTracking::computeManipulabilityJacobian(const Eigen::MatrixXd &jacobian) {
+    int rows = jacobian.rows(); // task space dim
+    int columns = jacobian.cols(); // joint space dim
+
     Eigen::Tensor<double, 3> derivJ = computeJacobianDerivative(jacobian);
 
     Eigen::array<int, 3> shuffling({1, 0, 2}); // Permutation of first two dimensions
     Eigen::Tensor<double, 3> permDerivJ = derivJ.shuffle(shuffling);
 
+    // Define default weight matrix 
+    if (manipulability->weightMatrix.rows() == 0) {
+        manipulability->weightMatrix.setIdentity(columns, columns);
+    }
+
     // Compute dJ/dq x_2 J
-    Eigen::Tensor<double, 3> dJtdq_J = tensorMatrixProduct(permDerivJ, jacobian);
+    Eigen::Tensor<double, 3> dJtdq_J = tensorMatrixProduct(permDerivJ, jacobian * manipulability->weightMatrix * manipulability->weightMatrix.transpose());
 
     // Compute dJ'/dq x_1 J + dJ/dq x_2 J
-    int rows = jacobian.rows(); // task space dim
-    int columns = jacobian.cols(); // joint space dim
     Eigen::array<int, 2> shufflingTranpose({1, 0}); // for transposing
     Eigen::Tensor<double, 3> manipJ(rows,rows,columns);
     for(int s = 0; s < columns; ++s) {