iKinSlv.cpp

Go to the documentation of this file.

/*
 * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT)
 * Copyright (C) 2006-2010 RobotCub Consortium
 * All rights reserved.
 *
 * This software may be modified and distributed under the terms
 * of the BSD-3-Clause license. See the accompanying LICENSE file for
 * details.
*/
 
#include <cstdlib>
#include <cmath>
#include <algorithm>
 
#include <yarp/os/Log.h>
#include <yarp/os/Network.h>
#include <yarp/os/Time.h>
 
#include <iCub/iKin/iKinVocabs.h>
#include <iCub/iKin/iKinSlv.h>
 
#define CARTSLV_DEFAULT_PER                 20      // [ms]
#define CARTSLV_DEFAULT_TOL                 1e-4
#define CARTSLV_DEFAULT_CONSTR_TOL          1e-6
#define CARTSLV_DEFAULT_MAXITER             200
#define CARTSLV_WEIGHT_2ND_TASK             0.01
#define CARTSLV_WEIGHT_3RD_TASK             0.01
#define CARTSLV_UNCTRLEDJNTS_THRES          1.0     // [deg]
 
using namespace std;
using namespace yarp::os;
using namespace yarp::sig;
using namespace yarp::dev;
using namespace yarp::math;
using namespace iCub::ctrl;
using namespace iCub::iKin;
 
 
/************************************************************************/
bool RpcProcessor::read(ConnectionReader &connection)
{
    if (!slv->isClosed())
    {
        Bottle cmd, reply;
        if (!cmd.read(connection))
            return false;
    
        slv->respond(cmd,reply);
        if (ConnectionWriter *writer=connection.getWriter())
            reply.write(*writer);
    
        return true;
    }
    else
        return false;
}
 
 
/************************************************************************/
InputPort::InputPort(CartesianSolver *_slv)
{
    slv=_slv;
 
    maxLen=7;
    xd.resize(maxLen,0.0);
    dof.resize(1,0.0);
 
    pose=IKINCTRL_POSE_FULL;
    contMode=false;
    isNew=false;
    token=0.0;
    pToken=NULL;
}
 
 
/************************************************************************/
void InputPort::set_dof(const Vector &_dof)
{
    lock_guard<mutex> lck(mtx);
    dof=_dof;
}
 
 
/************************************************************************/
Vector InputPort::get_dof()
{
    lock_guard<mutex> lck(mtx);
    Vector _dof=dof;
    return _dof;
}
 
 
/************************************************************************/
Vector InputPort::get_xd()
{
    lock_guard<mutex> lck(mtx);
    Vector _xd=xd;
    return _xd;
}
 
 
/************************************************************************/
void InputPort::reset_xd(const Vector &_xd)
{
    lock_guard<mutex> lck(mtx);
    xd=_xd;
    isNew=false;
}
 
 
/************************************************************************/
bool InputPort::isNewDataEvent()
{
    if (isNew)
    {
        isNew=false;
        return true;
    }
    else
        return false;
}
 
 
/************************************************************************/
bool InputPort::handleTarget(Bottle *b)
{
    if (b!=NULL)
    {
        lock_guard<mutex> lck(mtx);
        size_t len=std::min(b->size(),maxLen);
        for (size_t i=0; i<len; i++)
            xd[i]=b->get(i).asFloat64();
        return isNew=true;
    }
    else
        return false;
}
 
 
/************************************************************************/
bool InputPort::handleDOF(Bottle *b)
{
    if (b!=NULL)
    {
        slv->lock();
 
        mtx.lock();
        dof.resize(b->size());
        for (int i=0; i<b->size(); i++)
            dof[i]=b->get(i).asInt32();
        mtx.unlock();
 
        slv->unlock();
        return true;
    }
    else
        return false;
}
 
 
/************************************************************************/
bool InputPort::handlePose(const int newPose)
{
    if (newPose==IKINSLV_VOCAB_VAL_POSE_FULL)
    {
        if (pose==IKINCTRL_POSE_XYZ)
            isNew=true;
 
        pose=IKINCTRL_POSE_FULL;
        return true;
    }
    else if (newPose==IKINSLV_VOCAB_VAL_POSE_XYZ)
    {
        if (pose==IKINCTRL_POSE_FULL)
            isNew=true;
 
        pose=IKINCTRL_POSE_XYZ;
        return true;
    }
    else
        return false;
}
 
 
/************************************************************************/
bool InputPort::handleMode(const int newMode)
{
    if (newMode==IKINSLV_VOCAB_VAL_MODE_TRACK)
    {
        slv->lock();
        contMode=true;
        slv->unlock();
        return true;
    }
    else if (newMode==IKINSLV_VOCAB_VAL_MODE_SINGLE)
    {
        slv->lock();
        contMode=false;
        slv->unlock();
        return true;
    }
    else
        return false;
}
 
 
/************************************************************************/
void InputPort::onRead(Bottle &b)
{
    bool xdOptIn  =b.check(Vocab32::decode(IKINSLV_VOCAB_OPT_XD));
    bool dofOptIn =b.check(Vocab32::decode(IKINSLV_VOCAB_OPT_DOF));
    bool poseOptIn=b.check(Vocab32::decode(IKINSLV_VOCAB_OPT_POSE));
    bool modeOptIn=b.check(Vocab32::decode(IKINSLV_VOCAB_OPT_MODE));
 
    if (xdOptIn)
    {
        if (CartesianHelper::getTokenOption(b,&token))
            pToken=&token;
        else
            pToken=NULL;
    }
 
    if (modeOptIn)
        if (!handleMode(b.find(Vocab32::decode(IKINSLV_VOCAB_OPT_MODE)).asVocab32()))
            yWarning("%s: got incomplete %s command",slv->slvName.c_str(),
                     Vocab32::decode(IKINSLV_VOCAB_OPT_MODE).c_str());
 
    if (dofOptIn)
        if (!handleDOF(b.find(Vocab32::decode(IKINSLV_VOCAB_OPT_DOF)).asList()))
            yWarning("%s: expected %s data",slv->slvName.c_str(),
                     Vocab32::decode(IKINSLV_VOCAB_OPT_DOF).c_str());
 
    if (poseOptIn)
        if (!handlePose(b.find(Vocab32::decode(IKINSLV_VOCAB_OPT_POSE)).asVocab32()))
            yWarning("%s: got incomplete %s command",slv->slvName.c_str(),
                     Vocab32::decode(IKINSLV_VOCAB_OPT_POSE).c_str());
 
    if (slv->handleJointsRestPosition(b.find(Vocab32::decode(IKINSLV_VOCAB_OPT_REST_POS)).asList()) ||
        slv->handleJointsRestWeights(b.find(Vocab32::decode(IKINSLV_VOCAB_OPT_REST_WEIGHTS)).asList()))
    {
        slv->lock();
        slv->prepareJointsRestTask();
        slv->unlock();
    }
 
    // shall be the last handling
    if (xdOptIn)
    {    
        if (!handleTarget(b.find(Vocab32::decode(IKINSLV_VOCAB_OPT_XD)).asList()))
            yWarning("%s: expected %s data",slv->slvName.c_str(),
                     Vocab32::decode(IKINSLV_VOCAB_OPT_XD).c_str());
    }
    else
        yWarning("%s: missing %s data; it shall be present",
                 slv->slvName.c_str(),Vocab32::decode(IKINSLV_VOCAB_OPT_XD).c_str());
}
 
 
/************************************************************************/
void SolverCallback::exec(const Vector &xd, const Vector &q)
{
    slv->send(xd,slv->prt->chn->EndEffPose(),CTRL_RAD2DEG*q,slv->pToken);
}
 
 
/************************************************************************/
CartesianSolver::CartesianSolver(const string &_slvName) :
                                PeriodicThread((double)CARTSLV_DEFAULT_PER/1000.0)
{          
    // initialization
    slvName=_slvName;
    configured=false;
    closing=false;
    closed=false;
    interrupting=false;
    verbosity=false;
    timeout_detected=false;
    maxPartJoints=0;
    unctrlJointsNum=0;
    ping_robot_tmo=0.0;
 
    prt=NULL;
    slv=NULL;
    clb=NULL;
    inPort=NULL;
    outPort=NULL;
 
    // open rpc port
    rpcPort=new Port;
    cmdProcessor=new RpcProcessor(this);
    rpcPort->setReader(*cmdProcessor);
    rpcPort->open("/"+slvName+"/rpc");
 
    // token
    token=0.0;
    pToken=NULL;
}
 
 
/************************************************************************/
PolyDriver *CartesianSolver::waitPart(const Property &partOpt)
{    
    string partName=partOpt.find("part").asString();
    PolyDriver *pDrv=NULL;
 
    double t0=Time::now();
    while (Time::now()-t0<ping_robot_tmo)
    {
        delete pDrv;
 
        pDrv=new PolyDriver(const_cast<Property&>(partOpt));
        bool ok=pDrv->isValid();
 
        if (ok)
        {
            yInfo("%s: Checking if %s part is active ... ok",
                  slvName.c_str(),partName.c_str());
            return pDrv;
        }
        else
        {
            double dt=ping_robot_tmo-(Time::now()-t0);
            yInfo("%s: Checking if %s part is active ... not yet: still %.1f [s] to timeout expiry",
                  slvName.c_str(),partName.c_str(),dt>0.0?dt:0.0);
 
            double t1=Time::now();
            while (Time::now()-t1<1.0)
                Time::delay(0.1);
        }
 
        if (interrupting)
            break;
    }
 
    return pDrv;
}
 
 
/************************************************************************/
bool CartesianSolver::alignJointsBounds()
{
    hwLimits.resize(prt->chn->getN(),2);
    double min, max;
    int cnt=0;    
    
    yInfo("%s: aligning joints bounds ...",slvName.c_str());
    for (int i=0; i<prt->num; i++)
    {
        yInfo("part #%d: %s",i,prt->prp[i].find("part").asString().c_str());
        for (int j=0; j<jnt[i]; j++)
        {               
            if (!lim[i]->getLimits(rmp[i][j],&min,&max))
            {
                yError("joint #%d: failed getting limits!",cnt);
                return false;
            }
 
            yInfo("joint #%d: [%g, %g] deg",cnt,min,max);
            (*prt->chn)[cnt].setMin(CTRL_DEG2RAD*min);
            (*prt->chn)[cnt].setMax(CTRL_DEG2RAD*max);
 
            hwLimits(cnt,0)=min;
            hwLimits(cnt,1)=max;
        
            cnt++;
        }
    }
 
    swLimits=hwLimits;
    return true;
}
 
 
/************************************************************************/
bool CartesianSolver::setLimits(int axis, double min, double max)
{
    if (axis>=(int)prt->chn->getN())
        yError("#%d>#%d: requested out of range axis!",
               axis,prt->chn->getN());
    else if (min>max)
        yError("joint #%d: requested wrong bounds [%g,%g]!",
               axis,min,max);
    else if ((min>=hwLimits(axis,0)) && (max<=hwLimits(axis,1)))
    {
        (*prt->chn)[axis].setMin(CTRL_DEG2RAD*min);
        (*prt->chn)[axis].setMax(CTRL_DEG2RAD*max);
 
        swLimits(axis,0)=min;
        swLimits(axis,1)=max;
 
        return true;
    }
    else
        yWarning("joint #%d: requested out of range limit!",axis);
 
    return false;
}
 
 
/************************************************************************/
void CartesianSolver::countUncontrolledJoints()
{
    unctrlJointsNum=prt->chn->getN()-prt->chn->getDOF();
}
 
 
/************************************************************************/
void CartesianSolver::latchUncontrolledJoints(Vector &joints)
{
    if (unctrlJointsNum>0)
    {
        joints.resize(unctrlJointsNum);
        int j=0;
        
        for (unsigned int i=0; i<prt->chn->getN(); i++)
            if ((*prt->chn)[i].isBlocked())
                joints[j++]=prt->chn->getAng(i);
    }
    else
        joints.resize(1);
}
 
 
/************************************************************************/
void CartesianSolver::getFeedback(const bool wait)
{
    Vector fbTmp(maxPartJoints);
    int chainCnt=0;
 
    for (int i=0; i<prt->num; i++)
    {    
        bool ok=enc[i]->getEncoders(fbTmp.data());
        while (wait && !closing && !ok)
        {
            ok=enc[i]->getEncoders(fbTmp.data());
            Time::yield();
        }
 
        if (ok)
        {    
            for (int j=0; j<jnt[i]; j++)
            {
                double tmp=CTRL_DEG2RAD*fbTmp[rmp[i][j]];
            
                if ((*prt->chn)[chainCnt].isBlocked())
                    prt->chn->setBlockingValue(chainCnt,tmp);
                else
                    prt->chn->setAng(chainCnt,tmp);
            
                chainCnt++;
            }
        }
        else
        {    
            yWarning("%s: timeout detected on part %s!",
                     slvName.c_str(),prt->prp[i].find("part").asString().c_str());
 
            timeout_detected=true;
            chainCnt+=jnt[i];
        }
    }
}
 
 
/************************************************************************/
void CartesianSolver::initPos()
{
    lock();
    getFeedback(true);  // wait until all joints are acquired
    unlock();
 
    latchUncontrolledJoints(unctrlJointsOld);
    inPort->reset_xd(prt->chn->EndEffPose());
}
 
 
/************************************************************************/
void CartesianSolver::lock()
{
    mtx.lock();
}
 
 
/************************************************************************/
void CartesianSolver::unlock()
{
    mtx.unlock();
}
 
 
/************************************************************************/
void CartesianSolver::waitDOFHandling()
{
    unique_lock<mutex> lck(mtx_dofEvent);
    cv_dofEvent.wait(lck);
}
 
 
/************************************************************************/
void CartesianSolver::postDOFHandling()
{
    cv_dofEvent.notify_all();
}
 
 
/************************************************************************/
void CartesianSolver::fillDOFInfo(Bottle &reply)
{
    for (unsigned int i=0; i<prt->chn->getN(); i++)
        reply.addInt32(int(!(*prt->chn)[i].isBlocked()));
}
 
 
/************************************************************************/
void CartesianSolver::respond(const Bottle &command, Bottle &reply)
{
    if (command.size())
    {
        int vcb=command.get(0).asVocab32();
 
        if (!configured && vcb!=IKINSLV_VOCAB_CMD_CFG)
            reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
        else switch (vcb)
        {
            //-----------------
            case IKINSLV_VOCAB_CMD_GET:
            {
                if (command.size()>1)
                {
                    switch (command.get(1).asVocab32())
                    {
                        //-----------------
                        case IKINSLV_VOCAB_OPT_POSE:
                        {
                            reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
                    
                            if (ctrlPose==IKINCTRL_POSE_FULL)
                                reply.addVocab32(IKINSLV_VOCAB_VAL_POSE_FULL);
                            else
                                reply.addVocab32(IKINSLV_VOCAB_VAL_POSE_XYZ);
                    
                            break;
                        }
                    
                        //-----------------
                        case IKINSLV_VOCAB_OPT_PRIO:
                        {
                            reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
 
                            string priority=slv->get_posePriority();
                            if (priority=="position")
                                reply.addVocab32(IKINSLV_VOCAB_VAL_PRIO_XYZ);
                            else
                                reply.addVocab32(IKINSLV_VOCAB_VAL_PRIO_ANG);
 
                            break;
                        }
 
                        //-----------------
                        case IKINSLV_VOCAB_OPT_MODE:
                        {
                            reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
                    
                            if (inPort->get_contMode())
                                reply.addVocab32(IKINSLV_VOCAB_VAL_MODE_TRACK);
                            else
                                reply.addVocab32(IKINSLV_VOCAB_VAL_MODE_SINGLE);
                    
                            break;
                        }
                    
                        //-----------------
                        case IKINSLV_VOCAB_OPT_LIM:
                        {
                            if (command.size()>2)
                            {                    
                                int axis=command.get(2).asInt32();
                    
                                if (axis<(int)prt->chn->getN())
                                {
                                    reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
                                    reply.addFloat64(swLimits(axis,0));
                                    reply.addFloat64(swLimits(axis,1));
                                }
                                else
                                    reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
                            }
                            else
                                reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
                    
                            break;
                        }
                    
                        //-----------------
                        case IKINSLV_VOCAB_OPT_VERB:
                        {
                            reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
                    
                            if (verbosity)
                                reply.addVocab32(IKINSLV_VOCAB_VAL_ON);
                            else
                                reply.addVocab32(IKINSLV_VOCAB_VAL_OFF);
                    
                            break;
                        }
                    
                        //-----------------
                        case IKINSLV_VOCAB_OPT_DOF:
                        {
                            reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
                            Bottle &dofPart=reply.addList();
                            fillDOFInfo(dofPart);
                            break;
                        }
                    
                        //-----------------
                        case IKINSLV_VOCAB_OPT_REST_POS:
                        {
                            reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
                            handleJointsRestPosition(NULL,&reply);
                            break;
                        }
                    
                        //-----------------
                        case IKINSLV_VOCAB_OPT_REST_WEIGHTS:
                        {
                            reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
                            handleJointsRestWeights(NULL,&reply);
                            break;
                        }
                    
                        //-----------------
                        case IKINSLV_VOCAB_OPT_TASK2:
                        {
                            reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
                            Bottle &payLoad=reply.addList();
                            payLoad.addInt32(slv->get2ndTaskChain().getN());
            
                            Bottle &posPart=payLoad.addList();
                            for (size_t i=0; i<xd_2ndTask.length(); i++)
                                posPart.addFloat64(xd_2ndTask[i]);
 
                            Bottle &weightsPart=payLoad.addList();
                            for (size_t i=0; i<w_2ndTask.length(); i++)
                                weightsPart.addFloat64(w_2ndTask[i]);
            
                            break; 
                        }
            
                        //-----------------
                        case IKINSLV_VOCAB_OPT_CONVERGENCE:
                        {
                            reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
                            Bottle &payLoad=reply.addList();
 
                            Bottle &tol=payLoad.addList();
                            tol.addString("tol");
                            tol.addFloat64(slv->getTol());
 
                            Bottle &constr_tol=payLoad.addList();
                            constr_tol.addString("constr_tol");
                            constr_tol.addFloat64(slv->getConstrTol());
 
                            Bottle &maxIter=payLoad.addList();
                            maxIter.addString("max_iter");
                            maxIter.addInt32(slv->getMaxIter());
 
                            break;
                        }
 
                        //-----------------
                        default:
                            reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
                    }
                }
                else
                    reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
            
                break;
            }
            
            //-----------------
            case IKINSLV_VOCAB_CMD_SET:
            {
                if (command.size()>2)
                {
                    switch (command.get(1).asVocab32())
                    {
                        //-----------------
                        case IKINSLV_VOCAB_OPT_POSE:
                        {
                            if (inPort->handlePose(command.get(2).asVocab32()))
                                reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
                            else
                                reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
                    
                            break;
                        }
                    
                        //-----------------
                        case IKINSLV_VOCAB_OPT_PRIO:
                        {
                            int type=command.get(2).asVocab32();
                            if (type==IKINSLV_VOCAB_VAL_PRIO_XYZ)
                            {
                                slv->set_posePriority("position");
                                reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
                            }
                            else if (type==IKINSLV_VOCAB_VAL_PRIO_ANG)
                            {
                                slv->set_posePriority("orientation");
                                reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
                            }
                            else
                                reply.addVocab32(IKINSLV_VOCAB_REP_NACK); 
 
                            break;
                        }
 
                        //-----------------
                        case IKINSLV_VOCAB_OPT_MODE:
                        {
                            if (inPort->handleMode(command.get(2).asVocab32()))
                                reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
                            else
                                reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
                    
                            break;
                        }
                    
                        //-----------------
                        case IKINSLV_VOCAB_OPT_LIM:
                        {
                            if (command.size()>4)
                            {
                                int axis=command.get(2).asInt32();
                                double min=command.get(3).asFloat64();
                                double max=command.get(4).asFloat64();
                    
                                if (setLimits(axis,min,max))
                                    reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
                                else
                                    reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
                            }
                            else
                                reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
                    
                            break;
                        }
                    
                        //-----------------
                        case IKINSLV_VOCAB_OPT_VERB:
                        {
                            int sw=command.get(2).asVocab32();
                    
                            if (sw==IKINSLV_VOCAB_VAL_ON)
                            {
                                verbosity=true;    
                                reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
                            }
                            else if (sw==IKINSLV_VOCAB_VAL_OFF)
                            {   
                                verbosity=false; 
                                reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
                            }
                    
                            break;
                        }
                    
                        //-----------------
                        case IKINSLV_VOCAB_OPT_DOF:
                        {
                            if (inPort->handleDOF(command.get(2).asList()))
                            {                                    
                                waitDOFHandling();  // sleep till dof handling is done
                    
                                reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
                                Bottle &dofPart=reply.addList();
                                fillDOFInfo(dofPart);
                            }
                            else
                                reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
                    
                            break;
                        }
                    
                        //-----------------
                        case IKINSLV_VOCAB_OPT_REST_POS:
                        {
                            Bottle restPart;
                    
                            if (handleJointsRestPosition(command.get(2).asList(),&restPart))
                            {
                                lock();
                                prepareJointsRestTask();
                                unlock();
                    
                                reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
                                reply.append(restPart);
                            }
                            else
                                reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
                    
                            break;
                        }
                    
                        //-----------------
                        case IKINSLV_VOCAB_OPT_REST_WEIGHTS:
                        {
                            Bottle restPart;
                    
                            if (handleJointsRestWeights(command.get(2).asList(),&restPart))
                            {
                                lock();
                                prepareJointsRestTask();
                                unlock();
                    
                                reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
                                reply.append(restPart);
                            }
                            else
                                reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
                    
                            break;
                        }
                    
                        //-----------------
                        case IKINSLV_VOCAB_OPT_TIP_FRAME:
                        {
                            if (Bottle *tipPart=command.get(2).asList())
                            {
                                if (tipPart->size()>=7)
                                {
                                    Vector x(3);
                                    for (size_t i=0; i<x.length(); i++)
                                        x[i]=tipPart->get(i).asFloat64();
            
                                    Vector o(4);
                                    for (size_t i=0; i<o.length(); i++)
                                        o[i]=tipPart->get(i+x.length()).asFloat64();
            
                                    Matrix HN=axis2dcm(o);
                                    HN.setSubcol(x,0,3);
 
                                    lock();
                                    prt->chn->setHN(HN);
                                    unlock();
            
                                    reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
                                    break;
                                }
                            }
            
                            reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
                            break;
                        }
            
                        //-----------------
                        case IKINSLV_VOCAB_OPT_TASK2:
                        {
                            if (Bottle *payLoad=command.get(2).asList())
                            {
                                if (payLoad->size()>=3)
                                {
                                    int n=payLoad->get(0).asInt32();
                                    Bottle *posPart=payLoad->get(1).asList();
                                    Bottle *weightsPart=payLoad->get(2).asList();
            
                                    if ((posPart!=NULL) && (weightsPart!=NULL))
                                    {
                                        if ((posPart->size()>=3) && (weightsPart->size()>=3))
                                        {
                                            for (size_t i=0; i<xd_2ndTask.length(); i++)
                                                xd_2ndTask[i]=posPart->get(i).asFloat64();
            
                                            for (size_t i=0; i<w_2ndTask.length(); i++)
                                                w_2ndTask[i]=weightsPart->get(i).asFloat64();
            
                                            if (n>=0)
                                                slv->specify2ndTaskEndEff(n);
                                            else
                                                slv->get2ndTaskChain().clear();
 
                                            reply.addVocab32(IKINSLV_VOCAB_REP_ACK); 
                                            break;
                                        }
                                    }
                                }
                            }
           
                            reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
                            break;
                        }
 
                        //-----------------
                        case IKINSLV_VOCAB_OPT_CONVERGENCE:
                        {
                            if (Bottle *payLoad=command.get(2).asList())
                            {
                                int cnt=0;
                                if (payLoad->check("tol"))
                                {
                                    slv->setTol(payLoad->find("tol").asFloat64());
                                    cnt++;
                                }
 
                                if (payLoad->check("constr_tol"))
                                {
                                    slv->setConstrTol(payLoad->find("constr_tol").asFloat64());
                                    cnt++;
                                }
 
                                if (payLoad->check("max_iter"))
                                {
                                    slv->setMaxIter(payLoad->find("max_iter").asInt32());
                                    cnt++;
                                }
 
                                reply.addVocab32(cnt>0?IKINSLV_VOCAB_REP_ACK:IKINSLV_VOCAB_REP_NACK);
                            }
                            else
                                reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
 
                            break;
                        }
 
                        //-----------------
                        default:
                            reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
                    }
                }
                else
                    reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
            
                break;
            }
            
            //-----------------
            case IKINSLV_VOCAB_CMD_ASK:
            {
                Bottle *b_xd=getTargetOption(command);
                Bottle *b_q=getJointsOption(command);
            
                // some integrity checks
                if (b_xd==NULL)
                {
                    reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
                    break;
                }
                else if (b_xd->size()<3)    // at least the positional part must be given 
                {
                    reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
                    break;
                }
            
                lock();
            
                // get the target
                Vector xd(b_xd->size());
                for (size_t i=0; i<xd.length(); i++)
                    xd[i]=b_xd->get(i).asFloat64();
            
                // accounts for the starting DOF
                // if different from the actual one
                if (b_q!=NULL)
                {
                    size_t len=std::min((size_t)b_q->size(),(size_t)prt->chn->getDOF());
                    for (unsigned int i=0; i<len; i++)
                        (*prt->chn)(i).setAng(CTRL_DEG2RAD*b_q->get(i).asFloat64());
                }
                else
                    getFeedback();  // otherwise get the current configuration
            
                // account for the pose 
                if (command.check(Vocab32::decode(IKINSLV_VOCAB_OPT_POSE)))
                {
                    int pose=command.find(Vocab32::decode(IKINSLV_VOCAB_OPT_POSE)).asVocab32();
            
                    if (pose==IKINSLV_VOCAB_VAL_POSE_FULL)
                        slv->set_ctrlPose(IKINCTRL_POSE_FULL);
                    else if (pose==IKINSLV_VOCAB_VAL_POSE_XYZ)
                        slv->set_ctrlPose(IKINCTRL_POSE_XYZ);
                }
            
                // set things for the 3rd task
                for (unsigned int i=0; i<prt->chn->getDOF(); i++)
                    if (idx_3rdTask[i]!=0.0)
                        qd_3rdTask[i]=(*prt->chn)(i).getAng();
            
                // call the solver to converge
                double t0=Time::now();
                Vector q=solve(xd);
                double t1=Time::now();
            
                Vector x=prt->chn->EndEffPose(q);
            
                // change to degrees
                q*=CTRL_RAD2DEG;
            
                // dump on screen
                if (verbosity)
                    printInfo("ask",xd,x,q,t1-t0);
            
                // prepare the complete joints configuration
                Vector _q(prt->chn->getN());
                for (unsigned int i=0; i<prt->chn->getN(); i++)
                    _q[i]=CTRL_RAD2DEG*prt->chn->getAng(i);
            
                // fill the reply accordingly
                reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
                addVectorOption(reply,IKINSLV_VOCAB_OPT_X,x);
                addVectorOption(reply,IKINSLV_VOCAB_OPT_Q,_q);
 
                unlock();
            
                break;
            }
 
            //-----------------
            case IKINSLV_VOCAB_CMD_SUSP:
            {
                suspend();
                reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
                break;
            }
            
            //-----------------
            case IKINSLV_VOCAB_CMD_RUN:
            {
                if (!isRunning())
                {
                    initPos();
                    start();
                }
                else
                    resume();
            
                reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
                break;
            }
                        
            //-----------------
            case IKINSLV_VOCAB_CMD_STATUS:
            {
                reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
                if (isSuspended())
                    reply.addString("suspended");
                else if (isRunning())
                    reply.addString("running");
                else
                    reply.addString("not_started");
                break; 
            }
 
            //-----------------
            case IKINSLV_VOCAB_CMD_CFG:
            {
                Property options(command.toString().c_str());
                yInfo("Configuring with options: %s",options.toString().c_str());
            
                if (open(options))
                    reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
                else
                    reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
            
                break;
            }
            
            //-----------------
            case IKINSLV_VOCAB_CMD_HELP:
            {                    
                reply.addVocab32("many");
                reply.addString("***** commands");
                reply.addVocab32(IKINSLV_VOCAB_CMD_GET);
                reply.addVocab32(IKINSLV_VOCAB_CMD_SET);
                reply.addVocab32(IKINSLV_VOCAB_CMD_ASK);
                reply.addVocab32(IKINSLV_VOCAB_CMD_SUSP);
                reply.addVocab32(IKINSLV_VOCAB_CMD_RUN);
                reply.addVocab32(IKINSLV_VOCAB_CMD_STATUS);
                reply.addVocab32(IKINSLV_VOCAB_CMD_CFG);
                reply.addVocab32(IKINSLV_VOCAB_CMD_HELP);
                reply.addVocab32(IKINSLV_VOCAB_CMD_QUIT);
                reply.addString("***** options");
                reply.addVocab32(IKINSLV_VOCAB_OPT_MODE);
                reply.addVocab32(IKINSLV_VOCAB_OPT_POSE);
                reply.addVocab32(IKINSLV_VOCAB_OPT_DOF);
                reply.addVocab32(IKINSLV_VOCAB_OPT_LIM);
                reply.addVocab32(IKINSLV_VOCAB_OPT_VERB);
                reply.addVocab32(IKINSLV_VOCAB_OPT_TOKEN);
                reply.addVocab32(IKINSLV_VOCAB_OPT_REST_POS);
                reply.addVocab32(IKINSLV_VOCAB_OPT_REST_WEIGHTS);
                reply.addVocab32(IKINSLV_VOCAB_OPT_TIP_FRAME);
                reply.addVocab32(IKINSLV_VOCAB_OPT_TASK2);
                reply.addVocab32(IKINSLV_VOCAB_OPT_XD);
                reply.addVocab32(IKINSLV_VOCAB_OPT_X);
                reply.addVocab32(IKINSLV_VOCAB_OPT_Q);
                reply.addString("***** values");
                reply.addVocab32(IKINSLV_VOCAB_VAL_POSE_FULL);
                reply.addVocab32(IKINSLV_VOCAB_VAL_POSE_XYZ);
                reply.addVocab32(IKINSLV_VOCAB_VAL_MODE_TRACK);
                reply.addVocab32(IKINSLV_VOCAB_VAL_MODE_SINGLE);
                reply.addVocab32(IKINSLV_VOCAB_VAL_ON);
                reply.addVocab32(IKINSLV_VOCAB_VAL_OFF);
                break;
            }
            
            //-----------------
            case IKINSLV_VOCAB_CMD_QUIT:
            {
                reply.addVocab32(IKINSLV_VOCAB_REP_ACK);
                close();
                break;
            }
            
            //-----------------
            default:
                reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
        }
    }
    else
        reply.addVocab32(IKINSLV_VOCAB_REP_NACK);
}
 
 
/************************************************************************/
void CartesianSolver::send(const Vector &xd, const Vector &x, const Vector &q,
                           double *tok)
{       
    Bottle &b=outPort->prepare();
    b.clear();
 
    addVectorOption(b,IKINSLV_VOCAB_OPT_XD,xd);
    addVectorOption(b,IKINSLV_VOCAB_OPT_X,x);
    addVectorOption(b,IKINSLV_VOCAB_OPT_Q,q);
 
    if (tok!=NULL)
        addTokenOption(b,*tok);
 
    outPort->writeStrict();
}
 
 
/************************************************************************/
void CartesianSolver::printInfo(const string &typ, const Vector &xd,
                                const Vector &x, const Vector &q,
                                const double t)
{
    // ensure same length of vectors
    Vector x_=x.subVector(0,(unsigned int)xd.length()-1);
 
    printf("   Request type       = %s\n",typ.c_str());
    printf("  Target rxPose   [m] = %s\n",xd.toString().c_str());
    printf("  Target txPose   [m] = %s\n",x_.toString().c_str());
    printf("Target txJoints [deg] = %s\n",q.toString().c_str());
    printf("    computed in   [s] = %g\n",t);
}
 
 
/************************************************************************/
Vector &CartesianSolver::encodeDOF()
{
    dof.resize(prt->chn->getN());
    bool fullness=true;
 
    for (unsigned int i=0; i<prt->chn->getN(); i++)
        if (!(dof[i]=!(*prt->chn)[i].isBlocked()))
            fullness=false;
 
    fullDOF=fullness;
    return dof;
}
 
 
/************************************************************************/
bool CartesianSolver::decodeDOF(const Vector &_dof)
{
    unsigned int len=std::min(prt->chn->getN(),(unsigned int)_dof.length());
    for (unsigned int i=0; i<len; i++)
    {
        if (_dof[i]>1.0)
            continue;
        else if (_dof[i]!=0.0)
            prt->chn->releaseLink(i);
        else
            prt->chn->blockLink(i);
    }
 
    prepareJointsRestTask();
 
    return true;
}
 
 
/************************************************************************/
bool CartesianSolver::handleJointsRestPosition(const Bottle *options, Bottle *reply)
{
    bool ret=false;
 
    if (options!=NULL)
    {            
        size_t len=std::min((size_t)options->size(),restJntPos.length());
        for (unsigned int i=0; i<len; i++)
        {
            double val=CTRL_DEG2RAD*options->get(i).asFloat64();
            double min=(*prt->chn)[i].getMin();
            double max=(*prt->chn)[i].getMax();
 
            restJntPos[i]=std::min(std::max(val,min),max);
        }
 
        ret=true;
    }
 
    if (reply!=NULL)
    {
        Bottle &b=reply->addList();
        for (size_t i=0; i<restJntPos.length(); i++)
            b.addFloat64(CTRL_RAD2DEG*restJntPos[i]);
    }
 
    return ret;
}
 
 
/************************************************************************/
bool CartesianSolver::handleJointsRestWeights(const Bottle *options, Bottle *reply)
{
    bool ret=false;
 
    if (options!=NULL)
    {            
        size_t len=std::min((size_t)options->size(),restWeights.length());
        for (size_t i=0; i<len; i++)
        {
            double val=options->get(i).asInt32();
            restWeights[i]=std::max(val,0.0);
        }
 
        ret=true;
    }
 
    if (reply!=NULL)
    {
        Bottle &b=reply->addList();
        for (size_t i=0; i<restWeights.length(); i++)
            b.addFloat64(restWeights[i]);
    }
 
    return ret;
}
 
 
/************************************************************************/
bool CartesianSolver::isNewDOF(const Vector &_dof)
{
    size_t len=std::min((size_t)prt->chn->getN(),_dof.length());
    for (size_t i=0; i<len; i++)
    {
        if (_dof[i]>1.0)
            continue;
        else if (_dof[i]!=dof[i])
            return true;
    }
 
    return false;
}
 
 
/************************************************************************/
bool CartesianSolver::open(Searchable &options)
{    
    if (configured)
    {
        yWarning("%s already configured",slvName.c_str());
        return true;
    }
 
    prt=getPartDesc(options);
    if (prt==NULL)
    {
        yError("Detected errors while processing parts description!");
        return false;
    }
 
    Property DHTable; prt->lmb->toLinksProperties(DHTable);
    yInfo()<<"DH Table: "<<DHTable.toString();  // stream version to prevent long strings truncation
    
    if (options.check("ping_robot_tmo"))
        ping_robot_tmo=options.find("ping_robot_tmo").asFloat64();
 
    // open drivers
    int remainingJoints=prt->chn->getN();
    for (int i=0; i<prt->num; i++)
    {
        yInfo("Allocating device driver for %s ...",
              prt->prp[i].find("part").asString().c_str());
 
        PolyDriver *pDrv=(ping_robot_tmo>0.0)?
                         waitPart(prt->prp[i]):
                         new PolyDriver(prt->prp[i]);
 
        if (!pDrv->isValid())
        {
            delete pDrv;
            yError("Device driver not available!");            
            close();
            return false;
        }
 
        // create interfaces
        IControlLimits *pLim;
        IEncoders      *pEnc;
        int             joints;
 
        if (!pDrv->view(pLim) || !pDrv->view(pEnc))
        {
            delete pDrv;
            yError("Problems acquiring interfaces!");
            close();
            return false;
        }
 
        pEnc->getAxes(&joints);
 
        // this is for allocating vector
        // to read data from the interface
        if (joints>maxPartJoints)
            maxPartJoints=joints;
 
        // handle chain's bounds
        if (joints>remainingJoints)
            joints=remainingJoints;
 
        remainingJoints-=joints;
 
        int *rmpTmp=new int[joints];
        for (int j=0; j<joints; j++)
            rmpTmp[j]=prt->rvs[i]?(joints-j-1):j;
 
        drv.push_back(pDrv);
        lim.push_back(pLim);
        enc.push_back(pEnc);
        jnt.push_back(joints);
        rmp.push_back(rmpTmp);
    }
 
    // handle joints rest position and weights
    restJntPos.resize(prt->chn->getN(),0.0);
    restWeights.resize(prt->chn->getN(),0.0);
    handleJointsRestPosition(options.find("rest_pos").asList());
    handleJointsRestWeights(options.find("rest_weights").asList());
    prepareJointsRestTask();
 
    // dof here is not defined yet, so define it
    encodeDOF();
 
    // handle dof from options
    if (Bottle *v=options.find("dof").asList())
    {
        Vector _dof(v->size());
        for (size_t i=0; i<_dof.length(); i++)
            _dof[i]=v->get(i).asInt32();
 
        decodeDOF(_dof);
        encodeDOF();
    }
 
    // joints bounds alignment
    if (!alignJointsBounds())
    {
        yError("Unable to retrieve joints limits!");
        close();
        return false;
    }
 
    // parse configuration options
    period=options.check("period",Value(CARTSLV_DEFAULT_PER)).asInt32();
    setPeriod((double)period/1000.0);
 
    ctrlPose=IKINCTRL_POSE_FULL;
    if (options.check(Vocab32::decode(IKINSLV_VOCAB_OPT_POSE)))
        if (options.find(Vocab32::decode(IKINSLV_VOCAB_OPT_POSE)).asVocab32()==IKINSLV_VOCAB_VAL_POSE_XYZ)
            ctrlPose=IKINCTRL_POSE_XYZ;
 
    bool mode=false;
    if (options.check(Vocab32::decode(IKINSLV_VOCAB_OPT_MODE)))
        if (options.find(Vocab32::decode(IKINSLV_VOCAB_OPT_MODE)).asVocab32()==IKINSLV_VOCAB_VAL_MODE_TRACK)
            mode=true;
 
    if (options.check("verbosity"))
        if (options.find("verbosity").asVocab32()==IKINSLV_VOCAB_VAL_ON)
            verbosity=true;
 
    double tol=options.check("tol",Value(CARTSLV_DEFAULT_TOL)).asFloat64();
    double constr_tol=options.check("constr_tol",Value(CARTSLV_DEFAULT_CONSTR_TOL)).asFloat64();
    int maxIter=options.check("maxIter",Value(CARTSLV_DEFAULT_MAXITER)).asInt32();
 
    // instantiate the optimizer
    slv=new iKinIpOptMin(*prt->chn,ctrlPose,tol,constr_tol,maxIter);
 
    // instantiate solver callback object if required    
    if (options.check("interPoints"))
        if (options.find("interPoints").asVocab32()==IKINSLV_VOCAB_VAL_ON)
            clb=new SolverCallback(this);
 
    // enable scaling
    slv->setUserScaling(true,100.0,100.0,100.0);
 
    // enforce linear inequalities constraints, if any
    if (prt->cns!=NULL)
    {
        slv->attachLIC(*prt->cns);
        
        double lower_bound_inf, upper_bound_inf;        
        slv->getBoundsInf(lower_bound_inf,upper_bound_inf);
        slv->getLIC().getLowerBoundInf()=2.0*lower_bound_inf;
        slv->getLIC().getUpperBoundInf()=2.0*upper_bound_inf;
        slv->getLIC().update(NULL);
    }
 
    // set up 2nd task
    xd_2ndTask.resize(3,0.0);
    w_2ndTask.resize(3,0.0);
 
    // define input port
    inPort=new InputPort(this);
    inPort->useCallback();
    
    // init input port data
    inPort->set_dof(dof);
    inPort->get_pose()=ctrlPose;
    inPort->get_contMode()=contModeOld=mode;
 
    // define output port
    outPort=new BufferedPort<Bottle>;    
 
    // count uncontrolled joints
    countUncontrolledJoints();
 
    // get starting position
    initPos();
 
    configured=true;
 
    start();
 
    // open ports as very last thing, so that
    // the solver is completely operative
    // when it becomes yarp-visible
    inPort->open("/"+slvName+"/in");
    outPort->open("/"+slvName+"/out");
 
    return true;
}
 
 
/************************************************************************/
bool CartesianSolver::changeDOF(const Vector &_dof)
{
    if (isNewDOF(_dof))
    {
        // dof stuff
        Vector curDOF=dof;
        decodeDOF(_dof);
        inPort->set_dof(encodeDOF());
 
        if (dof==curDOF)
            return false;
 
        // update LinIneqConstr if any
        if (prt->cns!=NULL)
            prt->cns->update(NULL);
 
        // count uncontrolled joints
        countUncontrolledJoints();
 
        // get starting position
        getFeedback();
        latchUncontrolledJoints(unctrlJointsOld);
 
        return true;
    }
    else
        return false;
}
 
 
/************************************************************************/
void CartesianSolver::prepareJointsRestTask()
{
    unsigned int nDOF=prt->chn->getDOF();
    int offs=0;
    
    qd_3rdTask.resize(nDOF);
    w_3rdTask.resize(nDOF,1.0);
    idx_3rdTask.resize(nDOF,1.0);
 
    for (unsigned int i=0; i<prt->chn->getN(); i++)
    {
        if (!(*prt->chn)[i].isBlocked())
        {            
            qd_3rdTask[offs]=restJntPos[i];
            if (restWeights[i]!=0.0)
            {
                w_3rdTask[offs]=restWeights[i];
                idx_3rdTask[offs]=0.0;
            }
            offs++;
        }
    }
}
 
 
/************************************************************************/
Vector CartesianSolver::solve(Vector &xd)
{
    return slv->solve(prt->chn->getAng(),xd,
                      slv->get2ndTaskChain().getN()>0?CARTSLV_WEIGHT_2ND_TASK:0.0,xd_2ndTask,w_2ndTask,
                      CARTSLV_WEIGHT_3RD_TASK,qd_3rdTask,w_3rdTask,
                      NULL,NULL,clb);
}
 
 
/************************************************************************/
void CartesianSolver::interrupt()
{
    interrupting=true;
}
 
 
/************************************************************************/
void CartesianSolver::close()
{
    if (closed)
        return;
 
    closing=true;
 
    if (isRunning())
        stop();
 
    if (inPort!=NULL)
    {
        inPort->interrupt();
        inPort->close();
        delete inPort;
        inPort=NULL;
    }
 
    if (outPort!=NULL)
    {
        outPort->interrupt();
        outPort->close();
        delete outPort;
        outPort=NULL;
    }
 
    delete slv;
    delete clb;
    slv=NULL;
    clb=NULL;
 
    for (size_t i=0; i<drv.size(); i++)
    {
        delete drv[i];
        drv[i]=NULL;
    }
 
    for (size_t i=0; i<rmp.size(); i++)
    {
        delete[] rmp[i];
        rmp[i]=NULL;
    }
 
    drv.clear();
    lim.clear();
    enc.clear();
    jnt.clear();
    rmp.clear();
 
    if (prt!=NULL)
    {
        delete prt->lmb;
        delete prt->cns;
        delete prt;
        prt=NULL;
    }
 
    yInfo("%s closed",slvName.c_str());
    closed=true;
}
 
 
/************************************************************************/
bool CartesianSolver::threadInit()
{
    if (!configured)
        yError("%s not configured!",slvName.c_str());
    else
        yInfo("Starting %s at %d ms",slvName.c_str(),period);
 
    return configured;
}
 
 
/************************************************************************/
void CartesianSolver::afterStart(bool s)
{
    if (s)
        yInfo("%s started successfully",slvName.c_str());
    else
        yError("%s did not start!",slvName.c_str());
}
 
 
/************************************************************************/
void CartesianSolver::suspend()
{
    if (isSuspended())
        yWarning("%s is already suspended",slvName.c_str());
    else
    {
        PeriodicThread::suspend();
        yInfo("%s suspended",slvName.c_str());        
    }
}
 
 
/************************************************************************/
void CartesianSolver::resume()
{
    if (isSuspended())
    {        
        initPos();
        PeriodicThread::resume();
        yInfo("%s resumed",slvName.c_str());
    }
    else
        yWarning("%s is already running",slvName.c_str());
}
 
 
/************************************************************************/
void CartesianSolver::run()
{
    lock();
 
    // init conditions
    bool doSolve=false;
 
    // handle changeDOF()
    changeDOF(inPort->get_dof());    
 
    // wake up sleeping threads
    postDOFHandling();
 
    // get the current configuration
    getFeedback();
 
    // acquire uncontrolled joints configuration
    Vector unctrlJoints;
    if (!fullDOF)
    {
        latchUncontrolledJoints(unctrlJoints);
 
        // upon setting the mode to continuous,
        // latch the old state so that we skip any
        // adjustment
        if (inPort->get_contMode() && !contModeOld)
            unctrlJointsOld=unctrlJoints;
 
        // detect movements of uncontrolled joints
        double distExtMoves=CTRL_RAD2DEG*norm(unctrlJoints-unctrlJointsOld);        
 
        // run the solver if movements of uncontrolled joints
        // are detected and we are in continuous mode
        doSolve|=inPort->get_contMode() && (distExtMoves>CARTSLV_UNCTRLEDJNTS_THRES);
        if (doSolve && verbosity)
            yInfo("%s: detected movements on uncontrolled joints (norm=%g>%g deg)",
                  slvName.c_str(),distExtMoves,CARTSLV_UNCTRLEDJNTS_THRES);
    }
 
    // run the solver if any input is received
    if (inPort->isNewDataEvent())
    {    
        // update solver condition
        doSolve=true;        
    }
 
    // solver part
    if (doSolve)
    {
        // point to the desired pose
        Vector xd=inPort->get_xd();
        if (inPort->get_tokenPtr()) // latch token
        {
            token=*inPort->get_tokenPtr();
            pToken=&token;
        }
        else
            pToken=NULL;
 
        // set things for the 3rd task
        for (unsigned int i=0; i<prt->chn->getDOF(); i++)
            if (idx_3rdTask[i]!=0.0)
                qd_3rdTask[i]=(*prt->chn)(i).getAng();
 
        // update optimizer's options
        slv->set_ctrlPose(ctrlPose=inPort->get_pose());
 
        // call the solver to converge
        double t0=Time::now();
        Vector q=solve(xd);
        double t1=Time::now();
 
        // q is the estimation of the real qd,
        // so that x is the actual achieved pose
        Vector x=prt->chn->EndEffPose(q);
        
        // change to degrees
        q*=CTRL_RAD2DEG;
 
        // send data
        send(xd,x,q,pToken);
 
        // dump on screen
        if (verbosity)
            printInfo("go",xd,x,q,t1-t0);
 
        // save the values of uncontrolled joints
        if (!fullDOF)
            unctrlJointsOld=unctrlJoints; 
    }
 
    contModeOld=inPort->get_contMode();
 
    unlock();
}
 
 
/************************************************************************/
void CartesianSolver::threadRelease()
{    
    yInfo("Stopping %s ...",slvName.c_str());
}
 
 
/************************************************************************/
CartesianSolver::~CartesianSolver()
{
    close();
 
    rpcPort->interrupt();
    rpcPort->close();
    delete rpcPort;
    delete cmdProcessor;
}
 
 
/************************************************************************/
PartDescriptor *iCubArmCartesianSolver::getPartDesc(Searchable &options)
{
    type="right";
    string part_type=type;
    iKinLimbVersion version("1.0");
    if (options.check("type"))
    {
        type=options.find("type").asString();
        part_type=type.substr(0,type.find("_"));
        if ((part_type!="left") && (part_type!="right"))
            type=part_type="right";
 
        size_t underscore=type.find('_');
        if (underscore!=string::npos)
            version=iKinLimbVersion(type.substr(underscore+2));
    }
 
    string robot=options.check("robot",Value("icub")).asString();
    Property optTorso("(device remote_controlboard)");
    Property optArm("(device remote_controlboard)");
 
    string partTorso  ="torso";
    string remoteTorso="/"+robot+"/"+partTorso;
    string localTorso ="/"+slvName+"/"+partTorso;
    optTorso.put("remote",remoteTorso);
    optTorso.put("local",localTorso);
    optTorso.put("robot",robot);
    optTorso.put("part",partTorso);
 
    string partArm  =part_type=="left"?"left_arm":"right_arm";
    string remoteArm="/"+robot+"/"+partArm;
    string localArm ="/"+slvName+"/"+partArm;
    optArm.put("remote",remoteArm);
    optArm.put("local",localArm);
    optArm.put("robot",robot);
    optArm.put("part",partArm);
 
    PartDescriptor *p=new PartDescriptor;
    p->lmb=new iCubArm(type);
    p->chn=p->lmb->asChain();
    p->cns=new iCubAdditionalArmConstraints(*static_cast<iCubArm*>(p->lmb));
    p->prp.push_back(optTorso);
    p->prp.push_back(optArm);
    p->rvs.push_back(version<iKinLimbVersion("3.0"));   // torso
    p->rvs.push_back(false);                            // arm
    p->num=2;                                           // number of parts
 
    return p;
}
 
 
/************************************************************************/
bool iCubArmCartesianSolver::open(Searchable &options)
{
    // call father's open() method
    if (CartesianSolver::open(options))
    {
        // Identify the elbow xyz position to be used as 2nd task
        slv->specify2ndTaskEndEff(6);
 
        // try to keep elbow as low as possible
        xd_2ndTask[2]=-1.0;
        w_2ndTask[2]=1.0;
    }
 
    return configured;
}
 
 
/************************************************************************/
bool iCubArmCartesianSolver::decodeDOF(const Vector &_dof)
{
    // latch current status
    Vector newDOF=dof;
 
    // update desired status
    size_t len=std::min((size_t)prt->chn->getN(),_dof.length());
    for (size_t i=0; i<len; i++)
        newDOF[i]=_dof[i];
 
    // check whether shoulder's axes are treated properly
    if (!(((newDOF[3]!=0.0) && (newDOF[4]!=0)   && (newDOF[5]!=0)) || 
          ((newDOF[3]==0.0) && (newDOF[4]==0.0) && (newDOF[5]==0.0))))
    {        
        // restore previous condition:
        // they all shall be on or off
        newDOF[3]=newDOF[4]=newDOF[5]=dof[3];
 
        yWarning("%s: attempt to set one shoulder's joint differently from others",slvName.c_str());
    }
 
    return CartesianSolver::decodeDOF(newDOF);
}
 
 
/************************************************************************/
PartDescriptor *iCubLegCartesianSolver::getPartDesc(Searchable &options)
{
    type="right";
    string part_type=type;
    iKinLimbVersion version("1.0");
    if (options.check("type"))
    {
        type=options.find("type").asString();
        part_type=type.substr(0,type.find("_"));
        if ((part_type!="left") && (part_type!="right"))
            type=part_type="right";
 
        size_t underscore=type.find('_');
        if (underscore!=string::npos)
            version=iKinLimbVersion(type.substr(underscore+2));
    }
 
    string robot=options.check("robot",Value("icub")).asString();
    Property optLeg("(device remote_controlboard)");
 
    string partLeg  =part_type=="left"?"left_leg":"right_leg";
    string remoteLeg="/"+robot+"/"+partLeg;
    string localLeg ="/"+slvName+"/"+partLeg;
 
    optLeg.put("remote",remoteLeg);
    optLeg.put("local",localLeg);
    optLeg.put("robot",robot);
    optLeg.put("part",partLeg);
 
    PartDescriptor *p=new PartDescriptor;
    p->lmb=new iCubLeg(type);
    p->chn=p->lmb->asChain();
    p->cns=NULL;
    p->prp.push_back(optLeg);   
    p->rvs.push_back(false);
    p->num=1;
 
    return p;
}