C++ (Cpp) runTraj Examples

Example #1

File: hubo-read-trajectory-as-func.cpp Project: robodreamer/Robocpp-cmake

int huboLoop(int mode, bool compliance_mode, bool pause_feature) {
    double newRef[2] = {1.0, 0.0};
    // get initial values for hubo
    struct hubo_ref H_ref;
    memset( &H_ref,   0, sizeof(H_ref));
    hubo_virtual_t H_virtual;
    memset( &H_virtual, 0, sizeof(H_virtual));

    size_t fs;
    int r = ach_get( &chan_hubo_ref, &H_ref, sizeof(H_ref), &fs, NULL, ACH_O_COPY );
    if(ACH_OK != r) {
        if(hubo_debug) {
            //	printf("Ref ini r = %s\n",ach_result_to_string(r));
        }
    }
    else{   assert( sizeof(H_ref) == fs ); }

    struct hubo_state H_state;
    memset( &H_state, 0, sizeof(H_state) );

    r = ach_get( &chan_hubo_state, &H_state, sizeof(H_state), &fs, NULL, ACH_O_COPY );
    if (ACH_OK != r) {
        if (hubo_debug) {
            //printf("State ini r = %s\n", ach_result_to_string(r));
        }
    } else {
        assert( sizeof(H_state) == fs );
    }


    // time info
    struct timespec t;


    /* Sampling Period */
    double T = (double)interval/(double)NSEC_PER_SEC; // (sec)
    clock_gettime( 0,&t);
    // ------------------------------------------------------------------------------
    // ---------------[ DO NOT EDIT AVBOE THIS LINE]---------------------------------
    // ------------------------------------------------------------------------------

    //	char* fileName = "valve0.traj";

    runTraj(fileName,mode,  &H_ref, &t, &H_state, compliance_mode, pause_feature);

    // ------------------------------------------------------------------------------
    // ---------------[ DO NOT EDIT BELOW THIS LINE]---------------------------------
    // ------------------------------------------------------------------------------


    printf("Trajectory Finished\n");
    return 0;
}

Example #2

File: hubo-read-trajectory.c Project: thedancomplex/hubo-read-trajectory

int huboLoop() {
	double newRef[2] = {1.0, 0.0};
        // get initial values for hubo
        struct hubo_ref H_ref;
	memset( &H_ref,   0, sizeof(H_ref));

        size_t fs;
	int r = ach_get( &chan_hubo_ref, &H_ref, sizeof(H_ref), &fs, NULL, ACH_O_COPY );
	if(ACH_OK != r) {
		if(hubo_debug) {
                       	printf("Ref ini r = %s\n",ach_result_to_string(r));}
		}
	else{   assert( sizeof(H_ref) == fs ); }


        // time info
        struct timespec t;


	/* Sampling Period */
	double T = (double)interval/(double)NSEC_PER_SEC; // (sec)
	clock_gettime( 0,&t);
// ------------------------------------------------------------------------------
// ---------------[ DO NOT EDIT AVBOE THIS LINE]---------------------------------
// ------------------------------------------------------------------------------

//	char* fileName = "valve0.traj";

	runTraj(fileName, &H_ref, &t);


//	runTraj("ybTest1.traj",&H_ref_filter, &t);

//	runTraj("valve0.traj", &H_ref_filter, &t);
//	runTraj("valve1.traj", &H_ref_filter, &t);
//	runTraj("valve2.traj", &H_ref_filter, &t);
//	runTraj("valve1.traj", &H_ref_filter, &t);
//	runTraj("valve2.traj", &H_ref_filter, &t);
//	runTraj("valve1.traj", &H_ref_filter, &t);
//	runTraj("valve2.traj", &H_ref_filter, &t);
//	runTraj("valve1.traj", &H_ref_filter, &t);
//	runTraj("valve2.traj", &H_ref_filter, &t);
//	runTraj("valve1.traj", &H_ref_filter, &t);
//	runTraj("valve2.traj", &H_ref_filter, &t);
//	runTraj("valve3.traj", &H_ref_filter, &t);
// ------------------------------------------------------------------------------
// ---------------[ DO NOT EDIT BELOW THIS LINE]---------------------------------
// ------------------------------------------------------------------------------


	printf("Trajectory Finished\n");
	return 0;
}

Example #3

File: hubo-read-trajectory-as-func.cpp Project: robodreamer/Robocpp-cmake

int runTrajFunction(char* s, int mode,  bool compliance_mode, bool pause_feature) {
    /* open ach channel */
    int t1 = ach_open(&chan_hubo_ref, HUBO_CHAN_REF_NAME , NULL);
    assert( ACH_OK == t1);

    int t2 = ach_open(&chan_hubo_state, HUBO_CHAN_STATE_NAME, NULL);
    assert( ACH_OK == t2);
    // open to sim chan
    t1 = ach_open(&chan_hubo_from_sim, HUBO_CHAN_VIRTUAL_FROM_SIM_NAME, NULL);
    assert( ACH_OK == t1);


    printf("\n ======================================= \n");
    printf("     Hubo-Read-Trajectory-Function-Mode \n");
    printf(" ======================================= \n");
//    fflush(stdout);
    //    printf("into the func \n");

    double newRef[2] = {1.0, 0.0};
    // get initial values for hubo
    struct hubo_ref H_ref;
    memset( &H_ref,   0, sizeof(H_ref));
    hubo_virtual_t H_virtual;
    memset( &H_virtual, 0, sizeof(H_virtual));

//    printf("after creating \n");
//    fflush(stdout);
    size_t fs;
    int r = ach_get( &chan_hubo_ref, &H_ref, sizeof(H_ref), &fs, NULL, ACH_O_COPY );
//    printf("after r \n");
//    fflush(stdout);

    if(ACH_OK != r) {
        if(hubo_debug) {
            //printf("Ref ini r = %s\n",ach_result_to_string(r));
        }
    }
    else{   assert( sizeof(H_ref) == fs ); }

    //printf("after achget \n");
//    fflush(stdout);
    struct hubo_state H_state;
    memset( &H_state, 0, sizeof(H_state) );

    r = ach_get( &chan_hubo_state, &H_state, sizeof(H_state), &fs, NULL, ACH_O_COPY );
    if (ACH_OK != r) {
        if (hubo_debug) {
            //printf("State ini r = %s\n", ach_result_to_string(r));
        }
    } else {
        assert( sizeof(H_state) == fs );
    }
    //printf("after ach \n");

    // time info
    struct timespec t;


    /* Sampling Period */
    double T = (double)interval/(double)NSEC_PER_SEC; // (sec)
    clock_gettime( 0,&t);
    // ------------------------------------------------------------------------------
    // ---------------[ DO NOT EDIT AVBOE THIS LINE]---------------------------------
    // ------------------------------------------------------------------------------

    //      char* fileName = "valve0.traj";
    printf("Getting into trajectory \n");
    fflush(stdout);
    runTraj(s,mode,  &H_ref, &t, &H_state, compliance_mode, pause_feature);

    // ------------------------------------------------------------------------------
    // ---------------[ DO NOT EDIT BELOW THIS LINE]---------------------------------
    // ------------------------------------------------------------------------------


    printf("Trajectory Finished \n");
    fflush(stdout);
    return 0;


}