/home/slemaign/softs-local/BioMove3D-git/include/device.h

Go to the documentation of this file.

/****************************************************************************/
/****************************************************************************/

#ifndef _DEVICE_H
#define _DEVICE_H

#define JNT_NB_DOF_MAX     6

#define JNT_MAX_SIZE_NAME 80

#define NDOF_BASE           6
#define NDOF_BASE_TRANSLATE 3
#define NDOF_BASE_ROTATE    3

#define NB_POS_PARAM 6

#define MAX_TRANSITION 10

#ifdef LIGHT_PLANNER
#include <vector>
#endif

#include "p3d_sys.h"
#include "p3d_type.h"
#include "p3d_matrix.h"
#include "polyhedre.h"
#include "p3d_poly.h"
#include "dyna_list.h"              // modif Juan
#include "p3d.h"
#include "traj.h"
#include "localpath.h"

/*--------------------------------------------------------------------------*/
typedef struct s_read_jnt_link_data {
  struct jnt * jnt_link;
  int jnt_num;
  int robot_num;
  int flag_prev;
  double pos_param[NB_POS_PARAM];
} p3d_read_jnt_link_data;

/*--------------------------------------------------------------------------*/
typedef struct p3d_read_jnt_data {
  p3d_type_joint type;
  double scale;
  char name[JNT_MAX_SIZE_NAME];
  int flag_name;
  int prev_jnt;
  int flag_prev_jnt;
  int nb_dof;
  double * v;
  int flag_v;
  double * v_pos0;
  int flag_v_pos0;
  double * vmin;
  int flag_vmin;
  double * vmax;
  int flag_vmax;
  double * vmin_rand;
  int flag_vmin_rand;
  double * vmax_rand;
  int flag_vmax_rand;
  int * is_user;
  int flag_is_user;
  int * is_active_for_planner;
  int flag_is_active_for_planner;
  int nb_param;
  double * param;
  int flag_param;
  p3d_matrix4 pos;
  int flag_pos;
  int flag_relative_pos;
  double * velocity_max;
  int flag_velocity_max;
  double * acceleration_max;
  int flag_acceleration_max;
  double * jerk_max;
  int flag_jerk_max;
  double * torque_max;
  int flag_torque_max;
  
  int nb_links;
  struct s_read_jnt_link_data ** link_array;

} p3d_read_jnt_data;


typedef struct
{
  p3d_vector3 axis;

  double v;

  double old_v;

  double v0;

  double vmin;

  double vmax;

  
  double velocity_max;

  double acceleration_max;

  double jerk_max;

  double torque_max;
  
  double vmin_r;

  double vmax_r;

  int   is_user;

  int   is_active_for_planner;

  int   is_modified;

  int lock;

  bool circular;
} p3d_dof_data;

typedef struct link_between_joint
{
  p3d_matrix4 rel_pos;

  struct jnt * prev_jnt;

  struct jnt * next_jnt;

} p3d_link_between_joint;


typedef struct jnt
{
  p3d_type_joint   type;

  int kinematic_type;
#ifdef BIO

  int bio_jnt_type;  // modif Juan
  int bio_AAnumber;  // modif ljaillet
  int num_subrobot;
#endif

  int num;

  char  name[JNT_MAX_SIZE_NAME];

  int ui_kin_num;

  int kin_joint_id;

  int is_used_in_device_flag;

  struct rob  *rob;

  struct obj  *o;

  //  double v;

  //double vmin;

  //  double vmax;

  /*----------------------------------------------------------------------
   * Placement parameters
   */

  p3d_matrix4 pos0_obs;

  p3d_matrix4 pos0;

  p3d_point p0;

  p3d_point relative_p0;

  int   is_defined_relative;

  p3d_matrix4 abs_pos;

  int abs_pos_modified;

  p3d_matrix4 abs_pos_before_jnt;

  int abs_pos_before_jnt_modified;

  int pos_obj_modified;

  int pos_updated;

  p3d_matrix4   jnt_mat;

  int mat_modified;

  /*----------------------------------------------------------------------
   * The link to other joints.
   */

  struct jnt  *prev_jnt;

  struct jnt  **next_jnt;

  int   n_next_jnt;

  struct link_between_joint **link_jnt_arr;

  int        n_link_jnt;

  struct link_between_joint **link_jnt_owned_arr;

  int        n_link_jnt_owned;

  double  dist;

  p3d_prejac  *prejac;

  void  *end_user_data;

  int   index_dof;

  int   index_user_dof;

  int dof_equiv_nbr;

  int user_dof_equiv_nbr;

  p3d_dof_data  dof_data[JNT_NB_DOF_MAX];

} p3d_jnt, *pp3d_jnt;

