hpp-rbprm  4.10.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 
9 namespace hpp {
10  namespace pinocchio {
11  class RbPrmDevice; //fwd declaration of rbprmDevice class
12  typedef boost::shared_ptr <RbPrmDevice> RbPrmDevicePtr_t;
13  }
14  namespace core {
15 
18 
23  class HPP_CORE_DLLAPI RbprmNode : public Node
24  {
25  public :
30  RbprmNode (const ConfigurationPtr_t& configuration):
31  Node(configuration)
32  {}
36  RbprmNode (const ConfigurationPtr_t& configuration,
37  ConnectedComponentPtr_t connectedComponent):
38  Node(configuration,connectedComponent)
39  {}
40 
41  fcl::Vec3f getNormal(){
42  return normal_;
43  }
44 
45  void normal(double x, double y, double z){
46  fcl::Vec3f n(x,y,z);
47  normal_=n;
48  }
49 
50  void normal(fcl::Vec3f n){
51  normal_ = n;
52  }
53 
55  return collisionReport_;
56  }
57 
59  collisionReport_ = report;
60  }
61 
62 
63 
64 
65  void setV(MatrixXX V){V_ = V;}
66 
67  MatrixXX getV(){return V_;}
68 
69  void setIPHat(Matrix6X m){IP_hat_=m;}
70 
71  Matrix6X getIPhat(){return IP_hat_;}
72 
73  void setG(Matrix6X G){G_=G;}
74 
75  Matrix6X getG(){return G_;}
76 
77  void setH(Matrix63 H){H_=H;}
78 
79  Matrix63 getH(){return H_;}
80 
81  void seth(Vector6 h){h_=h;}
82 
83  Vector6 geth(){return h_;}
84 
85  void setNumberOfContacts(size_type n){numberOfContacts_ = n;}
86 
87  size_type getNumberOfContacts(){return numberOfContacts_;}
88 
89  void fillNodeMatrices(ValidationReportPtr_t report,bool rectangularContact, double sizeFootx, double sizeFooty, double m,double mu,pinocchio::RbPrmDevicePtr_t device);
90 
91  void chooseBestContactSurface(ValidationReportPtr_t report, hpp::pinocchio::RbPrmDevicePtr_t device );
92 
93 
94  Eigen::Quaterniond getQuaternion();
95  private:
96  fcl::Vec3f normal_;
97  RbprmValidationReportPtr_t collisionReport_;
98  // const polytope::ProjectedCone* giwc_; // useless now ?
99  // polytope::T_rotation_t rotContact_;
100  // polytope::vector_t posContact_;
101  MatrixXX V_;
102  Matrix6X IP_hat_;
103  Matrix6X G_; // not initialized yet
104  Matrix63 H_;
105  Vector6 h_;
106  size_type numberOfContacts_;
107 
108 
109  }; // class
110 
111  }//core
112 }//hpp
113 
114 
115 #endif // HPP_RBPRM_NODE_HH
hpp::core::RbprmNode::setV
void setV(MatrixXX V)
Definition: rbprm-node.hh:65
hpp::core::RbprmNode::setH
void setH(Matrix63 H)
Definition: rbprm-node.hh:77
hpp::core::Matrix6X
centroidal_dynamics::Matrix6X Matrix6X
Definition: rbprm-node.hh:20
hpp::core::RbprmNode::normal
void normal(fcl::Vec3f n)
Definition: rbprm-node.hh:50
hpp::core::RbprmNode::seth
void seth(Vector6 h)
Definition: rbprm-node.hh:81
hpp::core::RbprmNode
Definition: rbprm-node.hh:23
hpp::core::MatrixXX
centroidal_dynamics::MatrixXX MatrixXX
Definition: rbprm-node.hh:19
hpp::core::RbprmNode::getV
MatrixXX getV()
Definition: rbprm-node.hh:67
hpp::core::RbprmNode::getNormal
fcl::Vec3f getNormal()
Definition: rbprm-node.hh:41
hpp::core::RbprmNode::RbprmNode
RbprmNode(const ConfigurationPtr_t &configuration)
Definition: rbprm-node.hh:30
hpp::core::Matrix63
centroidal_dynamics::Matrix63 Matrix63
Definition: rbprm-node.hh:21
hpp::core::RbprmNode::RbprmNode
RbprmNode(const ConfigurationPtr_t &configuration, ConnectedComponentPtr_t connectedComponent)
Definition: rbprm-node.hh:36
hpp::core::RbprmNode::getG
Matrix6X getG()
Definition: rbprm-node.hh:75
hpp
Definition: algorithm.hh:27
hpp::core::RbprmNodePtr_t
RbprmNode * RbprmNodePtr_t
Definition: rbprm-node.hh:17
hpp::core::RbprmNode::collisionReport
void collisionReport(RbprmValidationReportPtr_t report)
Definition: rbprm-node.hh:58
hpp::core::HPP_PREDEF_CLASS
HPP_PREDEF_CLASS(RbprmNode)
hpp::core::RbprmNode::getH
Matrix63 getH()
Definition: rbprm-node.hh:79
rbprm-validation-report.hh
hpp::pinocchio::RbPrmDevicePtr_t
boost::shared_ptr< RbPrmDevice > RbPrmDevicePtr_t
Definition: dynamic-validation.hh:27
hpp::core::RbprmNode::setNumberOfContacts
void setNumberOfContacts(size_type n)
Definition: rbprm-node.hh:85
hpp::core::Vector6
centroidal_dynamics::Vector6 Vector6
Definition: rbprm-node.hh:22
hpp::core::RbprmNode::getReport
RbprmValidationReportPtr_t getReport()
Definition: rbprm-node.hh:54
hpp::core::RbprmNode::normal
void normal(double x, double y, double z)
Definition: rbprm-node.hh:45
hpp::core::RbprmNode::geth
Vector6 geth()
Definition: rbprm-node.hh:83
hpp::core::RbprmValidationReportPtr_t
boost::shared_ptr< RbprmValidationReport > RbprmValidationReportPtr_t
Definition: rbprm-validation-report.hh:29
hpp::core::RbprmNode::setG
void setG(Matrix6X G)
Definition: rbprm-node.hh:73
hpp::core::RbprmNode::getIPhat
Matrix6X getIPhat()
Definition: rbprm-node.hh:71
hpp::core::RbprmNode::getNumberOfContacts
size_type getNumberOfContacts()
Definition: rbprm-node.hh:87
hpp::core::RbprmNode::setIPHat
void setIPHat(Matrix6X m)
Definition: rbprm-node.hh:69