jacobian.hpp

Go to the documentation of this file.

/***************************************************************************
                         Jacobian.h -  description
                       -------------------------
    begin                : June 2006
    copyright            : (C) 2006 Erwin Aertbelien
    email                : firstname.lastname@mech.kuleuven.ac.be

 History (only major changes)( AUTHOR-Description ) :
 
 ***************************************************************************
 *   This library is free software; you can redistribute it and/or         *
 *   modify it under the terms of the GNU Lesser General Public            *
 *   License as published by the Free Software Foundation; either          *
 *   version 2.1 of the License, or (at your option) any later version.    *
 *                                                                         *
 *   This library is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
 *   Lesser General Public License for more details.                       *
 *                                                                         *
 *   You should have received a copy of the GNU Lesser General Public      *
 *   License along with this library; if not, write to the Free Software   *
 *   Foundation, Inc., 59 Temple Place,                                    *
 *   Suite 330, Boston, MA  02111-1307  USA                                *
 *                                                                         *
 ***************************************************************************/
#ifndef KDL_Jacobian_H
#define KDL_Jacobian_H

#include <kdl/traits.h>
#include <assert.h>
#include <bitset>
#include <vector>

#define MAXNROFDERIV 31

typedef std::bitset<MAXNROFDERIV> JacobianBitset;

template <typename T>
class Expr {
public:
    typedef typename T::exprType exprType; 
        typedef typename T::derivType derivType;

        typedef typename T::valueType valueType;

    T elem;
public:
    Expr(const T& _elem):elem(_elem) {}
};



template <typename T>
class Jacobian {
public:

    typedef T exprType;
        typedef typename Traits<T>::valueType valueType;

        typedef typename Traits<T>::derivType derivType;
private:
    valueType val;
    std::vector<derivType> der;
    int size;
        JacobianBitset hasderiv;
public:
        explicit Jacobian(int _size=0):
                val(),
                der(_size),
                size(_size),
                hasderiv() {
                assert(_size < MAXNROFDERIV);
        }
        
        void resize(int _size) {
                size=_size;
                hasderiv = JacobianBitset();
                der.resize(_size);
                assert(_size < MAXNROFDERIV);
        }



    Jacobian(const Jacobian<T>& arg): 
                val(arg.val),
                der(arg.size),
                size(arg.size),
                hasderiv(arg.hasderiv)
        {
        for (int i=0;i<size;++i) der[i]=arg.der[i];
    }

        Jacobian(const valueType& arg):val(arg),der(0),size(0),hasderiv() {
        }

    ~Jacobian() {
    }

    derivType deriv(int i) const{
                assert((0<=i)&&(i<size));
                assert(hasderiv[i]);
        return der[i];
    }

    derivType& deriv(int i) {
                assert((0<=i)&&(i<size));
                hasderiv[i]=true;
        return der[i];
    }

    valueType& value(){
        return val;
    }

    valueType value() const {
        return val;
    }


    int nrOfDeriv() const {
        return size;
    }

        bool hasDeriv(int i) const {
                return hasderiv[i];
        }

        bool hasDerivs() const {
                return hasderiv.any();
        }

        
        JacobianBitset getDerivs() const {
                return hasderiv;
        }

        void setDeriv(int i,bool s=true) {
                assert(!( (!s) && (size==0) ));
                hasderiv[i] = s;
        }

    template <class A>
    Jacobian<T>& operator=(const Expr<A>& e) {          
                const A& expr = e.elem;        
                hasderiv = expr.getDerivs();
                if (hasderiv.any()) {
                        assert(expr.nrOfDeriv()==nrOfDeriv());
                for (int i=0;i<size;++i)
                                if (hasderiv[i]) {
                                        der[i] = expr.deriv(i);
                                }
                }
                // it is important that the following is located after the 
                // der[i] assignments.  In this way, A = A*B is a correct use of
                // this routine. i.e. expr.deriv(i) could refer to expr.value()
                val = expr.value();
        return *this;
    }


    Jacobian<T>& operator=(const Jacobian<T>& expr) {           
                hasderiv = expr.hasderiv;
                if (hasderiv.any()) {
                        assert(expr.nrOfDeriv()==nrOfDeriv());
                        for (int i=0;i<size;++i) 
                                if (hasderiv[i]) {
                                        der[i] = expr.deriv(i);
                                }
                }
                val = expr.value();
        return *this;
    }

        Jacobian<T>& operator=(const T& expr) {
        val = expr;
                hasderiv.reset();
        return *this;
    } 
};



template <typename T>
class Cnst {
public:
    typedef T exprType;
        typedef typename Traits<T>::valueType valueType;

        typedef typename Traits<T>::derivType derivType;
private:
    valueType val;
public:
        //
        // -Default constructor 
        // -Default copy constructor
        // -Default destructor
        // are sufficient.


    derivType deriv(int i) const{
                assert(false); // should never be called, this is a CONSTANT object.
        return derivType();
    }

    derivType& deriv(int i) {
                assert(false); // should never be called, this is a CONSTANT object.
        return derivType();
    }

    valueType& value(){
        return val;
    }

    valueType value() const {
        return val;
    }


    int nrOfDeriv() const {
        return 0;
    }

        bool hasDeriv(int i) const {
                return false;
        }

        bool hasDerivs() const {
                return false;
        }

        
        JacobianBitset getDerivs() const {
                return JacobianBitset(0);
        }

        void setDeriv(int i,bool s=true) {
                assert(false);
        }


         Cnst<T>& operator=(const T& expr) {
        val = expr;
        return *this;
    } 
};









#endif //KDL_Jacobian