kinfam.sip

//Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
//
//Version: 1.0
//Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
//Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
//URL: http://www.orocos.org/kdl
//
//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., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA


template<TYPE>
%MappedType std::vector<TYPE>
{
%TypeHeaderCode
#include <vector>
%End

%ConvertFromTypeCode
    PyObject *l = PyList_New(sipCpp -> size());

    // Create the Python list of the correct length.
    if (!l)
        return NULL;

    // Go through each element in the C++ instance and convert it to a
    // wrapped P2d.
    for (int i = 0; i < (int)sipCpp->size(); ++i) {
        TYPE *cpp = new TYPE(sipCpp->at(i));
        PyObject *pobj = sipConvertFromInstance(cpp, sipClass_TYPE, sipTransferObj);

        // Get the Python wrapper for the Type instance, creating a new
        // one if necessary, and handle any ownership transfer.
        if (!pobj) {
            // There was an error so garbage collect the Python list.
            Py_DECREF(l);
            return NULL;
        }

        // Add the wrapper to the list.
        PyList_SET_ITEM(l, i, pobj);
    }

    // Return the Python list.
    return l;
%End

%ConvertToTypeCode
    // Check if type is compatible
    if (!sipIsErr) {
        // Must be any iterable
        PyObject *i = PyObject_GetIter(sipPy);
        bool iterable = (i != NULL);
        Py_XDECREF(i);
        return iterable;
    }

    // Iterate over the object
    PyObject *iterator = PyObject_GetIter(sipPy);
    PyObject *item;

    std::vector<TYPE> *V = new std::vector<TYPE>();

    while ((item = PyIter_Next(iterator)))
    {
        if (!sipCanConvertToInstance(item, sipClass_TYPE, SIP_NOT_NONE)) {
            PyErr_Format(PyExc_TypeError, "object in iterable cannot be converted to TYPE");
            *sipIsErr = 1;
            break;
        }

        int state;
        TYPE* p = reinterpret_cast<TYPE*>(
             sipConvertToInstance(item, sipClass_TYPE, 0, SIP_NOT_NONE, &state, sipIsErr));

        if (!*sipIsErr)
            V->push_back(*p);

        sipReleaseInstance(p, sipClass_TYPE, state);
        Py_DECREF(item);
    }

    Py_DECREF(iterator);

    if (*sipIsErr) {
        delete V;
        return 0;
    }

    *sipCppPtr = V;
    return sipGetState(sipTransferObj);
%End
};



class Joint{

%TypeHeaderCode
#include <kdl/joint.hpp>
#include <kdl/kinfam_io.hpp>
using namespace KDL;
%End


public:
    enum JointType {RotAxis,RotX,RotY,RotZ,TransAxis,TransX,TransY,TransZ,None};
    Joint(std::string name, JointType type=None,double scale=1,double offset=0,
              double inertia=0,double damping=0,double stiffness=0);
    Joint(JointType type=None,double scale=1,double offset=0,
          double inertia=0,const double damping=0,double stiffness=0);
    Joint(std::string name, Vector origin, Vector axis, JointType type, double scale=1, double offset=0,
	      double inertia=0, double damping=0, double stiffness=0);
    Joint(Vector origin, Vector axis, JointType type, double scale=1, double offset=0,
	      double inertia=0, double damping=0, double stiffness=0);
    
    Joint(const Joint& in);
    
    Frame pose(const double& q)const /Factory/ ;
    Twist twist(const double& qdot)const /Factory/ ;
    Vector JointAxis() const /Factory/;
    Vector JointOrigin() const /Factory/;
    std::string getName()const;
    const double& getInertia()const;
    const double& getDamping()const;
    const double& getStiffness()const;

    JointType getType() const;
    std::string getTypeName() const;
    const std::string* __repr__();
    %MethodCode
        std::ostringstream oss;
        oss<<(*sipCpp);
        std::string s(oss.str());
        sipRes=&s;
    %End
};

class RotationalInertia
{
%TypeHeaderCode
#include <kdl/rotationalinertia.hpp>
#include <kdl/kinfam_io.hpp>
using namespace KDL;
%End
public:
    RotationalInertia(double Ixx=0,double Iyy=0,double Izz=0,double Ixy=0,double Ixz=0,double Iyz=0);
        
    static RotationalInertia Zero()/Factory/;

    double __getitem__(int index);
%MethodCode
    if (a0 < 0 || a0 >= 9) {
        PyErr_SetString(PyExc_IndexError, "RotationalInertia index out of range");
        return 0;
    }
    sipRes=(*sipCpp).data[a0];
%End

