JacobiSingularity.h

Go to the documentation of this file.

/**********************************************************************************************************************
This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich.
Licensed under the BSD-2 license (see LICENSE file in main directory)
**********************************************************************************************************************/

#pragma once

namespace ct {
namespace rbd {

template <size_t ROWS, size_t COLS>
class JacobiSingularity
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef Eigen::Matrix<double, ROWS, COLS> Jacobian;
    typedef Eigen::Matrix<double, COLS, ROWS> PseudoInverse;
    typedef Eigen::Matrix<double, ROWS, 1> SingularValues;
    typedef Eigen::Matrix<double, ROWS, 1> EndeffectorVelocities;
    typedef Eigen::Matrix<double, COLS, 1> JointVelocities;

    enum ManipulabilityMethod
    {
        DIRECT,
        SINGULAR_VALUES
    };

public:
    JacobiSingularity(const Jacobian& J);

    double calc_condition_number() const;

    double calc_manipulability(ManipulabilityMethod method = DIRECT) const;

    SingularValues calc_singular_values() const;

    JointVelocities calc_joint_vel(const EndeffectorVelocities& des_ee_vel) const;

private:
    Jacobian J_;
};


}  // namespace rbd
}  // namespace ct


#include "implementation/JacobiSingularity.h"