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 //
86 // Public methods
87 //
88public:
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
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 //
142private:
143 //
144 // Private members
145 //
146private:
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_ */
Definition: rigid-body.hh:83
rigid_body_state_t & State()
Definition: rigid-body.hh:135
int initialize()
Initialize.
Definition: rigid-body.cpp:40
int inject_trajectory(unsigned int Axis, Eigen::VectorXd Trajectory)
Decouple degree of freedom by injected trajectory.
unsigned const & NbSamplingsPreviewed() const
Definition: rigid-body.hh:127
RigidBody()
Definition: rigid-body.cpp:30
~RigidBody()
Definition: rigid-body.cpp:38
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:79
void NbSamplingsPreviewed(unsigned N)
Definition: rigid-body.hh:128
rigid_body_state_t const & State() const
Definition: rigid-body.hh:136
void SamplingPeriodAct(double Ta)
Definition: rigid-body.hh:125
rigid_body_state_t increment_state(double Control)
Increment the state.
double const & Mass() const
Definition: rigid-body.hh:130
double const & SamplingPeriodSim() const
Definition: rigid-body.hh:121
void Mass(double Mass)
Definition: rigid-body.hh:131
double const & SamplingPeriodAct() const
Definition: rigid-body.hh:124
std::deque< rigid_body_state_t > & Trajectory()
Definition: rigid-body.hh:133
void SamplingPeriodSim(double T)
Definition: rigid-body.hh:122
\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:122
struct rigid_body_state_s & operator=(const rigid_body_state_s &RB)
Definition: rigid-body.cpp:125
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:139
Eigen::VectorXd Roll
Definition: rigid-body.hh:45