    void __setitem__(int i, double value);
%MethodCode
    if (a0 < 0 || a0 >= 9) {
        PyErr_SetString(PyExc_IndexError, "RotationalInertia index out of range");
        return 0;
    }
    (*sipCpp).data[a0]=a1;
%End

};
Vector operator*(RotationalInertia& Ia, Vector omega) const /Factory/;
RotationalInertia operator*(double a, const RotationalInertia& I)/Factory/;
RotationalInertia operator+(const RotationalInertia& Ia, const RotationalInertia& Ib)/Factory/;


class RigidBodyInertia
{
%TypeHeaderCode
#include <kdl/rigidbodyinertia.hpp>
#include <kdl/kinfam_io.hpp>
using namespace KDL;
%End
public:
    RigidBodyInertia(double m=0, const Vector& oc=Vector::Zero(), const RotationalInertia& Ic=RotationalInertia::Zero());
    static RigidBodyInertia Zero() /Factory/;        
    RigidBodyInertia RefPoint(const Vector& p) /Factory/;
    double getMass()const /Factory/;        
    Vector getCOG() const /Factory/;
    RotationalInertia getRotationalInertia() const /Factory/;
};
RigidBodyInertia operator*(double a,const RigidBodyInertia& I) /Factory/;
RigidBodyInertia operator+(const RigidBodyInertia& Ia,const RigidBodyInertia& Ib) /Factory/;
Wrench operator*(const RigidBodyInertia& I,const Twist& t) /Factory/;
RigidBodyInertia operator*(const Frame& T,const RigidBodyInertia& I) /Factory/;
RigidBodyInertia operator*(const Rotation& R,const RigidBodyInertia& I) /Factory/;

class Segment
{
    
%TypeHeaderCode
#include <kdl/segment.hpp>
#include <kdl/kinfam_io.hpp>
using namespace KDL;
%End
public:
    Segment(const std::string& name, const Joint& joint=Joint(Joint::None), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero());
    Segment(const Joint& joint=Joint(Joint::None), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero());
    Segment(const Segment& in);

    const std::string* __repr__();
%MethodCode
    std::stringstream ss;
    ss<<(*sipCpp);
    std::string s(ss.str());
    sipRes=&s;
%End
    
    const Frame& getFrameToTip()const /Factory/;
    Frame pose(const double& q)const /Factory/ ;
    Twist twist(const double& q,const double& qdot)const /Factory/ ;
    const std::string& getName()const /Factory/;
    const Joint& getJoint()const /Factory/;
    const RigidBodyInertia& getInertia()const /Factory/;
    void setInertia(const RigidBodyInertia& Iin);
};

class Chain
{
    
%TypeHeaderCode
#include <kdl/chain.hpp>
using namespace KDL;
%End

public:
    Chain();
    Chain(const Chain& in);
    
    void addSegment(const Segment& segment);
    void addChain(const Chain& chain);
        
    unsigned int getNrOfJoints()const;
    unsigned int getNrOfSegments()const;

    const Segment* getSegment(unsigned int nr)const /Factory/;
%MethodCode
    if (a0 < 0 || a0 >= (unsigned int)sipCpp->getNrOfSegments()) {
        PyErr_SetString(PyExc_IndexError, "Chain index out of range");
        return 0;
    }
    sipRes = &(sipCpp->getSegment(a0));
%End

};

class Tree {
%TypeHeaderCode
#include <kdl/tree.hpp>
using namespace KDL;
%End
public:
    Tree(const std::string& root_name="root");
    bool addSegment(const Segment& segment, const std::string& hook_name);
    unsigned int getNrOfJoints()const;
    unsigned int getNrOfSegments()const;
    Chain* getChain(const std::string& chain_root, const std::string& chain_tip)const /Factory/;
%MethodCode
    Chain* chain = new Chain();
    sipCpp->getChain(*a0, *a1, *chain);
    sipRes = chain;
%End
};

class JntArray{

%TypeHeaderCode
#include <kdl/jntarray.hpp>
using namespace KDL;
%End

public:
    JntArray(unsigned int size);
    JntArray(const JntArray& arg);
    unsigned int rows()const;
    unsigned int columns()const;
    void resize(unsigned int newSize);

    double __getitem__ (int index);
%MethodCode
    if (a0 < 0 || a0 >= (int)sipCpp->rows()) {
        PyErr_SetString(PyExc_IndexError, "JntArray index out of range");
        return 0;
    }
    sipRes=(*sipCpp)(a0);
%End
    
    void __setitem__(int index, double value);
%MethodCode
    if (a0 < 0 || a0 >= (int)sipCpp->rows()) {
        PyErr_SetString(PyExc_IndexError, "JntArray index out of range");
        return 0;
    }
    (*sipCpp)(a0)=a1;
%End

