GecodeAnalyzer.h
Go to the documentation of this file.
1 //
2 // Created by yijiangh on 1/17/18.
3 //
4 
5 #ifndef FRAMEFAB_MPP_GECODEANALYZER_H
6 #define FRAMEFAB_MPP_GECODEANALYZER_H
7 
8 #include <cmath>
9 #include <cstring>
10 
12 
13 // Gecode
14 // int variable & search engines
15 #include <gecode/driver.hh>
16 #include <gecode/int.hh>
17 #include <gecode/search.hh>
18 #include <gecode/minimodel.hh>
19 
20 namespace Gecode
21 {
22 class AssemblySequenceOptions : public Options
23 {
24  public:
25  std::vector<int> A;
26  std::vector<int> G;
27  std::vector<int> T;
28 
29  int n;
30  int m;
31 
32  // convert Eigen matrix to Gecode Matrix
33 
35  const std::vector<int>& _A,
36  const std::vector<int>& _G,
37  const std::vector<int>& _T,
38  const int _n,
39  const int _m)
40  : Options(s), A(_A), G(_G), T(_T), n(_n), m(_m)
41  {
42  }
43 };
44 
45 class AssemblySequence : public Script
46 {
47  protected:
49  IntVarArray o;
50 
51  public:
53  : Script(input_opt), opt(input_opt), o(*this, input_opt.n, 0, input_opt.n - 1)
54  {
55  int n = opt.n;
56  int m = opt.m;
57 
58  // create shared int array, ref. Gecode MPG Pg.70 - Tip 4.9
59  // connectivity matrix
60  IntArgs A(opt.A);
61  IntArgs G(opt.G);
62 
63  // collision matrix
64 // IntArgs T(opt.T);
65 
66  // C1: AllDiff
67  distinct(*this, o);
68 
69  // C2: Connectivity
70  for(int i=0; i < n; i++)
71  {
72  // get A's row O_i
73  IntVarArgs connect_num(i+1);
74 
75  for(int j=0; j < i; j++)
76  {
77  // connect_num[j] = A[o[j] + n*o[i]]
78  element(*this, A, o[j], n, o[i], n, connect_num[j]);
79  }
80 
81  element(*this, G, o[i], connect_num[i]);
82 
83  linear(*this, connect_num, IRT_GQ, 1);
84  }
85 
86  // C4: ExistValidEEFPose
87 // for(int i=0; i < n; i++)
88 // {
89 // // num of collision free prev edge j's num for each direction k
90 // IntVarArgs B(m);
91 // IntArgs free_num(m);
92 //
93 // for(int k=0; k < m; k++)
94 // {
95 // BoolVarArgs d_sum(i);
96 //
97 // for (int j = 0; j < i; j++)
98 // {
99 // // id = (o[i]*n + o[j])*m + k
100 // IntVar id = expr(*this, o[i]*(n*m) + o[j]*m + k);
101 //
102 // // d_sum[j] = T[o_i][o_j][k]
103 // element(*this, T, id, d_sum[j]);
104 // }
105 //
106 // // B[k] = sum{j} d_sum[j]
107 // linear(*this, d_sum, IRT_EQ, B[k]);
108 //
109 // free_num[k] = i-1;
110 // }
111 // count(*this, B, free_num, IRT_GQ, 1);
112 // }
113 
114 // branch(*this, o, INT_VAR_MIN_MIN(), INT_VAL_SPLIT_MIN());
115  }
116 
118  : Script(share, s), opt(s.opt)
119  {
120  o.update(*this, share, s.o);
121  }
122 
123  virtual Space *copy(bool share)
124  {
125  return new AssemblySequence(share, *this);
126  }
127 
128  void print(std::ostream& os) const
129  {
130  os << "\tAssembly Sequence(n: "
131  << opt.n << ", m: " << opt.m << ")"
132  << std::endl;
133 
134  os << "Order: " << o << std::endl;
135 
136 // Matrix<BoolVarArray> p(_p,opt.b,opt.v);
137 
138 // for (int i = 0; i<opt.v; i++)
139 // {
140 // os << "\t\t";
141 // for (int j = 0; j<opt.b; j++)
142 // {
143 // os << p(j, i) << " ";
144 // }
145 //
146 // os << std::endl;
147 // }
148 
149  os << std::endl;
150  }
151 };
152 
153 } // end Gecode namespace
154 
156 {
157  public:
159  {
161  COL_MAJOR
162  };
163 
164  typedef Eigen::MatrixXd MX;
165  typedef Eigen::Matrix3d M3;
166  typedef Eigen::VectorXd VX;
167  typedef Eigen::Vector3d V3;
168 
169  public:
170  explicit GecodeAnalyzer(
171  DualGraph *ptr_dualgraph,
172  QuadricCollision *ptr_collision,
173  Stiffness *ptr_stiffness,
174  FiberPrintPARM *ptr_parm,
175  char *ptr_path,
176  bool terminal_output,
177  bool file_output,
178  descartes_core::RobotModelPtr hotend_model,
179  moveit::core::RobotModelConstPtr moveit_model,
180  std::string hotend_group_name
181  ) noexcept
182  : SeqAnalyzer(ptr_dualgraph, ptr_collision, ptr_stiffness,
183  ptr_parm, ptr_path, terminal_output, file_output,
184  hotend_model, moveit_model, hotend_group_name){}
185  ~GecodeAnalyzer();
186 
187  public:
188  bool SeqPrint();
189  void debug();
190 
191  private:
192  void ComputeGecodeInput(
193  const std::vector<WF_edge*>& layer_e,
194  std::vector<int>& A, std::vector<int>& G, std::vector<int>& T, CSPDataMatrixStorageType m_type);
195 
196  public:
197  void PrintOutTimer();
198 
199  private:
200  std::vector<std::vector<WF_edge*>> layers_; // store dual_node's id for each layers
201 
202  double min_z_;
203  double max_z_;
204 
206 };
207 
208 #endif //FRAMEFAB_MPP_GECODEANALYZER_H
GLdouble n
AssemblySequenceOptions(const char *s, const std::vector< int > &_A, const std::vector< int > &_G, const std::vector< int > &_T, const int _n, const int _m)
Eigen::Matrix3d M3
const GLfloat * m
std::vector< std::vector< WF_edge * > > layers_
GecodeAnalyzer(DualGraph *ptr_dualgraph, QuadricCollision *ptr_collision, Stiffness *ptr_stiffness, FiberPrintPARM *ptr_parm, char *ptr_path, bool terminal_output, bool file_output, descartes_core::RobotModelPtr hotend_model, moveit::core::RobotModelConstPtr moveit_model, std::string hotend_group_name) noexcept
Eigen::MatrixXd MX
const AssemblySequenceOptions & opt
AssemblySequence(const AssemblySequenceOptions &input_opt)
Eigen::Vector3d V3
GLdouble s
virtual Space * copy(bool share)
AssemblySequence(bool share, AssemblySequence &s)
Definition: Timer.h:48
Eigen::VectorXd VX
void print(std::ostream &os) const


choreo_task_sequence_planner
Author(s): Yijiang Huang
autogenerated on Thu Jul 18 2019 04:03:14