- 3.0.2 rigid body dynamics module.
FixBaseNLOC.h
Go to the documentation of this file.
1 /**********************************************************************************************************************
2 This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich.
3 Licensed under the BSD-2 license (see LICENSE file in main directory)
4 **********************************************************************************************************************/
5 
6 #pragma once
7 
9 
10 namespace ct {
11 namespace rbd {
12 
16 template <class FIX_BASE_FD_SYSTEM>
18 {
19 public:
20  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
21 
22  static const bool eeForcesAreControlInputs = false;
23 
24  using FBSystem = FIX_BASE_FD_SYSTEM;
25 
26  static const size_t CONTROL_DIM = FBSystem::CONTROL_DIM;
27  static const size_t NJOINTS = FBSystem::NJOINTS;
28  static const size_t STATE_DIM = FBSystem::STATE_DIM;
29  static const size_t ACTUATOR_STATE_DIM = FBSystem::ACTUATOR_STATE_DIM;
30  using SCALAR = typename FBSystem::SCALAR;
31 
33  // using SystemLinearizer = ct::rbd::RbdLinearizer<FBSystem>;
34 
36  using NLOptConSolver = ct::optcon::NLOptConSolver<STATE_DIM, CONTROL_DIM, STATE_DIM / 2, STATE_DIM / 2, SCALAR>;
37 
46 
49 
50 
52  FixBaseNLOC() = default;
53 
56  const typename NLOptConSolver::Settings_t& nlocSettings,
57  std::shared_ptr<FBSystem> system = std::shared_ptr<FBSystem>(new FBSystem),
58  bool verbose = false,
59  std::shared_ptr<LinearizedSystem> linearizedSystem = nullptr);
60 
66  const typename NLOptConSolver::Settings_t& nlocSettings,
67  std::shared_ptr<FBSystem> system = std::shared_ptr<FBSystem>(new FBSystem),
68  bool verbose = false,
69  std::shared_ptr<LinearizedSystem> linearizedSystem = nullptr);
70 
71  void initialize(const RobotState_t& x0,
72  const core::Time& tf,
74  FeedbackArray u0_fb = FeedbackArray(),
76 
78  void initializeSteadyPose(const RobotState_t& x0,
79  const core::Time& tf,
80  const int N,
81  ControlVector& u_ref,
82  FeedbackMatrix K = FeedbackMatrix::Zero());
83 
86  const RobotState_t& xf,
87  const core::Time& tf,
88  const int N,
89  FeedbackMatrix K = FeedbackMatrix::Zero());
90 
93  const RobotState_t& xf,
94  const core::Time& tf,
95  const int N,
98  FeedbackMatrix K = FeedbackMatrix::Zero());
99 
100  bool runIteration();
101 
102  bool solve();
103 
105 
106  const core::TimeArray& getTimeArray();
107 
109 
111 
113 
114  const typename NLOptConSolver::Settings_t& getSettings() const;
115 
116  void changeCostFunction(std::shared_ptr<CostFunction> costFunction);
117 
118  std::shared_ptr<NLOptConSolver> getSolver();
119 
120 private:
121  std::shared_ptr<FBSystem> system_;
122  std::shared_ptr<LinearizedSystem> linearizedSystem_;
123  std::shared_ptr<CostFunction> costFunction_;
124  std::shared_ptr<ct::optcon::LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>> inputBoxConstraints_;
125  std::shared_ptr<ct::optcon::LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>> stateBoxConstraints_;
126  std::shared_ptr<ct::optcon::LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>> generalConstraints_;
127 
129 
130  std::shared_ptr<NLOptConSolver> nlocSolver_;
131 
132  size_t iteration_;
133 };
134 
135 } // namespace rbd
136 } // namespace ct
void initializeDirectInterpolation(const RobotState_t &x0, const RobotState_t &xf, const core::Time &tf, const int N, FeedbackMatrix K=FeedbackMatrix::Zero())
initialize fixed-base robot with a directly interpolated state trajectory and corresponding ID torque...
Definition: FixBaseNLOC-impl.h:102
NLOC for fixed base systems without an explicit contact model.
Definition: FixBaseNLOC.h:17
void initialize(const RobotState_t &x0, const core::Time &tf, StateVectorArray x_ref=StateVectorArray(), FeedbackArray u0_fb=FeedbackArray(), ControlVectorArray u0_ff=ControlVectorArray())
Definition: FixBaseNLOC-impl.h:59
const NLOptConSolver::Settings_t & getSettings() const
Definition: FixBaseNLOC-impl.h:220
bool runIteration()
Definition: FixBaseNLOC-impl.h:172
static const size_t NJOINTS
Definition: FixBaseNLOC.h:27
const StateVectorArray & getStateVectorArray()
Definition: FixBaseNLOC-impl.h:213
typename core::StateVector< STATE_DIM, SCALAR > StateVector
Definition: FixBaseNLOC.h:39
std::shared_ptr< NLOptConSolver > getSolver()
Definition: FixBaseNLOC-impl.h:232
static const size_t CONTROL_DIM
Definition: FixBaseNLOC.h:26
void initializeSteadyPose(const RobotState_t &x0, const core::Time &tf, const int N, ControlVector &u_ref, FeedbackMatrix K=FeedbackMatrix::Zero())
initialize fixed-base robot with a steady pose using inverse dynamics torques as feedforward ...
Definition: FixBaseNLOC-impl.h:73
typename FBSystem::SCALAR SCALAR
Definition: FixBaseNLOC.h:30
static EIGEN_MAKE_ALIGNED_OPERATOR_NEW const bool eeForcesAreControlInputs
Definition: FixBaseNLOC.h:22
joint acceleration
Definition: JointAcceleration.h:17
FIX_BASE_FD_SYSTEM FBSystem
Definition: FixBaseNLOC.h:24
FixBaseNLOC()=default
default constructor
typename core::FeedbackArray< STATE_DIM, CONTROL_DIM, SCALAR > FeedbackArray
Definition: FixBaseNLOC.h:44
bool verbose
Definition: rbdJITtests.h:15
static const size_t STATE_DIM
Definition: FixBaseNLOC.h:28
typename core::StateVectorArray< STATE_DIM, SCALAR > StateVectorArray
Definition: FixBaseNLOC.h:42
typename core::StateFeedbackController< STATE_DIM, CONTROL_DIM, SCALAR > StateFeedbackController
Definition: FixBaseNLOC.h:45
const StateFeedbackController & getSolution()
Definition: FixBaseNLOC-impl.h:188
bool solve()
Definition: FixBaseNLOC-impl.h:182
const FeedbackArray & getFeedbackArray()
Definition: FixBaseNLOC-impl.h:200
typename core::ControlVector< CONTROL_DIM, SCALAR > ControlVector
Definition: FixBaseNLOC.h:40
whole fix base robot state, i.e. Joint state, Actuator Dynamics (and fix-base pose) ...
Definition: FixBaseRobotState.h:26
static const size_t ACTUATOR_STATE_DIM
Definition: FixBaseNLOC.h:29
typename core::FeedbackMatrix< STATE_DIM, CONTROL_DIM, SCALAR > FeedbackMatrix
Definition: FixBaseNLOC.h:41
void changeCostFunction(std::shared_ptr< CostFunction > costFunction)
Definition: FixBaseNLOC-impl.h:226
typename core::ControlVectorArray< CONTROL_DIM, SCALAR > ControlVectorArray
Definition: FixBaseNLOC.h:43
const ControlVectorArray & getControlVectorArray()
Definition: FixBaseNLOC-impl.h:207
double Time
const core::TimeArray & getTimeArray()
Definition: FixBaseNLOC-impl.h:194