    const std::string* __repr__();
%MethodCode
    std::stringstream ss;
    ss<<sipCpp->data;
    std::string s(ss.str());
    sipRes=&s;
%End
};

void Add(const JntArray& src1,const JntArray& src2,JntArray& dest);
void Subtract(const JntArray& src1,const JntArray& src2,JntArray& dest);
void Multiply(const JntArray& src,const double& factor,JntArray& dest);
void Divide(const JntArray& src,const double& factor,JntArray& dest);
void MultiplyJacobian(const Jacobian& jac, const JntArray& src, Twist& dest);
void SetToZero(JntArray& array);
bool Equal(const JntArray& src1,const JntArray& src2,double eps=epsilon);
bool operator==(const JntArray& src1,const JntArray& src2);
//bool operator!=(const JntArray& src1,const JntArray& src2);

class JntArrayVel
{
%TypeHeaderCode
#include <kdl/jntarrayvel.hpp>
using namespace KDL;
%End

public:
    JntArray q;
    JntArray qdot;
    JntArrayVel(unsigned int size);
    JntArrayVel(const JntArray& q,const JntArray& qdot);
    JntArrayVel(const JntArray& q);
    void resize(unsigned int newSize);
    
    JntArray value()const /Factory/;
    JntArray deriv()const /Factory/;
};

void Add(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest);
void Add(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest);
void Subtract(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest);
void Subtract(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest);
void Multiply(const JntArrayVel& src,const double& factor,JntArrayVel& dest);
void Multiply(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest);
void Divide(const JntArrayVel& src,const double& factor,JntArrayVel& dest);
void Divide(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest);
void SetToZero(JntArrayVel& array);
bool Equal(const JntArrayVel& src1,const JntArrayVel& src2,double eps=epsilon);

class Jacobian
{
%TypeHeaderCode
#include <kdl/jntarray.hpp>
using namespace KDL;
%End
public:
    Jacobian(unsigned int size);
    Jacobian(const Jacobian& arg);
    unsigned int rows()const;
    unsigned int columns()const;
    void resize(unsigned int newNrOfColumns);

    double __getitem__ (SIP_PYTUPLE);
%MethodCode
    int i,j;
    PyArg_ParseTuple(a0,"ii",&i,&j);
    if (i < 0 || j < 0 || i > 5 || j >= (int)sipCpp->columns()) {
        PyErr_SetString(PyExc_IndexError, "Jacobian index out of range");
        return 0;
    }
    sipRes=(*sipCpp)(i,j);
%End
    
    void __setitem__(SIP_PYTUPLE,double value);
%MethodCode
    int i,j;
    PyArg_ParseTuple(a0,"ii",&i,&j);
    if (i < 0 || j < 0 || i > 5 || j >= (int)sipCpp->columns()) {
        PyErr_SetString(PyExc_IndexError, "Jacobian index out of range");
        return 0;
    }
    (*sipCpp)(i,j)=a1;
%End

    const std::string* __repr__();
%MethodCode
    std::stringstream ss;
    ss<<sipCpp->data;
    std::string s(ss.str());
    sipRes=&s;
%End
    Twist getColumn(unsigned int i) const /Factory/;
    void setColumn(unsigned int i,const Twist& t);
    
    void changeRefPoint(const Vector& base_AB);
    void changeBase(const Rotation& rot);
    void changeRefFrame(const Frame& frame);

};
void SetToZero(Jacobian& jac);
    
void changeRefPoint(const Jacobian& src1, const Vector& base_AB, Jacobian& dest);
void changeBase(const Jacobian& src1, const Rotation& rot, Jacobian& dest);
void changeRefFrame(const Jacobian& src1,const Frame& frame, Jacobian& dest);

class SolverI
{
%TypeHeaderCode
#include <kdl/solveri.hpp>
using namespace KDL;
%End
    virtual int getError() const;
    virtual const char* strError(const int error) const;
    virtual void updateInternalDataStructures() = 0;
};

class ChainFkSolverPos : SolverI
{
%TypeHeaderCode
#include <kdl/chainfksolver.hpp>
using namespace KDL;
%End
    virtual int JntToCart(const JntArray& q_in, Frame& p_out,int segmentNr=-1)=0;
};

class ChainFkSolverVel : SolverI
{
%TypeHeaderCode
#include <kdl/chainfksolver.hpp>
using namespace KDL;
%End
    virtual int JntToCart(const JntArrayVel& q_in, FrameVel& p_out,int
    segmentNr=-1)=0;
};

