vrep_perturbations.cpp

Go to the documentation of this file.

#include <humanoid_simulation/humanoid_simulation.h>

#define PI 3.14159265

using namespace std;

static device_state state;

HDCallbackCode HDCALLBACK UpdateCalibrationCallback(void *pUserData);
HDboolean CheckCalibration();
HDSchedulerHandle gCallbackHandle = HD_INVALID_HANDLE;

ros::Subscriber simulation_state_sub;
ros::Publisher command_right_platform_pub;
ros::Publisher command_left_platform_pub;

geometry_msgs::PoseStamped right_platform;
geometry_msgs::PoseStamped left_platform;

geometry_msgs::Vector3 right_platform_init;
geometry_msgs::Vector3 left_platform_init;

geometry_msgs::Point pelvis_target;
geometry_msgs::Point pelvis_target_init;
geometry_msgs::PointStamped pelvis_target_stamped;

int simulator_state(4);
double simulationTime (0);
double time_inc = 0.05;
double poly_time;
string move_type = "move_down";

string plane;
string amplitude;
string frequency;


HDCallbackCode HDCALLBACK SchedulerCallback(void *pUserData)
{
    /* Scheduler callback responsible for device data extraction and force rendering.
     * Calling operations within a haptic frame ensures consistency for the data being used, since the device
     * state remains the same within the frame. */
     
    HDErrorInfo error;
    
    HDint nButtons = 0;

    hdBeginFrame(hdGetCurrentDevice());
    
    hdGetDoublev(HD_CURRENT_POSITION, state.device_position); 
    hdGetDoublev(HD_CURRENT_GIMBAL_ANGLES, state.gimbal_angle);

    hdGetIntegerv(HD_CURRENT_BUTTONS, &nButtons);

    state.button_state[0] = 
        (nButtons & HD_DEVICE_BUTTON_1) ? HD_TRUE : HD_FALSE;
   
    hdEndFrame(hdGetCurrentDevice());

    state.gimbal_angle[0] = state.gimbal_angle[0];
    state.gimbal_angle[1] = -state.gimbal_angle[1];
    state.gimbal_angle[2] = -state.gimbal_angle[2];
    
    /*cout << "Gimbal angles: " << "(" << state.gimbal_angle[0]*(180/M_PI) << ", " << state.gimbal_angle[1]*(180/M_PI) << "," << state.gimbal_angle[2]*(180/M_PI) << ")" << endl;*/
    /*cout << "Device position: " << "(" << state.device_position[0]/1700 << ", " << state.device_position[1]/1700 << "," << state.device_position[2]/1700 << ")" << endl;*/
 
    if (HD_DEVICE_ERROR(error = hdGetError()))
    {
        hduPrintError(stderr, &error, "Error during main scheduler callback\n");
        if (hduIsSchedulerError(&error))
            return HD_CALLBACK_DONE;
    }

    return HD_CALLBACK_CONTINUE;
}
     

/* Implements the rotary motion for the platform in the sagittal and frontal planes (x and y axis), 
 * based on the input parameters inserted by the user - amplitude and frequency, or on the joystick gimbal angles. */
