debug.cpp

#include <iostream>
#include <cstdio>
#include <cstdlib>
#include <signal.h>
#include <string.h>
#include <math.h>
#include <assert.h>
#include "VREPClient.hpp"

/**
 * VREPClient instance
 * Globale variable
 */
static VREPClient VREP;

using namespace std;

static void print_image(unsigned char* image, int* resolution)
{
    FILE* f = fopen("cam.ppm","w");
    fprintf(f,"P3\n%d %d\n255\n",resolution[0],resolution[1]);
    for(int i=resolution[0]-1; i>=0; i--)
    {
        for(int j=0; j<resolution[1]; j++)
        {
            int index = 3*(i * resolution[1] + j);
            fprintf(f,"%hhu %hhu %hhu  ", image[index], image[index+1], image[index+2]);
        }
        fprintf(f,"\n");
    }

    fclose(f);
    return;
}

static void exiting(bool success = true)
{
    //Close server connection
    VREP.stop();
    VREP.disconnect();
    if (success) {
        exit(EXIT_SUCCESS);
    } else {
        exit(EXIT_FAILURE);
    }
}

static void signal_handler(int sig, siginfo_t *siginfo, void *context)
{
    cout << endl << "Exiting..." << endl;
    exiting();
}

static void attachSignalHandler()
{
    struct sigaction action;
    bzero(&action, sizeof(action));
    action.sa_sigaction = &signal_handler;
    action.sa_flags = SA_SIGINFO;
    if (sigaction(SIGINT, &action, NULL) < 0) {
        cerr << "Unable to register signal handler" << endl;
        exit(EXIT_FAILURE);
    }
}

static void displayInitialState()
{
    //Display founded motors
    size_t countMotors = VREP.countMotors();
    cout << "Registered motors: " << countMotors << endl;
    for (size_t i=0;i<countMotors;i++) {
        cout << "[" << i << "] ";
        cout << VREP.getMotor(i).getName() << " ";
        cout << "minPos=" << VREP.getMotor(i).getMinPos() << " ";
        cout << "maxPos=" << VREP.getMotor(i).getMaxPos() << " ";
        cout << "TorqueMax=" << VREP.getMotor(i).getTorqueMax() << endl;
    }
    //And force sensors
    size_t countForceSensors = VREP.countForceSensors();
    cout << "Registered force sensors: " << countForceSensors << endl;
    for (size_t i=0;i<countForceSensors;i++) {
        cout << "[" << i << "] ";
        cout << VREP.getForceSensor(i).getName() << endl;
    }
    //And vision sensors
    size_t countVisionSensors = VREP.countVisionSensors();
    cout << "Registered vision sensors: " << countVisionSensors << endl;
    for (size_t i=0;i<countVisionSensors;i++) {
        cout << "[" << i << "] ";
        cout << VREP.getVisionSensor(i).getName() << endl;
    }
}

static void displayState()
{
    size_t countMotors = VREP.countMotors();
    //Display motor states
    for (size_t i=0;i<countMotors;i++) {
        cout << "   #[" << i << "] ";
        cout << VREP.getMotor(i).getName() << " ";
        cout << "pos=" << VREP.getMotor(i).readPos() << " ";
        cout << "torque=" << VREP.getMotor(i).readTorque() << endl;
    }
    size_t countForceSensors = VREP.countForceSensors();
    //Display force sensors value
    for (size_t i=0;i<countForceSensors;i++) {
        cout << "   *[" << i << "] ";
        cout << VREP.getForceSensor(i).getName() << " ";
        cout << "force=" << VREP.getForceSensor(i).readForceNorm() << " ";
        cout << "torque=" << VREP.getForceSensor(i).readTorqueNorm() << endl;
    }
    size_t countVisionSensors = VREP.countVisionSensors();
    //Display vision sensors value
    for (size_t i=0;i<countVisionSensors;i++) {
        cout << "   *[" << i << "] ";
        cout << VREP.getVisionSensor(i).getName() << " ";
    }
    //Display accelerometer sensor
    cout << "   -    Accelerometer ";
    cout << "X=" << VREP.readAccelerometerX() << " ";
    cout << "Y=" << VREP.readAccelerometerY() << " ";
    cout << "Z=" << VREP.readAccelerometerZ() << endl;
    //Display position tracker
    cout << "   -    Position tracker ";
    cout << "X=" << VREP.readPositionTrackerX() << " ";
    cout << "Y=" << VREP.readPositionTrackerY() << " ";
    cout << "Z=" << VREP.readPositionTrackerZ() << endl;
}

double PI = 3.14159265359;

double sinParams[20][3] = {
    //phase, amplitude, offset
    {0, 0, 0},          // Hanche D 1
    {0, 0.2, -0.2},  // Hanche D 2
    {0, 0, 0},          // Cuisse D
    {0, 0, 0},          // Genou D
    //{PI/2, 0.2, 0.2},   // Cheville D
    {0, -0.2, 0.2},   // Cheville D
    {0, 0, 0},          // Pied D
    {0, 0, 0},          // Hanche G 1
    {0, 0, 0},          // Hanche G 2
    {0, -0.8, 0.8},     // Cuisse G
    {0, 0.8, -0.8},         // Genou G
    {0, 0, 0},          // Cheville G
    {0, -0.1, 0.1},     // Pied G
    {0, 0, 0},          // Epaule D
    {0, -1, -1},          // Bras D
    {0, 0, 0},          // Coude D
    {0, 0, 0},          // Epaule G
    {0, 0, 1.57},       // Bras G
    {0, 0, 0},          // Coude G
    {0, 0, 0},          // Cou
    {0, 0, 0}           // Tete
};

static void moveMotorsStep(double t)
{
    int n = 20;
    for (size_t i = 0; i < n; i++) {
        double phase = sinParams[i][0];
        double amplitude = sinParams[i][1];
        double offset = sinParams[i][2];
        double pos = amplitude * sin(t + phase) + offset;
        VREP.getMotor(i).writePos(pos);
    }
}

int main(int argc, char* argv[])
{
    //Network parameters
    int port = 0;
    char* ip = NULL;
    
    //Parse input arguments
    if (argc != 3) {
        cerr << "Bad usage. Usage: ./command [ip address] [port number]" << endl;
        cerr << "Provide network parameters to connect to V-REP server" << endl;
        return EXIT_FAILURE;
    } else {
        ip = argv[1];
        port = atoi(argv[2]);
    }

    //Signal attaching
    attachSignalHandler();

    try {
        //Connection to V-REP
        cout << "Connecting to V-REP server " << ip << ":" << port << endl;
        VREP.connect(ip, port);
        //Display initial state
        displayInitialState();
        //Main Loop
        cout << "Starting simulation" << endl;
        VREP.start();
        for (double t=0;t<60.0;t+=0.050) {
            //Display state
            cout << "Simulation step t=" << t << endl;
            displayState();
            //Do next step
            VREP.nextStep();
            //Compute motors move
            moveMotorsStep(t);
            print_image(VREP.getVisionSensor(0).getImage(),
                VREP.getVisionSensor(0).getResolution());
        }
        //End simulation
        cout << "Stopping simulation" << endl;
        VREP.stop();
    } catch (string str) {
        cerr << "Exception error: " << str << endl;
        exiting(false);
    }

    exiting();
}