Loading...
Searching...
No Matches
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>
30#include <privatepgtypes.hh>
31
32namespace 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
83class RigidBody {
84 //
85 // Public methods
86 //
87 public:
88 RigidBody();
89
90 ~RigidBody();
91
93 int interpolate(std::deque<COMState> &COMStates,
94 std::deque<ZMPPosition> &ZMPRefPositions, int CurrentPosition,
95 double CX, double CY);
96
100 int initialize();
101
106
113 int inject_trajectory(unsigned int Axis, Eigen::VectorXd Trajectory);
114
117 linear_dynamics_t const &Dynamics(dynamics_e) const;
118 linear_dynamics_t &Dynamics(dynamics_e);
119
120 inline double const &SamplingPeriodSim() const { return T_; }
121 inline void SamplingPeriodSim(double T) { T_ = T; }
122
123 inline double const &SamplingPeriodAct() const { return Ta_; }
124 inline void SamplingPeriodAct(double Ta) { Ta_ = Ta; }
125
126 inline unsigned const &NbSamplingsPreviewed() const { return N_; }
127 inline void NbSamplingsPreviewed(unsigned N) { N_ = N; }
128
129 inline double const &Mass() const { return Mass_; }
130 inline void Mass(double Mass) { Mass_ = Mass; }
131
132 inline std::deque<rigid_body_state_t> &Trajectory() { return Trajectory_; }
133
134 inline rigid_body_state_t &State() { return State_; }
135 inline rigid_body_state_t const &State() const { return State_; }
137
138 //
139 // Private member functions
140 //
141 private:
142 //
143 // Private members
144 //
145 private:
147 rigid_body_state_t State_;
148
150 std::deque<rigid_body_state_t> Trajectory_;
151
154 linear_dynamics_t PositionDynamics_, VelocityDynamics_, AccelerationDynamics_,
155 JerkDynamics_, CoPDynamics_;
157
159 double T_;
160
163 double Tr_;
164
166 double Ta_;
167
169 unsigned int N_;
170
172 double Mass_;
173};
174} // namespace PatternGeneratorJRL
175#endif /* _RIGID_BODY_ */
Definition rigid-body.hh:83
rigid_body_state_t & State()
Definition rigid-body.hh:134
int initialize()
Initialize.
Definition rigid-body.cpp:39
int inject_trajectory(unsigned int Axis, Eigen::VectorXd Trajectory)
Decouple degree of freedom by injected trajectory.
unsigned const & NbSamplingsPreviewed() const
Definition rigid-body.hh:126
RigidBody()
Definition rigid-body.cpp:30
~RigidBody()
Definition rigid-body.cpp:37
int interpolate(std::deque< COMState > &COMStates, std::deque< ZMPPosition > &ZMPRefPositions, int CurrentPosition, double CX, double CY)
Interpolate.
linear_dynamics_t const & Dynamics(dynamics_e) const
Definition rigid-body.cpp:77
void NbSamplingsPreviewed(unsigned N)
Definition rigid-body.hh:127
rigid_body_state_t const & State() const
Definition rigid-body.hh:135
void SamplingPeriodAct(double Ta)
Definition rigid-body.hh:124
rigid_body_state_t increment_state(double Control)
Increment the state.
double const & Mass() const
Definition rigid-body.hh:129
double const & SamplingPeriodSim() const
Definition rigid-body.hh:120
void Mass(double Mass)
Definition rigid-body.hh:130
double const & SamplingPeriodAct() const
Definition rigid-body.hh:123
std::deque< rigid_body_state_t > & Trajectory()
Definition rigid-body.hh:132
void SamplingPeriodSim(double T)
Definition rigid-body.hh:121
\doc Simulate a rigid body
Definition patterngeneratorinterface.hh:41
linear_dynamics_s linear_dynamics_t
Definition rigid-body.hh:80
Definition rigid-body.hh:59
void clear()
Definition rigid-body.hh:74
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > S
State matrix.
Definition rigid-body.hh:70
Eigen::MatrixXd UT
Transpose of control matrix.
Definition rigid-body.hh:67
Eigen::MatrixXd Um1
Inverse of control matrix.
Definition rigid-body.hh:64
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > U
Control matrix.
Definition rigid-body.hh:61
dynamics_e Type
Definition rigid-body.hh:72
State vectors.
Definition rigid-body.hh:35
Eigen::VectorXd Y
Definition rigid-body.hh:39
rigid_body_state_s()
Definition rigid-body.cpp:118
struct rigid_body_state_s & operator=(const rigid_body_state_s &RB)
Definition rigid-body.cpp:120
Eigen::VectorXd Yaw
Definition rigid-body.hh:46
Eigen::VectorXd Pitch
Definition rigid-body.hh:44
Eigen::VectorXd X
Definition rigid-body.hh:38
Eigen::VectorXd Z
Definition rigid-body.hh:40
void reset()
Definition rigid-body.cpp:134
Eigen::VectorXd Roll
Definition rigid-body.hh:45