typedef struct lm_list_param
{
  p3d_localpath_type lpl_type;
  void  *lm_param;
  struct lm_list_param  *next;

} lm_list_param_str, *plm_list_param_str;

#ifdef MULTIGRAPH

typedef struct multiGraphJoint{
  int nbJoints;
  int * joints;
  int * cntrts;
}p3d_multiGraphJoint;

typedef struct multiGraph{
  char * envName;
  char * robotName;
  int nbGraphs;
  struct graph ** graphs;
  int * active;
  struct multiGraphJoint ** mgJoints;
  int * usedJoint;
  int involvesCp;
  struct flatSuperGraph * fsg;
} p3d_multiGraph;
#endif

#ifdef MULTILOCALPATH

typedef struct multiLocalPathJoint{
        int nbJoints;
        int nbDofs;
        int * joints;
        char gpName[30];
        p3d_group_type  gpType;
        p3d_localpath_type      lplType;
        plm_list_param_str local_method_params;

}p3d_multiLocalPathJoint;

typedef struct multiLocalPath{
        int nblpGp;
        int * active;
        struct multiLocalPathJoint ** mlpJoints;
        p3d_traj        **t;
} p3d_multiLocalPath;
#endif

typedef enum {
  P3D_ROB_DEFAULT_DISPLAY, 
  P3D_ROB_NO_DISPLAY, 
  P3D_ROB_UNLIT_BLACK_DISPLAY, 
  P3D_ROB_UNLIT_RED_DISPLAY, 
  P3D_ROB_RED_DISPLAY, 
  P3D_ROB_UNLIT_GREEN_DISPLAY, 
  P3D_ROB_GREEN_DISPLAY, 
  P3D_ROB_UNLIT_BLUE_DISPLAY, 
  P3D_ROB_BLUE_DISPLAY, 
  P3D_ROB_CUSTOM_COLOR_DISPLAY,  
  P3D_ROB_UNLIT_CUSTOM_COLOR_DISPLAY,  
} p3d_rob_display_mode;


