rigid-body-system.hh

Go to the documentation of this file.

/*
 * Copyright 2011
 *
 * Andrei Herdt
 *
 * JRL, CNRS/AIST, INRIA Grenoble-Rhone-Alpes
 *
 * This file is part of walkGenJrl.
 * walkGenJrl is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * walkGenJrl is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Lesser Public License for more details.
 * You should have received a copy of the GNU Lesser General Public License
 * along with walkGenJrl.  If not, see <http://www.gnu.org/licenses/>.
 *
 */
 
 
#ifndef _RIGID_BODY_SYSTEM_
#define _RIGID_BODY_SYSTEM_
 
#include <FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.h>
 
#include <PreviewControl/SupportFSM.hh>
#include <PreviewControl/rigid-body.hh>
#include <jrl/walkgen/pinocchiorobot.hh>
#include <privatepgtypes.hh>
 
namespace PatternGeneratorJRL {
class RigidBodySystem {
  //
  // Public methods
  //
 public:
  RigidBodySystem(SimplePluginManager *SPM, PinocchioRobot *aPR,
                  SupportFSM *FSM);
 
  ~RigidBodySystem();
 
  void initialize();
 
  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);
 
  int update(const std::deque<support_state_t> &SupportStates_deq,
             const std::deque<FootAbsolutePosition> &LeftFootTraj_deq,
             const std::deque<FootAbsolutePosition> &RightFootTraj_deq);
 
  int compute_dyn_cjerk();
 
  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);
 
  linear_dynamics_t const &DynamicsCoPJerk() const { return CoPDynamicsJerk_; }
  linear_dynamics_t &DynamicsCoPJerk() { return CoPDynamicsJerk_; }
 
  inline RigidBody const &CoM() const { return CoM_; };
  inline void CoM(const RigidBody &CoM) { CoM_ = CoM; };
 
  inline RigidBody const &LeftFoot() const { return LeftFoot_; };
  inline RigidBody &LeftFoot() { return LeftFoot_; };
  inline void LeftFoot(const RigidBody &LeftFoot) { LeftFoot_ = LeftFoot; };
 
  inline RigidBody const &RightFoot() const { return RightFoot_; };
  inline RigidBody &RightFoot() { return RightFoot_; };
  inline void RightFoot(const RigidBody &RightFoot) { RightFoot_ = RightFoot; };
 
  inline double SamplingPeriodSim() const { return T_; }
  inline void SamplingPeriodSim(double T) { T_ = T; }
 
  inline double SamplingPeriodAct() const { return Ta_; }
  inline void SamplingPeriodAct(double Ta) { Ta_ = Ta; }
 
  inline unsigned NbSamplingsPreviewed() const { return N_; }
  inline void NbSamplingsPreviewed(unsigned N) { N_ = N; }
 
  inline double Mass() const { return mass_; }
  inline void Mass(double Mass) { mass_ = Mass; }
 
  inline double CoMHeight() const { return CoMHeight_; }
  inline void CoMHeight(double Height) { CoMHeight_ = Height; }
 
  inline bool multiBody() const { return multiBody_; }
  inline void multiBody(bool multiBody) { multiBody_ = multiBody; }
 
  std::deque<support_state_t> &SupportTrajectory() {
    return SupportTrajectory_deq_;
  }
 
  //
  // Private methods
  //
 private:
  int compute_dyn_cop(unsigned nbSteps);
 
  int compute_dyn_cjerk(linear_dynamics_t &Dynamics);
 
  int compute_foot_zero_dynamics(
      const std::deque<support_state_t> &SupportStates_deq,
      linear_dynamics_t &LeftFootDynamics,
      linear_dynamics_t &RightFootDynamics);
 
  int compute_foot_pol_dynamics(
      const std::deque<support_state_t> &SupportStates_deq,
      linear_dynamics_t &LeftFootDynamics,
      linear_dynamics_t &RightFootDynamics);
 
  int compute_foot_cjerk_dynamics(
      const std::deque<support_state_t> &SupportStates_deq,
      linear_dynamics_t &LeftFootDynamics,
      linear_dynamics_t &RightFootDynamics);
 
  int initialize_trajectories();
 
  int precompute_trajectories(
      const std::deque<support_state_t> &SupportStates_deq);
 
  int compute_sbar(double *Spbar, double *Sabar, double T, double Td);
 
  int compute_ubar(double *Upbar, double *Uabar, double T, double Td);
 
  //
  // Private members
  //
 private:
  RigidBody CoM_, LeftFoot_, RightFoot_, LeftWrist_, RightWrist_;
 
  linear_dynamics_t CoPDynamicsJerk_;
 
  std::deque<rigid_body_state_t> FlyingFootTrajectory_deq_;
 
  std::deque<support_state_t> SupportTrajectory_deq_;
  // \}
 
  std::deque<double> GRF_deq_;
 
  double mass_;
 
  double CoMHeight_;
 
  double T_;
 
  double Tr_;
 
  double Ta_;
 
  unsigned int N_;
 
  bool multiBody_;
 
  OnLineFootTrajectoryGeneration *OFTG_;
 
  SupportFSM *FSM_;
 
  PinocchioRobot *PR_;
};
}  // namespace PatternGeneratorJRL
#endif /* _RIGID_BODY_SYSTEM_ */