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