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

Go to the documentation of this file.

/* (jcortes) */
/****************************************************************************/
#include "p3d.h"

#ifndef CNTRT_H
#define CNTRT_H

#define DOF_WITHOUT_CNTRT 0
#define DOF_PASSIF 2
#define DOF_ACTIF  1

/**********************
    PARALLEL SYS
***********************/

/*-----------------------------------------------------------------------------*/
typedef struct s_tri_attach {
  p3d_matrix4          Ttri2pp;
  int                  index_att[3];
  double               l12;
  double               l13;
  double               l23;
  double               angle12;
  struct s_tri_attach  *next;
} p3d_tri_attach;


/*-----------------------------------------------------------------------------*/
#define MAX_N_MANIPULATORS 10
typedef struct s_parallel {
  struct jnt            *base_jntPt;
  struct jnt            *platform_jntPt;
  int (*fct_parplatf_shoot)(struct s_parallel*, configPt);
  double                platform_dof_min_bounds[6];
  double                platform_dof_max_bounds[6];
  int                   n_manipulators;
  struct jnt            *att_jnt[MAX_N_MANIPULATORS];
  int                   index_link_platf[MAX_N_MANIPULATORS];
  double                dpa[MAX_N_MANIPULATORS];
  struct cntrt          *manip_cntrt[MAX_N_MANIPULATORS];
  struct s_tri_attach   *triangles;
} p3d_parallel;


/**********************
        RLG
***********************/

/* clasement of joints for RLG */
#define RJA1 1
#define RJA2 2
#define RJB1 3
#define RJB2 4
#define RJB3 5
#define RJC0 6
#define RJC1 7
#define RJD1 8
#define RJD2 9
#define TJ   10

typedef struct s_shell {
  double rext;
  double rint;
  double alpha;
} p3d_shell;

typedef struct s_rlg_chain_data {
  struct jnt  *jnt;
  int          num_dof_jnt;
  int          jtype;
  double       lmax;
  double       lmin;
  double       dat[4];
  struct s_shell shell;
  double       totrml;
  double       totmMdif;
  p3d_matrix4  Tref;
  double       refval;
} p3d_rlg_chain_data, *pp3d_rlg_chain_data;

typedef struct s_rlg_chain {
  int                      nlinksrlgch;
  struct s_rlg_chain_data  **rlg_data;
  int (*rlg_chain_fct)(struct cntrt* ct, configPt q);
} p3d_rlg_chain, *pp3d_rlg_chain;


typedef struct s_rlg_base {
  struct jnt               *basejntPt;
  int (*rlg_base_fct)(struct cntrt*, configPt);
  double                   rmax, rmin;
  p3d_matrix4              Tbm;
  double                   lbm;
} p3d_rlg_base, *pp3d_rlg_base;



/*-----------------------------------------------------------------------------*/
typedef struct s_rlg {
  struct s_rlg_chain  *rlgchPt;
  struct s_rlg_base   *rlgbsPt;
} p3d_rlg, *pp3d_rlg;

/**********************
      CONSTRAINTS
***********************/

#define MAX_ARGU_CNTRT 30

typedef struct cntrt {
  int            num;
  char           namecntrt[40];
  int            active;
  int (*fct_cntrt)(struct cntrt *ct, int iksol, configPt, double dl);
  int            nactjnts, npasjnts;
  int            ndval, nival;
  struct jnt *   actjnts[MAX_ARGU_CNTRT];
  int            act_jnt_dof[MAX_ARGU_CNTRT];
  int            act_rob_dof[MAX_ARGU_CNTRT];
  int            actjnt_state[MAX_ARGU_CNTRT];
  struct jnt *   pasjnts[MAX_ARGU_CNTRT];
  int            pas_jnt_dof[MAX_ARGU_CNTRT];
  int            pas_rob_dof[MAX_ARGU_CNTRT];
  int            argu_i[MAX_ARGU_CNTRT];
  double         argu_d[MAX_ARGU_CNTRT];

  int            markedForSingularity;//this singularity is marked for singularity don't compute the cntrt        0
  int            nSingularities;//number of singularities
  struct singularity *singularities[MAX_ARGU_CNTRT];//array of singularities

  p3d_matrix4    Tatt;
  p3d_matrix4    Tatt2;
  p3d_matrix4    Tatt_default;
  p3d_matrix4    Tbase;
  p3d_matrix4    TSingularity;
  struct obj *   col_pairs[2][MAX_ARGU_CNTRT];
  struct cntrt   *next_cntrt;
  struct cntrt   *prev_cntrt;
  int            enchained_rob_dof[MAX_ARGU_CNTRT];
  int            nenchained;
  struct cntrt   **enchained;
  struct cntrt   *reshoot_ct;
  int            nctud;
  struct cntrt   **ct_to_update;
  struct s_rlg      *rlgPt;
  struct s_parallel *parallel_sys_data;
  struct s_bio_6R_ik_data *bio_ik_data;
  struct cntrt_management *cntrt_manager;
  int           nbSol; //Number of possible solutions. This parameter is filled in the set_function of the constraint.
} p3d_cntrt, *pp3d_cntrt;

typedef struct singJntVal {
  int jntNum; //The joint number
  double *val; //The joint singular value
} p3d_singJntVal, *pp3d_singJntVal;


typedef struct singularity {
  int nJnt; //the number of jnt for this singularity
  p3d_singJntVal *singJntVal[MAX_ARGU_CNTRT]; //array of singular joint values composing this singularity
  int nRel; //the number of relation between two classes
  int classes[MAX_ARGU_CNTRT][2]; //two columns array of the classes that this singularity can connect
} p3d_singularity, *pp3d_singularity;

typedef struct cntrt_management {

  p3d_cntrt   **cntrts;

  int         ncntrts;

  int         nb_dof;

  int         *in_cntrt;

  struct s_dbl_list   *cntrt_call_list;

} p3d_cntrt_management, *pp3d_cntrt_management;

// struct lessJnt {
//   bool operator() (const struct jnt* jnt1, const struct jnt* jnt2) const
//   {return jnt1->num < jnt2->num;}
// };
// 
// class Constraint{
//   private:
//     int id;
//     std::string name;
//     int active;
//     std::multimap <struct jnt*, std::vector <int>, lessJnt> actjnts;
//     std::multimap <struct jnt*, std::vector <int>, lessJnt> pasjnts;
//     std::vector <int> argu_i;
//     std::vector <double> argu_d;
//     p3d_matrix4 Tatt;
//     p3d_matrix4 Tatt2;
//     std::vector <pair <struct obj*, struct obj*>> col_pairs;
// //     struct obj * col_pairs[2][MAX_ARGU_CNTRT];
// //     struct cntrt *next_cntrt;
// //     struct cntrt *prev_cntrt;
//     int enchained_rob_dof[MAX_ARGU_CNTRT];
//     int nenchained;
//     std::vector <Constraint&> enchained;
//     Constraint& reshoot_ct;
//     int nctud;
//     std::vector <Constraint&> ct_to_update;
//     struct s_rlg *rlgPt;
//     struct s_parallel *parallel_sys_data;
//     struct s_bio_6R_ik_data *bio_ik_data;
//     struct cntrt_management *cntrt_manager;
// 
//     p3d_matrix4 Tbase;
//     p3d_matrix4 TSingularity;
//     int markedForSingularity;//this singularity is marked for singularity don't compute the cntrt 0
//     int nbSol;
//     int nSingularities;//number of singularities
//     std::vector<struct singularity* > singularities;//vector of singularities
// };

#endif