25 #ifndef _RIGID_BODY_SYSTEM_
26 #define _RIGID_BODY_SYSTEM_
28 #include <FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.h>
32 #include <privatepgtypes.hh>
58 int interpolate(solution_t Result, std::deque<ZMPPosition> &FinalZMPTraj_deq,
59 std::deque<COMState> &FinalCOMTraj_deq,
60 std::deque<FootAbsolutePosition> &FinalLeftFootTraj_deq,
61 std::deque<FootAbsolutePosition> &FinalRightFootTraj_deq);
70 int update(
const std::deque<support_state_t> &SupportStates_deq,
71 const std::deque<FootAbsolutePosition> &LeftFootTraj_deq,
72 const std::deque<FootAbsolutePosition> &RightFootTraj_deq);
92 double time,
const solution_t &Result,
93 const std::deque<support_state_t> &SupportStates_deq,
94 const std::deque<double> &PreviewedSupportAngles_deq,
95 std::deque<FootAbsolutePosition> &LeftFootTraj_deq,
96 std::deque<FootAbsolutePosition> &RightFootTraj_deq);
123 inline double Mass()
const {
return mass_; }
127 inline void CoMHeight(
double Height) { CoMHeight_ = Height; }
133 return SupportTrajectory_deq_;
146 int compute_dyn_cop(
unsigned nbSteps);
162 int compute_foot_zero_dynamics(
163 const std::deque<support_state_t> &SupportStates_deq,
174 int compute_foot_pol_dynamics(
175 const std::deque<support_state_t> &SupportStates_deq,
186 int compute_foot_cjerk_dynamics(
187 const std::deque<support_state_t> &SupportStates_deq,
192 int initialize_trajectories();
196 int precompute_trajectories(
197 const std::deque<support_state_t> &SupportStates_deq);
206 int compute_sbar(
double *Spbar,
double *Sabar,
double T,
double Td);
215 int compute_ubar(
double *Upbar,
double *Uabar,
double T,
double Td);
222 RigidBody CoM_, LeftFoot_, RightFoot_, LeftWrist_, RightWrist_;
232 std::deque<rigid_body_state_t> FlyingFootTrajectory_deq_;
235 std::deque<support_state_t> SupportTrajectory_deq_;
239 std::deque<double> GRF_deq_;
264 OnLineFootTrajectoryGeneration *OFTG_;
Definition: pinocchiorobot.hh:57
Definition: rigid-body-system.hh:35
double CoMHeight() const
Definition: rigid-body-system.hh:126
double SamplingPeriodSim() const
Definition: rigid-body-system.hh:114
std::deque< support_state_t > & SupportTrajectory()
Definition: rigid-body-system.hh:132
void multiBody(bool multiBody)
Definition: rigid-body-system.hh:130
int compute_dyn_cjerk()
Initialize dynamics of the body center Suppose a piecewise constant jerk.
Definition: rigid-body-system.cpp:380
void RightFoot(const RigidBody &RightFoot)
Definition: rigid-body-system.hh:112
void initialize()
Initialize.
Definition: rigid-body-system.cpp:44
void SamplingPeriodSim(double T)
Definition: rigid-body-system.hh:115
linear_dynamics_t & DynamicsCoPJerk()
Definition: rigid-body-system.hh:101
void Mass(double Mass)
Definition: rigid-body-system.hh:124
RigidBody const & LeftFoot() const
Definition: rigid-body-system.hh:106
void LeftFoot(const RigidBody &LeftFoot)
Definition: rigid-body-system.hh:108
int update(const std::deque< support_state_t > &SupportStates_deq, const std::deque< FootAbsolutePosition > &LeftFootTraj_deq, const std::deque< FootAbsolutePosition > &RightFootTraj_deq)
Update feet matrices.
Definition: rigid-body-system.cpp:216
unsigned NbSamplingsPreviewed() const
Definition: rigid-body-system.hh:120
double Mass() const
Definition: rigid-body-system.hh:123
double SamplingPeriodAct() const
Definition: rigid-body-system.hh:117
void CoM(const RigidBody &CoM)
Definition: rigid-body-system.hh:104
~RigidBodySystem()
Definition: rigid-body-system.cpp:39
RigidBody & RightFoot()
Definition: rigid-body-system.hh:111
RigidBody const & RightFoot() const
Definition: rigid-body-system.hh:110
int generate_trajectories(double time, const solution_t &Result, const std::deque< support_state_t > &SupportStates_deq, const std::deque< double > &PreviewedSupportAngles_deq, std::deque< FootAbsolutePosition > &LeftFootTraj_deq, std::deque< FootAbsolutePosition > &RightFootTraj_deq)
Generate final trajectories.
Definition: rigid-body-system.cpp:739
linear_dynamics_t const & DynamicsCoPJerk() const
Definition: rigid-body-system.hh:100
void CoMHeight(double Height)
Definition: rigid-body-system.hh:127
int interpolate(solution_t Result, std::deque< ZMPPosition > &FinalZMPTraj_deq, std::deque< COMState > &FinalCOMTraj_deq, std::deque< FootAbsolutePosition > &FinalLeftFootTraj_deq, std::deque< FootAbsolutePosition > &FinalRightFootTraj_deq)
Interpolate.
RigidBodySystem(SimplePluginManager *SPM, PinocchioRobot *aPR, SupportFSM *FSM)
Definition: rigid-body-system.cpp:30
bool multiBody() const
Definition: rigid-body-system.hh:129
RigidBody const & CoM() const
Definition: rigid-body-system.hh:103
RigidBody & LeftFoot()
Definition: rigid-body-system.hh:107
void NbSamplingsPreviewed(unsigned N)
Definition: rigid-body-system.hh:121
void SamplingPeriodAct(double Ta)
Definition: rigid-body-system.hh:118
Definition: rigid-body.hh:83
Finite state machine to determine the support parameters.
Definition: SupportFSM.hh:35
\doc Simulate a rigid body
Definition: patterngeneratorinterface.hh:41
Definition: rigid-body.hh:59