eigen_types.hpp

Go to the documentation of this file.

/* SPDX-FileCopyrightText: 2009 Benoit Bolsee
 *
 * SPDX-License-Identifier: LGPL-2.1-or-later */
 
#ifndef EIGEN_TYPES_HPP_
#define EIGEN_TYPES_HPP_
 
#include <Eigen/Core>
#include "kdl/frames.hpp"
#include "kdl/tree.hpp"
#include "kdl/chain.hpp"
#include "kdl/jacobian.hpp"
#include "kdl/jntarray.hpp"
 
 
namespace iTaSC{
 
using KDL::Twist;
using KDL::Frame;
using KDL::Joint;
using KDL::Inertia;
using KDL::SegmentMap;
using KDL::Tree;
using KDL::JntArray;
using KDL::Jacobian;
using KDL::Segment;
using KDL::Rotation;
using KDL::Vector;
using KDL::Vector2;
using KDL::Chain;
 
extern const Frame F_identity;
 
#define e_scalar double
#define e_vector Eigen::Matrix<e_scalar, Eigen::Dynamic, 1>
#define e_zero_vector Eigen::Matrix<e_scalar, Eigen::Dynamic, 1>::Zero
#define e_matrix Eigen::Matrix<e_scalar, Eigen::Dynamic, Eigen::Dynamic>
#define e_matrix6 Eigen::Matrix<e_scalar,6,6>
#define e_identity_matrix Eigen::Matrix<e_scalar, Eigen::Dynamic, Eigen::Dynamic>::Identity
#define e_scalar_vector Eigen::Matrix<e_scalar, Eigen::Dynamic, 1>::Constant
#define e_zero_matrix Eigen::Matrix<e_scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero
#define e_random_matrix Eigen::Matrix<e_scalar, Eigen::Dynamic, Eigen::Dynamic>::Random
#define e_vector6 Eigen::Matrix<e_scalar,6,1>
#define e_vector3 Eigen::Matrix<e_scalar,3,1>
 
class Range {
public:
    int start;
    int count;
    Range(int _start, int _count) { start = _start; count=_count; }
    Range(const Range& other) { start=other.start; count=other.count; }
};
 
template<typename MatrixType> inline Eigen::Block<MatrixType> project(MatrixType& m, Range r)
{
    return Eigen::Block<MatrixType>(m,r.start,0,r.count,1);
}
 
template<typename MatrixType> inline Eigen::Block<MatrixType> project(MatrixType& m, Range r, Range c)
{
    return Eigen::Block<MatrixType>(m,r.start,c.start,r.count,c.count);
}
 
template<typename Derived> inline static int changeBase(Eigen::MatrixBase<Derived>& J, const Frame& T) {
 
    if (J.rows() != 6)
        return -1;
    for (int j = 0; j < J.cols(); ++j) {
        typename Derived::ColXpr Jj = J.col(j);
        Twist arg;
        for(unsigned int i=0;i<6;++i)
            arg(i)=Jj[i];
        Twist tmp(T*arg);
        for(unsigned int i=0;i<6;++i)
            Jj[i]=e_scalar(tmp(i));
    }
    return 0;
}
 
}
#endif /* UBLAS_TYPES_HPP_ */