ORCA: Optimization-based framework for Robotic Control Applications
RobotModel.h
Go to the documentation of this file.
1 //| This file is a part of the ORCA framework.
2 //|
3 //| Copyright 2018, Fuzzy Logic Robotics
4 //| Copyright 2017, ISIR / Universite Pierre et Marie Curie (UPMC)
5 //|
6 //| Main contributor(s): Antoine Hoarau, Ryan Lober, and
7 //| Fuzzy Logic Robotics <info@fuzzylogicrobotics.com>
8 //|
9 //| ORCA is a whole-body reactive controller framework for robotics.
10 //|
11 //| This software is governed by the CeCILL-C license under French law and
12 //| abiding by the rules of distribution of free software. You can use,
13 //| modify and/ or redistribute the software under the terms of the CeCILL-C
14 //| license as circulated by CEA, CNRS and INRIA at the following URL
15 //| "http://www.cecill.info".
16 //|
17 //| As a counterpart to the access to the source code and rights to copy,
18 //| modify and redistribute granted by the license, users are provided only
19 //| with a limited warranty and the software's author, the holder of the
20 //| economic rights, and the successive licensors have only limited
21 //| liability.
22 //|
23 //| In this respect, the user's attention is drawn to the risks associated
24 //| with loading, using, modifying and/or developing or reproducing the
25 //| software by the user in light of its specific status of free software,
26 //| that may mean that it is complicated to manipulate, and that also
27 //| therefore means that it is reserved for developers and experienced
28 //| professionals having in-depth computer knowledge. Users are therefore
29 //| encouraged to load and test the software's suitability as regards their
30 //| requirements in conditions enabling the security of their systems and/or
31 //| data to be ensured and, more generally, to use and operate it in the
32 //| same conditions as regards security.
33 //|
34 //| The fact that you are presently reading this means that you have had
35 //| knowledge of the CeCILL-C license and that you accept its terms.
36 
37 #pragma once
38 
39 #include "orca/utils/Utils.h"
40 #include "orca/math/Utils.h"
42 
43 namespace orca
44 {
45 namespace robot
46 {
51 struct RobotState
52 {
53  void resize(int nrOfInternalDOFs)
54  {
55  jointPos.resize(nrOfInternalDOFs);
56  jointVel.resize(nrOfInternalDOFs);
57  reset();
58  }
59 
60  void reset()
61  {
62  world_H_base.setIdentity();
63  jointPos.setZero();
64  baseVel.setZero();
65  jointVel.setZero();
66  gravity[0] = 0;
67  gravity[1] = 0;
68  gravity[2] = -9.81;
69  }
70 
71  Eigen::Matrix4d world_H_base;
72  Eigen::VectorXd jointPos;
73  Eigen::Matrix<double,6,1> baseVel;
74  Eigen::VectorXd jointVel;
75  Eigen::Vector3d gravity;
76 };
77 
79 {
80  void resize(int nrOfInternalDOFs)
81  {
82  jointAcc.resize(nrOfInternalDOFs);
83  baseAcc.setZero();
84  jointAcc.setZero();
85  }
86 
87  void random()
88  {
89  baseAcc.setRandom();
90  jointAcc.setRandom();
91  }
92 
93  void setZero()
94  {
95  baseAcc.setZero();
96  jointAcc.setZero();
97  }
98 
99  Eigen::Matrix<double,6,1> baseAcc;
100  Eigen::VectorXd jointAcc;
101 };
102 
103 
109 {
110 public:
111  using Ptr = std::shared_ptr<RobotModel>;
117  RobotModel(const std::string& name="");
118  virtual ~RobotModel();
125  bool loadModelFromFile(const std::string& modelFile);
132  bool loadModelFromString(const std::string &modelString);
138  const std::string& getUrdfUrl() const;
144  const std::string& getUrdfString() const;
154  void setRobotState(const Eigen::Matrix4d& world_H_base
155  , const Eigen::VectorXd& jointPos
156  , const Eigen::Matrix<double,6,1>& baseVel
157  , const Eigen::VectorXd& jointVel
158  , const Eigen::Vector3d& gravity);
166  void setRobotState(const Eigen::VectorXd& jointPos
167  , const Eigen::VectorXd& jointVel
168  , const Eigen::Vector3d& global_gravity_vector);
175  void setRobotState(const Eigen::VectorXd& jointPos
176  , const Eigen::VectorXd& jointVel);
181  const std::string& getName() const;
186  void print() const;
195  void setBaseFrame(const std::string& fixed_base_or_free_floating_frame);
201  const std::string& getBaseFrame() const;
207  void setGravity(const Eigen::Vector3d& global_gravity_vector);
213  unsigned int getNrOfDegreesOfFreedom() const;
219  unsigned int getConfigurationSpaceDimension() const;
226  bool frameExists(const std::string& frame_name) const;
227 
235  const Eigen::Matrix4d& getRelativeTransform(const std::string& refFrameName, const std::string& frameName);
242  const Eigen::Matrix4d& getTransform(const std::string& frameName);
243 
252  bool addAdditionalFrameToLink (const std::string &linkName, const std::string &frameName, const Eigen::Matrix4d& link_H_frame);
253 
254  const Eigen::Matrix<double,6,1>& getFrameVel(const std::string& frameName);
255  const Eigen::Matrix<double,6,1>& getFrameBiasAcc(const std::string& frameName);
256 
257  const Eigen::MatrixXd& getFreeFloatingMassMatrix();
258  const Eigen::MatrixXd& getMassMatrix();
259  const Eigen::VectorXd& getJointPos() const;
260  const Eigen::VectorXd& getJointVel() const;
261  const Eigen::VectorXd& getMinJointPos();
262  const Eigen::VectorXd& getMaxJointPos();
263  const Eigen::MatrixXd& getJacobian(const std::string& frameName);
264  const Eigen::MatrixXd& getRelativeJacobian(const std::string& refFrameName, const std::string& frameName);
265  const Eigen::MatrixXd& getFrameFreeFloatingJacobian(const std::string& frameName);
266  const Eigen::VectorXd& generalizedBiasForces();
267  const Eigen::VectorXd& getJointGravityTorques();
268  const Eigen::VectorXd& getJointCoriolisTorques();
269  const Eigen::VectorXd& getJointGravityAndCoriolisTorques();
270  unsigned int getNrOfJoints() const;
271  std::string getJointName(unsigned int idx) const;
272  unsigned int getJointIndex(const std::string& jointName) const;
273  const std::vector<std::string>& getLinkNames() const;
274  const std::vector<std::string>& getFrameNames() const;
275  const std::vector<std::string>& getJointNames() const;
276  bool isInitialized() const;
277  void onRobotInitializedCallback(std::function<void(void)> cb);
278 protected:
279 
280  enum RobotModelImplType { iDynTree, KDL };
281  RobotModelImplType robot_kinematics_type_ = iDynTree;
282 
283  std::function<void(void)> robot_initialized_cb_;
284  bool is_initialized_ = false;
285 
292 
293  bool loadFromParameters();
294 private:
295 
296  template<RobotModelImplType type = iDynTree> class RobotModelImpl;
297  std::unique_ptr<RobotModelImpl<> > impl_;
298 
299 };
300 
301 } // namespace robot
302 } // namespace orca
Represents a set of parameters that can be loaded from a YAML file.
Definition: ConfigurableOrcaObject.h:14
Eigen::VectorXd jointPos
Definition: RobotModel.h:72
common::Parameter< std::string > urdf_str_
Definition: RobotModel.h:287
common::Parameter< std::string > name_
Definition: RobotModel.h:291
void random()
Definition: RobotModel.h:87
Eigen::VectorXd jointVel
Definition: RobotModel.h:74
std::shared_ptr< ConfigurableOrcaObject > Ptr
Definition: ConfigurableOrcaObject.h:17
Eigen::Matrix4d world_H_base
Definition: RobotModel.h:71
Eigen::Vector3d gravity
Definition: RobotModel.h:75
Eigen::VectorXd jointAcc
Definition: RobotModel.h:100
The robot model class allow to make kinematics and dynamics computations.
Definition: RobotModel.h:108
Eigen::Matrix< double, 6, 1 > baseAcc
Definition: RobotModel.h:99
common::Parameter< Eigen::VectorXd > home_joint_positions_
Definition: RobotModel.h:290
Definition: RobotModel.h:78
common::Parameter< Eigen::Vector3d > gravity_
Definition: RobotModel.h:289
Eigen::Matrix< double, 6, 1 > baseVel
Definition: RobotModel.h:73
void setZero()
Definition: RobotModel.h:93
RobotModelImplType
Definition: RobotModel.h:280
common::Parameter< std::string > base_frame_
Definition: RobotModel.h:288
Definition: CartesianAccelerationPID.h:44
void reset()
Definition: RobotModel.h:60
void resize(int nrOfInternalDOFs)
Definition: RobotModel.h:80
common::Parameter< std::string > urdf_url_
Definition: RobotModel.h:286
void resize(int nrOfInternalDOFs)
Definition: RobotModel.h:53
Definition: RobotModel.h:51
std::function< void(void)> robot_initialized_cb_
Definition: RobotModel.h:283