Commit 81b862e5 authored by Luca's avatar Luca
Browse files

First real commit

parent dfb8a0e1
# 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)
// -*- 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);
}
// -*- 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; }