Explore the use of ICartesianControl::setTaskVelocities() method

!test-settaskvelocities.md

test-settaskvelocities

Helper gist for robotology/community#208.

Running the test

Dependencies (in order)

1. iCub_SIM
2. yarprobotinterface --context simCartesianControl --config no_legs.xml
3. iKinCartesianSolver --context simCartesianControl --part left_arm

Launching the module

$ ./test-settaskvelocities --Ts sample_time --V target_velocity --Ttraj p2p_trajectory_time --method method_type

Available Options are:

1. sample_time to specify the sampling time in [s] (default is 0.01 s).
2. target_velocity to specify the magnitude of the task space velocity in [m/s] (default is 0.07 m/s).
3. p2p_trajectory_time to specify the reactivity of the Cartesian controller in terms of point-to-point trajectory time in [s] (default is 0.4 s).
4. method_type to specify the method used to generate the task space velocity; options are ["task-velocities" | "pseudo-inverse"] (default is "task-velocities").

CMakeLists.txt

# Copyright: (C) 2017 iCub Facility - Istituto Italiano di Tecnologia
# Authors: Ugo Pattacini
# CopyPolicy: Released under the terms of the GNU GPL v2.0.

cmake_minimum_required(VERSION 3.0.0)
project(test-settaskvelocities)

find_package(YARP REQUIRED)
find_package(ICUB REQUIRED)
add_definitions(-D_USE_MATH_DEFINES)
include_directories(${YARP_INCLUDE_DIRS} ${ICUB_INCLUDE_DIRS})
add_executable(${PROJECT_NAME} main.cpp)
target_link_libraries(${PROJECT_NAME} ${YARP_LIBRARIES} iKin ctrlLib)

main.cpp

// Copyright: (C) 2017 iCub Facility - Istituto Italiano di Tecnologia
// Authors: Ugo Pattacini
// CopyPolicy: Released under the terms of the GNU GPL v2.0.

#include <string>
#include <deque>
#include <cmath>

#include <yarp/os/all.h>
#include <yarp/dev/all.h>
#include <yarp/sig/all.h>
#include <yarp/math/Math.h>
#include <yarp/math/SVD.h>

#include <iCub/ctrl/pids.h>
#include <iCub/iKin/iKinFwd.h>

using namespace std;
using namespace yarp::os;
using namespace yarp::dev;
using namespace yarp::sig;
using namespace yarp::math;
using namespace iCub::ctrl;
using namespace iCub::iKin;

#define DEG2RAD     (M_PI/180.0)
#define RAD2DEG     (180.0/M_PI)

class TargetHandler : public RateThread
{
    RpcClient sim;
    Vector pos;

    Vector transform(const Vector &x)
    {
        Matrix T(4,4);
        T(0,1)=-1.0;
        T(1,2)=1.0;  T(1,3)=0.5976;
        T(2,0)=-1.0; T(2,3)=-0.026;
        T(3,3)=1.0;

        Vector x_=x;
        x_.push_back(1.0);
        return (T*x_).subVector(0,2);
    }

public:
    TargetHandler(const double Ts, const Vector &pos_) :
        RateThread(int(1000.0*Ts)),
        pos(pos_) { }

    bool threadInit()
    {
        sim.open("/sim");
        if (!Network::connect(sim.getName(),"/icubSim/world"))
        {
            sim.close();
            return false;
        }

        Vector x=transform(pos);

        Bottle cmd,rep;
        cmd.addString("world");
        cmd.addString("mk");
        cmd.addString("ssph");
        cmd.addDouble(0.03);
        cmd.addDouble(x[0]);
        cmd.addDouble(x[1]);
        cmd.addDouble(x[2]);
        cmd.addDouble(1.0);
        cmd.addDouble(0.0);
        cmd.addDouble(0.0);
        cmd.addInt(0);

        sim.write(cmd,rep);

        return true;
    }

    void threadRelease()
    {
        sim.close();
    }

    void setPos(const Vector &pos_)
    {
        pos=pos_;
    }

    void run()
    {
        Vector x=transform(pos);

        Bottle cmd,rep;
        cmd.addString("world");
        cmd.addString("set");
        cmd.addString("ssph");
        cmd.addInt(1);
        cmd.addDouble(x[0]);
        cmd.addDouble(x[1]);
        cmd.addDouble(x[2]);

        sim.write(cmd,rep);
    }
};

class CtrlModule: public RFModule
{
    PolyDriver drvCart;
    PolyDriver drvTorso;
    PolyDriver drvArm;
    
    iCubArm arm;
    TargetHandler *TH;
    Integrator *I;

    VectorOf<int> jntTorso;
    VectorOf<int> jntArm;

    int context;
    string method;
    double Ts,Ttraj,V;