typedef struct rob
{
  char  *name;

  int num;

  int ui_kin_num;

  int is_used_in_env_flag;

  int kinematic_type;

  struct env  *env;

  /*----------------------------------------------------------------------
   * corps du robots
   */
  int no;
  p3d_obj **o,*ocur;

  int  nb_dof;

  int  nb_user_dof;

  int njoints;
  p3d_jnt **joints;

  p3d_jnt *j_modif;

  p3d_jnt *first_joint;

  p3d_matrix4 * first_abs_pos;

  int   nt;

  p3d_traj  **t;
  p3d_traj  *tcur;

  p3d_box box;
  double rx;
  double ry;
  double rz;
  double vmin_rot[NDOF_BASE_ROTATE];
  double vmax_rot[NDOF_BASE_ROTATE];

  int coll;

  p3d_BB BB;

  int nconf;
  config_namePt *conf;
  config_namePt confcur;
  configPt  ROBOT_POS;
  configPt  ROBOT_GOTO;
  configPt transitionConfigs[MAX_TRANSITION];
  int nTransition;
  // modif Juan ( not used yet ! )
  int *ikSolPos;
  int *ikSolGoto;
  int *ikSolTransition[MAX_TRANSITION];
  int *ikSol;
  // fmodif Juan
  int sdk_id_qinit;
  int sdk_id_qgoal;

  double *length_array;

  p3d_localpath_type lpl_type;
  plm_list_param_str local_method_params;

  struct graph  *GRAPH;

  struct cntrt_management *cntrt_manager;  // modif. Juan

  void    *user_appli;

  configPt currect_q_inv;  // temporary modif

#ifdef BIO
  int nbAA;
  int nb_flexible_sc;
  p3d_jnt** list_firstjnts_flexible_sc;
  int  nb_ligand_dof;
  int num_subrobot_ligand;
  p3d_jnt** list_AA_firstjnt;
#endif

  p3d_rob_display_mode display_mode;  
// TODO: Because of the removal of hri_planner from BioMove3D (to libhri), this flag is problematic:
// - libmove3d p3d_rob struct is compiled without this part
// - libhri/move3d-studio use this part, resulting in structs that have different sizes.
//#ifdef HRI_PLANNER_GUI
//

/* Modif Luis */
  int show_pos_area;     // boolean show/hide semidisc of position area
  double max_pos_range;  // Max Range distance for the position area
  double min_pos_range;  // Min Range distance for the position area
  double angle_range;    // aperture angle (rad) of acepted "in front of" positions (range:[-angle/2,angle/2])


  // Camera's Properties
  int show_view_field;   // boolean show/hide cone of camera field of view

  double cam_pos[3];     // Camera Position point in the robot X,Y,Z
  double cam_dir[3];     // and orientation point X,Y,Z
  int    cam_axe;        // Axe where the camera is looking forward
  double cam_pan;        // Camera's pan angle
  double cam_tilt;       // Camera's tilt angle
  double cam_max_range;  // Max Range of view of the camera
  double cam_min_range;  // Min Range of view of the camera (end of de cone)
  double cam_h_angle;    // Camera Horizontal caption angle
  double cam_v_angle;    // Camera Vertical caption angle
  int    cam_body_index; // Index of the body of the robot with attached camera

  //Perspective
  int caption_selected;
  void *lookatpoint; //used later as a psp_obs_vertex* from perspective.h

  /* End Modif Luis */

//#endif

#ifdef MULTIGRAPH
  struct multiGraph *mg;
#endif

#ifdef MULTILOCALPATH
  struct multiLocalPath *mlp;
#endif
#ifdef LIGHT_PLANNER
  int graspNbJoints;
  p3d_jnt** graspJoints;
  p3d_jnt* baseJnt;
  p3d_jnt* curObjectJnt;
  double relativeZRotationBaseObject;
  int * isUserDof;
  int nbCcCntrts; //Closed Chain constraints
  struct cntrt ** ccCntrts;
  configPt openChainConf;
  configPt closedChainConf;
  double configCostThreshold;
  struct graph* preComputedGraphs[4]; //Save the precomputed Graphs: 0 = Platform Graph without the object, 1 = Upper Body Graph without the object, 2 = Platform Graph with the object, 3 = Upper Body with the object
#endif
#if defined(FK_CNTRT) && defined(LIGHT_PLANNER)
  int nbFkCntrts; //Forward kinematics constraints for the virtual object used by the closed chain constraints 
  struct cntrt ** fkCntrts;
#endif
#if  defined(LIGHT_PLANNER)
  int isCarryingObject;
//  struct rob *carriedObject; /*!< pointer to the carried object (a freeflyer robot) */
  std::vector<class ArmManipulationData> *armManipulationData;
#endif
#ifdef DPG
  int nbDpgCells;
  class DpgCell ** dpgCells;
#endif
  int draw_custom_color;
  int draw_transparent;
  int inhibitCollisionTolerance; 
} p3d_rob, *pp3d_rob;

#endif /* #ifndef _DEVICE_H */