designer.hpp
Go to the documentation of this file.
1 #ifndef SOBEC_DESIGNER
2 #define SOBEC_DESIGNER
3 
4 #include <pinocchio/fwd.hpp>
5 // Include pinocchio first
6 #include <Eigen/Dense>
7 #include <pinocchio/algorithm/model.hpp>
8 #include <pinocchio/spatial/se3.hpp>
9 #include <string>
10 #include <vector>
11 
12 #include "pinocchio/multibody/data.hpp"
13 #include "pinocchio/multibody/model.hpp"
14 
15 namespace sobec {
16 
18  public:
19  std::string urdfPath = "";
20  std::string srdfPath = "";
21  std::string robotDescription = "";
22  std::vector<std::string> controlledJointsNames;
23 
24  std::string leftFootName = "";
25  std::string rightFootName = "";
26 };
27 
29  private:
30  RobotDesignerSettings settings_;
31 
32  std::vector<unsigned long> controlled_joints_id_;
33  unsigned long leftFootId_, rightFootId_;
34 
35  pinocchio::Model rModelComplete_, rModel_;
36  pinocchio::Data rDataComplete_, rData_;
37  // std::vector<pinocchio::JointIndex> pinocchioControlledJoints_;
38 
39  Eigen::VectorXd q0Complete_, q0_;
40  Eigen::VectorXd v0Complete_, v0_;
41  Eigen::VectorXd x0_;
42 
43  Eigen::Vector3d com_position_;
44  Eigen::Vector3d LF_position_;
45  Eigen::Vector3d RF_position_;
46 
47  // Memori allocations
48  double mass_ = 0;
49 
50  public:
51  RobotDesigner();
52  RobotDesigner(const RobotDesignerSettings &settings);
53  void initialize(const RobotDesignerSettings &settings);
54  bool initialized_ = false;
55 
56  void updateReducedModel(const Eigen::VectorXd &x);
57  void updateCompleteModel(const Eigen::VectorXd &x);
58 
59  const pinocchio::SE3 &get_LF_frame();
60  const pinocchio::SE3 &get_RF_frame();
61 
62  double getRobotMass();
63 
64  const pinocchio::Model &get_rModel() { return rModel_; }
65  const pinocchio::Model &get_rModelComplete() { return rModelComplete_; }
66  const pinocchio::Data &get_rData() { return rData_; }
67  const pinocchio::Data &get_rDataComplete() { return rDataComplete_; }
68  const Eigen::VectorXd &get_q0() { return q0_; }
69  const Eigen::VectorXd &get_v0() { return v0_; }
70  const Eigen::VectorXd &get_q0Complete() { return q0Complete_; }
71  const Eigen::VectorXd &get_v0Complete() { return v0Complete_; }
72  const Eigen::VectorXd &get_x0() { return x0_; }
73 
74  const std::string &get_LF_name() { return settings_.leftFootName; }
75  const std::string &get_RF_name() { return settings_.rightFootName; }
76  const pinocchio::FrameIndex &get_LF_id() { return leftFootId_; }
77  const pinocchio::FrameIndex &get_RF_id() { return rightFootId_; }
78  const RobotDesignerSettings &get_settings() { return settings_; }
79  const std::vector<unsigned long> &get_controlledJointsIDs() {
80  return controlled_joints_id_;
81  }
82 
83  const Eigen::Vector3d &get_LF_position() { return LF_position_; }
84  const Eigen::Vector3d &get_RF_position() { return RF_position_; }
85  const Eigen::Vector3d &get_com_position() { return com_position_; }
86 };
87 
88 } // namespace sobec
89 #endif // SOBEC_DESIGNER
sobec::RobotDesigner::get_x0
const Eigen::VectorXd & get_x0()
Definition: designer.hpp:72
sobec::RobotDesignerSettings::srdfPath
std::string srdfPath
Definition: designer.hpp:20
sobec::RobotDesigner::getRobotMass
double getRobotMass()
Definition: designer.cpp:136
sobec::RobotDesigner::get_LF_frame
const pinocchio::SE3 & get_LF_frame()
Definition: designer.cpp:128
sobec::RobotDesigner::initialized_
bool initialized_
Definition: designer.hpp:54
sobec::RobotDesigner::get_controlledJointsIDs
const std::vector< unsigned long > & get_controlledJointsIDs()
Definition: designer.hpp:79
sobec::RobotDesigner::get_rModel
const pinocchio::Model & get_rModel()
Definition: designer.hpp:64
sobec::RobotDesigner::get_com_position
const Eigen::Vector3d & get_com_position()
Definition: designer.hpp:85
sobec::RobotDesignerSettings
Definition: designer.hpp:17
sobec::RobotDesignerSettings::leftFootName
std::string leftFootName
Definition: designer.hpp:24
sobec::RobotDesigner::get_RF_id
const pinocchio::FrameIndex & get_RF_id()
Definition: designer.hpp:77
sobec::RobotDesigner
Definition: designer.hpp:28
sobec::RobotDesignerSettings::rightFootName
std::string rightFootName
Definition: designer.hpp:25
sobec::RobotDesigner::updateReducedModel
void updateReducedModel(const Eigen::VectorXd &x)
Definition: designer.cpp:105
sobec::RobotDesignerSettings::controlledJointsNames
std::vector< std::string > controlledJointsNames
Definition: designer.hpp:22
sobec::RobotDesigner::get_LF_position
const Eigen::Vector3d & get_LF_position()
Definition: designer.hpp:83
sobec::RobotDesignerSettings::urdfPath
std::string urdfPath
Definition: designer.hpp:19
sobec::RobotDesigner::updateCompleteModel
void updateCompleteModel(const Eigen::VectorXd &x)
Definition: designer.cpp:116
sobec
Definition: activation-quad-ref.hpp:19
sobec::RobotDesigner::get_settings
const RobotDesignerSettings & get_settings()
Definition: designer.hpp:78
sobec::RobotDesigner::get_rDataComplete
const pinocchio::Data & get_rDataComplete()
Definition: designer.hpp:67
sobec::RobotDesigner::get_rModelComplete
const pinocchio::Model & get_rModelComplete()
Definition: designer.hpp:65
sobec::RobotDesigner::get_rData
const pinocchio::Data & get_rData()
Definition: designer.hpp:66
sobec::RobotDesigner::get_LF_id
const pinocchio::FrameIndex & get_LF_id()
Definition: designer.hpp:76
sobec::newcontacts::x
@ x
Definition: contact1d.hpp:26
sobec::RobotDesigner::get_LF_name
const std::string & get_LF_name()
Definition: designer.hpp:74
sobec::RobotDesigner::get_q0
const Eigen::VectorXd & get_q0()
Definition: designer.hpp:68
sobec::RobotDesignerSettings::robotDescription
std::string robotDescription
Definition: designer.hpp:21
sobec::RobotDesigner::get_RF_frame
const pinocchio::SE3 & get_RF_frame()
Definition: designer.cpp:132
sobec::RobotDesigner::get_v0
const Eigen::VectorXd & get_v0()
Definition: designer.hpp:69
sobec::RobotDesigner::get_v0Complete
const Eigen::VectorXd & get_v0Complete()
Definition: designer.hpp:71
sobec::RobotDesigner::RobotDesigner
RobotDesigner()
Definition: designer.cpp:10
sobec::RobotDesigner::get_q0Complete
const Eigen::VectorXd & get_q0Complete()
Definition: designer.hpp:70
sobec::RobotDesigner::get_RF_name
const std::string & get_RF_name()
Definition: designer.hpp:75
sobec::RobotDesigner::initialize
void initialize(const RobotDesignerSettings &settings)
Definition: designer.cpp:16
sobec::RobotDesigner::get_RF_position
const Eigen::Vector3d & get_RF_position()
Definition: designer.hpp:84