    void updateChain()
    {
        IEncoders *ienc;
        drvTorso.view(ienc);
        Vector encs(3);
        ienc->getEncoders(encs.data());

        for (int i=0; i<2; i++)
            arm.setAng(i,DEG2RAD*encs[2-i]);

        drvArm.view(ienc);
        encs.resize(16);
        ienc->getEncoders(encs.data());
        for (int i=0; i<7; i++)
            arm.setAng(3+i,DEG2RAD*encs[i]);
    }

public:
    CtrlModule() : arm("left") { }

    bool configure(ResourceFinder &rf)
    {
        method=rf.check("method",Value("task-velocities")).asString();
        Ts=rf.check("Ts",Value(0.01)).asDouble();
        Ttraj=rf.check("Ttraj",Value(0.4)).asDouble();
        V=rf.check("V",Value(0.07)).asDouble();        

        if ((method!="task-velocities") && (method!="pseudo-inverse"))
            method="task-velocities";

        if (method=="task-velocities")
        {
            Property opt;
            opt.put("device","cartesiancontrollerclient");
            opt.put("remote","/icubSim/cartesianController/left_arm");
            opt.put("local","/local/cart");

            if (!drvCart.open(opt))
                return false;

            ICartesianControl *icart;
            drvCart.view(icart);
            icart->storeContext(&context);

            Vector dof(10,1.0);
            icart->setDOF(dof,dof);
            icart->setTrajTime(Ttraj);

            Vector x,o;
            icart->getPose(x,o);
            I=new Integrator(Ts,x);
        }
        else
        {
            Property optTorso;
            optTorso.put("device","remote_controlboard");
            optTorso.put("remote","/icubSim/torso");
            optTorso.put("local","/local/joint/torso");

            Property optArm;
            optArm.put("device","remote_controlboard");
            optArm.put("remote","/icubSim/left_arm");
            optArm.put("local","/local/joint/arm");

            if (!drvTorso.open(optTorso))
                return false;

            if (!drvArm.open(optArm))
            {
                drvTorso.close();
                return false;
            }

            deque<IControlLimits*> limits;
            IControlLimits *ilim;
            drvTorso.view(ilim);
            limits.push_back(ilim);
            drvArm.view(ilim);
            limits.push_back(ilim);
            arm.alignJointsBounds(limits);

            for (int i=0; i<3; i++)
                arm.releaseLink(i);
            updateChain();

            for (int i=0; i<3; i++)
                jntTorso.push_back(i);
            for (int i=0; i<7; i++)
                jntArm.push_back(i);

            VectorOf<int> modes;
            modes.resize(7,VOCAB_CM_VELOCITY);

            IControlMode2 *imod;
            drvTorso.view(imod);
            imod->setControlModes(jntTorso.size(),jntTorso.getFirst(),modes.getFirst());

            drvArm.view(imod);
            imod->setControlModes(jntArm.size(),jntArm.getFirst(),modes.getFirst());

            Vector x=arm.EndEffPosition();
            I=new Integrator(Ts,x);
        }

        TH=new TargetHandler(Ts*10.0,I->get());
        TH->start();

        return true;
    }

    bool close()
    {
        if (method=="task-velocities")
        {
            ICartesianControl *icart;
            drvCart.view(icart);
            icart->stopControl();
            icart->restoreContext(context);
            drvCart.close();
        }
        else
        {
            IVelocityControl2 *ivel;
            drvTorso.view(ivel);
            ivel->stop(jntTorso.size(),jntTorso.getFirst());

            drvArm.view(ivel);
            ivel->stop(jntArm.size(),jntArm.getFirst());

            drvTorso.close();
            drvArm.close();
        }

        TH->stop();        
        delete TH;
        delete I;

        return true;
    }

    double getPeriod()
    {
        return Ts;
    }

    bool updateModule()
    {
        Vector vel(3,0.0); vel[1]=V;
        TH->setPos(I->integrate(vel));

        if (method=="task-velocities")
        {
            ICartesianControl *icart;
            drvCart.view(icart);
            icart->setTaskVelocities(vel,Vector(4,0.0));            
        }
        else
        {
            updateChain();
            Matrix J=arm.GeoJacobian();
            Vector qdot=RAD2DEG*(pinv(J)*cat(vel,Vector(3,0.0)));

            double tmp=qdot[0];
            qdot[0]=qdot[2];
            qdot[2]=tmp;

            IVelocityControl2 *ivel;
            drvTorso.view(ivel);
            ivel->velocityMove(jntTorso.size(),jntTorso.getFirst(),qdot.subVector(0,2).data());

            drvArm.view(ivel);
            ivel->velocityMove(jntArm.size(),jntArm.getFirst(),qdot.subVector(3,9).data());
        }

        return true;
    }
};

int main(int argc, char *argv[])
{
    Network yarp;
    if (!yarp.checkNetwork())
        return 1;

    CtrlModule mod;
    ResourceFinder rf;
    rf.configure(argc,argv);
    return mod.runModule(rf);
}