First real commit

.gitignore

+/build

CMakeLists.txt

+# Copyright: 2012 iCub Facility, Istituto Italiano di Tecnologia
+# Author: Lorenzo Natale
+# CopyPolicy: Released under the terms of the GNU GPL v2.0.
+# 
+
+cmake_minimum_required(VERSION 3.5)
+project(velocityController)
+
+find_package(YARP)
+
+file(GLOB source *.cpp)
+file(GLOB header *.h)
+
+source_group("Source Files" FILES ${source})
+source_group("Header Files" FILES ${header})
+
+add_executable(velocityController ${source} ${header})
+
+target_link_libraries(velocityController ${YARP_LIBRARIES})
+
+install(TARGETS velocityController DESTINATION bin)

velocityControllerModule.cpp

+// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
+//
+// A tutorial on how to use the Cartesian Interface to control a limb
+// in the operational space.
+//
+// Author: Ugo Pattacini - <ugo.pattacini@iit.it>
+
+#include <cstdio>
+#include <cmath>
+#include <iostream>
+#include <fstream>
+
+#include <yarp/os/Network.h>
+#include <yarp/os/RFModule.h>
+#include <yarp/os/PeriodicThread.h>
+#include <yarp/os/Time.h>
+#include <yarp/sig/Vector.h>
+#include <yarp/math/Math.h>
+
+#include <yarp/dev/CartesianControl.h>
+#include <yarp/dev/GazeControl.h>
+#include <yarp/dev/PolyDriver.h>
+
+#include "velocityControllerThread.h"
+
+
+#define TASK_PERIOD          0.1     // [s] delta_time between two successive trajectory steps
+#define CTRL_THREAD_PER      0.005  // [s]
+#define MAX_TORSO_PITCH     30.0    // [deg]
+
+using namespace std;
+using namespace yarp::os;
+using namespace yarp::dev;
+using namespace yarp::sig;
+using namespace yarp::math;
+
+
+class CtrlModule: public RFModule
+{
+protected:
+    CtrlThread *thr;
+
+public:
+    virtual bool configure(ResourceFinder &rf)
+    {
+        thr=new CtrlThread(CTRL_THREAD_PER,rf);
+        if (!thr->start())
+        {
+            delete thr;
+            return false;
+        }
+        return true;
+    }
+
+    virtual bool close()
+    {
+        thr->stop();
+        delete thr;
+
+        return true;
+    }
+
+    virtual double getPeriod()    { return 1.0;  }
+    virtual bool   updateModule() { return true; }
+};
+
+int main(int argc, char *argv[])
+{
+    Network yarp;
+    if (!yarp.checkNetwork())
+    {
+        fprintf(stdout,"Error: yarp server does not seem available\n");
+        return 1;
+    }
+
+    CtrlModule mod;
+
+    ResourceFinder rf;
+    rf.configure(argc, argv);
+
+
+    if (rf.check("help"))
+    {
+        cout << endl << "IMU IDENTIFIER module" << endl;
+        cout << endl << "Options:" << endl;
+        cout << "   --context      path:   where to find the called resource. Default gazeStabilization." << endl;
+        cout << "   --from         from:   the name of the .ini file. Default iCubBreather.ini." << endl;
+        cout << "   --name         name:   the name of the module. Default iCubBreather." << endl;
+        cout << "   --robot        robot:  the name of the robot. Default icub." << endl;
+        cout << "   --rate         rate:   the period used by the thread. Default 500ms." << endl;
+        cout << "   --noiseStd     double: standard deviation of the noise. Default 1.0." << endl;
+        cout << "   --refSpeeds    double: The reference speeds at the joints. Default 5.0." << endl;
+        cout << "   --verbosity    int:    verbosity level. Default 0." << endl;
+        cout << "   --autoStart    flag:   if to autostart the module or not. Default no." << endl;
+        cout << endl;
+        return 0;
+    }
+
+    //default settings
+    rf.setDefault("robot","icubSim");
+    rf.setDefault("part","right_arm");
+    rf.setDefault("torso", "OFF");
+    rf.setDefault("file_path","/home/luca/Downloads/test_file.txt");
+
+    return mod.runModule(rf);
+}

