Scene.cpp

Go to the documentation of this file.

/* $Id: Scene.cpp 35814 2011-03-27 07:56:29Z campbellbarton $
 * Scene.cpp
 *
 *  Created on: Jan 5, 2009
 *      Author: rubensmits
 */

#include "Scene.hpp"
#include "ControlledObject.hpp"
#include "kdl/utilities/svd_eigen_HH.hpp"
#include <cstdio>

namespace iTaSC {

class SceneLock : public ControlledObject::JointLockCallback {
private:
        Scene* m_scene;
        Range  m_qrange;

public:
        SceneLock(Scene* scene) :
          m_scene(scene), m_qrange(0,0) {}
        virtual ~SceneLock() {}

        void setRange(Range& range)
        {
                m_qrange = range;
        }
        // lock a joint, no need to update output
        virtual void lockJoint(unsigned int q_nr, unsigned int ndof)
        {
                q_nr += m_qrange.start;
                project(m_scene->m_Wq, Range(q_nr, ndof), m_qrange).setZero();
        }
        // lock a joint and update output in view of reiteration
        virtual void lockJoint(unsigned int q_nr, unsigned int ndof, double* qdot)
        {
                q_nr += m_qrange.start;
                project(m_scene->m_Wq, Range(q_nr, ndof), m_qrange).setZero();
                // update the ouput vector so that the movement of this joint will be 
                // taken into account and we can put the joint back in its initial position
                // which means that the jacobian doesn't need to be changed
                for (unsigned int i=0 ;i<ndof ; ++i, ++q_nr) {
                        m_scene->m_ydot -= m_scene->m_A.col(q_nr)*qdot[i];
                }
        }
};

Scene::Scene():
        m_A(), m_B(), m_Atemp(), m_Wq(), m_Jf(), m_Jq(), m_Ju(), m_Cf(), m_Cq(), m_Jf_inv(),
        m_Vf(),m_Uf(), m_Wy(), m_ydot(), m_qdot(), m_xdot(), m_Sf(),m_tempf(),
        m_ncTotal(0),m_nqTotal(0),m_nuTotal(0),m_nsets(0),
        m_solver(NULL),m_cache(NULL) 
{
        m_minstep = 0.01;
        m_maxstep = 0.06;
}

Scene::~Scene() 
{
        ConstraintMap::iterator constraint_it;
        while ((constraint_it = constraints.begin()) != constraints.end()) {
                delete constraint_it->second;
                constraints.erase(constraint_it);
        }
        ObjectMap::iterator object_it;
        while ((object_it = objects.begin()) != objects.end()) {
                delete object_it->second;
                objects.erase(object_it);
        }
}

bool Scene::setParam(SceneParam paramId, double value)
{
        switch (paramId) {
        case MIN_TIMESTEP:
                m_minstep = value;
                break;
        case MAX_TIMESTEP:              
                m_maxstep = value;
                break;
        default:
                return false;
        }
        return true;
}

bool Scene::addObject(const std::string& name, Object* object, UncontrolledObject* base, const std::string& baseFrame)
{
        // finalize the object before adding
        object->finalize();
    //Check if Object is controlled or uncontrolled.
    if(object->getType()==Object::Controlled){
                int baseFrameIndex = base->addEndEffector(baseFrame);
                if (baseFrameIndex < 0)
                        return false;
                std::pair<ObjectMap::iterator, bool> result;
                if (base->getNrOfCoordinates() == 0) {
                        // base is fixed object, no coordinate range
                        result = objects.insert(ObjectMap::value_type(
                                        name, new Object_struct(object,base,baseFrameIndex,
                                                Range(m_nqTotal,object->getNrOfCoordinates()),
                                                Range(m_ncTotal,((ControlledObject*)object)->getNrOfConstraints()),
                                                Range(0,0))));
                } else {
                        // base is a moving object, must be in list already
                    ObjectMap::iterator base_it;
                        for (base_it=objects.begin(); base_it != objects.end(); base_it++) {
                                if (base_it->second->object == base)
                                        break;
                        }
                        if (base_it == objects.end())
                                return false;
                        result = objects.insert(ObjectMap::value_type(
                                        name, new Object_struct(object,base,baseFrameIndex,
                                                Range(m_nqTotal,object->getNrOfCoordinates()),
                                                Range(m_ncTotal,((ControlledObject*)object)->getNrOfConstraints()),
                                                base_it->second->coordinaterange)));
                }
                if (!result.second) {
                        return false;
                }
        m_nqTotal+=object->getNrOfCoordinates();
        m_ncTotal+=((ControlledObject*)object)->getNrOfConstraints();
        return true;
    }
    if(object->getType()==Object::UnControlled){
                if ((WorldObject*)base != &Object::world)
                        return false;
                std::pair<ObjectMap::iterator,bool> result = objects.insert(ObjectMap::value_type(
                        name,new Object_struct(object,base,0,
                                                Range(0,0),
                                                Range(0,0),
                                                Range(m_nuTotal,object->getNrOfCoordinates()))));
        if(!result.second)
            return false;
        m_nuTotal+=object->getNrOfCoordinates();
        return true;
    }
    return false;
}

bool Scene::addConstraintSet(const std::string& name,ConstraintSet* task,const std::string& object1,const std::string& object2, const std::string& ee1, const std::string& ee2)
{
    //Check if objects exist:
    ObjectMap::iterator object1_it = objects.find(object1);
    ObjectMap::iterator object2_it = objects.find(object2);
    if(object1_it==objects.end()||object2_it==objects.end())
        return false;
        int ee1_index = object1_it->second->object->addEndEffector(ee1);
        int ee2_index = object2_it->second->object->addEndEffector(ee2);
        if (ee1_index < 0 || ee2_index < 0)
                return false;
        std::pair<ConstraintMap::iterator,bool> result = 
                constraints.insert(ConstraintMap::value_type(name,new ConstraintSet_struct(
                        task,object1_it,ee1_index,object2_it,ee2_index,
                        Range(m_ncTotal,task->getNrOfConstraints()),Range(6*m_nsets,6))));
    if(!result.second)
        return false;
    m_ncTotal+=task->getNrOfConstraints();
    m_nsets+=1;
    return true;
}

bool Scene::addSolver(Solver* _solver){
    if(m_solver==NULL){
        m_solver=_solver;
        return true;
    }
    else
        return false;
}

bool Scene::addCache(Cache* _cache){
    if(m_cache==NULL){
        m_cache=_cache;
        return true;
    }
    else
        return false;
}

bool Scene::initialize(){

    //prepare all matrices:
        if (m_ncTotal == 0 || m_nqTotal == 0 || m_nsets == 0)
                return false;

    m_A = e_zero_matrix(m_ncTotal,m_nqTotal);
        if (m_nuTotal > 0) {
                m_B = e_zero_matrix(m_ncTotal,m_nuTotal);
                m_xdot = e_zero_vector(m_nuTotal);
                m_Ju = e_zero_matrix(6*m_nsets,m_nuTotal);
        }
    m_Atemp = e_zero_matrix(m_ncTotal,6*m_nsets);
    m_ydot = e_zero_vector(m_ncTotal);
    m_qdot = e_zero_vector(m_nqTotal);
    m_Wq = e_zero_matrix(m_nqTotal,m_nqTotal);
    m_Wy = e_zero_vector(m_ncTotal);
    m_Jq = e_zero_matrix(6*m_nsets,m_nqTotal);
    m_Jf = e_zero_matrix(6*m_nsets,6*m_nsets);
    m_Jf_inv = m_Jf;
    m_Cf = e_zero_matrix(m_ncTotal,m_Jf.rows());
    m_Cq = e_zero_matrix(m_ncTotal,m_nqTotal);

    bool result=true;
        // finalize all objects
        for (ObjectMap::iterator it=objects.begin(); it!=objects.end(); ++it) {
                Object_struct* os = it->second;

                os->object->initCache(m_cache);
                if (os->constraintrange.count > 0)
                        project(m_Cq,os->constraintrange,os->jointrange) = (((ControlledObject*)(os->object))->getCq());
        }

        m_ytask.resize(m_ncTotal);
        bool toggle=true;
        int cnt = 0;
    //Initialize all ConstraintSets:
    for(ConstraintMap::iterator it=constraints.begin();it!=constraints.end();++it){
        //Calculate the external pose:
                ConstraintSet_struct* cs = it->second;
                Frame external_pose;
                getConstraintPose(cs->task, cs, external_pose);
        result&=cs->task->initialise(external_pose);
                cs->task->initCache(m_cache);
                for (int i=0; i<cs->constraintrange.count; i++, cnt++) {
                        m_ytask[cnt] = toggle;
                }
                toggle = !toggle;
                project(m_Cf,cs->constraintrange,cs->featurerange)=cs->task->getCf();
    }

    if(m_solver!=NULL)
        m_solver->init(m_nqTotal,m_ncTotal,m_ytask);
    else
        return false;


    return result;
}

bool Scene::getConstraintPose(ConstraintSet* constraint, void *_param, KDL::Frame& _pose)
{
        // function called from constraint when they need to get the external pose
        ConstraintSet_struct* cs = (ConstraintSet_struct*)_param;
        // verification, the pointer MUST match
        assert (constraint == cs->task);
        Object_struct* ob1 = cs->object1->second;
        Object_struct* ob2 = cs->object2->second;
        //Calculate the external pose:
        _pose=(ob1->base->getPose(ob1->baseFrameIndex)*ob1->object->getPose(cs->ee1index)).Inverse()*(ob2->base->getPose(ob2->baseFrameIndex)*ob2->object->getPose(cs->ee2index));
        return true;
}

bool Scene::update(double timestamp, double timestep, unsigned int numsubstep, bool reiterate, bool cache, bool interpolate)
{
        // we must have valid timestep and timestamp
        if (timestamp < KDL::epsilon || timestep < 0.0)
                return false;
        Timestamp ts;
        ts.realTimestamp = timestamp;
        // initially we start with the full timestep to allow velocity estimation over the full interval
        ts.realTimestep = timestep;
        setCacheTimestamp(ts);
        ts.substep = 0;
        // for reiteration don't load cache
        // reiteration=additional iteration with same timestamp if application finds the convergence not good enough
        ts.reiterate = (reiterate) ? 1 : 0;
        ts.interpolate = (interpolate) ? 1 : 0;
        ts.cache = (cache) ? 1 : 0;
        ts.update = 1;
        ts.numstep = (numsubstep & 0xFF);
        bool autosubstep = (numsubstep == 0) ? true : false;
        if (numsubstep < 1)
                numsubstep = 1;
        double timesubstep = timestep/numsubstep;
        double timeleft = timestep;

        if (timeleft == 0.0) {
                // this special case correspond to a request to cache data
                for(ObjectMap::iterator it=objects.begin();it!=objects.end();++it){
                        it->second->object->pushCache(ts);
                }
                //Update the Constraints
                for(ConstraintMap::iterator it=constraints.begin();it!=constraints.end();++it){
                        it->second->task->pushCache(ts);
                }
                return true;
        }

        // double maxqdot; // UNUSED
        e_scalar nlcoef;
        SceneLock lockCallback(this);
        Frame external_pose;
        bool locked;

        // initially we keep timestep unchanged so that update function compute the velocity over
        while (numsubstep > 0) {
                // get objects
                for(ObjectMap::iterator it=objects.begin();it!=objects.end();++it) {
                        Object_struct* os = it->second;
                        if (os->object->getType()==Object::Controlled) {
                                ((ControlledObject*)(os->object))->updateControlOutput(ts);
                                if (os->constraintrange.count > 0) {
                                        project(m_ydot, os->constraintrange) = ((ControlledObject*)(os->object))->getControlOutput();
                                        project(m_Wy, os->constraintrange) = ((ControlledObject*)(os->object))->getWy();
                                        // project(m_Cq,os->constraintrange,os->jointrange) = (((ControlledObject*)(os->object))->getCq());
                                }
                                if (os->jointrange.count > 0) {
                                        project(m_Wq,os->jointrange,os->jointrange) = ((ControlledObject*)(os->object))->getWq();
                                }
                        }
                        if (os->object->getType()==Object::UnControlled && ((UncontrolledObject*)os->object)->getNrOfCoordinates() != 0) {
                    ((UncontrolledObject*)(os->object))->updateCoordinates(ts);
                                if (!ts.substep) {
                                        // velocity of uncontrolled object remains constant during substepping
                                        project(m_xdot,os->coordinaterange) = ((UncontrolledObject*)(os->object))->getXudot();
                                }
                        }
                }

                //get new Constraints values
                for(ConstraintMap::iterator it=constraints.begin();it!=constraints.end();++it) {
                        ConstraintSet_struct* cs = it->second;
                        Object_struct* ob1 = cs->object1->second;
                        Object_struct* ob2 = cs->object2->second;

                        if (ob1->base->updated() || ob1->object->updated() || ob2->base->updated() || ob2->object->updated()) {
                                // the object from which the constraint depends have changed position
                                // recompute the constraint pose
                                getConstraintPose(cs->task, cs, external_pose);
                                cs->task->initialise(external_pose);
                        }
                        cs->task->updateControlOutput(ts);
                project(m_ydot,cs->constraintrange)=cs->task->getControlOutput();
                        if (!ts.substep || cs->task->substep()) {
                                project(m_Wy,cs->constraintrange)=(cs->task)->getWy();
                                //project(m_Cf,cs->constraintrange,cs->featurerange)=cs->task->getCf();
                        }

                        project(m_Jf,cs->featurerange,cs->featurerange)=cs->task->getJf();
                        //std::cout << "Jf = " << Jf << std::endl;
                        //Transform the reference frame of this jacobian to the world reference frame
                        Eigen::Block<e_matrix> Jf_part = project(m_Jf,cs->featurerange,cs->featurerange);
                        changeBase(Jf_part,ob1->base->getPose(ob1->baseFrameIndex)*ob1->object->getPose(cs->ee1index));
                        //std::cout << "Jf_w = " << Jf << std::endl;

                        //calculate the inverse of Jf
                        KDL::svd_eigen_HH(project(m_Jf,cs->featurerange,cs->featurerange),m_Uf,m_Sf,m_Vf,m_tempf);
                        for(unsigned int i=0;i<6;++i)
                                if(m_Sf(i)<KDL::epsilon)
                                        m_Uf.col(i).setConstant(0.0);
                                else
                                        m_Uf.col(i)*=(1/m_Sf(i));
                        project(m_Jf_inv,cs->featurerange,cs->featurerange)=(m_Vf*m_Uf.transpose()).lazy();

                        //Get the robotjacobian associated with this constraintset
                        //Each jacobian is expressed in robot base frame => convert to world reference
                        //and negate second robot because it is taken reversed when closing the loop:
                        if(ob1->object->getType()==Object::Controlled){
                                project(m_Jq,cs->featurerange,ob1->jointrange) = (((ControlledObject*)(ob1->object))->getJq(cs->ee1index));
                                //Transform the reference frame of this jacobian to the world reference frame:
                                Eigen::Block<e_matrix> Jq_part = project(m_Jq,cs->featurerange,ob1->jointrange);
                                changeBase(Jq_part,ob1->base->getPose(ob1->baseFrameIndex));
                                // if the base of this object is moving, get the Ju part
                                if (ob1->base->getNrOfCoordinates() != 0) {
                                        // Ju is already computed for world reference frame
                                        project(m_Ju,cs->featurerange,ob1->coordinaterange)=ob1->base->getJu(ob1->baseFrameIndex);
                                }
                        } else if (ob1->object->getType() == Object::UnControlled && ((UncontrolledObject*)ob1->object)->getNrOfCoordinates() != 0) {
                                // object1 is uncontrolled moving object
                                project(m_Ju,cs->featurerange,ob1->coordinaterange)=((UncontrolledObject*)ob1->object)->getJu(cs->ee1index);
                        }
                        if(ob2->object->getType()==Object::Controlled){
                                //Get the robotjacobian associated with this constraintset
                                // process a special case where object2 and object1 are equal but using different end effector
                                if (ob1->object == ob2->object) {
                                        // we must create a temporary matrix
                                        e_matrix JqTemp(((ControlledObject*)(ob2->object))->getJq(cs->ee2index));
                                        //Transform the reference frame of this jacobian to the world reference frame:
                                        changeBase(JqTemp,ob2->base->getPose(ob2->baseFrameIndex));
                                        // substract in place
                                        project(m_Jq,cs->featurerange,ob2->jointrange) -= JqTemp;
                                } else {
                                        project(m_Jq,cs->featurerange,ob2->jointrange) = -(((ControlledObject*)(ob2->object))->getJq(cs->ee2index));
                                        //Transform the reference frame of this jacobian to the world reference frame:
                                        Eigen::Block<e_matrix> Jq_part = project(m_Jq,cs->featurerange,ob2->jointrange);
                                        changeBase(Jq_part,ob2->base->getPose(ob2->baseFrameIndex));
                                }
                                if (ob2->base->getNrOfCoordinates() != 0) {
                                        // if base is the same as first object or first object base, 
                                        // that portion of m_Ju has been set already => substract inplace
                                        if (ob2->base == ob1->base || ob2->base == ob1->object) {
                                                project(m_Ju,cs->featurerange,ob2->coordinaterange) -= ob2->base->getJu(ob2->baseFrameIndex);
                                        } else {
                                                project(m_Ju,cs->featurerange,ob2->coordinaterange) = -ob2->base->getJu(ob2->baseFrameIndex);
                                        }
                                }
                        } else if (ob2->object->getType() == Object::UnControlled && ((UncontrolledObject*)ob2->object)->getNrOfCoordinates() != 0) {
                                if (ob2->object == ob1->base || ob2->object == ob1->object) {
                                        project(m_Ju,cs->featurerange,ob2->coordinaterange) -= ((UncontrolledObject*)ob2->object)->getJu(cs->ee2index);
                                } else {
                                        project(m_Ju,cs->featurerange,ob2->coordinaterange) = -((UncontrolledObject*)ob2->object)->getJu(cs->ee2index);
                                }
                        }
                }

            //Calculate A
                m_Atemp=(m_Cf*m_Jf_inv).lazy();
                m_A = m_Cq-(m_Atemp*m_Jq).lazy();
                if (m_nuTotal > 0) {
                        m_B=(m_Atemp*m_Ju).lazy();
                        m_ydot += (m_B*m_xdot).lazy();
                }

                //Call the solver with A, Wq, Wy, ydot to solver qdot:
                if(!m_solver->solve(m_A,m_Wy,m_ydot,m_Wq,m_qdot,nlcoef))
                        // this should never happen
                        return false;
                //send result to the objects
                for(ObjectMap::iterator it=objects.begin();it!=objects.end();++it) {
                        Object_struct* os = it->second;
                        if(os->object->getType()==Object::Controlled)
                                ((ControlledObject*)(os->object))->setJointVelocity(project(m_qdot,os->jointrange));
                }
                // compute the constraint velocity
                for(ConstraintMap::iterator it=constraints.begin();it!=constraints.end();++it){
                        ConstraintSet_struct* cs = it->second;
                        Object_struct* ob1 = cs->object1->second;
                        Object_struct* ob2 = cs->object2->second;
                        //Calculate the twist of the world reference frame due to the robots (Jq*qdot+Ju*chiudot):
                        e_vector6 external_vel = e_zero_vector(6);
                        if (ob1->jointrange.count > 0)
                                external_vel += (project(m_Jq,cs->featurerange,ob1->jointrange)*project(m_qdot,ob1->jointrange)).lazy();
                        if (ob2->jointrange.count > 0)
                                external_vel += (project(m_Jq,cs->featurerange,ob2->jointrange)*project(m_qdot,ob2->jointrange)).lazy();
                        if (ob1->coordinaterange.count > 0)
                                external_vel += (project(m_Ju,cs->featurerange,ob1->coordinaterange)*project(m_xdot,ob1->coordinaterange)).lazy();
                        if (ob2->coordinaterange.count > 0)
                                external_vel += (project(m_Ju,cs->featurerange,ob2->coordinaterange)*project(m_xdot,ob2->coordinaterange)).lazy();
                        //the twist caused by the constraint must be opposite because of the closed loop
                        //estimate the velocity of the joints using the inverse jacobian
                        e_vector6 estimated_chidot = project(m_Jf_inv,cs->featurerange,cs->featurerange)*(-external_vel);
                        cs->task->setJointVelocity(estimated_chidot);
                }

                if (autosubstep) {
                        // automatic computing of substep based on maximum joint change
                        // and joint limit gain variation
                        // We will pass the joint velocity to each object and they will recommend a maximum timestep
                        timesubstep = timeleft;
                        // get armature max joint velocity to estimate the maximum duration of integration
                        // maxqdot = m_qdot.cwise().abs().maxCoeff(); // UNUSED
                        double maxsubstep = nlcoef*m_maxstep;
                        if (maxsubstep < m_minstep)
                                maxsubstep = m_minstep;
                        if (timesubstep > maxsubstep)
                                timesubstep = maxsubstep;
                        for(ObjectMap::iterator it=objects.begin();it!=objects.end();++it){
                                Object_struct* os = it->second;
                                if(os->object->getType()==Object::Controlled)
                                        ((ControlledObject*)(os->object))->getMaxTimestep(timesubstep);
                        }
                        for(ConstraintMap::iterator it=constraints.begin();it!=constraints.end();++it){
                                ConstraintSet_struct* cs = it->second;
                                cs->task->getMaxTimestep(timesubstep);
                        }
                        // use substep that are even dividers of timestep for more regularity
                        maxsubstep = 2.0*floor(timestep/2.0/timesubstep-0.66666);
                        timesubstep = (maxsubstep < 0.0) ? timestep : timestep/(2.0+maxsubstep);
                        if (timesubstep >= timeleft-(m_minstep/2.0)) {
                                timesubstep = timeleft;
                                numsubstep = 1;
                                timeleft = 0.;
                        } else {
                                numsubstep = 2;
                                timeleft -= timesubstep;
                        }
                }
                if (numsubstep > 1) {
                        ts.substep = 1;
                } else {
                        // set substep to false for last iteration so that controlled output 
                        // can be updated in updateKinematics() and model_update)() before next call to Secne::update()
                        ts.substep = 0;
                }
                // change timestep so that integration is done correctly
                ts.realTimestep = timesubstep;

                do {
                        ObjectMap::iterator it;
                        Object_struct* os;
                        locked = false;
                        for(it=objects.begin();it!=objects.end();++it){
                                os = it->second;
                                if (os->object->getType()==Object::Controlled) {
                                        lockCallback.setRange(os->jointrange);
                                        if (((ControlledObject*)os->object)->updateJoint(ts, lockCallback)) {
                                                // this means one of the joint was locked and we must rerun
                                                // the solver to update the remaining joints
                                                locked = true;
                                                break;
                                        }
                                }
                        }
                        if (locked) {
                                // Some rows of m_Wq have been cleared so that the corresponding joint will not move
                                if(!m_solver->solve(m_A,m_Wy,m_ydot,m_Wq,m_qdot,nlcoef))
                                        // this should never happen
                                        return false;

                                //send result to the objects
                                for(it=objects.begin();it!=objects.end();++it) {
                                        os = it->second;
                                        if(os->object->getType()==Object::Controlled)
                                                ((ControlledObject*)(os->object))->setJointVelocity(project(m_qdot,os->jointrange));
                                }
                        }
                } while (locked);

                //Update the Objects
                for(ObjectMap::iterator it=objects.begin();it!=objects.end();++it){
                        it->second->object->updateKinematics(ts);
                        // mark this object not updated since the constraint will be updated anyway
                        // this flag is only useful to detect external updates
                        it->second->object->updated(false);
                }
                //Update the Constraints
                for(ConstraintMap::iterator it=constraints.begin();it!=constraints.end();++it){
                        ConstraintSet_struct* cs = it->second;
                        //Calculate the external pose:
                        getConstraintPose(cs->task, cs, external_pose);
                        cs->task->modelUpdate(external_pose,ts);
                        // update the constraint output and cache
                        cs->task->updateKinematics(ts);
                }
                numsubstep--;
        }
        return true;
}

}