void setPlatformPose(double& time)
{
    double A = atof(amplitude.c_str());
    double fc = atof(frequency.c_str());
    double w = 2*PI*fc;
    
    double roll = A*sin(w*time);
    double pitch = A*sin(w*time);

    if (plane == "lat") 
    {
        roll = 0;
        pitch = pitch;
    } 
    else if (plane == "sag") 
    {
        roll = roll;
        pitch = 0;
    }
       
    /*right_platform.pose.position.x = cos(state.gimbal_angle[2]/scale_div-right_platform_init.y)*0.125; 
    right_platform.pose.position.y = 0;
    right_platform.pose.position.z = 0.07-sin(state.gimbal_angle[2]/scale_div-right_platform_init.y)*0.125;
   
    right_platform.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(state.gimbal_angle[1]/scale_div-right_platform_init.x, state.gimbal_angle[2]/scale_div-right_platform_init.y, 0);   
    
       
    left_platform.pose.position.x = -cos(state.gimbal_angle[2]/scale_div-right_platform_init.y)*0.125;
    left_platform.pose.position.y = 0;
    left_platform.pose.position.z = 0.07+sin(state.gimbal_angle[2]/scale_div-right_platform_init.y)*0.125;
    
    left_platform.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(state.gimbal_angle[1]/scale_div-right_platform_init.x, state.gimbal_angle[2]/scale_div-right_platform_init.y, 0);*/
   
    
    right_platform.pose.position.x = cos(pitch*PI/180)*0.125; 
    right_platform.pose.position.y = 0;
    right_platform.pose.position.z = 0.07-sin(pitch*PI/180)*0.125;
    
    right_platform.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll*PI/180, pitch*PI/180, 0);   
    
    right_platform.header.stamp.sec = simulationTime*100000;
    
    left_platform.pose.position.x = -cos(pitch*PI/180)*0.125;
    left_platform.pose.position.y = 0;
    left_platform.pose.position.z = 0.07+sin(pitch*PI/180)*0.125;
    
    left_platform.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll*PI/180, pitch*PI/180, 0);
   
    left_platform.header.stamp.sec = simulationTime*100000;
    
    cout << "Platform's roll: " << roll << "   pitch: " << pitch << endl;
    cout << "--------------------------------------" << endl;
    
    command_right_platform_pub.publish(right_platform);
    command_left_platform_pub.publish(left_platform);
}


void simulationCallback(const vrep_common::VrepInfo& msg)
{
    if (simulator_state == 5 && msg.simulatorState.data == 4) /* Simulation stopped. */
      ros::shutdown();
          
    simulator_state = msg.simulatorState.data;   
    simulationTime = msg.simulationTime.data;
 
    int scale_div = 3;
    
    if (simulationTime == 0)
    {
       right_platform_init.x = (state.gimbal_angle[1])/scale_div;
       right_platform_init.y = (state.gimbal_angle[2])/scale_div;
       right_platform_init.z = 0; /*(state.gimbal_angle[0])/scale_div;*/
    }
    
    setPlatformPose(simulationTime);
}
   

void setupMessaging(ros::NodeHandle nh)
{   
    simulation_state_sub = nh.subscribe("/vrep/info", 1000, simulationCallback);
    
    command_right_platform_pub = nh.advertise<geometry_msgs::PoseStamped>("/vrep/command_right_platform",1000);
   
    command_left_platform_pub = nh.advertise<geometry_msgs::PoseStamped>("/vrep/command_left_platform",1000);
}

    
void mainLoop(int argc, char **argv)
{
    ros::init(argc, argv, "platform_control");
    ros::NodeHandle nh;
    
    if (!ros::param::get("plane", plane))
        ROS_ERROR("Failed to read symbols on parameter server");
    else if (!ros::param::get("amplitude", amplitude))
        ROS_ERROR("Failed to read symbols on parameter server");
    else if (!ros::param::get("frequency", frequency))
        ROS_ERROR("Failed to read symbols on parameter server");
    
    setupMessaging(nh);
    
    ros::spin();
}


HDCallbackCode HDCALLBACK UpdateCalibrationCallback(void *pUserData)
{
    HDenum *calibrationStyle = (HDenum *) pUserData;
   
    if (hdCheckCalibration() == HD_CALIBRATION_NEEDS_UPDATE)
    {
        hdUpdateCalibration(*calibrationStyle);
    }

    return HD_CALLBACK_DONE;
}