velocityControllerThread.cpp

+// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
+//
+// A tutorial on how to use the Cartesian Interface to control a limb
+// in the operational space.
+//
+// Author: Ugo Pattacini - <ugo.pattacini@iit.it>
+
+#include "velocityControllerThread.h"
+
+#include <cstdio>
+#include <cmath>
+#include <iostream>
+#include <fstream>
+
+#include <yarp/os/Network.h>
+#include <yarp/os/RFModule.h>
+#include <yarp/os/PeriodicThread.h>
+#include <yarp/os/Time.h>
+#include <yarp/sig/Vector.h>
+#include <yarp/math/Math.h>
+
+#include <yarp/dev/all.h>
+//#include <yarp/dev/CartesianControl.h>
+//#include <yarp/dev/GazeControl.h>
+//#include <yarp/dev/PolyDriver.h>
+
+#define TASK_PERIOD    0.15     // [s] delta_time between two successive trajectory steps
+#define CTRL_THREAD_PER     0.005  // [s]
+#define MAX_TORSO_PITCH     30.0    // [deg]
+
+using namespace std;
+using namespace yarp::os;
+using namespace yarp::dev;
+using namespace yarp::sig;
+using namespace yarp::math;
+
+
+CtrlThread::CtrlThread(const double period, yarp::os::ResourceFinder rf) : PeriodicThread(period) {
+    this->rf = rf;
+    // we wanna raise an event each time the arm is at 20%
+    // of the trajectory (or 80% far from the target)
+    //cartesianEventParameters.type="motion-ongoing";
+    //cartesianEventParameters.motionOngoingCheckPoint=0.2;
+}
+
+CtrlThread::~CtrlThread(){};
+
+bool CtrlThread::threadInit() {
+    std::string robotName = rf.find("robot").asString();
+    std::string partName = rf.find("part").asString();
+    enable_torso = rf.find("torso").asBool();
+    enable_gaze = rf.find("gaze").asBool();
+    std::string file_path = rf.find("file_path").asString();
+
+    std::cout << "Module settings:\n";
+    std::cout << "robot: " << robotName.c_str() << '\n';
+    std::cout << "part: " << partName.c_str() << '\n';
+    std::cout << "enable torso: " << enable_torso << '\n';
+    std::cout << "enable gaze: " << enable_gaze << '\n';
+    std::cout << "file path: " << file_path.c_str() << '\n';
+
+    //read .txt from file_path
+    std::ifstream file;
+    file.open(file_path.c_str());
+    if(!file.is_open())
+    {
+        std::cout << "Error: file not found\n";
+        return false;
+    }
+    string param_string;
+    double value;
+    std::getline(file, param_string);
+
+    std::stringstream stream(param_string);
+    while (stream >> value){
+        time.push_back(value);
+    }
+
+    //print time
+    yInfo( "time vector : %s" , time.toString().c_str());
+
+    //open cartesain interface
+    Property option("(device cartesiancontrollerclient)");
+    option.put("remote","/" + robotName + "/cartesianController/" + partName);
+    option.put("local","/velocity_controller_client/" + partName);
+
+    if (!this->client.open(option)) {
+        printf("Device not available.\n");
+        return false;
+    }
+
+    // open the view
+    this->client.view(icart);
+
+    // latch the controller context in order to preserve
+    // it after closing the module
+    // the context contains the dofs status, the tracking mode,
+    // the resting positions, the limits and so on.
+    icart->storeContext(&startup_context_id);
+
+    if (enable_gaze) {
+        // open the gaze interface
+        Property option_gaze("gazecontrollerclient");
+        option_gaze.put("remote","iKinGazeCtrl");
+        option_gaze.put("local","/velocity_controller_gaze_client/" + partName);
+
+        PolyDriver client_gaze(option);
+
+        if (client_gaze.isValid()) {
+            client_gaze.view(igaze);
+        }
+
+        igaze->restoreContext(startup_context_id);
+    }
+
+    // impose some restriction on the torso pitch
+    if (enable_torso) {
+        // get the torso dofs
+        Vector newDof, curDof;
+        icart->getDOF(curDof);
+        newDof=curDof;
+        // enable the torso yaw and pitch
+        // disable the torso roll
+        newDof[0] = 1;
+        newDof[1] = 0;
+        newDof[2] = 1;
+        // send the request for dofs reconfiguration
+        icart->setDOF(newDof,curDof);
+        limitTorsoPitch();
+    }
+
+    // print out some info about the controller
+    Bottle info;
+    icart->getInfo(info);
+    fprintf(stdout,"info = %s\n",info.toString().c_str());
+
+    // register the event, attaching the callback
+    icart->registerEvent(*this);
+
+    A.resize(3);
+    B.resize(3);
+    xd.resize(3);
+    od.resize(4);
+    x_dot.resize(3);
+    o_dot.resize(4);
+
+    od[0] = 0.0;
+    od[1] = 0.0;
+    od[2] = 1.0;
+    od[3] = M_PI;
+
+    A[0] = -0.157;
+    A[1] = 0.275;
+    A[2] = 0.324;
+
+    B[0] = -0.253;
+    B[1] = 0.256;
+    B[2] = -0.026;
+
+
+
+    distance = sqrt(pow(B[0] - A[0], 2) + pow(B[1] - A[1], 2) + pow(B[2] - A[2], 2));
+    xd = A;
+
+    //to get the direction of the segment A->B we compute the simple element-wise B-A
+    AB.resize(3);
+    AB[0] = B[0] - A[0];
+    AB[1] = B[1] - A[1];
+    AB[2] = B[2] - A[2];
+
+    /* set the position controller */
+    Property optionJoints;
+    optionJoints.put("device", "remote_controlboard");
+    optionJoints.put("local", "/test/client");         //local port names
+    optionJoints.put("remote", "/" + robotName + "/" + partName);                        //where we connect to
+    optionJoints.put("writeStrict","on");
+
+
+    // if (!robotDevice.open(optionJoints)) {
+    //     printf("Device not available.  Here are the known devices:\n");
+    //     printf("%s", Drivers::factory().toString().c_str());
+    // }
+    robotDevice.open(optionJoints);
+
+    if (!robotDevice.isValid()) {
+        printf("Device not available.  Here are the known devices:\n");
+        printf("%s", Drivers::factory().toString().c_str());
+        return 0;
+    }
+
+    // bool ok;
+    // ok = this->robotDevice.view(pos);
+    // ok = ok && this->robotDevice.view(encs);
+
+    bool ok;
+    ok = robotDevice.view(encs);
+    ok = robotDevice.view(pos);
+    ok = ok && robotDevice.view(ictrl);
+    ok = ok && robotDevice.view(idir);
+
+    if (!ok) {
+        yError("Problems acquiring interfaces\n");
+        return 0;
+    }
+
+    idir->getAxes(&njoints);
+    yDebug("njoints = %d", njoints);
+    encoders.resize(njoints);
+
+    // //changing the control mode
+    // for (int i = 0; i < 16; i++) {
+    //     ictrl->setControlMode(i, VOCAB_CM_POSITION_DIRECT);
+    // }
+
+    while(!encs->getEncoders(encoders.data()))
+    {
+        Time::delay(0.1);
+        printf(".");
+    }
+
+    trial_n = 0;
+
+    return true;
+}
+
+void CtrlThread::afterStart(bool s) {
+    if (s)
+        fprintf(stdout,"Thread started successfully\n");
+    else
+        fprintf(stdout,"Thread did not start\n");
+}
+
+void CtrlThread::run() {
+    //wait for new commands on the port
+    fprintf(stdout,"NEW ACTION:\n");
+
+
+    printf("Going to start position\n\n");
+
+    icart->goToPose(A, od, 4);
+    Time::delay(4);
+
+    //plan trajectory
+    planAndPlayTrajectory();
+
+    Time::delay(4);
+
+}
+
+void CtrlThread::threadRelease() {
+    // we require an immediate stop
+    // before closing the client for safety reason
+    icart->stopControl();
+
+    // it's a good rule to restore the controller
+    // context as it was before opening the module
+    icart->restoreContext(startup_context_id);
+
+    client.close();
+}
+
+// ask the solver to compute the joint angles that allow to reach that position
+void CtrlThread::planTrajectory(){
+
+    trajectory.resize(time.length(),njoints+3);//+3 for the torso
+
+    Vector q_start;
+    Vector q_final;
+    q_start.resize(njoints);
+    q_final.resize(njoints);
+    Vector xd_final = xd;
+    Vector od_final = od;
+
+    encs->getEncoders(q_start.data());
+
+    printf("START PLANNING: \n");
+    for (int i = 0; i < time.length(); i++) {
+        xd[0] = A[0] + AB[0] * (time[i] / distance);
+        xd[1] = A[1] + AB[1] * (time[i] / distance);
+        xd[2] = A[2] + AB[2] * (time[i] / distance);
+
+        icart->askForPose(q_start,xd,od, xd_final,od_final,q_final);
+        //icart->askForPosition(q_start,xd, xd_final,od_final,q_final); //forget the orientation
+
+        //add joints config to the trajectory
+        trajectory.setRow(i, q_final);
+
+        q_start = q_final;
+    }
+}
+
+void CtrlThread::playTrajectory(){
+
+  //get number of actuated joints
+  int actuated_joints;
+  //idir->getAxes(&actuated_joints);
+  pos->getAxes(&actuated_joints);
+  printf("actuated joints: %i \n", actuated_joints);
+
+  //set all joints in position direct mode
+  for (int i = 0; i < actuated_joints; i++) {
+     ictrl->setControlMode(i, VOCAB_CM_POSITION_DIRECT);
+     //ictrl->setControlMode(i, VOCAB_CM_POSITION);
+  }
+
+  // read encoders
+  //yInfo("the plan is: \n\n%s", trajectory.toString().c_str());
+
+  encs->getEncoders(encoders.data());
+  yInfo("start from: %s", encoders.toString().c_str());
+  std::array<int,4> ar = {0,1,2,3};
+  // Vector to_control;
+  // to_control.resize(4);
+  // to_control[0] = 0;
+  // to_control[1] = 1;
+  // to_control[2] = 2;
+  // to_control[3] = 3;
+
+  for (size_t i = 0; i < trajectory.rows(); i++) {
+
+    Vector command = trajectory.getRow(i).subVector(3,7);
+    yInfo("going   to: %s", command.toString().c_str());
+    //pos->positionMove(command.data());
+    idir->setPositions(command.data());
+    Time::delay(0.5);
+  }
+  //pos->positionMove(trajectory.getRow(trajectory.rows()).subVector(3,7).data());
+  Time::delay(10);
+  yInfo("azione finita da 10 sec: Cambio control mode");
+  //set all joints in position direct mode (to use the cartesian solver)
+  // for (int i = 0; i < actuated_joints; i++) {
+  //    ictrl->setControlMode(i, VOCAB_CM_POSITION_DIRECT);
+  // }
+
+  Time::delay(5);
+
+
+
+}
+
+void CtrlThread::planAndPlayTrajectory() {
+    // we keep the orientation of the left arm constant:
+    // we want the middle finger to point forward (end-effector x-axis)
+    // with the palm turned down (end-effector y-axis points leftward);
+    // to achieve that it is enough to rotate the root frame of pi around z-axis
+
+
+    //sum the values in the vector
+    double total_time = 0;
+    for (int i = 0; i < time.size(); i++) {
+        total_time += time[i];
+    }
+
+    //AB is multiplied by a value between 0 and 1
+    printf("Going to home position\n\n");
+    icart->goToPose(A, od, 1.0);
+    Time::delay(4);
+
+    size_t n_steps = time.length();
+
+    Matrix velocityReport(n_steps, 3);
+
+    Vector xd_old = xd;
+    Vector od_old = od;
+
+    double t_start = Time::now();
+    for (int i = 0; i < n_steps; i++) {
+        //update time variable
+        t = Time::now();
+
+        //moltiplico il vettore AB per il valore [0,1] della traiettoria realizzata
+        float next_distance = time[i];
+        xd[0] = A[0] + AB[0] * (next_distance / distance);
+        xd[1] = A[1] + AB[1] * (next_distance / distance);
+        xd[2] = A[2] + AB[2] * (next_distance / distance);
+
+        //icart->setTaskVelocities((xd-xd_old)/TASK_PERIOD, (od-od_old)/TASK_PERIOD);
+        icart->goToPose(xd,od, TASK_PERIOD+0.15); //per 0.15 era 0.15
+
+        xd_old = xd;
+        od_old = od;
+
+        if (enable_gaze) {
+            igaze->lookAtFixationPointSync(xd);
+        }
+
+        // wait until the next step
+        while (Time::now() - t < TASK_PERIOD) {
+            Time::delay(0.001);
+        }
+        printf("%.2f%%  going to xd | %f %f %f |  in  %f sec. \n", (next_distance / distance), xd[0], xd[1], xd[2], Time::now() - t);
+        icart->getTaskVelocities(x_dot, o_dot);
+        velocityReport.setRow(i, x_dot);
+    }
+
+    //print time elapsed
+    printf("Action performed in %zu steps, for a total of %f sec. , it was planned to last %f sec. \n",
+           n_steps,
+           Time::now() - t_start,
+           n_steps * TASK_PERIOD);
+
+    //write velocity report to file
+
+    cout << "writing report after " << n_steps << "steps" << endl;
+    std::ofstream myfile;
+    myfile.open("velocityReport-" + to_string(trial_n) + ".csv", std::ofstream::out);
+    myfile << velocityReport.toString();
+    myfile.close();
+    trial_n++;
+
+
+}
+
+void CtrlThread::limitTorsoPitch() {
+    int axis=0; // pitch joint
+    double min, max;
+
+    // sometimes it may be helpful to reduce
+    // the range of variability of the joints;
+    // for example here we don't want the torso
+    // to lean out more than 30 degrees forward
+
+    // we keep the lower limit
+    icart->getLimits(axis,&min,&max);
+    icart->setLimits(axis,min,MAX_TORSO_PITCH);
+}
+
+void CtrlThread::printStatus() {
+    Vector x,o,xdhat,odhat,qdhat;
+
+    // we get the current arm pose in the
+    // operational space
+    icart->getPose(x,o);
+
+    // we get the final destination of the arm
+    // as found by the solver: it differs a bit
+    // from the desired pose according to the tolerances
+    icart->getDesired(xdhat,odhat,qdhat);
+
+    double e_x=norm(xdhat-x);
+    double e_o=norm(odhat-o);
+
+    fprintf(stdout,"+++++++++\n");
+    fprintf(stdout,"xd          [m] = %s\n",xd.toString().c_str());
+    fprintf(stdout,"xdhat       [m] = %s\n",xdhat.toString().c_str());
+    fprintf(stdout,"x           [m] = %s\n",x.toString().c_str());
+    fprintf(stdout,"od        [rad] = %s\n",od.toString().c_str());
+    fprintf(stdout,"odhat     [rad] = %s\n",odhat.toString().c_str());
+    fprintf(stdout,"o         [rad] = %s\n",o.toString().c_str());
+    fprintf(stdout,"norm(e_x)   [m] = %g\n",e_x);
+    fprintf(stdout,"norm(e_o) [rad] = %g\n",e_o);
+    fprintf(stdout,"---------\n\n");
+}
+
+
+class CtrlModule: public RFModule
+{
+protected:
+    CtrlThread *thr;
+
+public:
+    virtual bool configure(ResourceFinder &rf)
+    {
+        thr=new CtrlThread(CTRL_THREAD_PER,rf);
+        if (!thr->start())
+        {
+            delete thr;
+            return false;
+        }
+
+        return true;
+    }
+
+    virtual bool close()
+    {
+        thr->stop();
+        delete thr;
+
+        return true;
+    }
+
+    virtual double getPeriod()    { return 1.0;  }
+    virtual bool   updateModule() { return true; }