class ChainFkSolverPos_recursive : ChainFkSolverPos
{
%TypeHeaderCode
#include <kdl/chainfksolverpos_recursive.hpp>
using namespace KDL;
%End

public:
    ChainFkSolverPos_recursive(const Chain& chain);
    virtual int JntToCart(const JntArray& q_in, Frame& p_out,int segmentNr=-1);
    virtual void updateInternalDataStructures();
};

class ChainFkSolverVel_recursive : ChainFkSolverVel
{
%TypeHeaderCode
#include <kdl/chainfksolvervel_recursive.hpp>
using namespace KDL;
%End
public:
    ChainFkSolverVel_recursive(const Chain& chain);
    virtual int JntToCart(const JntArrayVel& q_in ,FrameVel& out,int
    segmentNr=-1 );
    virtual void updateInternalDataStructures();
};

class ChainIkSolverPos : SolverI {
%TypeHeaderCode
#include <kdl/chainiksolver.hpp>
using namespace KDL;
%End
public:
    virtual int CartToJnt(const JntArray& q_init , const Frame& p_in, JntArray& q_out )=0;
    virtual void updateInternalDataStructures()=0;
};

class ChainIkSolverVel : SolverI {
%TypeHeaderCode
#include <kdl/chainiksolver.hpp>
using namespace KDL;
%End
public:
    virtual int CartToJnt(const JntArray& q_in , const Twist& v_in , JntArray& qdot_out )=0;
    virtual int CartToJnt(const JntArray& q_init , const FrameVel& v_in , JntArrayVel& q_out )=0;
    virtual void updateInternalDataStructures()=0;
};

class ChainIkSolverPos_NR : ChainIkSolverPos
{
%TypeHeaderCode
#include <kdl/chainiksolverpos_nr.hpp>
using namespace KDL;
%End
public:
    ChainIkSolverPos_NR(const Chain& chain,ChainFkSolverPos& fksolver,ChainIkSolverVel& iksolver,
                        unsigned int maxiter=100,double eps=epsilon);
    
    virtual int CartToJnt(const JntArray& q_init , const Frame& p_in ,JntArray& q_out);
    virtual void updateInternalDataStructures();
};

class ChainIkSolverPos_NR_JL : ChainIkSolverPos
{
%TypeHeaderCode
#include <kdl/chainiksolverpos_nr_jl.hpp>
using namespace KDL;
%End
public:
    ChainIkSolverPos_NR_JL(const Chain& chain,const JntArray &q_min,const JntArray &q_max, 
                        ChainFkSolverPos& fksolver,ChainIkSolverVel& iksolver,
                        unsigned int maxiter=100,double eps=epsilon);
    
    virtual int CartToJnt(const JntArray& q_init , const Frame& p_in ,JntArray& q_out);
    virtual void updateInternalDataStructures();
};

class ChainIkSolverVel_pinv : ChainIkSolverVel
{
%TypeHeaderCode
#include <kdl/chainiksolvervel_pinv.hpp>
using namespace KDL;
%End
public:
    ChainIkSolverVel_pinv(const Chain& chain,double eps=0.00001,int maxiter=150);
        
    virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out);
    virtual void updateInternalDataStructures();
};

class ChainIkSolverVel_wdls : ChainIkSolverVel
{
%TypeHeaderCode
#include <kdl/chainiksolvervel_wdls.hpp>
using namespace KDL;
%End
public:
    ChainIkSolverVel_wdls(const Chain& chain,double eps=0.00001,int maxiter=150);
        
    virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out);
    virtual void updateInternalDataStructures();

    void setWeightTS(SIP_PYLIST);
%MethodCode
    //void setWeightTS(const Eigen::MatrixXd& Mx);
    //Mx has to be a 6x6 Matrix

    Py_ssize_t numRows,numCols;
    double c_item;
    PyObject *list=a0;
    numRows=PyList_Size(list);
    PyObject *temp1;
    temp1=PyList_GetItem(list,0);
    numCols=PyList_Size(temp1);
    if (numRows!=numCols) {
       sipIsErr=1; //todo: raise exception
    }
    if (numRows!=6) {
       sipIsErr=1; //todo: raise exception
    }
    Eigen::MatrixXd Mx;
    Mx=Eigen::MatrixXd::Identity(numRows,numCols);
    
    for (Py_ssize_t r=0;r<numRows;r++) {
        PyObject *row;
        row=PyList_GetItem(list,r);
        if (numCols!=PyList_Size(row)) {
           sipIsErr=1; //todo: raise exception
        }
        for (Py_ssize_t c=0;c<numCols;c++) {
            PyObject *item;
            item=PyList_GetItem(row,c);
            c_item=PyFloat_AsDouble(item);
            Mx(r,c)= c_item;
        }
    }
    sipCpp->setWeightTS(Mx);