HDboolean CheckCalibration()
{
    HDint supportedCalibrationStyles;
    HDenum calibrationStyle;
    
    /* Choose a calibration style. Some devices may support multiple types of calibration. 
     * In that case, prefer auto calibration over inkwell calibration, and prefer inkwell calibration over reset encoders. */
    
    hdGetIntegerv(HD_CALIBRATION_STYLE, &supportedCalibrationStyles);
    
    if (supportedCalibrationStyles & HD_CALIBRATION_ENCODER_RESET)
    {
        calibrationStyle = HD_CALIBRATION_ENCODER_RESET;
        /* Calibration style encoder reset: the device needs to be put in a reset position to be calibrated. */
    }
    
    if (supportedCalibrationStyles & HD_CALIBRATION_INKWELL)
    {
        calibrationStyle = HD_CALIBRATION_INKWELL;
        /* Calibration style inkwell: the device needs to be put into a fixture, inkwell, before calibration can be performed. */
    }
    
    if (supportedCalibrationStyles & HD_CALIBRATION_AUTO)
    {
        calibrationStyle = HD_CALIBRATION_AUTO;
        /* Calibration style auto: the device will gather new calibration information as the armature is moved. */
    }
    
    /* Some haptic devices are calibrated when the gimbal is placed into the device inkwell and updateCalibration is called.  
     * This form of calibration is always performed after the servoloop has started running. For devices that require inkwell reset,
     * such as the PHANToM Omni, calibration is successfully performed whenever the device is put into the inkwell. */
    
    HDenum status = hdCheckCalibration();
 
    if (status == HD_CALIBRATION_OK)
    {
        return HD_TRUE;
    }
    else if (status == HD_CALIBRATION_NEEDS_MANUAL_INPUT)
    {
        cout << "Calibration requires manual input." << endl;
        return HD_FALSE;
    }
    else if (status == HD_CALIBRATION_NEEDS_UPDATE)
    {
        /* Synchronous scheduler callback to check the joystick internal calibration parameters. */
        hdScheduleSynchronous(UpdateCalibrationCallback, &calibrationStyle, HD_DEFAULT_SCHEDULER_PRIORITY);
        
        if (HD_DEVICE_ERROR(hdGetError()))
        {
            cout << "\nFailed to update calibration.\n" << endl;
            return HD_FALSE;
        }
        else
        {
            cout << "\nCalibration updated successfully.\n" << endl;
            return HD_TRUE;
        }
    }
    else
    {
        assert(!"Unknown calibration status");
        return HD_FALSE;
    }
}

 
int main(int argc, char **argv)
{
    /*if (argv[1] == NULL || argv[2] == NULL || argv[3] == NULL)
        ROS_ERROR("Insert the plane, amplitude and frequency of the rotary motion.");
    else 
    {
        plane = argv[1];
        amplitude = atoi(argv[2]);
        frequency = atof(argv[3]);
    }*/
    
    HDErrorInfo error;
    
    /* General pattern of use for the HDAPI: */
     
    /* Initialize the device. */
    HHD hHD = hdInitDevice(HD_DEFAULT_DEVICE);
    if (HD_DEVICE_ERROR(error = hdGetError()))
    {
        hduPrintError(stderr, &error, "Failed to initialize the device");
        fprintf(stderr, "\nPress any key to quit.\n");
        getch();
        
        return -1;           
    }
    ROS_INFO("Found %s.\n\n", hdGetString(HD_DEVICE_MODEL_TYPE));
    
    /* Create asynchronous scheduler callback. */
    gCallbackHandle = hdScheduleAsynchronous(SchedulerCallback, 0, HD_MAX_SCHEDULER_PRIORITY);
    if (HD_DEVICE_ERROR(error = hdGetError()))
    {
        hduPrintError(stderr, &error, "Failed to schedule servoloop callback");

        hdDisableDevice(hHD);
        
        fprintf(stderr, "\nPress any key to quit.\n");
        getch();
        
        return -1;    
    }
        
    /* Enable force output. */        
    hdEnable(HD_FORCE_OUTPUT);
    
    /* Start the scheduler - responsible for managing a high frequency thread 
     *for sending forces and retrieving state information from the device. */
    hdSetSchedulerRate(1000);
    hdStartScheduler();
    if (HD_DEVICE_ERROR(error = hdGetError()))
    {
        hduPrintError(stderr, &error, "Failed to start the scheduler");
        
        fprintf(stderr, "\nPress any key to quit.\n");
        getch();
        return -1;           
    }
    
    /* A parallel loop is started where the ROS node will be running, if the device is properly calibrated. */
    if (CheckCalibration())
        mainLoop(argc, argv);

    /* When the application is terminated (ROS node is shutted down), the device and scheduler are cleanup. */
    hdStopScheduler();
    hdUnschedule(gCallbackHandle);
    hdDisableDevice(hHD);
    
    return 0;   
}