From 0cefd1852bb2f6a59506cd501a2d47fed4e17008 Mon Sep 17 00:00:00 2001
From: Mamoun Gharbi <magharbi@laas.fr>
Date: Mon, 12 Oct 2015 17:58:58 +0200
Subject: [PATCH] [wip/move3d-studio] Update to version 1.3
- GTP interface (including socket via msgConnector)
- MHO interface
- Making real time video with theV key(time paramtrized trajectories)
- Function to save a FlyCrane traj
- Button for determinietic shortcut (remove useless)
- New Test buttons in mainWindow.cpp
- Directed PRM checbox
- Checkboxes for metric and sampling (kinodynamic)
---
move3d-studio/Makefile | 2 +-
move3d-studio/depend.mk | 2 +-
move3d-studio/distinfo | 6 +-
move3d-studio/patches/patch-aa | 2647 --------------------------------
4 files changed, 5 insertions(+), 2652 deletions(-)
delete mode 100644 move3d-studio/patches/patch-aa
diff --git a/move3d-studio/Makefile b/move3d-studio/Makefile
index 5e4e205b..6cdbafc4 100644
--- a/move3d-studio/Makefile
+++ b/move3d-studio/Makefile
@@ -4,7 +4,7 @@
PKGNAME= move3d-studio-${VERSION}
DISTNAME= move3d-studio-${VERSION}
-VERSION= 1.2.1
+VERSION= 1.3
PKGREVISION= 3
CATEGORIES= path
diff --git a/move3d-studio/depend.mk b/move3d-studio/depend.mk
index 207bd40f..1affe28b 100644
--- a/move3d-studio/depend.mk
+++ b/move3d-studio/depend.mk
@@ -17,7 +17,7 @@ SYSTEM_SEARCH.move3d-studio=\
bin/move3d-qt-studio
DEPEND_USE+= move3d-studio
-DEPEND_ABI.move3d-studio?=move3d-studio>=1.2.1
+DEPEND_ABI.move3d-studio?=move3d-studio>=1.3
DEPEND_DIR.move3d-studio?=../../wip/move3d-studio
endif # MOVE3DSTUDIO_DEPEND_MK ----------------------------------------
diff --git a/move3d-studio/distinfo b/move3d-studio/distinfo
index fcb048df..9c5068a5 100644
--- a/move3d-studio/distinfo
+++ b/move3d-studio/distinfo
@@ -1,4 +1,4 @@
-SHA1 (move3d-studio-1.2.1.tar.gz) = 634504cf962a6cffa4136e91de22395a4667b64d
-RMD160 (move3d-studio-1.2.1.tar.gz) = 1968343ae3ed724ca0b9cc31946efd150cdc9638
-Size (move3d-studio-1.2.1.tar.gz) = 1312291 bytes
+SHA1 (move3d-studio-1.3.tar.gz) = 2e2e4ae16b0c4bb7a6757e9ec757c0fea4fdabcb
+RMD160 (move3d-studio-1.3.tar.gz) = 8f197229cf6c16e9c5da11ccdeb7570742293cf8
+Size (move3d-studio-1.3.tar.gz) = 1381636 bytes
SHA1 (patch-aa) = 061bebfc7a04ce479ca8a78b8f82a35f7159d6fe
diff --git a/move3d-studio/patches/patch-aa b/move3d-studio/patches/patch-aa
deleted file mode 100644
index bfddb880..00000000
--- a/move3d-studio/patches/patch-aa
+++ /dev/null
@@ -1,2647 +0,0 @@
---- src/move3d-qt-gui-libs/CMakeLists.txt
-+++ src/move3d-qt-gui-libs/CMakeLists.txt
-@@ -324,9 +324,6 @@ ENDMACRO(GENERATE_DOCUMENTATION)
- # -------------------------------------------------------------------------------
- # Compiler and Compilation flags
- # -------------------------------------------------------------------------------
--SET(CMAKE_CXX_COMPILER g++)
--SET(CMAKE_C_COMPILER g++)
--
- IF(UNIX)
- ADD_DEFINITIONS(-DUNIX)
- IF(APPLE)
---- src/move3d-qt-gui-libs/src/qtMainInterface/mainwindow.cpp
-+++ src/move3d-qt-gui-libs/src/qtMainInterface/mainwindow.cpp
-@@ -32,7 +32,7 @@
- // Warning contains boost function that conlicts with Qt
- //#include "planner_cxx/API/Trajectory/RoboptimTrajectory.h"
- #include "planner/TrajectoryOptim/Classic/costOptimization.hpp"
--#include "API/Grids/GridToGraph/gridtograph.hpp"
-+//#include "API/Grids/GridToGraph/gridtograph.hpp"
- #include "API/Search/GraphState.hpp"
- #include "API/Grids/PointCloud.hpp"
- #include "API/Grids/BaseGrid.hpp"
---- src/move3d-qt/CMakeLists.txt
-+++ src/move3d-qt/CMakeLists.txt
-@@ -166,8 +166,6 @@ SET(INCLUDE_DIRS ${INCLUDE_DIRS} ${MOVE3D-QTGUI_INCLUDE_DIR})
- #message(${INCLUDE_DIRS})
- #----------------------------------------------------------------------
-
--SET(CMAKE_CXX_COMPILER g++)
--SET(CMAKE_C_COMPILER g++)
-
- IF(UNIX)
- ADD_DEFINITIONS(-DUNIX)
-@@ -286,9 +284,9 @@ ENDIF(USE_GBM)
-
- IF(QT_LIBRARY)
- QT4_WRAP_CPP(MOC_OUTFILES main.hpp)
--ADD_EXECUTABLE(move3d-qt-studio main.cpp move3d.c ${MOC_OUTFILES})
-+ADD_EXECUTABLE(move3d-qt-studio main.cpp move3d.cpp ${MOC_OUTFILES})
- ELSE()
--ADD_EXECUTABLE(move3d-qt-studio main.cpp move3d.c)
-+ADD_EXECUTABLE(move3d-qt-studio main.cpp move3d.cpp)
- ENDIF()
-
- SET_TARGET_PROPERTIES(move3d-qt-studio PROPERTIES LINKER_LANGUAGE CXX)
---- src/move3d-qt/move3d.c
-+++ /dev/null
-@@ -1,640 +0,0 @@
--#include <Util-pkg.h>
--#include <P3d-pkg.h>
--
--#include <move3d-headless.h>
--
--#ifdef P3D_PLANNER
--#include <Planner-pkg.h>
--#endif
--#ifdef P3D_COLLISION_CHECKING
--#include <Collision-pkg.h>
--#endif
--
--#include <Bio-pkg.h>
--#include <UdpClient.h>
--
--#include <locale.h>
--
--// TODO
--#undef USE_GLUT
--#if defined(USE_GLUT)
--#if defined(MACOSX)
--#include <glut.h>
--#else
--#include <GL/freeglut.h>
--#endif
--#endif
--
--#if defined( QT_GL ) && defined( CXX_PLANNER )
--#include <project.hpp>
--#include <cppToQt.hpp>
--#endif
--
--#include "planner_handler.hpp"
--
--#ifdef LIGHT_PLANNER
--#include "../lightPlanner/proto/DlrPlanner.h"
--#include "../lightPlanner/proto/DlrParser.h"
--#include "../lightPlanner/proto/lightPlannerApi.h"
--
--#if defined( MULTILOCALPATH ) && defined( GRASP_PLANNING )
--#include "../lightPlanner/proto/ManipulationTestFunctions.hpp"
--#endif
--#endif
--
--#include "../graphic/proto/g3d_glut.hpp"
--
--#if defined( CXX_PLANNER ) || defined( MOVE3D_CORE )
--#include "API/planningAPI.hpp"
--#include "planner/cost_space.hpp"
--#include <boost/bind.hpp>
--#endif
--#ifdef BIO_BALL
--#include "ball_energy.hpp"
--#endif
--
--#include <iostream>
--using namespace std;
--
--// External varialbe handled when compiled with move3d studio
--bool move3d_studio_load_settings = false;
--
--static int FILTER_TO_BE_SET_ACTIVE = FALSE;
--//extern void g3d_create_main_form(void);
--//extern void g3d_loop(void);
--//extern void p3d_col_set_mode(int);
--//extern void kcd_set_user_defined_small_volume(double);
--//extern double p3d_get_env_dmax(void);
--static void use(void);
--
--
--
--int mainMhp(int argc, char ** argv)
--{
-- cout << "Main Mhp" << endl;
--
-- //int main(int argc, char ** argv) {
--
--
-- // modif Pepijn apropos dmax and tol
-- int user_dmax_to_be_set = FALSE; /* Modif. Pepijn on dmax */
-- double user_dmax = 0.0; /* Modif. Pepijn on dmax */
-- int user_obj_tol_to_be_set = FALSE; /* Modif. Carl on tolerance */
-- double user_obj_tol = 0.0;
-- // end modif Pepijn
--
-- double user_volume, user_size; /* Modif. Carl on volume */
--#ifndef BIO
-- const char *file;
--#endif
-- char file_directory[200];
-- char filename[200];
-- char scenario[200];
-- char dlrReadFile[200];
-- char dlrSaveFile[200];
-- int i = 0;
-- /* carl: */
-- int seed_set = FALSE;
-- int dir_set = FALSE;
-- int file_set = FALSE;
-- int scenario_set = FALSE;
-- int col_det_set = FALSE;
-- int col_mode_to_be_set = p3d_col_mode_pqp; /* Nic p3d_col_mode_v_collide;*/
-- int ccntrt_active = TRUE;
--
-- // Tests
-- int manip_test_run = FALSE;
-- int manip_test_id = 0;
--
-- // init English C
-- if (! setlocale(LC_ALL, "C"))
-- fprintf(stderr, "There was an error while setting the locale to \"C\"\n");
--
-- int grid_set = FALSE;
-- std::string grid_path;
-- // modif Brice SALVA
--
--#ifdef BIO
-- int usrAnswer = FALSE;
-- file_name_list* file_list = NULL;
-- char name[PSF_MAX_NAME_LENGTH];
-- int is_p3d = FALSE;
--#endif
-- if (! setlocale(LC_ALL, "C")) fprintf(stderr, "There was an error while setting the locale to \"C\"\n");
--
-- // End modif Brice SALVA
--
-- /* lecture des arguments */
-- /* carl: */
-- i = 1;
-- while (i < argc) {
-- if (strcmp(argv[i], "-debug") == 0) {
-- basic_alloc_debugon();
-- i++;
-- } else if (strcmp(argv[i], "-d") == 0) {
-- ++i;
-- if ((i < argc)) {
-- strcpy(file_directory, argv[i]);
-- dir_set = TRUE;
-- ++i;
-- } else {
-- use();
-- return 0;
-- }
-- } else if (strcmp(argv[i], "-f") == 0) {
-- ++i;
-- if ((i < argc)) {
-- strcpy(filename, argv[i]);
-- file_set = TRUE;
-- ++i;
-- } else {
-- use();
-- return 0;
-- }
-- } else if (strcmp(argv[i], "-sc") == 0) {
-- ++i;
-- if ((i < argc)) {
-- strcpy(scenario, argv[i]);
-- scenario_set = TRUE;
-- ++i;
-- } else {
-- use();
-- return 0;
-- }
-- }
-- else if (strcmp(argv[i], "-setgui") == 0) {
-- ++i;
-- move3d_studio_load_settings = true;
-- }
-- else if (strcmp(argv[i], "-nogui") == 0) {
-- ++i;
-- // Nothing to do, fix this in main
-- }
--#ifdef P3D_COLLISION_CHECKING
-- else if (strcmp(argv[i], "-o") == 0) {
-- set_collision_by_object(FALSE);
-- ++i;
-- } else if (strcmp(argv[i], "-x") == 0) {
-- FILTER_TO_BE_SET_ACTIVE = TRUE;
-- ++i;
-- } else if (strcmp(argv[i], "-nkcdd") == 0) {
-- set_return_kcd_distance_estimate(FALSE);
-- ++i;
-- }
--#endif
-- else if (strcmp(argv[i], "-s") == 0) {
-- ++i;
-- if ((i < argc)) {
-- p3d_init_random_seed(atoi(argv[i]));
-- seed_set = TRUE;
-- ++i;
-- } else {
-- use();
-- return 0;
-- }
-- } else if (strcmp(argv[i], "-v") == 0) {
-- ++i;
-- if (i < argc) {
--
-- user_size = atof(argv[i]);
-- user_volume = user_size * user_size * user_size;
--#ifdef P3D_COLLISION_CHECKING
-- kcd_set_user_defined_small_volume(user_volume);
--#endif
-- ++i;
-- } else {
-- use();
-- return 0;
-- }
-- } else if (strcmp(argv[i], "-vol") == 0) {
-- ++i;
-- if (i < argc) {
--#ifdef P3D_COLLISION_CHECKING
-- kcd_set_user_defined_small_volume(atof(argv[i]));
--#endif
-- ++i;
-- } else {
-- use();
-- return 0;
-- }
-- } else if (strcmp(argv[i], "-tol") == 0) {
-- ++i;
-- if (i < argc) {
-- user_obj_tol = atof(argv[i]) ;
-- user_obj_tol_to_be_set = TRUE;
-- ++i;
-- } else {
-- use();
-- return 0;
-- }
-- } else if (strcmp(argv[i], "-dmax") == 0) {
-- ++i;
-- if (i < argc) {
-- user_dmax = atof(argv[i]) ;
-- user_dmax_to_be_set = TRUE;
-- ++i;
-- } else {
-- use();
-- return 0;
-- }
-- } else if (strcmp(argv[i], "-stat") == 0) {
-- ++i;
--#ifdef P3D_PLANNER
-- enableStats();
--#endif
-- }
--#if defined(HRI_COSTSPACE) && defined(QT_GL)
-- else if (strcmp(argv[i], "-grid") == 0) {
-- ++i;
-- if ((i < argc)) {
-- grid_path = argv[i];
-- //printf("Grid is at : %s\n",gridPath.c_str());
-- grid_set = TRUE;
-- ++i;
-- } else {
-- use();
-- return 0;
-- }
-- }
--#endif
-- else if (strcmp(argv[i], "-udp") == 0) {
-- }
--
--#ifdef LIGHT_PLANNER
-- else if (strcmp(argv[i], "-no-ccntrt") == 0) {
-- printf("Deactivate ccntrt at startup\n");
-- ccntrt_active = FALSE;
-- ++i;
-- }
-- else if (strcmp(argv[i], "-test") == 0) {
-- ++i;
-- manip_test_run = TRUE;
-- if (i < argc) {
-- manip_test_id = atoi(argv[i]) ;
-- ++i;
-- } else {
-- manip_test_run = FALSE;
-- use();
-- return 0;
-- }
-- }
--#endif
--
-- else if (strcmp(argv[i], "-udp") == 0) {
-- std::string serverIp(argv[i+1]);
-- int port = 0;
-- sscanf(argv[i+2], "%d", &port);
-- globalUdpClient = new UdpClient(serverIp, port);
-- i += 3;
-- }
-- else if (strcmp(argv[i], "-dlr") == 0) {
-- strcpy(dlrReadFile, argv[i + 1]);
-- strcpy(dlrSaveFile, argv[i + 2]);
-- i += 3;
-- } else if (strcmp(argv[i], "-c") == 0) {
-- ++i;
-- cout << "Coll mod " << argv[i] << endl;
-- if (strcmp(argv[i], "vcollide") == 0) {
-- col_mode_to_be_set = p3d_col_mode_v_collide;
-- col_det_set = TRUE;
-- ++i;
-- }
--#ifdef P3D_COLLISION_CHECKING
-- else if (strcmp(argv[i], "kcd") == 0) {
-- col_mode_to_be_set = p3d_col_mode_kcd;
-- set_DO_KCD_GJK(TRUE);
-- col_det_set = TRUE;
-- ++i;
-- }
--#ifdef PQP
-- else if (strcmp(argv[i], "pqp") == 0) {
-- printf("Colmod pqp");
-- col_mode_to_be_set= p3d_col_mode_pqp;
-- col_det_set = TRUE;
-- ++i;
-- cout << "Coll mod PQP " << endl;
-- }
--#endif
--#endif
-- else if (strcmp(argv[i], "bio") == 0) {
-- col_mode_to_be_set = p3d_col_mode_bio;
-- col_det_set = TRUE;
-- ++i;
-- } else if (strcmp(argv[i], "kng") == 0) {
-- col_mode_to_be_set = p3d_col_mode_kcd;
--#ifdef P3D_COLLISION_CHECKING
-- set_DO_KCD_GJK(FALSE);
--#endif
-- col_det_set = TRUE;
-- ++i;
-- } else if (strcmp(argv[i], "gjk") == 0) {
-- col_mode_to_be_set = p3d_col_mode_gjk;
-- col_det_set = TRUE;
-- ++i;
-- } else if (strcmp(argv[i], "none") == 0) {
-- col_mode_to_be_set = p3d_col_mode_none;
-- col_det_set = TRUE;
-- ++i;
-- } else {
-- use();
-- return 0;
-- }
-- } else {
-- use();
-- return 0;
-- }
-- }
--
-- if (!dir_set)
-- strcpy(file_directory, "../../demo");
--
-- if (!seed_set)
-- p3d_init_random();
--
-- if (!col_det_set){
-- // modif Juan
--
-- //check that the HOME_MOVE3D environment variable exists:
-- if(getenv("HOME_MOVE3D")==NULL) {
-- printf("%s: %d: main(): The environment variable \"HOME_MOVE3D\" is not defined. This might cause some problems or crashes (e.g. with video capture).\n", __FILE__,__LINE__);
-- }
--
--
--#ifdef GRASP_PLANNING
-- col_mode_to_be_set= p3d_col_mode_pqp;
--#ifndef PQP
-- printf("%s: %d: main(): GRASP_PLANNING must be compiled with PQP.\n", __FILE__,__LINE__);
-- printf("Program must quit.\n");
-- return 1;
--#endif
--#else
-- col_mode_to_be_set = p3d_col_mode_kcd;
--#endif
-- }
--#ifdef P3D_COLLISION_CHECKING
-- if (col_mode_to_be_set != p3d_col_mode_v_collide)
-- set_collision_by_object(FALSE);
-- /* : carl */
-- if (col_mode_to_be_set == p3d_col_mode_v_collide) {
-- p3d_filter_switch_filter_mechanism(FILTER_TO_BE_SET_ACTIVE);
-- }
-- /* begin added KCD FILTER */
-- else if (col_mode_to_be_set == p3d_col_mode_kcd) {
-- p3d_filter_switch_filter_mechanism(FILTER_TO_BE_SET_ACTIVE);
-- }
--#endif
-- /* end added KCD FILTER */
--
--
-- /* lecture du fichier environnement */
-- p3d_set_directory(file_directory);
--
-- // init English C
-- if (! setlocale(LC_ALL, "C"))
-- fprintf(stderr, "There was an error while setting the locale to \"C\"\n");
--
--
-- while (!p3d_get_desc_number(P3D_ENV)) {
--
--#ifdef BIO
-- if (file_set == TRUE) {
-- // if (!filename) {
-- // exit(0);
-- // }
--
--#ifndef PQP
-- p3d_col_set_mode(p3d_col_mode_none);
--#else
-- p3d_col_set_mode(col_mode_to_be_set);
--#endif
-- p3d_BB_set_mode_close();
--
-- if (!p3d_read_desc(filename))
-- {
-- file_set = FALSE;
-- }
-- }
-- if (file_set == FALSE) {
--
-- printf("Error : give a p3d filename as argument, or use the XFORMS module.\n");
-- exit(0);
-- }
--#else
--
-- if (file_set == TRUE) {
-- file = filename;
-- } else {
--
-- }
-- if (!file) {
-- exit(0);
-- }
--
--#ifdef P3D_COLLISION_CHECKING
-- p3d_col_set_mode(p3d_col_mode_none);
-- p3d_BB_set_mode_close();
--#endif
--
-- printf("\n");
-- printf(" ----------------------------\n");
-- printf(" -- p3d file parsing start --\n");
-- printf(" ----------------------------\n");
-- printf("\n");
--
-- p3d_read_desc((char *) file);
--
--#endif
-- if (!p3d_get_desc_number(P3D_ENV)) {
-- printf("loading done...\n");
-- }
-- }
--
-- printf("\n");
-- printf(" ----------------------------\n");
-- printf(" -- p3d file parsing done --\n");
-- printf(" ----------------------------\n");
-- printf("\n");
--
-- MY_ALLOC_INFO("After p3d_read_desc");
--
-- printf("Nb poly : %d\n", p3d_poly_get_nb());
--
-- /* for start-up with currently chosen collision detector: */
-- /* MY_ALLOC_INFO("Before initialization of a collision detector"); */
--#ifdef P3D_COLLISION_CHECKING
-- p3d_col_set_mode(col_mode_to_be_set);
-- p3d_col_start(col_mode_to_be_set);
--#endif
--
--
--#if defined( MOVE3D_CORE )
-- if (ENV.getBool(Env::isCostSpace))
-- {
-- GlobalCostSpace::initialize();
-- }
--#endif
--
-- /* modif Pepijn april 2001
-- * this changes have to be made after the initialistion of the collision checker
-- * because in p3d_col_start KCD migth be initialised, and during this initialisation
-- * there is automaticly calculated a dmax
-- * So if the user wants to set his own dmax this most be done after this initialisation
-- * INTERNAL NOTE: in this case the users are the developpers of MOVE3D, normally the clients
-- * who purchase Move3d don't know that this option exists
-- */
-- if (user_dmax_to_be_set) {
-- if (user_dmax < EPS4) {
-- printf("WARNING: User chose dmax too small --> new value set to 0.0001 (EPS4)\n");
-- user_dmax = EPS4;
-- }
-- p3d_set_env_dmax(user_dmax);
-- }
-- if (user_obj_tol_to_be_set) {
-- if (user_obj_tol < 0.0) {
-- printf("WARNING: Negative tolerance, tolerance is set to 0.0\n");
-- user_obj_tol = 0.0;
-- }
-- p3d_set_env_object_tolerance(user_obj_tol);
-- }
--
-- printf("Env dmax = %f\n",p3d_get_env_dmax());
-- printf("Env Object tol = %f\n",p3d_get_env_object_tolerance());
-- /* always set tolerance even if the user didn't specify any options
-- * it's possible that Kcd has calculated automaticly a dmax
-- */
-- /* Normally p3d_col_set_tolerance(); is called when initialising
-- * the sliders, in case the sliders are not used we have to use
-- * p3d_col_set_tolerance()
-- */
--
-- //printf("MAX_DDLS %d\n", MAX_DDLS);
--
-- // modif Juan
--#ifdef BIO
-- if (col_mode_to_be_set == p3d_col_mode_bio) {
-- bio_set_num_subrobot_AA();
-- bio_set_num_subrobot_ligand();
-- bio_set_bio_jnt_types();
-- bio_set_bio_jnt_AAnumbers();
-- bio_set_list_AA_first_jnt();
-- bio_set_AAtotal_number();
-- bio_set_nb_flexible_sc();
-- bio_set_list_firstjnts_flexible_sc();
-- if (XYZ_ROBOT->num_subrobot_ligand != -1)
-- bio_set_nb_dof_ligand();
--#ifdef BIO_BALL
-- new BallEnergy(XYZ_ENV);
-- if(global_costSpace)
-- {
-- global_costSpace->addCost("BALLmmff94",
-- boost::bind(&BallEnergy::computeEnergy, XYZ_ENV->energyComputer, _1));
-- global_costSpace->setCost("BALLmmff94");
-- }
--#endif
-- }
--#endif
-- // fmodif Juan
--
--#if defined(LIGHT_PLANNER) && defined(FK_CNTRT)
-- for(int i=0; i<XYZ_ENV->nr; i++)
-- { p3d_create_FK_cntrts(XYZ_ENV->robot[i]); }
--#endif
--
--#ifdef P3D_CONSTRAINTS
-- // Modif Mokhtar Initialisation For Multisolutions constraints
-- p3d_init_iksol(XYZ_ROBOT->cntrt_manager);
--#endif
--
-- /* creation du FORM main */
--
-- /*
-- * needs to be run after main form has been created
-- */
-- if (scenario_set == TRUE)
-- {
-- p3d_rw_scenario_init_name();
-- p3d_read_scenario(scenario);
-- }
--
-- //Set the robots to initial Pos if defined
-- for(i = 0; i < XYZ_ENV->nr; i++){
-- if(!p3d_isNullConfig(XYZ_ENV->robot[i], XYZ_ENV->robot[i]->ROBOT_POS)){
-- p3d_set_and_update_this_robot_conf(XYZ_ENV->robot[i], XYZ_ENV->robot[i]->ROBOT_POS);
-- }
-- }
-- //Exection Of Dlr Planner
-- // do{
-- // DlrPlanner* planner = new DlrPlanner(dlrSaveFile);
-- // DlrParser parser(dlrReadFile, planner);
-- // if(parser.parse()){
-- // planner->process();
-- // }else{
-- // sleep(2);
-- // }
-- // free(planner);
-- // }while(1);
-- /* go into loop */
--
--#if defined(LIGHT_PLANNER) && defined(PQP)
-- if(ENV.getBool(Env::startWithFKCntrt))
-- {
-- p3d_rob* MyRobot = p3d_get_robot_by_name_containing ( ( char* ) "ROBOT" );
-- deactivateCcCntrts(MyRobot,-1);
-- }
--#endif
--
--#if defined(HRI_COSTSPACE) && defined(QT_GL)
-- if(grid_set)
-- {
-- std::cout << "Reading HRICS Grid " << grid_path << std::endl;
-- qt_load_HRICS_Grid(grid_path);
-- }
--#endif
--
-- // double c, color[4];
-- // srand(time(NULL));
-- // c= rand()/((double)RAND_MAX+1);
-- // g3d_rgb_from_hue(c, color);
-- // g3d_set_win_floor_color(g3d_get_cur_win(), color[0], color[1], color[2]);
--
--#if defined( LIGHT_PLANNER ) && !defined( QT_GL )
-- if (manip_test_run)
-- {
-- printf("Test functions : ManipulationTestFunctions\n");
-- new qtG3DWindow();
--
--#if defined( MOVE3D_CORE )
-- // Creates the wrapper to the project
-- // Be carefull to initialize in the right thread
-- global_Project = new Project(new Scene(XYZ_ENV));
--#endif
--
-- ManipulationTestFunctions tests;
--
-- if(!tests.runTest(manip_test_id))
-- {
-- std::cout << "ManipulationTestFunctions::Fail" << std::endl;
-- }
-- }
--#endif
-- return 0;
--}
--
--/* fonction de rappel des formats des arguments */
--static void use(void) {
--
-- printf("main : ERROR : wrong arguments !\n move3d -d data_directory -f file -sc scenario -s random_seed -c collision_detector -tol tolerance -dmax dist [-v size | -vol volume] -o -x -nkcdd\n");
-- printf("collision_detector may be: icollide, vcollide, solid, kcd, kng, none\n");
-- printf("(kng == kcd without gjk)\n");
-- printf("-o: consider polyhedrons as objects (only for vcollide)\n");
-- printf("-x: do filtering by robot body (default: don't filter)\n");
-- printf("-v: (only with kcd) minimal relevant size (in 1 dimension) \n");
-- printf("-dmax: for Vcollide & nkcdd => maximum penetration distance \n");
-- printf(" KCD => artificial enlargement of the obstacles \n");
-- printf("-tol: (only with kcd) object tolerance (enlarge objects for the collision checker) \n");
-- printf("-vol: (only with kcd) minimal relevant size (volume) \n");
-- printf("(internal note, kcd: volume of a box around a facet == (surface of box)^(3/2) )\n");
-- printf("-nkcdd: (only with -c kcd) return only boolean reply, don't use distance \n");
-- printf("-no-ccntrt: deactivate closed chain constraint at startup \n");
--}
--
---- /dev/null
-+++ src/move3d-qt/move3d.cpp
-@@ -0,0 +1,640 @@
-+#include <Util-pkg.h>
-+#include <P3d-pkg.h>
-+
-+#include <move3d-headless.h>
-+
-+#ifdef P3D_PLANNER
-+#include <Planner-pkg.h>
-+#endif
-+#ifdef P3D_COLLISION_CHECKING
-+#include <Collision-pkg.h>
-+#endif
-+
-+#include <Bio-pkg.h>
-+#include <UdpClient.h>
-+
-+#include <locale.h>
-+
-+// TODO
-+#undef USE_GLUT
-+#if defined(USE_GLUT)
-+#if defined(MACOSX)
-+#include <glut.h>
-+#else
-+#include <GL/freeglut.h>
-+#endif
-+#endif
-+
-+#if defined( QT_GL ) && defined( CXX_PLANNER )
-+#include <project.hpp>
-+#include <cppToQt.hpp>
-+#endif
-+
-+#include "planner_handler.hpp"
-+
-+#ifdef LIGHT_PLANNER
-+#include "../lightPlanner/proto/DlrPlanner.h"
-+#include "../lightPlanner/proto/DlrParser.h"
-+#include "../lightPlanner/proto/lightPlannerApi.h"
-+
-+#if defined( MULTILOCALPATH ) && defined( GRASP_PLANNING )
-+#include "../lightPlanner/proto/ManipulationTestFunctions.hpp"
-+#endif
-+#endif
-+
-+#include "../graphic/proto/g3d_glut.hpp"
-+
-+#if defined( CXX_PLANNER ) || defined( MOVE3D_CORE )
-+#include "API/planningAPI.hpp"
-+#include "planner/cost_space.hpp"
-+#include <boost/bind.hpp>
-+#endif
-+#ifdef BIO_BALL
-+#include "ball_energy.hpp"
-+#endif
-+
-+#include <iostream>
-+using namespace std;
-+
-+// External varialbe handled when compiled with move3d studio
-+bool move3d_studio_load_settings = false;
-+
-+static int FILTER_TO_BE_SET_ACTIVE = FALSE;
-+//extern void g3d_create_main_form(void);
-+//extern void g3d_loop(void);
-+//extern void p3d_col_set_mode(int);
-+//extern void kcd_set_user_defined_small_volume(double);
-+//extern double p3d_get_env_dmax(void);
-+static void use(void);
-+
-+
-+
-+int mainMhp(int argc, char ** argv)
-+{
-+ cout << "Main Mhp" << endl;
-+
-+ //int main(int argc, char ** argv) {
-+
-+
-+ // modif Pepijn apropos dmax and tol
-+ int user_dmax_to_be_set = FALSE; /* Modif. Pepijn on dmax */
-+ double user_dmax = 0.0; /* Modif. Pepijn on dmax */
-+ int user_obj_tol_to_be_set = FALSE; /* Modif. Carl on tolerance */
-+ double user_obj_tol = 0.0;
-+ // end modif Pepijn
-+
-+ double user_volume, user_size; /* Modif. Carl on volume */
-+#ifndef BIO
-+ const char *file;
-+#endif
-+ char file_directory[200];
-+ char filename[200];
-+ char scenario[200];
-+ char dlrReadFile[200];
-+ char dlrSaveFile[200];
-+ int i = 0;
-+ /* carl: */
-+ int seed_set = FALSE;
-+ int dir_set = FALSE;
-+ int file_set = FALSE;
-+ int scenario_set = FALSE;
-+ int col_det_set = FALSE;
-+ int col_mode_to_be_set = p3d_col_mode_pqp; /* Nic p3d_col_mode_v_collide;*/
-+ int ccntrt_active = TRUE;
-+
-+ // Tests
-+ int manip_test_run = FALSE;
-+ int manip_test_id = 0;
-+
-+ // init English C
-+ if (! setlocale(LC_ALL, "C"))
-+ fprintf(stderr, "There was an error while setting the locale to \"C\"\n");
-+
-+ int grid_set = FALSE;
-+ std::string grid_path;
-+ // modif Brice SALVA
-+
-+#ifdef BIO
-+ int usrAnswer = FALSE;
-+ file_name_list* file_list = NULL;
-+ char name[PSF_MAX_NAME_LENGTH];
-+ int is_p3d = FALSE;
-+#endif
-+ if (! setlocale(LC_ALL, "C")) fprintf(stderr, "There was an error while setting the locale to \"C\"\n");
-+
-+ // End modif Brice SALVA
-+
-+ /* lecture des arguments */
-+ /* carl: */
-+ i = 1;
-+ while (i < argc) {
-+ if (strcmp(argv[i], "-debug") == 0) {
-+ basic_alloc_debugon();
-+ i++;
-+ } else if (strcmp(argv[i], "-d") == 0) {
-+ ++i;
-+ if ((i < argc)) {
-+ strcpy(file_directory, argv[i]);
-+ dir_set = TRUE;
-+ ++i;
-+ } else {
-+ use();
-+ return 0;
-+ }
-+ } else if (strcmp(argv[i], "-f") == 0) {
-+ ++i;
-+ if ((i < argc)) {
-+ strcpy(filename, argv[i]);
-+ file_set = TRUE;
-+ ++i;
-+ } else {
-+ use();
-+ return 0;
-+ }
-+ } else if (strcmp(argv[i], "-sc") == 0) {
-+ ++i;
-+ if ((i < argc)) {
-+ strcpy(scenario, argv[i]);
-+ scenario_set = TRUE;
-+ ++i;
-+ } else {
-+ use();
-+ return 0;
-+ }
-+ }
-+ else if (strcmp(argv[i], "-setgui") == 0) {
-+ ++i;
-+ move3d_studio_load_settings = true;
-+ }
-+ else if (strcmp(argv[i], "-nogui") == 0) {
-+ ++i;
-+ // Nothing to do, fix this in main
-+ }
-+#ifdef P3D_COLLISION_CHECKING
-+ else if (strcmp(argv[i], "-o") == 0) {
-+ set_collision_by_object(FALSE);
-+ ++i;
-+ } else if (strcmp(argv[i], "-x") == 0) {
-+ FILTER_TO_BE_SET_ACTIVE = TRUE;
-+ ++i;
-+ } else if (strcmp(argv[i], "-nkcdd") == 0) {
-+ set_return_kcd_distance_estimate(FALSE);
-+ ++i;
-+ }
-+#endif
-+ else if (strcmp(argv[i], "-s") == 0) {
-+ ++i;
-+ if ((i < argc)) {
-+ p3d_init_random_seed(atoi(argv[i]));
-+ seed_set = TRUE;
-+ ++i;
-+ } else {
-+ use();
-+ return 0;
-+ }
-+ } else if (strcmp(argv[i], "-v") == 0) {
-+ ++i;
-+ if (i < argc) {
-+
-+ user_size = atof(argv[i]);
-+ user_volume = user_size * user_size * user_size;
-+#ifdef P3D_COLLISION_CHECKING
-+ kcd_set_user_defined_small_volume(user_volume);
-+#endif
-+ ++i;
-+ } else {
-+ use();
-+ return 0;
-+ }
-+ } else if (strcmp(argv[i], "-vol") == 0) {
-+ ++i;
-+ if (i < argc) {
-+#ifdef P3D_COLLISION_CHECKING
-+ kcd_set_user_defined_small_volume(atof(argv[i]));
-+#endif
-+ ++i;
-+ } else {
-+ use();
-+ return 0;
-+ }
-+ } else if (strcmp(argv[i], "-tol") == 0) {
-+ ++i;
-+ if (i < argc) {
-+ user_obj_tol = atof(argv[i]) ;
-+ user_obj_tol_to_be_set = TRUE;
-+ ++i;
-+ } else {
-+ use();
-+ return 0;
-+ }
-+ } else if (strcmp(argv[i], "-dmax") == 0) {
-+ ++i;
-+ if (i < argc) {
-+ user_dmax = atof(argv[i]) ;
-+ user_dmax_to_be_set = TRUE;
-+ ++i;
-+ } else {
-+ use();
-+ return 0;
-+ }
-+ } else if (strcmp(argv[i], "-stat") == 0) {
-+ ++i;
-+#ifdef P3D_PLANNER
-+ enableStats();
-+#endif
-+ }
-+#if defined(HRI_COSTSPACE) && defined(QT_GL)
-+ else if (strcmp(argv[i], "-grid") == 0) {
-+ ++i;
-+ if ((i < argc)) {
-+ grid_path = argv[i];
-+ //printf("Grid is at : %s\n",gridPath.c_str());
-+ grid_set = TRUE;
-+ ++i;
-+ } else {
-+ use();
-+ return 0;
-+ }
-+ }
-+#endif
-+ else if (strcmp(argv[i], "-udp") == 0) {
-+ }
-+
-+#ifdef LIGHT_PLANNER
-+ else if (strcmp(argv[i], "-no-ccntrt") == 0) {
-+ printf("Deactivate ccntrt at startup\n");
-+ ccntrt_active = FALSE;
-+ ++i;
-+ }
-+ else if (strcmp(argv[i], "-test") == 0) {
-+ ++i;
-+ manip_test_run = TRUE;
-+ if (i < argc) {
-+ manip_test_id = atoi(argv[i]) ;
-+ ++i;
-+ } else {
-+ manip_test_run = FALSE;
-+ use();
-+ return 0;
-+ }
-+ }
-+#endif
-+
-+ else if (strcmp(argv[i], "-udp") == 0) {
-+ std::string serverIp(argv[i+1]);
-+ int port = 0;
-+ sscanf(argv[i+2], "%d", &port);
-+ globalUdpClient = new UdpClient(serverIp, port);
-+ i += 3;
-+ }
-+ else if (strcmp(argv[i], "-dlr") == 0) {
-+ strcpy(dlrReadFile, argv[i + 1]);
-+ strcpy(dlrSaveFile, argv[i + 2]);
-+ i += 3;
-+ } else if (strcmp(argv[i], "-c") == 0) {
-+ ++i;
-+ cout << "Coll mod " << argv[i] << endl;
-+ if (strcmp(argv[i], "vcollide") == 0) {
-+ col_mode_to_be_set = p3d_col_mode_v_collide;
-+ col_det_set = TRUE;
-+ ++i;
-+ }
-+#ifdef P3D_COLLISION_CHECKING
-+ else if (strcmp(argv[i], "kcd") == 0) {
-+ col_mode_to_be_set = p3d_col_mode_kcd;
-+ set_DO_KCD_GJK(TRUE);
-+ col_det_set = TRUE;
-+ ++i;
-+ }
-+#ifdef PQP
-+ else if (strcmp(argv[i], "pqp") == 0) {
-+ printf("Colmod pqp");
-+ col_mode_to_be_set= p3d_col_mode_pqp;
-+ col_det_set = TRUE;
-+ ++i;
-+ cout << "Coll mod PQP " << endl;
-+ }
-+#endif
-+#endif
-+ else if (strcmp(argv[i], "bio") == 0) {
-+ col_mode_to_be_set = p3d_col_mode_bio;
-+ col_det_set = TRUE;
-+ ++i;
-+ } else if (strcmp(argv[i], "kng") == 0) {
-+ col_mode_to_be_set = p3d_col_mode_kcd;
-+#ifdef P3D_COLLISION_CHECKING
-+ set_DO_KCD_GJK(FALSE);
-+#endif
-+ col_det_set = TRUE;
-+ ++i;
-+ } else if (strcmp(argv[i], "gjk") == 0) {
-+ col_mode_to_be_set = p3d_col_mode_gjk;
-+ col_det_set = TRUE;
-+ ++i;
-+ } else if (strcmp(argv[i], "none") == 0) {
-+ col_mode_to_be_set = p3d_col_mode_none;
-+ col_det_set = TRUE;
-+ ++i;
-+ } else {
-+ use();
-+ return 0;
-+ }
-+ } else {
-+ use();
-+ return 0;
-+ }
-+ }
-+
-+ if (!dir_set)
-+ strcpy(file_directory, "../../demo");
-+
-+ if (!seed_set)
-+ p3d_init_random();
-+
-+ if (!col_det_set){
-+ // modif Juan
-+
-+ //check that the HOME_MOVE3D environment variable exists:
-+ if(getenv("HOME_MOVE3D")==NULL) {
-+ printf("%s: %d: main(): The environment variable \"HOME_MOVE3D\" is not defined. This might cause some problems or crashes (e.g. with video capture).\n", __FILE__,__LINE__);
-+ }
-+
-+
-+#ifdef GRASP_PLANNING
-+ col_mode_to_be_set= p3d_col_mode_pqp;
-+#ifndef PQP
-+ printf("%s: %d: main(): GRASP_PLANNING must be compiled with PQP.\n", __FILE__,__LINE__);
-+ printf("Program must quit.\n");
-+ return 1;
-+#endif
-+#else
-+ col_mode_to_be_set = p3d_col_mode_kcd;
-+#endif
-+ }
-+#ifdef P3D_COLLISION_CHECKING
-+ if (col_mode_to_be_set != p3d_col_mode_v_collide)
-+ set_collision_by_object(FALSE);
-+ /* : carl */
-+ if (col_mode_to_be_set == p3d_col_mode_v_collide) {
-+ p3d_filter_switch_filter_mechanism(FILTER_TO_BE_SET_ACTIVE);
-+ }
-+ /* begin added KCD FILTER */
-+ else if (col_mode_to_be_set == p3d_col_mode_kcd) {
-+ p3d_filter_switch_filter_mechanism(FILTER_TO_BE_SET_ACTIVE);
-+ }
-+#endif
-+ /* end added KCD FILTER */
-+
-+
-+ /* lecture du fichier environnement */
-+ p3d_set_directory(file_directory);
-+
-+ // init English C
-+ if (! setlocale(LC_ALL, "C"))
-+ fprintf(stderr, "There was an error while setting the locale to \"C\"\n");
-+
-+
-+ while (!p3d_get_desc_number(P3D_ENV)) {
-+
-+#ifdef BIO
-+ if (file_set == TRUE) {
-+ // if (!filename) {
-+ // exit(0);
-+ // }
-+
-+#ifndef PQP
-+ p3d_col_set_mode(p3d_col_mode_none);
-+#else
-+ p3d_col_set_mode(col_mode_to_be_set);
-+#endif
-+ p3d_BB_set_mode_close();
-+
-+ if (!p3d_read_desc(filename))
-+ {
-+ file_set = FALSE;
-+ }
-+ }
-+ if (file_set == FALSE) {
-+
-+ printf("Error : give a p3d filename as argument, or use the XFORMS module.\n");
-+ exit(0);
-+ }
-+#else
-+
-+ if (file_set == TRUE) {
-+ file = filename;
-+ } else {
-+
-+ }
-+ if (!file) {
-+ exit(0);
-+ }
-+
-+#ifdef P3D_COLLISION_CHECKING
-+ p3d_col_set_mode(p3d_col_mode_none);
-+ p3d_BB_set_mode_close();
-+#endif
-+
-+ printf("\n");
-+ printf(" ----------------------------\n");
-+ printf(" -- p3d file parsing start --\n");
-+ printf(" ----------------------------\n");
-+ printf("\n");
-+
-+ p3d_read_desc((char *) file);
-+
-+#endif
-+ if (!p3d_get_desc_number(P3D_ENV)) {
-+ printf("loading done...\n");
-+ }
-+ }
-+
-+ printf("\n");
-+ printf(" ----------------------------\n");
-+ printf(" -- p3d file parsing done --\n");
-+ printf(" ----------------------------\n");
-+ printf("\n");
-+
-+ MY_ALLOC_INFO("After p3d_read_desc");
-+
-+ printf("Nb poly : %d\n", p3d_poly_get_nb());
-+
-+ /* for start-up with currently chosen collision detector: */
-+ /* MY_ALLOC_INFO("Before initialization of a collision detector"); */
-+#ifdef P3D_COLLISION_CHECKING
-+ p3d_col_set_mode(col_mode_to_be_set);
-+ p3d_col_start(col_mode_to_be_set);
-+#endif
-+
-+
-+#if defined( MOVE3D_CORE )
-+ if (ENV.getBool(Env::isCostSpace))
-+ {
-+ GlobalCostSpace::initialize();
-+ }
-+#endif
-+
-+ /* modif Pepijn april 2001
-+ * this changes have to be made after the initialistion of the collision checker
-+ * because in p3d_col_start KCD migth be initialised, and during this initialisation
-+ * there is automaticly calculated a dmax
-+ * So if the user wants to set his own dmax this most be done after this initialisation
-+ * INTERNAL NOTE: in this case the users are the developpers of MOVE3D, normally the clients
-+ * who purchase Move3d don't know that this option exists
-+ */
-+ if (user_dmax_to_be_set) {
-+ if (user_dmax < EPS4) {
-+ printf("WARNING: User chose dmax too small --> new value set to 0.0001 (EPS4)\n");
-+ user_dmax = EPS4;
-+ }
-+ p3d_set_env_dmax(user_dmax);
-+ }
-+ if (user_obj_tol_to_be_set) {
-+ if (user_obj_tol < 0.0) {
-+ printf("WARNING: Negative tolerance, tolerance is set to 0.0\n");
-+ user_obj_tol = 0.0;
-+ }
-+ p3d_set_env_object_tolerance(user_obj_tol);
-+ }
-+
-+ printf("Env dmax = %f\n",p3d_get_env_dmax());
-+ printf("Env Object tol = %f\n",p3d_get_env_object_tolerance());
-+ /* always set tolerance even if the user didn't specify any options
-+ * it's possible that Kcd has calculated automaticly a dmax
-+ */
-+ /* Normally p3d_col_set_tolerance(); is called when initialising
-+ * the sliders, in case the sliders are not used we have to use
-+ * p3d_col_set_tolerance()
-+ */
-+
-+ //printf("MAX_DDLS %d\n", MAX_DDLS);
-+
-+ // modif Juan
-+#ifdef BIO
-+ if (col_mode_to_be_set == p3d_col_mode_bio) {
-+ bio_set_num_subrobot_AA();
-+ bio_set_num_subrobot_ligand();
-+ bio_set_bio_jnt_types();
-+ bio_set_bio_jnt_AAnumbers();
-+ bio_set_list_AA_first_jnt();
-+ bio_set_AAtotal_number();
-+ bio_set_nb_flexible_sc();
-+ bio_set_list_firstjnts_flexible_sc();
-+ if (XYZ_ROBOT->num_subrobot_ligand != -1)
-+ bio_set_nb_dof_ligand();
-+#ifdef BIO_BALL
-+ new BallEnergy(XYZ_ENV);
-+ if(global_costSpace)
-+ {
-+ global_costSpace->addCost("BALLmmff94",
-+ boost::bind(&BallEnergy::computeEnergy, XYZ_ENV->energyComputer, _1));
-+ global_costSpace->setCost("BALLmmff94");
-+ }
-+#endif
-+ }
-+#endif
-+ // fmodif Juan
-+
-+#if defined(LIGHT_PLANNER) && defined(FK_CNTRT)
-+ for(int i=0; i<XYZ_ENV->nr; i++)
-+ { p3d_create_FK_cntrts(XYZ_ENV->robot[i]); }
-+#endif
-+
-+#ifdef P3D_CONSTRAINTS
-+ // Modif Mokhtar Initialisation For Multisolutions constraints
-+ p3d_init_iksol(XYZ_ROBOT->cntrt_manager);
-+#endif
-+
-+ /* creation du FORM main */
-+
-+ /*
-+ * needs to be run after main form has been created
-+ */
-+ if (scenario_set == TRUE)
-+ {
-+ p3d_rw_scenario_init_name();
-+ p3d_read_scenario(scenario);
-+ }
-+
-+ //Set the robots to initial Pos if defined
-+ for(i = 0; i < XYZ_ENV->nr; i++){
-+ if(!p3d_isNullConfig(XYZ_ENV->robot[i], XYZ_ENV->robot[i]->ROBOT_POS)){
-+ p3d_set_and_update_this_robot_conf(XYZ_ENV->robot[i], XYZ_ENV->robot[i]->ROBOT_POS);
-+ }
-+ }
-+ //Exection Of Dlr Planner
-+ // do{
-+ // DlrPlanner* planner = new DlrPlanner(dlrSaveFile);
-+ // DlrParser parser(dlrReadFile, planner);
-+ // if(parser.parse()){
-+ // planner->process();
-+ // }else{
-+ // sleep(2);
-+ // }
-+ // free(planner);
-+ // }while(1);
-+ /* go into loop */
-+
-+#if defined(LIGHT_PLANNER) && defined(PQP)
-+ if(ENV.getBool(Env::startWithFKCntrt))
-+ {
-+ p3d_rob* MyRobot = p3d_get_robot_by_name_containing ( ( char* ) "ROBOT" );
-+ deactivateCcCntrts(MyRobot,-1);
-+ }
-+#endif
-+
-+#if defined(HRI_COSTSPACE) && defined(QT_GL)
-+ if(grid_set)
-+ {
-+ std::cout << "Reading HRICS Grid " << grid_path << std::endl;
-+ qt_load_HRICS_Grid(grid_path);
-+ }
-+#endif
-+
-+ // double c, color[4];
-+ // srand(time(NULL));
-+ // c= rand()/((double)RAND_MAX+1);
-+ // g3d_rgb_from_hue(c, color);
-+ // g3d_set_win_floor_color(g3d_get_cur_win(), color[0], color[1], color[2]);
-+
-+#if defined( LIGHT_PLANNER ) && !defined( QT_GL )
-+ if (manip_test_run)
-+ {
-+ printf("Test functions : ManipulationTestFunctions\n");
-+ new qtG3DWindow();
-+
-+#if defined( MOVE3D_CORE )
-+ // Creates the wrapper to the project
-+ // Be carefull to initialize in the right thread
-+ global_Project = new Project(new Scene(XYZ_ENV));
-+#endif
-+
-+ ManipulationTestFunctions tests;
-+
-+ if(!tests.runTest(manip_test_id))
-+ {
-+ std::cout << "ManipulationTestFunctions::Fail" << std::endl;
-+ }
-+ }
-+#endif
-+ return 0;
-+}
-+
-+/* fonction de rappel des formats des arguments */
-+static void use(void) {
-+
-+ printf("main : ERROR : wrong arguments !\n move3d -d data_directory -f file -sc scenario -s random_seed -c collision_detector -tol tolerance -dmax dist [-v size | -vol volume] -o -x -nkcdd\n");
-+ printf("collision_detector may be: icollide, vcollide, solid, kcd, kng, none\n");
-+ printf("(kng == kcd without gjk)\n");
-+ printf("-o: consider polyhedrons as objects (only for vcollide)\n");
-+ printf("-x: do filtering by robot body (default: don't filter)\n");
-+ printf("-v: (only with kcd) minimal relevant size (in 1 dimension) \n");
-+ printf("-dmax: for Vcollide & nkcdd => maximum penetration distance \n");
-+ printf(" KCD => artificial enlargement of the obstacles \n");
-+ printf("-tol: (only with kcd) object tolerance (enlarge objects for the collision checker) \n");
-+ printf("-vol: (only with kcd) minimal relevant size (volume) \n");
-+ printf("(internal note, kcd: volume of a box around a facet == (surface of box)^(3/2) )\n");
-+ printf("-nkcdd: (only with -c kcd) return only boolean reply, don't use distance \n");
-+ printf("-no-ccntrt: deactivate closed chain constraint at startup \n");
-+}
-+
---- src/move3d-remote/CMakeLists.txt
-+++ src/move3d-remote/CMakeLists.txt
-@@ -222,7 +222,7 @@ ENDMACRO(SMP_QT_GENERATE_UI_HEADERS)
- # -------------------------------------------------------------
- file(GLOB source_files
- main-remote.cpp
--move3d.c
-+move3d.cpp
- sparkwidget.cpp
- niutwidget.cpp
- camerawidget.cpp
-@@ -295,8 +295,6 @@ QT4_WRAP_CPP(SMP_QT_MOC ${SMP_QT_FILES_FOR_MOC})
- # -------------------------------------------------------------
- # --- Compiler and Compilation flags --------------------------
- # -------------------------------------------------------------
--SET(CMAKE_CXX_COMPILER g++)
--SET(CMAKE_C_COMPILER g++)
-
- IF(UNIX)
- ADD_DEFINITIONS(-DUNIX)
---- src/move3d-remote/move3d.c
-+++ /dev/null
-@@ -1,643 +0,0 @@
--#include <Util-pkg.h>
--#include <P3d-pkg.h>
--
--#include <move3d-headless.h>
--
--#ifdef P3D_PLANNER
--#include <Planner-pkg.h>
--#endif
--#ifdef P3D_COLLISION_CHECKING
--#include <Collision-pkg.h>
--#endif
--
--#include <Bio-pkg.h>
--#include <UdpClient.h>
--
--#include <locale.h>
--
--// TODO
--#undef USE_GLUT
--#if defined(USE_GLUT)
--#if defined(MACOSX)
--#include <glut.h>
--#else
--#include <GL/freeglut.h>
--#endif
--#endif
--
--#if defined( QT_GL ) && defined( CXX_PLANNER )
--#include <project.hpp>
--#include <cppToQt.hpp>
--#endif
--
--#include "planner_handler.hpp"
--
--#ifdef LIGHT_PLANNER
--#include "../lightPlanner/proto/DlrPlanner.h"
--#include "../lightPlanner/proto/DlrParser.h"
--#include "../lightPlanner/proto/lightPlannerApi.h"
--
--#if defined( MULTILOCALPATH ) && defined( GRASP_PLANNING )
--#include "../lightPlanner/proto/ManipulationTestFunctions.hpp"
--#endif
--#endif
--
--
--
--#include "../graphic/proto/g3d_glut.hpp"
--
--#if defined( CXX_PLANNER ) || defined( MOVE3D_CORE )
--#include "API/planningAPI.hpp"
--#include "planner/cost_space.hpp"
--#include <boost/bind.hpp>
--#endif
--#ifdef BIO_BALL
--#include "ball_energy.hpp"
--#endif
--
--#include <iostream>
--using namespace std;
--
--// External varialbe handled when compiled with move3d studio
--bool move3d_studio_load_settings = false;
--
--static int FILTER_TO_BE_SET_ACTIVE = FALSE;
--//extern void g3d_create_main_form(void);
--//extern void g3d_loop(void);
--//extern void p3d_col_set_mode(int);
--//extern void kcd_set_user_defined_small_volume(double);
--//extern double p3d_get_env_dmax(void);
--static void use(void);
--
--int mainMhp(int argc, char ** argv) {
--
-- cout << "Main Mhp" << endl;
--
-- //int main(int argc, char ** argv) {
--
--
-- // modif Pepijn apropos dmax and tol
-- int user_dmax_to_be_set = FALSE; /* Modif. Pepijn on dmax */
-- double user_dmax = 0.0; /* Modif. Pepijn on dmax */
-- int user_obj_tol_to_be_set = FALSE; /* Modif. Carl on tolerance */
-- double user_obj_tol = 0.0;
-- // end modif Pepijn
--
-- double user_volume, user_size; /* Modif. Carl on volume */
--#ifndef BIO
-- const char *file;
--#endif
-- char file_directory[200];
-- char filename[200];
-- char scenario[200];
-- char dlrReadFile[200];
-- char dlrSaveFile[200];
-- int i = 0;
-- /* carl: */
-- int seed_set = FALSE;
-- int dir_set = FALSE;
-- int file_set = FALSE;
-- int scenario_set = FALSE;
-- int col_det_set = FALSE;
-- int col_mode_to_be_set = p3d_col_mode_kcd; /* Nic p3d_col_mode_v_collide;*/
-- int ccntrt_active = TRUE;
--
-- // Tests
-- int manip_test_run = FALSE;
-- int manip_test_id = 0;
--
-- // init English C
-- if (! setlocale(LC_ALL, "C"))
-- fprintf(stderr, "There was an error while setting the locale to \"C\"\n");
--
-- int grid_set = FALSE;
-- std::string grid_path;
-- // modif Brice SALVA
--
--#ifdef BIO
-- int usrAnswer = FALSE;
-- file_name_list* file_list = NULL;
-- char name[PSF_MAX_NAME_LENGTH];
-- int is_p3d = FALSE;
--#endif
-- if (! setlocale(LC_ALL, "C")) fprintf(stderr, "There was an error while setting the locale to \"C\"\n");
--
-- // End modif Brice SALVA
--
-- /* lecture des arguments */
-- /* carl: */
-- i = 1;
-- while (i < argc) {
-- if (strcmp(argv[i], "-debug") == 0) {
-- basic_alloc_debugon();
-- i++;
-- } else if (strcmp(argv[i], "-d") == 0) {
-- ++i;
-- if ((i < argc)) {
-- strcpy(file_directory, argv[i]);
-- dir_set = TRUE;
-- ++i;
-- } else {
-- use();
-- return 0;
-- }
-- } else if (strcmp(argv[i], "-f") == 0) {
-- ++i;
-- if ((i < argc)) {
-- strcpy(filename, argv[i]);
-- file_set = TRUE;
-- ++i;
-- } else {
-- use();
-- return 0;
-- }
-- } else if (strcmp(argv[i], "-sc") == 0) {
-- ++i;
-- if ((i < argc)) {
-- strcpy(scenario, argv[i]);
-- scenario_set = TRUE;
-- ++i;
-- } else {
-- use();
-- return 0;
-- }
-- }
-- else if (strcmp(argv[i], "-setgui") == 0) {
-- ++i;
-- move3d_studio_load_settings = true;
-- }
--#ifdef P3D_COLLISION_CHECKING
-- else if (strcmp(argv[i], "-o") == 0) {
-- set_collision_by_object(FALSE);
-- ++i;
-- } else if (strcmp(argv[i], "-x") == 0) {
-- FILTER_TO_BE_SET_ACTIVE = TRUE;
-- ++i;
-- } else if (strcmp(argv[i], "-nkcdd") == 0) {
-- set_return_kcd_distance_estimate(FALSE);
-- ++i;
-- }
--#endif
-- else if (strcmp(argv[i], "-s") == 0) {
-- ++i;
-- if ((i < argc)) {
-- p3d_init_random_seed(atoi(argv[i]));
-- seed_set = TRUE;
-- ++i;
-- } else {
-- use();
-- return 0;
-- }
-- } else if (strcmp(argv[i], "-v") == 0) {
-- ++i;
-- if (i < argc) {
--
-- user_size = atof(argv[i]);
-- user_volume = user_size * user_size * user_size;
--#ifdef P3D_COLLISION_CHECKING
-- kcd_set_user_defined_small_volume(user_volume);
--#endif
-- ++i;
-- } else {
-- use();
-- return 0;
-- }
-- } else if (strcmp(argv[i], "-vol") == 0) {
-- ++i;
-- if (i < argc) {
--#ifdef P3D_COLLISION_CHECKING
-- kcd_set_user_defined_small_volume(atof(argv[i]));
--#endif
-- ++i;
-- } else {
-- use();
-- return 0;
-- }
-- } else if (strcmp(argv[i], "-tol") == 0) {
-- ++i;
-- if (i < argc) {
-- user_obj_tol = atof(argv[i]) ;
-- user_obj_tol_to_be_set = TRUE;
-- ++i;
-- } else {
-- use();
-- return 0;
-- }
-- } else if (strcmp(argv[i], "-dmax") == 0) {
-- ++i;
-- if (i < argc) {
-- user_dmax = atof(argv[i]) ;
-- user_dmax_to_be_set = TRUE;
-- ++i;
-- } else {
-- use();
-- return 0;
-- }
-- } else if (strcmp(argv[i], "-stat") == 0) {
-- ++i;
--#ifdef P3D_PLANNER
-- enableStats();
--#endif
-- }
--#if defined(HRI_COSTSPACE) && defined(QT_GL)
-- else if (strcmp(argv[i], "-grid") == 0) {
-- ++i;
-- if ((i < argc)) {
-- grid_path = argv[i];
-- //printf("Grid is at : %s\n",gridPath.c_str());
-- grid_set = TRUE;
-- ++i;
-- } else {
-- use();
-- return 0;
-- }
-- }
--#endif
-- else if (strcmp(argv[i], "-udp") == 0) {
-- }
--
--#ifdef LIGHT_PLANNER
-- else if (strcmp(argv[i], "-no-ccntrt") == 0) {
-- printf("Deactivate ccntrt at startup\n");
-- ccntrt_active = FALSE;
-- ++i;
-- }
-- else if (strcmp(argv[i], "-test") == 0) {
-- ++i;
-- manip_test_run = TRUE;
-- if (i < argc) {
-- manip_test_id = atoi(argv[i]) ;
-- ++i;
-- } else {
-- manip_test_run = FALSE;
-- use();
-- return 0;
-- }
-- }
--#endif
--
-- else if (strcmp(argv[i], "-udp") == 0) {
-- std::string serverIp(argv[i+1]);
-- int port = 0;
-- sscanf(argv[i+2], "%d", &port);
-- globalUdpClient = new UdpClient(serverIp, port);
-- i += 3;
-- }
-- else if (strcmp(argv[i], "-dlr") == 0) {
-- strcpy(dlrReadFile, argv[i + 1]);
-- strcpy(dlrSaveFile, argv[i + 2]);
-- i += 3;
-- } else if (strcmp(argv[i], "-c") == 0) {
-- ++i;
-- cout << "Coll mod " << argv[i] << endl;
-- if (strcmp(argv[i], "vcollide") == 0) {
-- col_mode_to_be_set = p3d_col_mode_v_collide;
-- col_det_set = TRUE;
-- ++i;
-- }
--#ifdef P3D_COLLISION_CHECKING
-- else if (strcmp(argv[i], "kcd") == 0) {
-- col_mode_to_be_set = p3d_col_mode_kcd;
-- set_DO_KCD_GJK(TRUE);
-- col_det_set = TRUE;
-- ++i;
-- }
--#ifdef PQP
-- else if (strcmp(argv[i], "pqp") == 0) {
-- printf("Colmod pqp");
-- col_mode_to_be_set= p3d_col_mode_pqp;
-- col_det_set = TRUE;
-- ++i;
-- cout << "Coll mod PQP " << endl;
-- }
--#endif
--#endif
--
-- else if (strcmp(argv[i], "bio") == 0) {
-- col_mode_to_be_set = p3d_col_mode_bio;
-- col_det_set = TRUE;
-- ++i;
-- } else if (strcmp(argv[i], "kng") == 0) {
-- col_mode_to_be_set = p3d_col_mode_kcd;
--#ifdef P3D_COLLISION_CHECKING
-- set_DO_KCD_GJK(FALSE);
--#endif
-- col_det_set = TRUE;
-- ++i;
-- } else if (strcmp(argv[i], "gjk") == 0) {
-- col_mode_to_be_set = p3d_col_mode_gjk;
-- col_det_set = TRUE;
-- ++i;
-- } else if (strcmp(argv[i], "none") == 0) {
-- col_mode_to_be_set = p3d_col_mode_none;
-- col_det_set = TRUE;
-- ++i;
-- } else {
-- use();
-- return 0;
-- }
-- } else {
-- use();
-- return 0;
-- }
-- }
--
-- if (!dir_set)
-- strcpy(file_directory, "../../demo");
--
-- if (!seed_set)
-- p3d_init_random();
--
-- if (!col_det_set){
-- // modif Juan
--
-- //check that the HOME_MOVE3D environment variable exists:
-- if(getenv("HOME_MOVE3D")==NULL) {
-- printf("%s: %d: main(): The environment variable \"HOME_MOVE3D\" is not defined. This might cause some problems or crashes (e.g. with video capture).\n", __FILE__,__LINE__);
-- }
--
--
--#ifdef GRASP_PLANNING
-- col_mode_to_be_set= p3d_col_mode_pqp;
--#ifndef PQP
-- printf("%s: %d: main(): GRASP_PLANNING must be compiled with PQP.\n", __FILE__,__LINE__);
-- printf("Program must quit.\n");
-- return 1;
--#endif
--#else
-- col_mode_to_be_set = p3d_col_mode_kcd;
--#endif
-- }
--#ifdef P3D_COLLISION_CHECKING
-- if (col_mode_to_be_set != p3d_col_mode_v_collide)
-- set_collision_by_object(FALSE);
-- /* : carl */
-- if (col_mode_to_be_set == p3d_col_mode_v_collide) {
-- p3d_filter_switch_filter_mechanism(FILTER_TO_BE_SET_ACTIVE);
-- }
-- /* begin added KCD FILTER */
-- else if (col_mode_to_be_set == p3d_col_mode_kcd) {
-- p3d_filter_switch_filter_mechanism(FILTER_TO_BE_SET_ACTIVE);
-- }
--#endif
-- /* end added KCD FILTER */
--
--
-- /* lecture du fichier environnement */
-- p3d_set_directory(file_directory);
--
-- // init English C
-- if (! setlocale(LC_ALL, "C"))
-- fprintf(stderr, "There was an error while setting the locale to \"C\"\n");
--
--
-- while (!p3d_get_desc_number(P3D_ENV)) {
--
--#ifdef BIO
-- if (file_set == TRUE) {
-- // if (!filename) {
-- // exit(0);
-- // }
--
--#ifndef PQP
-- p3d_col_set_mode(p3d_col_mode_none);
--#else
-- p3d_col_set_mode(col_mode_to_be_set);
--#endif
-- p3d_BB_set_mode_close();
-- if (!p3d_read_desc(filename)) {
--
-- file_set = FALSE;
--
-- }
-- }
-- if (file_set == FALSE) {
--
-- printf("Error : give a p3d filename as argument, or use the XFORMS module.\n");
-- exit(0);
--
-- }
--#else
--
-- if (file_set == TRUE) {
-- file = filename;
-- } else {
--
-- }
-- if (!file) {
-- exit(0);
-- }
--
--#ifdef P3D_COLLISION_CHECKING
-- p3d_col_set_mode(p3d_col_mode_none);
-- p3d_BB_set_mode_close();
--#endif
--
-- printf("\n");
-- printf(" ----------------------------\n");
-- printf(" -- p3d file parsing start --\n");
-- printf(" ----------------------------\n");
-- printf("\n");
--
-- p3d_read_desc((char *) file);
--
--#endif
-- if (!p3d_get_desc_number(P3D_ENV)) {
-- printf("loading done...\n");
--
-- }
-- }
--
-- printf("\n");
-- printf(" ----------------------------\n");
-- printf(" -- p3d file parsing done --\n");
-- printf(" ----------------------------\n");
-- printf("\n");
--
-- MY_ALLOC_INFO("After p3d_read_desc");
--
-- printf("Nb poly : %d\n", p3d_poly_get_nb());
--
-- /* for start-up with currently chosen collision detector: */
-- /* MY_ALLOC_INFO("Before initialization of a collision detector"); */
--#ifdef P3D_COLLISION_CHECKING
-- p3d_col_set_mode(col_mode_to_be_set);
-- p3d_col_start(col_mode_to_be_set);
--#endif
--
--
--#if defined( MOVE3D_CORE )
-- if (ENV.getBool(Env::isCostSpace))
-- {
-- GlobalCostSpace::initialize();
-- }
--#endif
--
-- /* modif Pepijn april 2001
-- * this changes have to be made after the initialistion of the collision checker
-- * because in p3d_col_start KCD migth be initialised, and during this initialisation
-- * there is automaticly calculated a dmax
-- * So if the user wants to set his own dmax this most be done after this initialisation
-- * INTERNAL NOTE: in this case the users are the developpers of MOVE3D, normally the clients
-- * who purchase Move3d don't know that this option exists
-- */
-- if (user_dmax_to_be_set) {
-- if (user_dmax < EPS4) {
-- printf("WARNING: User chose dmax too small --> new value set to 0.0001 (EPS4)\n");
-- user_dmax = EPS4;
-- }
-- p3d_set_env_dmax(user_dmax);
-- }
-- if (user_obj_tol_to_be_set) {
-- if (user_obj_tol < 0.0) {
-- printf("WARNING: Negative tolerance, tolerance is set to 0.0\n");
-- user_obj_tol = 0.0;
-- }
-- p3d_set_env_object_tolerance(user_obj_tol);
-- }
--
-- printf("Env dmax = %f\n",p3d_get_env_dmax());
-- printf("Env Object tol = %f\n",p3d_get_env_object_tolerance());
-- /* always set tolerance even if the user didn't specify any options
-- * it's possible that Kcd has calculated automaticly a dmax
-- */
-- /* Normally p3d_col_set_tolerance(); is called when initialising
-- * the sliders, in case the sliders are not used we have to use
-- * p3d_col_set_tolerance()
-- */
--
-- //printf("MAX_DDLS %d\n", MAX_DDLS);
--
-- // modif Juan
--#ifdef BIO
-- if (col_mode_to_be_set == p3d_col_mode_bio) {
-- bio_set_num_subrobot_AA();
-- bio_set_num_subrobot_ligand();
-- bio_set_bio_jnt_types();
-- bio_set_bio_jnt_AAnumbers();
-- bio_set_list_AA_first_jnt();
-- bio_set_AAtotal_number();
-- bio_set_nb_flexible_sc();
-- bio_set_list_firstjnts_flexible_sc();
-- if (XYZ_ROBOT->num_subrobot_ligand != -1)
-- bio_set_nb_dof_ligand();
--#ifdef BIO_BALL
-- new BallEnergy(XYZ_ENV);
-- if(global_costSpace)
-- {
-- global_costSpace->addCost("BALLmmff94",
-- boost::bind(&BallEnergy::computeEnergy, XYZ_ENV->energyComputer, _1));
-- global_costSpace->setCost("BALLmmff94");
-- }
--#endif
-- }
--#endif
-- // fmodif Juan
--
--#if defined(LIGHT_PLANNER) && defined(FK_CNTRT)
-- for(int i=0; i<XYZ_ENV->nr; i++)
-- { p3d_create_FK_cntrts(XYZ_ENV->robot[i]); }
--#endif
--
--#ifdef P3D_CONSTRAINTS
-- // Modif Mokhtar Initialisation For Multisolutions constraints
-- p3d_init_iksol(XYZ_ROBOT->cntrt_manager);
--#endif
--
-- /* creation du FORM main */
--
-- /*
-- * needs to be run after main form has been created
-- */
-- if (scenario_set == TRUE) {
--
-- p3d_rw_scenario_init_name();
-- p3d_read_scenario(scenario);
--
-- }
--
-- //Set the robots to initial Pos if defined
-- for(i = 0; i < XYZ_ENV->nr; i++){
-- if(!p3d_isNullConfig(XYZ_ENV->robot[i], XYZ_ENV->robot[i]->ROBOT_POS)){
-- p3d_set_and_update_this_robot_conf(XYZ_ENV->robot[i], XYZ_ENV->robot[i]->ROBOT_POS);
-- }
-- }
-- //Exection Of Dlr Planner
-- // do{
-- // DlrPlanner* planner = new DlrPlanner(dlrSaveFile);
-- // DlrParser parser(dlrReadFile, planner);
-- // if(parser.parse()){
-- // planner->process();
-- // }else{
-- // sleep(2);
-- // }
-- // free(planner);
-- // }while(1);
-- /* go into loop */
--
--#if defined(LIGHT_PLANNER) && defined(PQP)
-- if(ENV.getBool(Env::startWithFKCntrt))
-- {
-- p3d_rob* MyRobot = p3d_get_robot_by_name_containing ( ( char* ) "ROBOT" );
-- deactivateCcCntrts(MyRobot,-1);
-- }
--#endif
--
--#if defined(HRI_COSTSPACE) && defined(QT_GL)
-- if(grid_set)
-- {
-- std::cout << "Reading HRICS Grid " << grid_path << std::endl;
-- qt_load_HRICS_Grid(grid_path);
-- }
--#endif
--//#ifdef QT_GL
--// sem->release();
--//#endif
--
-- // double c, color[4];
-- // srand(time(NULL));
-- // c= rand()/((double)RAND_MAX+1);
-- // g3d_rgb_from_hue(c, color);
-- // g3d_set_win_floor_color(g3d_get_cur_win(), color[0], color[1], color[2]);
--
--#if defined( LIGHT_PLANNER ) && !defined( QT_GL )
-- if (manip_test_run)
-- {
-- printf("Test functions : ManipulationTestFunctions\n");
-- new qtG3DWindow();
--
--#if defined( MOVE3D_CORE )
-- // Creates the wrapper to the project
-- // Be carefull to initialize in the right thread
-- global_Project = new Project(new Scene(XYZ_ENV));
--#endif
--
-- ManipulationTestFunctions tests;
--
-- if(!tests.runTest(manip_test_id))
-- {
-- std::cout << "ManipulationTestFunctions::Fail" << std::endl;
-- }
-- }
--#endif
-- return 0;
--}
--
--/* fonction de rappel des formats des arguments */
--static void use(void) {
--
-- printf("main : ERROR : wrong arguments !\n move3d -d data_directory -f file -sc scenario -s random_seed -c collision_detector -tol tolerance -dmax dist [-v size | -vol volume] -o -x -nkcdd\n");
-- printf("collision_detector may be: icollide, vcollide, solid, kcd, kng, none\n");
-- printf("(kng == kcd without gjk)\n");
-- printf("-o: consider polyhedrons as objects (only for vcollide)\n");
-- printf("-x: do filtering by robot body (default: don't filter)\n");
-- printf("-v: (only with kcd) minimal relevant size (in 1 dimension) \n");
-- printf("-dmax: for Vcollide & nkcdd => maximum penetration distance \n");
-- printf(" KCD => artificial enlargement of the obstacles \n");
-- printf("-tol: (only with kcd) object tolerance (enlarge objects for the collision checker) \n");
-- printf("-vol: (only with kcd) minimal relevant size (volume) \n");
-- printf("(internal note, kcd: volume of a box around a facet == (surface of box)^(3/2) )\n");
-- printf("-nkcdd: (only with -c kcd) return only boolean reply, don't use distance \n");
-- printf("-no-ccntrt: deactivate closed chain constraint at startup \n");
--}
--
---- /dev/null
-+++ src/move3d-remote/move3d.cpp
-@@ -0,0 +1,643 @@
-+#include <Util-pkg.h>
-+#include <P3d-pkg.h>
-+
-+#include <move3d-headless.h>
-+
-+#ifdef P3D_PLANNER
-+#include <Planner-pkg.h>
-+#endif
-+#ifdef P3D_COLLISION_CHECKING
-+#include <Collision-pkg.h>
-+#endif
-+
-+#include <Bio-pkg.h>
-+#include <UdpClient.h>
-+
-+#include <locale.h>
-+
-+// TODO
-+#undef USE_GLUT
-+#if defined(USE_GLUT)
-+#if defined(MACOSX)
-+#include <glut.h>
-+#else
-+#include <GL/freeglut.h>
-+#endif
-+#endif
-+
-+#if defined( QT_GL ) && defined( CXX_PLANNER )
-+#include <project.hpp>
-+#include <cppToQt.hpp>
-+#endif
-+
-+#include "planner_handler.hpp"
-+
-+#ifdef LIGHT_PLANNER
-+#include "../lightPlanner/proto/DlrPlanner.h"
-+#include "../lightPlanner/proto/DlrParser.h"
-+#include "../lightPlanner/proto/lightPlannerApi.h"
-+
-+#if defined( MULTILOCALPATH ) && defined( GRASP_PLANNING )
-+#include "../lightPlanner/proto/ManipulationTestFunctions.hpp"
-+#endif
-+#endif
-+
-+
-+
-+#include "../graphic/proto/g3d_glut.hpp"
-+
-+#if defined( CXX_PLANNER ) || defined( MOVE3D_CORE )
-+#include "API/planningAPI.hpp"
-+#include "planner/cost_space.hpp"
-+#include <boost/bind.hpp>
-+#endif
-+#ifdef BIO_BALL
-+#include "ball_energy.hpp"
-+#endif
-+
-+#include <iostream>
-+using namespace std;
-+
-+// External varialbe handled when compiled with move3d studio
-+bool move3d_studio_load_settings = false;
-+
-+static int FILTER_TO_BE_SET_ACTIVE = FALSE;
-+//extern void g3d_create_main_form(void);
-+//extern void g3d_loop(void);
-+//extern void p3d_col_set_mode(int);
-+//extern void kcd_set_user_defined_small_volume(double);
-+//extern double p3d_get_env_dmax(void);
-+static void use(void);
-+
-+int mainMhp(int argc, char ** argv) {
-+
-+ cout << "Main Mhp" << endl;
-+
-+ //int main(int argc, char ** argv) {
-+
-+
-+ // modif Pepijn apropos dmax and tol
-+ int user_dmax_to_be_set = FALSE; /* Modif. Pepijn on dmax */
-+ double user_dmax = 0.0; /* Modif. Pepijn on dmax */
-+ int user_obj_tol_to_be_set = FALSE; /* Modif. Carl on tolerance */
-+ double user_obj_tol = 0.0;
-+ // end modif Pepijn
-+
-+ double user_volume, user_size; /* Modif. Carl on volume */
-+#ifndef BIO
-+ const char *file;
-+#endif
-+ char file_directory[200];
-+ char filename[200];
-+ char scenario[200];
-+ char dlrReadFile[200];
-+ char dlrSaveFile[200];
-+ int i = 0;
-+ /* carl: */
-+ int seed_set = FALSE;
-+ int dir_set = FALSE;
-+ int file_set = FALSE;
-+ int scenario_set = FALSE;
-+ int col_det_set = FALSE;
-+ int col_mode_to_be_set = p3d_col_mode_kcd; /* Nic p3d_col_mode_v_collide;*/
-+ int ccntrt_active = TRUE;
-+
-+ // Tests
-+ int manip_test_run = FALSE;
-+ int manip_test_id = 0;
-+
-+ // init English C
-+ if (! setlocale(LC_ALL, "C"))
-+ fprintf(stderr, "There was an error while setting the locale to \"C\"\n");
-+
-+ int grid_set = FALSE;
-+ std::string grid_path;
-+ // modif Brice SALVA
-+
-+#ifdef BIO
-+ int usrAnswer = FALSE;
-+ file_name_list* file_list = NULL;
-+ char name[PSF_MAX_NAME_LENGTH];
-+ int is_p3d = FALSE;
-+#endif
-+ if (! setlocale(LC_ALL, "C")) fprintf(stderr, "There was an error while setting the locale to \"C\"\n");
-+
-+ // End modif Brice SALVA
-+
-+ /* lecture des arguments */
-+ /* carl: */
-+ i = 1;
-+ while (i < argc) {
-+ if (strcmp(argv[i], "-debug") == 0) {
-+ basic_alloc_debugon();
-+ i++;
-+ } else if (strcmp(argv[i], "-d") == 0) {
-+ ++i;
-+ if ((i < argc)) {
-+ strcpy(file_directory, argv[i]);
-+ dir_set = TRUE;
-+ ++i;
-+ } else {
-+ use();
-+ return 0;
-+ }
-+ } else if (strcmp(argv[i], "-f") == 0) {
-+ ++i;
-+ if ((i < argc)) {
-+ strcpy(filename, argv[i]);
-+ file_set = TRUE;
-+ ++i;
-+ } else {
-+ use();
-+ return 0;
-+ }
-+ } else if (strcmp(argv[i], "-sc") == 0) {
-+ ++i;
-+ if ((i < argc)) {
-+ strcpy(scenario, argv[i]);
-+ scenario_set = TRUE;
-+ ++i;
-+ } else {
-+ use();
-+ return 0;
-+ }
-+ }
-+ else if (strcmp(argv[i], "-setgui") == 0) {
-+ ++i;
-+ move3d_studio_load_settings = true;
-+ }
-+#ifdef P3D_COLLISION_CHECKING
-+ else if (strcmp(argv[i], "-o") == 0) {
-+ set_collision_by_object(FALSE);
-+ ++i;
-+ } else if (strcmp(argv[i], "-x") == 0) {
-+ FILTER_TO_BE_SET_ACTIVE = TRUE;
-+ ++i;
-+ } else if (strcmp(argv[i], "-nkcdd") == 0) {
-+ set_return_kcd_distance_estimate(FALSE);
-+ ++i;
-+ }
-+#endif
-+ else if (strcmp(argv[i], "-s") == 0) {
-+ ++i;
-+ if ((i < argc)) {
-+ p3d_init_random_seed(atoi(argv[i]));
-+ seed_set = TRUE;
-+ ++i;
-+ } else {
-+ use();
-+ return 0;
-+ }
-+ } else if (strcmp(argv[i], "-v") == 0) {
-+ ++i;
-+ if (i < argc) {
-+
-+ user_size = atof(argv[i]);
-+ user_volume = user_size * user_size * user_size;
-+#ifdef P3D_COLLISION_CHECKING
-+ kcd_set_user_defined_small_volume(user_volume);
-+#endif
-+ ++i;
-+ } else {
-+ use();
-+ return 0;
-+ }
-+ } else if (strcmp(argv[i], "-vol") == 0) {
-+ ++i;
-+ if (i < argc) {
-+#ifdef P3D_COLLISION_CHECKING
-+ kcd_set_user_defined_small_volume(atof(argv[i]));
-+#endif
-+ ++i;
-+ } else {
-+ use();
-+ return 0;
-+ }
-+ } else if (strcmp(argv[i], "-tol") == 0) {
-+ ++i;
-+ if (i < argc) {
-+ user_obj_tol = atof(argv[i]) ;
-+ user_obj_tol_to_be_set = TRUE;
-+ ++i;
-+ } else {
-+ use();
-+ return 0;
-+ }
-+ } else if (strcmp(argv[i], "-dmax") == 0) {
-+ ++i;
-+ if (i < argc) {
-+ user_dmax = atof(argv[i]) ;
-+ user_dmax_to_be_set = TRUE;
-+ ++i;
-+ } else {
-+ use();
-+ return 0;
-+ }
-+ } else if (strcmp(argv[i], "-stat") == 0) {
-+ ++i;
-+#ifdef P3D_PLANNER
-+ enableStats();
-+#endif
-+ }
-+#if defined(HRI_COSTSPACE) && defined(QT_GL)
-+ else if (strcmp(argv[i], "-grid") == 0) {
-+ ++i;
-+ if ((i < argc)) {
-+ grid_path = argv[i];
-+ //printf("Grid is at : %s\n",gridPath.c_str());
-+ grid_set = TRUE;
-+ ++i;
-+ } else {
-+ use();
-+ return 0;
-+ }
-+ }
-+#endif
-+ else if (strcmp(argv[i], "-udp") == 0) {
-+ }
-+
-+#ifdef LIGHT_PLANNER
-+ else if (strcmp(argv[i], "-no-ccntrt") == 0) {
-+ printf("Deactivate ccntrt at startup\n");
-+ ccntrt_active = FALSE;
-+ ++i;
-+ }
-+ else if (strcmp(argv[i], "-test") == 0) {
-+ ++i;
-+ manip_test_run = TRUE;
-+ if (i < argc) {
-+ manip_test_id = atoi(argv[i]) ;
-+ ++i;
-+ } else {
-+ manip_test_run = FALSE;
-+ use();
-+ return 0;
-+ }
-+ }
-+#endif
-+
-+ else if (strcmp(argv[i], "-udp") == 0) {
-+ std::string serverIp(argv[i+1]);
-+ int port = 0;
-+ sscanf(argv[i+2], "%d", &port);
-+ globalUdpClient = new UdpClient(serverIp, port);
-+ i += 3;
-+ }
-+ else if (strcmp(argv[i], "-dlr") == 0) {
-+ strcpy(dlrReadFile, argv[i + 1]);
-+ strcpy(dlrSaveFile, argv[i + 2]);
-+ i += 3;
-+ } else if (strcmp(argv[i], "-c") == 0) {
-+ ++i;
-+ cout << "Coll mod " << argv[i] << endl;
-+ if (strcmp(argv[i], "vcollide") == 0) {
-+ col_mode_to_be_set = p3d_col_mode_v_collide;
-+ col_det_set = TRUE;
-+ ++i;
-+ }
-+#ifdef P3D_COLLISION_CHECKING
-+ else if (strcmp(argv[i], "kcd") == 0) {
-+ col_mode_to_be_set = p3d_col_mode_kcd;
-+ set_DO_KCD_GJK(TRUE);
-+ col_det_set = TRUE;
-+ ++i;
-+ }
-+#ifdef PQP
-+ else if (strcmp(argv[i], "pqp") == 0) {
-+ printf("Colmod pqp");
-+ col_mode_to_be_set= p3d_col_mode_pqp;
-+ col_det_set = TRUE;
-+ ++i;
-+ cout << "Coll mod PQP " << endl;
-+ }
-+#endif
-+#endif
-+
-+ else if (strcmp(argv[i], "bio") == 0) {
-+ col_mode_to_be_set = p3d_col_mode_bio;
-+ col_det_set = TRUE;
-+ ++i;
-+ } else if (strcmp(argv[i], "kng") == 0) {
-+ col_mode_to_be_set = p3d_col_mode_kcd;
-+#ifdef P3D_COLLISION_CHECKING
-+ set_DO_KCD_GJK(FALSE);
-+#endif
-+ col_det_set = TRUE;
-+ ++i;
-+ } else if (strcmp(argv[i], "gjk") == 0) {
-+ col_mode_to_be_set = p3d_col_mode_gjk;
-+ col_det_set = TRUE;
-+ ++i;
-+ } else if (strcmp(argv[i], "none") == 0) {
-+ col_mode_to_be_set = p3d_col_mode_none;
-+ col_det_set = TRUE;
-+ ++i;
-+ } else {
-+ use();
-+ return 0;
-+ }
-+ } else {
-+ use();
-+ return 0;
-+ }
-+ }
-+
-+ if (!dir_set)
-+ strcpy(file_directory, "../../demo");
-+
-+ if (!seed_set)
-+ p3d_init_random();
-+
-+ if (!col_det_set){
-+ // modif Juan
-+
-+ //check that the HOME_MOVE3D environment variable exists:
-+ if(getenv("HOME_MOVE3D")==NULL) {
-+ printf("%s: %d: main(): The environment variable \"HOME_MOVE3D\" is not defined. This might cause some problems or crashes (e.g. with video capture).\n", __FILE__,__LINE__);
-+ }
-+
-+
-+#ifdef GRASP_PLANNING
-+ col_mode_to_be_set= p3d_col_mode_pqp;
-+#ifndef PQP
-+ printf("%s: %d: main(): GRASP_PLANNING must be compiled with PQP.\n", __FILE__,__LINE__);
-+ printf("Program must quit.\n");
-+ return 1;
-+#endif
-+#else
-+ col_mode_to_be_set = p3d_col_mode_kcd;
-+#endif
-+ }
-+#ifdef P3D_COLLISION_CHECKING
-+ if (col_mode_to_be_set != p3d_col_mode_v_collide)
-+ set_collision_by_object(FALSE);
-+ /* : carl */
-+ if (col_mode_to_be_set == p3d_col_mode_v_collide) {
-+ p3d_filter_switch_filter_mechanism(FILTER_TO_BE_SET_ACTIVE);
-+ }
-+ /* begin added KCD FILTER */
-+ else if (col_mode_to_be_set == p3d_col_mode_kcd) {
-+ p3d_filter_switch_filter_mechanism(FILTER_TO_BE_SET_ACTIVE);
-+ }
-+#endif
-+ /* end added KCD FILTER */
-+
-+
-+ /* lecture du fichier environnement */
-+ p3d_set_directory(file_directory);
-+
-+ // init English C
-+ if (! setlocale(LC_ALL, "C"))
-+ fprintf(stderr, "There was an error while setting the locale to \"C\"\n");
-+
-+
-+ while (!p3d_get_desc_number(P3D_ENV)) {
-+
-+#ifdef BIO
-+ if (file_set == TRUE) {
-+ // if (!filename) {
-+ // exit(0);
-+ // }
-+
-+#ifndef PQP
-+ p3d_col_set_mode(p3d_col_mode_none);
-+#else
-+ p3d_col_set_mode(col_mode_to_be_set);
-+#endif
-+ p3d_BB_set_mode_close();
-+ if (!p3d_read_desc(filename)) {
-+
-+ file_set = FALSE;
-+
-+ }
-+ }
-+ if (file_set == FALSE) {
-+
-+ printf("Error : give a p3d filename as argument, or use the XFORMS module.\n");
-+ exit(0);
-+
-+ }
-+#else
-+
-+ if (file_set == TRUE) {
-+ file = filename;
-+ } else {
-+
-+ }
-+ if (!file) {
-+ exit(0);
-+ }
-+
-+#ifdef P3D_COLLISION_CHECKING
-+ p3d_col_set_mode(p3d_col_mode_none);
-+ p3d_BB_set_mode_close();
-+#endif
-+
-+ printf("\n");
-+ printf(" ----------------------------\n");
-+ printf(" -- p3d file parsing start --\n");
-+ printf(" ----------------------------\n");
-+ printf("\n");
-+
-+ p3d_read_desc((char *) file);
-+
-+#endif
-+ if (!p3d_get_desc_number(P3D_ENV)) {
-+ printf("loading done...\n");
-+
-+ }
-+ }
-+
-+ printf("\n");
-+ printf(" ----------------------------\n");
-+ printf(" -- p3d file parsing done --\n");
-+ printf(" ----------------------------\n");
-+ printf("\n");
-+
-+ MY_ALLOC_INFO("After p3d_read_desc");
-+
-+ printf("Nb poly : %d\n", p3d_poly_get_nb());
-+
-+ /* for start-up with currently chosen collision detector: */
-+ /* MY_ALLOC_INFO("Before initialization of a collision detector"); */
-+#ifdef P3D_COLLISION_CHECKING
-+ p3d_col_set_mode(col_mode_to_be_set);
-+ p3d_col_start(col_mode_to_be_set);
-+#endif
-+
-+
-+#if defined( MOVE3D_CORE )
-+ if (ENV.getBool(Env::isCostSpace))
-+ {
-+ GlobalCostSpace::initialize();
-+ }
-+#endif
-+
-+ /* modif Pepijn april 2001
-+ * this changes have to be made after the initialistion of the collision checker
-+ * because in p3d_col_start KCD migth be initialised, and during this initialisation
-+ * there is automaticly calculated a dmax
-+ * So if the user wants to set his own dmax this most be done after this initialisation
-+ * INTERNAL NOTE: in this case the users are the developpers of MOVE3D, normally the clients
-+ * who purchase Move3d don't know that this option exists
-+ */
-+ if (user_dmax_to_be_set) {
-+ if (user_dmax < EPS4) {
-+ printf("WARNING: User chose dmax too small --> new value set to 0.0001 (EPS4)\n");
-+ user_dmax = EPS4;
-+ }
-+ p3d_set_env_dmax(user_dmax);
-+ }
-+ if (user_obj_tol_to_be_set) {
-+ if (user_obj_tol < 0.0) {
-+ printf("WARNING: Negative tolerance, tolerance is set to 0.0\n");
-+ user_obj_tol = 0.0;
-+ }
-+ p3d_set_env_object_tolerance(user_obj_tol);
-+ }
-+
-+ printf("Env dmax = %f\n",p3d_get_env_dmax());
-+ printf("Env Object tol = %f\n",p3d_get_env_object_tolerance());
-+ /* always set tolerance even if the user didn't specify any options
-+ * it's possible that Kcd has calculated automaticly a dmax
-+ */
-+ /* Normally p3d_col_set_tolerance(); is called when initialising
-+ * the sliders, in case the sliders are not used we have to use
-+ * p3d_col_set_tolerance()
-+ */
-+
-+ //printf("MAX_DDLS %d\n", MAX_DDLS);
-+
-+ // modif Juan
-+#ifdef BIO
-+ if (col_mode_to_be_set == p3d_col_mode_bio) {
-+ bio_set_num_subrobot_AA();
-+ bio_set_num_subrobot_ligand();
-+ bio_set_bio_jnt_types();
-+ bio_set_bio_jnt_AAnumbers();
-+ bio_set_list_AA_first_jnt();
-+ bio_set_AAtotal_number();
-+ bio_set_nb_flexible_sc();
-+ bio_set_list_firstjnts_flexible_sc();
-+ if (XYZ_ROBOT->num_subrobot_ligand != -1)
-+ bio_set_nb_dof_ligand();
-+#ifdef BIO_BALL
-+ new BallEnergy(XYZ_ENV);
-+ if(global_costSpace)
-+ {
-+ global_costSpace->addCost("BALLmmff94",
-+ boost::bind(&BallEnergy::computeEnergy, XYZ_ENV->energyComputer, _1));
-+ global_costSpace->setCost("BALLmmff94");
-+ }
-+#endif
-+ }
-+#endif
-+ // fmodif Juan
-+
-+#if defined(LIGHT_PLANNER) && defined(FK_CNTRT)
-+ for(int i=0; i<XYZ_ENV->nr; i++)
-+ { p3d_create_FK_cntrts(XYZ_ENV->robot[i]); }
-+#endif
-+
-+#ifdef P3D_CONSTRAINTS
-+ // Modif Mokhtar Initialisation For Multisolutions constraints
-+ p3d_init_iksol(XYZ_ROBOT->cntrt_manager);
-+#endif
-+
-+ /* creation du FORM main */
-+
-+ /*
-+ * needs to be run after main form has been created
-+ */
-+ if (scenario_set == TRUE) {
-+
-+ p3d_rw_scenario_init_name();
-+ p3d_read_scenario(scenario);
-+
-+ }
-+
-+ //Set the robots to initial Pos if defined
-+ for(i = 0; i < XYZ_ENV->nr; i++){
-+ if(!p3d_isNullConfig(XYZ_ENV->robot[i], XYZ_ENV->robot[i]->ROBOT_POS)){
-+ p3d_set_and_update_this_robot_conf(XYZ_ENV->robot[i], XYZ_ENV->robot[i]->ROBOT_POS);
-+ }
-+ }
-+ //Exection Of Dlr Planner
-+ // do{
-+ // DlrPlanner* planner = new DlrPlanner(dlrSaveFile);
-+ // DlrParser parser(dlrReadFile, planner);
-+ // if(parser.parse()){
-+ // planner->process();
-+ // }else{
-+ // sleep(2);
-+ // }
-+ // free(planner);
-+ // }while(1);
-+ /* go into loop */
-+
-+#if defined(LIGHT_PLANNER) && defined(PQP)
-+ if(ENV.getBool(Env::startWithFKCntrt))
-+ {
-+ p3d_rob* MyRobot = p3d_get_robot_by_name_containing ( ( char* ) "ROBOT" );
-+ deactivateCcCntrts(MyRobot,-1);
-+ }
-+#endif
-+
-+#if defined(HRI_COSTSPACE) && defined(QT_GL)
-+ if(grid_set)
-+ {
-+ std::cout << "Reading HRICS Grid " << grid_path << std::endl;
-+ qt_load_HRICS_Grid(grid_path);
-+ }
-+#endif
-+//#ifdef QT_GL
-+// sem->release();
-+//#endif
-+
-+ // double c, color[4];
-+ // srand(time(NULL));
-+ // c= rand()/((double)RAND_MAX+1);
-+ // g3d_rgb_from_hue(c, color);
-+ // g3d_set_win_floor_color(g3d_get_cur_win(), color[0], color[1], color[2]);
-+
-+#if defined( LIGHT_PLANNER ) && !defined( QT_GL )
-+ if (manip_test_run)
-+ {
-+ printf("Test functions : ManipulationTestFunctions\n");
-+ new qtG3DWindow();
-+
-+#if defined( MOVE3D_CORE )
-+ // Creates the wrapper to the project
-+ // Be carefull to initialize in the right thread
-+ global_Project = new Project(new Scene(XYZ_ENV));
-+#endif
-+
-+ ManipulationTestFunctions tests;
-+
-+ if(!tests.runTest(manip_test_id))
-+ {
-+ std::cout << "ManipulationTestFunctions::Fail" << std::endl;
-+ }
-+ }
-+#endif
-+ return 0;
-+}
-+
-+/* fonction de rappel des formats des arguments */
-+static void use(void) {
-+
-+ printf("main : ERROR : wrong arguments !\n move3d -d data_directory -f file -sc scenario -s random_seed -c collision_detector -tol tolerance -dmax dist [-v size | -vol volume] -o -x -nkcdd\n");
-+ printf("collision_detector may be: icollide, vcollide, solid, kcd, kng, none\n");
-+ printf("(kng == kcd without gjk)\n");
-+ printf("-o: consider polyhedrons as objects (only for vcollide)\n");
-+ printf("-x: do filtering by robot body (default: don't filter)\n");
-+ printf("-v: (only with kcd) minimal relevant size (in 1 dimension) \n");
-+ printf("-dmax: for Vcollide & nkcdd => maximum penetration distance \n");
-+ printf(" KCD => artificial enlargement of the obstacles \n");
-+ printf("-tol: (only with kcd) object tolerance (enlarge objects for the collision checker) \n");
-+ printf("-vol: (only with kcd) minimal relevant size (volume) \n");
-+ printf("(internal note, kcd: volume of a box around a facet == (surface of box)^(3/2) )\n");
-+ printf("-nkcdd: (only with -c kcd) return only boolean reply, don't use distance \n");
-+ printf("-no-ccntrt: deactivate closed chain constraint at startup \n");
-+}
-+
---
-1.7.9.5
-
--
GitLab