%End

    void setWeightJS(SIP_PYLIST);
%MethodCode
    //void setWeightJS(const Eigen::MatrixXd& Mx);
    //Mx has to be a simetric positive definite Matrix
    //unsigned int nOfJoints=sipCpp->chain.getNrOfJoints(); //To check that we are receiving valid data dimensions. This doesn't work, chain is a private member. todo: How can we check for this?
    Py_ssize_t numRows,numCols;
    double c_item;
    PyObject *list=a0;
    numRows=PyList_Size(list);
    PyObject *temp1;
    temp1=PyList_GetItem(list,0);
    numCols=PyList_Size(temp1);
    if (numRows!=numCols) {
       sipIsErr=1; //todo: raise exception
    }
    Eigen::MatrixXd Mq;
    Mq=Eigen::MatrixXd::Identity(numRows,numCols);
    for (Py_ssize_t r=0;r<numRows;r++) {
        PyObject *row;
        row=PyList_GetItem(list,r);
        if (numCols!=PyList_Size(row)) {
           sipIsErr=1; //todo: raise exception
        }
        for (Py_ssize_t c=0;c<numCols;c++) {
            PyObject *item;
            item=PyList_GetItem(row,c);
            c_item=PyFloat_AsDouble(item);
            Mq(r,c)= c_item;
        }
    }
    sipCpp->setWeightJS(Mq);
%End

    void setLambda(const double& lambda);

};


class ChainIkSolverPos_LMA : ChainIkSolverPos
{
%TypeHeaderCode
#include <kdl/chainiksolverpos_lma.hpp>
using namespace KDL;
%End
public:
    ChainIkSolverPos_LMA(const Chain& chain, double eps=0.00001, int _maxiter=500, double _eps_joints=0.000000000000001);

    virtual int CartToJnt(const JntArray& q_init , const Frame& p_in ,JntArray& q_out);
    virtual void updateInternalDataStructures();
};


class ChainIkSolverVel_pinv_nso : ChainIkSolverVel
{
%TypeHeaderCode
#include <kdl/chainiksolvervel_pinv_nso.hpp>
using namespace KDL;
%End
public:
    ChainIkSolverVel_pinv_nso(const Chain& chain,double eps=0.00001,int maxiter=150, double alpha=0.25);
        
    virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out);
    virtual void updateInternalDataStructures();

    virtual int setWeights(const JntArray &weights);

    virtual int setOptPos(const JntArray &opt_pos);

    virtual int setAlpha(const double alpha);

    const JntArray& getWeights()const /Factory/;

    const JntArray& getOptPos()const /Factory/;

    const double& getAlpha()const /Factory/;
};

class ChainIkSolverVel_pinv_givens : ChainIkSolverVel
{
%TypeHeaderCode
#include <kdl/chainiksolvervel_pinv_givens.hpp>
using namespace KDL;
%End
public:
    ChainIkSolverVel_pinv_givens(const Chain& chain);
        
    virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out);
    virtual void updateInternalDataStructures();
};

class ChainJntToJacSolver : SolverI
{
%TypeHeaderCode
#include <kdl/chainjnttojacsolver.hpp>
using namespace KDL;
%End
public:
    ChainJntToJacSolver(const Chain& chain);
    int JntToJac(const JntArray& q_in,Jacobian& jac, int seg_nr=-1);
    virtual void updateInternalDataStructures();
};

class ChainJntToJacDotSolver : SolverI
{
%TypeHeaderCode
#include <kdl/chainjnttojacdotsolver.hpp>
using namespace KDL;
%End
public:
    ChainJntToJacDotSolver(const Chain& chain);
    int JntToJacDot(const JntArrayVel& q_in,Jacobian& jac, int seg_nr=-1);
    virtual void updateInternalDataStructures();
};

class ChainIdSolver : SolverI
{
%TypeHeaderCode
#include <kdl/chainidsolver.hpp>
using namespace KDL;
%End
    virtual int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const std::vector<Wrench>& f_ext,JntArray &torques)=0;
};

class ChainIdSolver_RNE : ChainIdSolver
{
%TypeHeaderCode
#include <kdl/chainidsolver_recursive_newton_euler.hpp>
using namespace KDL;
%End

public:
	ChainIdSolver_RNE(const Chain& chain,Vector grav);
	int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const std::vector<Wrench>& f_ext,JntArray &torques);
	virtual void updateInternalDataStructures();
};