ConstraintSet.cpp

Go to the documentation of this file.

/* $Id: ConstraintSet.cpp 37836 2011-06-27 03:36:14Z campbellbarton $
 * ConstraintSet.cpp
 *
 *  Created on: Jan 5, 2009
 *      Author: rubensmits
 */

#include "ConstraintSet.hpp"
#include "kdl/utilities/svd_eigen_HH.hpp"

namespace iTaSC {

ConstraintSet::ConstraintSet(unsigned int _nc,double accuracy,unsigned int maximum_iterations):
    m_nc(_nc),
    m_Cf(e_zero_matrix(m_nc,6)),
    m_Wy(e_scalar_vector(m_nc,1.0)),
    m_y(m_nc),m_ydot(e_zero_vector(m_nc)),m_chi(e_zero_vector(6)),
    m_S(6),m_temp(6),m_tdelta(6),
    m_Jf(e_identity_matrix(6,6)),
    m_U(e_identity_matrix(6,6)),m_V(e_identity_matrix(6,6)),m_B(e_zero_matrix(6,6)),
    m_Jf_inv(e_zero_matrix(6,6)),
        m_internalPose(F_identity), m_externalPose(F_identity),
        m_constraintCallback(NULL), m_constraintParam(NULL), 
        m_toggle(false),m_substep(false),
    m_threshold(accuracy),m_maxIter(maximum_iterations)
{
        m_maxDeltaChi = e_scalar(0.52);
}

ConstraintSet::ConstraintSet():
    m_nc(0), 
        m_internalPose(F_identity), m_externalPose(F_identity),
        m_constraintCallback(NULL), m_constraintParam(NULL), 
        m_toggle(false),m_substep(false),
        m_threshold(0.0),m_maxIter(0)
{
        m_maxDeltaChi = e_scalar(0.52);
}

void ConstraintSet::reset(unsigned int _nc,double accuracy,unsigned int maximum_iterations)
{
    m_nc = _nc;
    m_Jf = e_identity_matrix(6,6);
    m_Cf = e_zero_matrix(m_nc,6);
    m_U = e_identity_matrix(6,6);
        m_V = e_identity_matrix(6,6);
        m_B = e_zero_matrix(6,6);
    m_Jf_inv = e_zero_matrix(6,6),
    m_Wy = e_scalar_vector(m_nc,1.0),
    m_chi = e_zero_vector(6);
    m_chidot = e_zero_vector(6);
        m_y = e_zero_vector(m_nc);
        m_ydot = e_zero_vector(m_nc);
        m_S = e_zero_vector(6);
        m_temp = e_zero_vector(6);
        m_tdelta = e_zero_vector(6);
    m_threshold = accuracy;
        m_maxIter = maximum_iterations;
}

ConstraintSet::~ConstraintSet() {

}

void ConstraintSet::modelUpdate(Frame& _external_pose,const Timestamp& timestamp)
{
    m_chi+=m_chidot*timestamp.realTimestep;
        m_externalPose = _external_pose;

    //update the internal pose and Jf
    updateJacobian();
        //check if loop is already closed, if not update the pose and Jf
    unsigned int iter=0;
    while(iter<5&&!closeLoop())
        iter++;
}

double ConstraintSet::getMaxTimestep(double& timestep)
{
        e_scalar maxChidot = m_chidot.cwise().abs().maxCoeff();
        if (timestep*maxChidot > m_maxDeltaChi) {
                timestep = m_maxDeltaChi/maxChidot;
        }
        return timestep;
}

bool ConstraintSet::initialise(Frame& init_pose){
    m_externalPose=init_pose;
        // get current Jf
    updateJacobian();

    unsigned int iter=0;
    while(iter<m_maxIter&&!closeLoop()){
        iter++;
    }
    if (iter<m_maxIter)
        return true;
    else
        return false;
}

bool ConstraintSet::setControlParameter(int id, ConstraintAction action, double data, double timestep)
{
        ConstraintValues values;
        ConstraintSingleValue value;
        values.values = &value;
        values.number = 0;
        values.action = action;
        values.id = id;
        value.action = action;
        value.id = id;
        switch (action) {
        case ACT_NONE:
                return true;
        case ACT_VALUE:
                value.yd = data;
                values.number = 1;
                break;
        case ACT_VELOCITY:
                value.yddot = data;
                values.number = 1;
                break;
        case ACT_TOLERANCE:
                values.tolerance = data;
                break;
        case ACT_FEEDBACK:
                values.feedback = data;
                break;
        case ACT_ALPHA:
                values.alpha = data;
                break;
        default:
                assert(action==ACT_NONE);
                break;
        }
        return setControlParameters(&values, 1, timestep);
}

bool ConstraintSet::closeLoop(){
    //Invert Jf
    //TODO: svd_boost_Macie has problems if Jf contains zero-rows
    //toggle=!toggle;
    //svd_boost_Macie(Jf,U,S,V,B,temp,1e-3*threshold,toggle);
        int ret = KDL::svd_eigen_HH(m_Jf,m_U,m_S,m_V,m_temp);
    if(ret<0)
        return false;

        // the reference point and frame of the jacobian is the base frame
        // m_externalPose-m_internalPose is the twist to extend the end effector
        // to get the required pose => change the reference point to the base frame
        Twist twist_delta(diff(m_internalPose,m_externalPose));
        twist_delta=twist_delta.RefPoint(-m_internalPose.p);
    for(unsigned int i=0;i<6;i++)
        m_tdelta(i)=twist_delta(i);
    //TODO: use damping in constraintset inversion?
    for(unsigned int i=0;i<6;i++)
        if(m_S(i)<m_threshold){
                        m_B.row(i).setConstant(0.0);
        }else
            m_B.row(i) = m_U.col(i)/m_S(i);

    m_Jf_inv=(m_V*m_B).lazy();

    m_chi+=(m_Jf_inv*m_tdelta).lazy();
    updateJacobian();
        // m_externalPose-m_internalPose in end effector frame
        // this is just to compare the pose, a different formula would work too
        return Equal(m_internalPose.Inverse()*m_externalPose,F_identity,m_threshold);

}
}