hpp-rbprm  4.11.0
Implementation of RB-PRM planner using hpp.
rbprm-node.hh
Go to the documentation of this file.
1 #ifndef HPP_RBPRM_NODE_HH
2 #define HPP_RBPRM_NODE_HH
3 
4 #include <hpp/core/node.hh>
6 #include <hpp/centroidal-dynamics/centroidal_dynamics.hh>
7 
8 namespace hpp {
9 namespace pinocchio {
10 class RbPrmDevice; // fwd declaration of rbprmDevice class
11 typedef std::shared_ptr<RbPrmDevice> RbPrmDevicePtr_t;
12 } // namespace pinocchio
13 namespace core {
14 
17 
22 class HPP_CORE_DLLAPI RbprmNode : public Node {
23  public:
28  RbprmNode(const ConfigurationPtr_t& configuration) : Node(configuration) {}
32  RbprmNode(const ConfigurationPtr_t& configuration, ConnectedComponentPtr_t connectedComponent)
33  : Node(configuration, connectedComponent) {}
34 
35  fcl::Vec3f getNormal() { return normal_; }
36 
37  void normal(double x, double y, double z) {
38  fcl::Vec3f n(x, y, z);
39  normal_ = n;
40  }
41 
42  void normal(fcl::Vec3f n) { normal_ = n; }
43 
44  RbprmValidationReportPtr_t getReport() { return collisionReport_; }
45 
46  void collisionReport(RbprmValidationReportPtr_t report) { collisionReport_ = report; }
47 
48  void setV(MatrixXX V) { V_ = V; }
49 
50  MatrixXX getV() { return V_; }
51 
52  void setIPHat(Matrix6X m) { IP_hat_ = m; }
53 
54  Matrix6X getIPhat() { return IP_hat_; }
55 
56  void setG(Matrix6X G) { G_ = G; }
57 
58  Matrix6X getG() { return G_; }
59 
60  void setH(Matrix63 H) { H_ = H; }
61 
62  Matrix63 getH() { return H_; }
63 
64  void seth(Vector6 h) { h_ = h; }
65 
66  Vector6 geth() { return h_; }
67 
68  void setNumberOfContacts(size_type n) { numberOfContacts_ = n; }
69 
70  size_type getNumberOfContacts() { return numberOfContacts_; }
71 
72  void fillNodeMatrices(ValidationReportPtr_t report, bool rectangularContact, double sizeFootx, double sizeFooty,
73  double m, double mu, pinocchio::RbPrmDevicePtr_t device);
74 
75  void chooseBestContactSurface(ValidationReportPtr_t report, hpp::pinocchio::RbPrmDevicePtr_t device);
76 
77  Eigen::Quaterniond getQuaternion();
78 
79  private:
80  fcl::Vec3f normal_;
81  RbprmValidationReportPtr_t collisionReport_;
82  // const polytope::ProjectedCone* giwc_; // useless now ?
83  // polytope::T_rotation_t rotContact_;
84  // polytope::vector_t posContact_;
85  MatrixXX V_;
86  Matrix6X IP_hat_;
87  Matrix6X G_; // not initialized yet
88  Matrix63 H_;
89  Vector6 h_;
90  size_type numberOfContacts_;
91 
92 }; // class
93 
94 } // namespace core
95 } // namespace hpp
96 
97 #endif // HPP_RBPRM_NODE_HH
Vector6 geth()
Definition: rbprm-node.hh:66
void collisionReport(RbprmValidationReportPtr_t report)
Definition: rbprm-node.hh:46
std::shared_ptr< RbPrmDevice > RbPrmDevicePtr_t
Definition: dynamic-validation.hh:27
void setG(Matrix6X G)
Definition: rbprm-node.hh:56
fcl::Vec3f getNormal()
Definition: rbprm-node.hh:35
Definition: algorithm.hh:27
Matrix63 getH()
Definition: rbprm-node.hh:62
RbprmNode * RbprmNodePtr_t
Definition: rbprm-node.hh:16
void setV(MatrixXX V)
Definition: rbprm-node.hh:48
void setH(Matrix63 H)
Definition: rbprm-node.hh:60
RbprmValidationReportPtr_t getReport()
Definition: rbprm-node.hh:44
Matrix6X getG()
Definition: rbprm-node.hh:58
Definition: rbprm-node.hh:22
centroidal_dynamics::MatrixXX MatrixXX
Definition: rbprm-node.hh:18
MatrixXX getV()
Definition: rbprm-node.hh:50
size_type getNumberOfContacts()
Definition: rbprm-node.hh:70
centroidal_dynamics::Matrix63 Matrix63
Definition: rbprm-node.hh:20
void setNumberOfContacts(size_type n)
Definition: rbprm-node.hh:68
HPP_PREDEF_CLASS(RbPrmDevice)
std::shared_ptr< RbprmValidationReport > RbprmValidationReportPtr_t
Definition: rbprm-validation-report.hh:29
void normal(double x, double y, double z)
Definition: rbprm-node.hh:37
Matrix6X getIPhat()
Definition: rbprm-node.hh:54
void setIPHat(Matrix6X m)
Definition: rbprm-node.hh:52
RbprmNode(const ConfigurationPtr_t &configuration)
Definition: rbprm-node.hh:28
centroidal_dynamics::Matrix6X Matrix6X
Definition: rbprm-node.hh:19
RbprmNode(const ConfigurationPtr_t &configuration, ConnectedComponentPtr_t connectedComponent)
Definition: rbprm-node.hh:32
void normal(fcl::Vec3f n)
Definition: rbprm-node.hh:42
centroidal_dynamics::Vector6 Vector6
Definition: rbprm-node.hh:21
void seth(Vector6 h)
Definition: rbprm-node.hh:64