rigid-body.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2011
3  *
4  * Andrei Herdt
5  *
6  * JRL, CNRS/AIST, INRIA Grenoble-Rhone-Alpes
7  *
8  * This file is part of walkGenJrl.
9  * walkGenJrl is free software: you can redistribute it and/or modify
10  * it under the terms of the GNU Lesser General Public License as published by
11  * the Free Software Foundation, either version 3 of the License, or
12  * (at your option) any later version.
13  *
14  * walkGenJrl is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU General Lesser Public License for more details.
18  * You should have received a copy of the GNU Lesser General Public License
19  * along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>.
20  *
21  */
22 
24 
25 #ifndef _RIGID_BODY_
26 #define _RIGID_BODY_
27 
28 #include <deque>
29 #include <jrl/walkgen/pgtypes.hh>
30 #include <privatepgtypes.hh>
31 
32 namespace PatternGeneratorJRL {
33 
38  Eigen::VectorXd X;
39  Eigen::VectorXd Y;
40  Eigen::VectorXd Z;
44  Eigen::VectorXd Pitch;
45  Eigen::VectorXd Roll;
46  Eigen::VectorXd Yaw;
48 
50 
51  void reset();
52 
54 };
56 
61  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> U;
62 
64  Eigen::MatrixXd Um1;
65 
67  Eigen::MatrixXd UT;
68 
70  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> S;
71 
72  dynamics_e Type;
73 
74  void clear() {
75  U.setZero();
76  UT.setZero();
77  S.setZero();
78  }
79 };
82 
83 class RigidBody {
84 
85  //
86  // Public methods
87  //
88 public:
89  RigidBody();
90 
91  ~RigidBody();
92 
94  int interpolate(std::deque<COMState> &COMStates,
95  std::deque<ZMPPosition> &ZMPRefPositions, int CurrentPosition,
96  double CX, double CY);
97 
101  int initialize();
102 
106  rigid_body_state_t increment_state(double Control);
107 
114  int inject_trajectory(unsigned int Axis, Eigen::VectorXd Trajectory);
115 
118  linear_dynamics_t const &Dynamics(dynamics_e) const;
119  linear_dynamics_t &Dynamics(dynamics_e);
120 
121  inline double const &SamplingPeriodSim() const { return T_; }
122  inline void SamplingPeriodSim(double T) { T_ = T; }
123 
124  inline double const &SamplingPeriodAct() const { return Ta_; }
125  inline void SamplingPeriodAct(double Ta) { Ta_ = Ta; }
126 
127  inline unsigned const &NbSamplingsPreviewed() const { return N_; }
128  inline void NbSamplingsPreviewed(unsigned N) { N_ = N; }
129 
130  inline double const &Mass() const { return Mass_; }
131  inline void Mass(double Mass) { Mass_ = Mass; }
132 
133  inline std::deque<rigid_body_state_t> &Trajectory() { return Trajectory_; }
134 
135  inline rigid_body_state_t &State() { return State_; }
136  inline rigid_body_state_t const &State() const { return State_; }
138 
139  //
140  // Private member functions
141  //
142 private:
143  //
144  // Private members
145  //
146 private:
148  rigid_body_state_t State_;
149 
151  std::deque<rigid_body_state_t> Trajectory_;
152 
155  linear_dynamics_t PositionDynamics_, VelocityDynamics_, AccelerationDynamics_,
156  JerkDynamics_, CoPDynamics_;
158 
160  double T_;
161 
164  double Tr_;
165 
167  double Ta_;
168 
170  unsigned int N_;
171 
173  double Mass_;
174 };
175 } // namespace PatternGeneratorJRL
176 #endif /* _RIGID_BODY_ */
PatternGeneratorJRL::RigidBody::increment_state
rigid_body_state_t increment_state(double Control)
Increment the state.
PatternGeneratorJRL::linear_dynamics_s
Definition: rigid-body.hh:59
PatternGeneratorJRL::RigidBody::NbSamplingsPreviewed
void NbSamplingsPreviewed(unsigned N)
Definition: rigid-body.hh:128
PatternGeneratorJRL::rigid_body_state_s
State vectors.
Definition: rigid-body.hh:35
PatternGeneratorJRL::RigidBody::SamplingPeriodAct
void SamplingPeriodAct(double Ta)
Definition: rigid-body.hh:125
PatternGeneratorJRL::RigidBody::inject_trajectory
int inject_trajectory(unsigned int Axis, Eigen::VectorXd Trajectory)
Decouple degree of freedom by injected trajectory.
PatternGeneratorJRL::rigid_body_state_s::operator=
struct rigid_body_state_s & operator=(const rigid_body_state_s &RB)
Definition: rigid-body.cpp:125
pgtypes.hh
PatternGeneratorJRL::RigidBody::initialize
int initialize()
Initialize.
Definition: rigid-body.cpp:40
PatternGeneratorJRL::rigid_body_state_s::Y
Eigen::VectorXd Y
Definition: rigid-body.hh:39
PatternGeneratorJRL::RigidBody
Definition: rigid-body.hh:83
PatternGeneratorJRL::linear_dynamics_t
linear_dynamics_s linear_dynamics_t
Definition: rigid-body.hh:80
PatternGeneratorJRL::RigidBody::NbSamplingsPreviewed
const unsigned & NbSamplingsPreviewed() const
Definition: rigid-body.hh:127
PatternGeneratorJRL::RigidBody::~RigidBody
~RigidBody()
Definition: rigid-body.cpp:38
PatternGeneratorJRL::RigidBody::RigidBody
RigidBody()
Definition: rigid-body.cpp:30
PatternGeneratorJRL::linear_dynamics_s::clear
void clear()
Definition: rigid-body.hh:74
PatternGeneratorJRL::linear_dynamics_s::UT
Eigen::MatrixXd UT
Transpose of control matrix.
Definition: rigid-body.hh:67
PatternGeneratorJRL::linear_dynamics_s::U
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > U
Control matrix.
Definition: rigid-body.hh:61
PatternGeneratorJRL::linear_dynamics_s::Um1
Eigen::MatrixXd Um1
Inverse of control matrix.
Definition: rigid-body.hh:64
PatternGeneratorJRL::RigidBody::Dynamics
const linear_dynamics_t & Dynamics(dynamics_e) const
Definition: rigid-body.cpp:79
PatternGeneratorJRL::rigid_body_state_s::X
Eigen::VectorXd X
Definition: rigid-body.hh:38
PatternGeneratorJRL::RigidBody::State
const rigid_body_state_t & State() const
Definition: rigid-body.hh:136
PatternGeneratorJRL::rigid_body_state_s::reset
void reset()
Definition: rigid-body.cpp:139
PatternGeneratorJRL
\doc Simulate a rigid body
Definition: patterngeneratorinterface.hh:41
PatternGeneratorJRL::RigidBody::interpolate
int interpolate(std::deque< COMState > &COMStates, std::deque< ZMPPosition > &ZMPRefPositions, int CurrentPosition, double CX, double CY)
Interpolate.
PatternGeneratorJRL::linear_dynamics_s::Type
dynamics_e Type
Definition: rigid-body.hh:72
PatternGeneratorJRL::RigidBody::SamplingPeriodSim
const double & SamplingPeriodSim() const
Definition: rigid-body.hh:121
PatternGeneratorJRL::rigid_body_state_s::Pitch
Eigen::VectorXd Pitch
Definition: rigid-body.hh:44
PatternGeneratorJRL::RigidBody::Trajectory
std::deque< rigid_body_state_t > & Trajectory()
Definition: rigid-body.hh:133
PatternGeneratorJRL::rigid_body_state_s::Roll
Eigen::VectorXd Roll
Definition: rigid-body.hh:45
PatternGeneratorJRL::RigidBody::SamplingPeriodAct
const double & SamplingPeriodAct() const
Definition: rigid-body.hh:124
PatternGeneratorJRL::linear_dynamics_s::S
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > S
State matrix.
Definition: rigid-body.hh:70
PatternGeneratorJRL::RigidBody::Mass
void Mass(double Mass)
Definition: rigid-body.hh:131
PatternGeneratorJRL::rigid_body_state_s::Z
Eigen::VectorXd Z
Definition: rigid-body.hh:40
PatternGeneratorJRL::rigid_body_state_s::rigid_body_state_s
rigid_body_state_s()
Definition: rigid-body.cpp:122
PatternGeneratorJRL::rigid_body_state_s::Yaw
Eigen::VectorXd Yaw
Definition: rigid-body.hh:46
PatternGeneratorJRL::RigidBody::Mass
const double & Mass() const
Definition: rigid-body.hh:130
PatternGeneratorJRL::RigidBody::SamplingPeriodSim
void SamplingPeriodSim(double T)
Definition: rigid-body.hh:122
PatternGeneratorJRL::RigidBody::State
rigid_body_state_t & State()
Definition: rigid-body.hh:135