Moby
ImpactConstraintHandler.h
1 /****************************************************************************
2  * Copyright 2011 Evan Drumwright
3  * This library is distributed under the terms of the Apache V2.0
4  * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0).
5  ****************************************************************************/
6 
7 #ifndef _IMPACT_EVENT_HANDLER_H
8 #define _IMPACT_EVENT_HANDLER_H
9 
10 #include <list>
11 #include <vector>
12 #include <map>
13 #ifdef USE_QLCPD
14 #include <Moby/QLCPD.h>
15 #endif
16 #ifdef HAVE_IPOPT
17 #include <coin/IpTNLP.hpp>
18 #include <coin/IpIpoptApplication.hpp>
19 #include <Moby/NQP_IPOPT.h>
20 #include <Moby/LCP_IPOPT.h>
21 #endif
22 #include <Ravelin/LinAlgd.h>
23 #include <Moby/Base.h>
24 #include <Moby/Types.h>
25 #include <Moby/LCP.h>
26 #include <Moby/SparseJacobian.h>
27 #include <Moby/UnilateralConstraint.h>
28 #include <Moby/UnilateralConstraintProblemData.h>
29 
30 namespace Moby {
31 
32 class ConstraintSimulator;
33 class NQP_IPOPT;
34 class LCP_IPOPT;
35 
37 class ImpactConstraintHandler
38 {
39  friend class ConstraintSimulator;
40  friend class ConstraintStabilization;
41 
42  public:
43  ImpactConstraintHandler();
44  void process_constraints(const std::vector<UnilateralConstraint>& constraints);
45  static boost::shared_ptr<Ravelin::DynamicBodyd> get_super_body(boost::shared_ptr<Ravelin::SingleBodyd> sb);
46 
48  bool use_ip_solver;
49 
51  unsigned ip_max_iterations;
52 
54  double ip_eps;
55 
56  private:
57  static void compute_signed_dist_dot_Jacobian(UnilateralConstraintProblemData& q, Ravelin::MatrixNd& J);
58  void solve_frictionless_lcp(UnilateralConstraintProblemData& q, Ravelin::VectorNd& z);
59  void apply_visc_friction_model_to_connected_constraints(const std::list<UnilateralConstraint*>& constraints, const std::list<boost::shared_ptr<Ravelin::SingleBodyd> >& single_bodies);
60  void apply_no_slip_model_to_connected_constraints(const std::list<UnilateralConstraint*>& constraints, const std::list<boost::shared_ptr<Ravelin::SingleBodyd> >& single_bodies);
61  void apply_ap_model_to_connected_constraints(const std::list<UnilateralConstraint*>& constraints, const std::list<boost::shared_ptr<Ravelin::SingleBodyd> >& single_bodies);
62  static void update_from_stacked(UnilateralConstraintProblemData& q, const Ravelin::VectorNd& z);
63  static void update_from_stacked(UnilateralConstraintProblemData& q);
64  double calc_min_constraint_velocity(const UnilateralConstraintProblemData& q) const;
65  void update_constraint_velocities_from_impulses(UnilateralConstraintProblemData& q);
66  bool apply_restitution(const UnilateralConstraintProblemData& q, Ravelin::VectorNd& z) const;
67  bool apply_restitution(UnilateralConstraintProblemData& q) const;
68  static bool use_qp_solver(const UnilateralConstraintProblemData& epd);
69  void apply_visc_friction_model(UnilateralConstraintProblemData& epd);
70  void apply_no_slip_model(UnilateralConstraintProblemData& epd);
71  void apply_ap_model(UnilateralConstraintProblemData& epd);
72  void solve_qp(Ravelin::VectorNd& z, UnilateralConstraintProblemData& epd);
73  void solve_nqp(Ravelin::VectorNd& z, UnilateralConstraintProblemData& epd);
74  void apply_model(const std::vector<UnilateralConstraint>& constraints);
75  void apply_model_to_connected_constraints(const std::list<UnilateralConstraint*>& constraints, const std::list<boost::shared_ptr<Ravelin::SingleBodyd> >& single_bodies);
76  void compute_problem_data(UnilateralConstraintProblemData& epd, const std::list<boost::shared_ptr<Ravelin::SingleBodyd> >& single_bodies);
77  void solve_lcp(UnilateralConstraintProblemData& epd, Ravelin::VectorNd& z);
78  void solve_qp_work(UnilateralConstraintProblemData& epd, Ravelin::VectorNd& z);
79  double calc_ke(UnilateralConstraintProblemData& epd, const Ravelin::VectorNd& z);
80  void update_problem(const UnilateralConstraintProblemData& qorig, UnilateralConstraintProblemData& qnew);
81  void update_solution(const UnilateralConstraintProblemData& q, const Ravelin::VectorNd& x, const std::vector<bool>& working_set, unsigned jidx, Ravelin::VectorNd& z);
82  void solve_nqp_work(UnilateralConstraintProblemData& epd, Ravelin::VectorNd& z);
83  void propagate_impulse_data(const UnilateralConstraintProblemData& epd);
84  void apply_impulses(const UnilateralConstraintProblemData& epd);
85  static void contact_select(const std::vector<int>& cn_indices, const std::vector<int>& beta_nbeta_c_indices, const Ravelin::VectorNd& x, Ravelin::VectorNd& cn, Ravelin::VectorNd& beta_c);
86  static void contact_select(const std::vector<int>& cn_indices, const std::vector<int>& beta_nbeta_c_indices, const Ravelin::MatrixNd& m, Ravelin::MatrixNd& cn_rows, Ravelin::MatrixNd& beta_c_rows);
87  static double sqr(double x) { return x*x; }
88  void setup_QP(UnilateralConstraintProblemData& epd, Ravelin::SharedMatrixNd& H, Ravelin::SharedVectorNd& c, Ravelin::SharedMatrixNd& M, Ravelin::SharedVectorNd& q, Ravelin::SharedMatrixNd& A, Ravelin::SharedVectorNd& b);
89  static void get_full_rank_implicit_constraints(const SparseJacobian& J, std::vector<bool>& active);
90  static Ravelin::MatrixNd& mult(const std::vector<Ravelin::MatrixNd>& inertias, const Ravelin::MatrixNd& X, Ravelin::MatrixNd& B);
91  static Ravelin::MatrixNd& to_dense(const std::vector<Ravelin::MatrixNd>& J, Ravelin::MatrixNd& B);
92  static void get_generalized_velocity(const UnilateralConstraintProblemData& epd, Ravelin::VectorNd& v);
93  static void compute_limit_components(const Ravelin::MatrixNd& X, UnilateralConstraintProblemData& epd);
94  static void compute_X(UnilateralConstraintProblemData& epd, Ravelin::MatrixNd& X);
95  static void update_generalized_velocities(const UnilateralConstraintProblemData& epd, const Ravelin::VectorNd& dv);
96  static void add_contact_to_Jacobian(const UnilateralConstraint& c, SparseJacobian& Cn, SparseJacobian& Cs, SparseJacobian& Ct, const std::map<boost::shared_ptr<Ravelin::DynamicBodyd>, unsigned>& gc_map, unsigned contact_index);
97  static void add_contact_dir_to_Jacobian(boost::shared_ptr<Ravelin::RigidBodyd> rb, boost::shared_ptr<Ravelin::ArticulatedBodyd> ab, SparseJacobian& C, const Ravelin::Vector3d& contact_point, const Ravelin::Vector3d& d, const std::map<boost::shared_ptr<Ravelin::DynamicBodyd>, unsigned>& gc_map, unsigned contact_index);
98  static double calc_signed_dist(boost::shared_ptr<Ravelin::SingleBodyd> sb1, boost::shared_ptr<Ravelin::SingleBodyd> sb2);
99 
100  Ravelin::LinAlgd _LA;
101  LCP _lcp;
102 
103  // persistent constraint data
104  UnilateralConstraintProblemData _epd;
105 
106  // a pointer to the simulator
107  boost::shared_ptr<ConstraintSimulator> _simulator;
108 
109  // temporaries for compute_problem_data(), solve_qp_work(), solve_lcp(), and apply_impulses()
110  Ravelin::MatrixNd _MM;
111  Ravelin::VectorNd _zlast, _v;
112 
113  // temporaries for solve_qp_work() and solve_nqp_work()
114  Ravelin::VectorNd _workv, _new_Cn_v;
115 
116  // temporaries shared between solve_lcp(), solve_qp(), and solve_nqp()
117  Ravelin::VectorNd _a, _b;
118 
119  // temporaries for solve_qp() and solve_nqp()
120  Ravelin::VectorNd _z;
121 
122  // temporaries for solve_frictionless_lcp()
123  Ravelin::VectorNd _cs_visc, _ct_visc;
124 
125  // temporaries for solve_no_slip_lcp()
126  Ravelin::MatrixNd _rJx_iM_JxT, _Y, _Q_X_XT, _workM, _workM2;
127  Ravelin::VectorNd _YXv, _Xv, _cs_ct_alphax;
128 
129  // interior-point solver "application"
130  #ifdef HAVE_IPOPT
131  Ipopt::IpoptApplication _app;
132  #endif
133 
134  // temporaries shared between solve_nqp_work() and solve_lcp()
135  Ravelin::MatrixNd _A;
136 
137  // temporaries for solve_nqp_work()
138  #ifdef HAVE_IPOPT
139  Ipopt::SmartPtr <NQP_IPOPT> _ipsolver;
140  Ipopt::SmartPtr <LCP_IPOPT> _lcpsolver;
141  #endif
142  Ravelin::MatrixNd _RTH;
143  Ravelin::VectorNd _w, _workv2, _x;
144 
145  // temporaries for solve_lcp()
146  Ravelin::MatrixNd _AU, _AV, _B, _C, _D;
147  Ravelin::VectorNd _AS, _alpha_x, _qq, _Cn_vplus;
148 
149  // QLCPD solver
150  #ifdef USE_QLCPD
151  QLCPD _qp;
152  #endif
153 
154 /*
155  // temporaries for IPOPT
156  boost::shared_ptr<double> _h_obj, _cJac;
157  std::vector<boost::shared_ptr<double> > _h_con;
158  unsigned _nnz_h_obj;
159  std::vector<unsigned> _nnz_h_con;
160  boost::shared_ptr<unsigned> _h_iRow, _h_jCol, _cJac_iRow, _cJac_jCol;
161 */
162 }; // end class
163 } // end namespace
164 
165 #endif
166 
Moby::SparseJacobian
A Sparse Jacobian representation along with multiplication routines.
Definition: SparseJacobian.h:30
Moby::ImpactConstraintHandler::get_super_body
static boost::shared_ptr< Ravelin::DynamicBodyd > get_super_body(boost::shared_ptr< Ravelin::SingleBodyd > sb)
Gets the super body (articulated if any)
Definition: ImpactConstraintHandler.cpp:1737
Moby::ConstraintSimulator
An virtual class for simulation with constraints.
Definition: ConstraintSimulator.h:29
Moby::ImpactConstraintHandler
Defines the mechanism for handling impact constraints.
Definition: ImpactConstraintHandler.h:37
Moby::ImpactConstraintHandler::ImpactConstraintHandler
ImpactConstraintHandler()
Sets up the default parameters for the impact event handler.
Definition: ImpactConstraintHandler.cpp:46
Moby::ImpactConstraintHandler::ip_eps
double ip_eps
The tolerance for to the interior-point solver (default 1e-6)
Definition: ImpactConstraintHandler.h:54
Moby::ConstraintStabilization
Projected constraint stabilization mechanism.
Definition: ConstraintStabilization.h:21
Moby::ImpactConstraintHandler::use_ip_solver
bool use_ip_solver
If set to true, uses the interior-point solver (default is false)
Definition: ImpactConstraintHandler.h:48
Moby::ImpactConstraintHandler::ip_max_iterations
unsigned ip_max_iterations
The maximum number of iterations to use for the interior-point solver.
Definition: ImpactConstraintHandler.h:51
Moby::UnilateralConstraint
Container class for describing a unilateral constraint in the simulation.
Definition: UnilateralConstraint.h:27
Moby::ImpactConstraintHandler::process_constraints
void process_constraints(const std::vector< UnilateralConstraint > &constraints)
Definition: ImpactConstraintHandler.cpp:75
Generated on Wed Mar 23 2016 16:29:51 for Moby by   doxygen 1.8.6