crocoddyl  1.6.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
impulse-cop-position.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2020, University of Duisburg-Essen, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_MULTIBODY_COSTS_IMPULSE_COP_POSITION_HPP_
10 #define CROCODDYL_MULTIBODY_COSTS_IMPULSE_COP_POSITION_HPP_
11 
12 #include "crocoddyl/multibody/fwd.hpp"
13 #include "crocoddyl/core/cost-base.hpp"
14 #include "crocoddyl/multibody/states/multibody.hpp"
15 #include "crocoddyl/multibody/impulse-base.hpp"
16 #include "crocoddyl/multibody/impulses/impulse-3d.hpp"
17 #include "crocoddyl/multibody/impulses/impulse-6d.hpp"
18 #include "crocoddyl/multibody/data/impulses.hpp"
19 #include "crocoddyl/multibody/frames.hpp"
20 #include "crocoddyl/multibody/data/multibody.hpp"
21 #include "crocoddyl/core/activations/quadratic-barrier.hpp"
22 #include "crocoddyl/core/utils/exception.hpp"
23 
24 namespace crocoddyl {
25 
57 template <typename _Scalar>
59  public:
60  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
61 
62  typedef _Scalar Scalar;
73  typedef typename MathBase::VectorXs VectorXs;
74  typedef typename MathBase::MatrixXs MatrixXs;
75  typedef Eigen::Matrix<Scalar, 4, 6> Matrix46;
76 
84  CostModelImpulseCoPPositionTpl(boost::shared_ptr<StateMultibody> state,
85  boost::shared_ptr<ActivationModelAbstract> activation,
86  const FrameCoPSupport& cop_support);
87 
97  CostModelImpulseCoPPositionTpl(boost::shared_ptr<StateMultibody> state, const FrameCoPSupport& cop_support);
99 
110  virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
111  const Eigen::Ref<const VectorXs>& u);
112 
123  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
124  const Eigen::Ref<const VectorXs>& u);
125 
135  virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract* const data);
136 
137  protected:
141  virtual void set_referenceImpl(const std::type_info& ti, const void* pv);
142 
146  virtual void get_referenceImpl(const std::type_info& ti, void* pv) const;
147 
148  using Base::activation_;
149  using Base::nu_;
150  using Base::state_;
151 
152  private:
153  FrameCoPSupport cop_support_;
154 };
155 
156 template <typename _Scalar>
158  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
159 
160  typedef _Scalar Scalar;
167  typedef typename MathBase::MatrixXs MatrixXs;
168 
169  template <template <typename Scalar> class Model>
170  CostDataImpulseCoPPositionTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
171  : Base(model, data), Arr_Ru(model->get_activation()->get_nr(), model->get_state()->get_nv()) {
172  Arr_Ru.setZero();
173 
174  // Check that proper shared data has been passed
176  if (d == NULL) {
177  throw_pretty("Invalid argument: the shared data should be derived from DataCollectorImpulse");
178  }
179 
180  // Get the active 6d impulse (avoids data casting at runtime)
181  FrameCoPSupport cop_support = model->template get_reference<FrameCoPSupport>();
182  const boost::shared_ptr<StateMultibody>& state = boost::static_pointer_cast<StateMultibody>(model->get_state());
183  std::string frame_name = state->get_pinocchio()->frames[cop_support.get_id()].name;
184  bool found_impulse = false;
185  for (typename ImpulseModelMultiple::ImpulseDataContainer::iterator it = d->impulses->impulses.begin();
186  it != d->impulses->impulses.end(); ++it) {
187  if (it->second->frame == cop_support.get_id()) {
188  ImpulseData3DTpl<Scalar>* d3d = dynamic_cast<ImpulseData3DTpl<Scalar>*>(it->second.get());
189  if (d3d != NULL) {
190  throw_pretty("Domain error: a 6d impulse model is required in " + frame_name +
191  "in order to compute the CoP");
192  break;
193  }
194  ImpulseData6DTpl<Scalar>* d6d = dynamic_cast<ImpulseData6DTpl<Scalar>*>(it->second.get());
195  if (d6d != NULL) {
196  found_impulse = true;
197  impulse = it->second;
198  break;
199  }
200  }
201  }
202  if (!found_impulse) {
203  throw_pretty("Domain error: there isn't defined impulse data for " + frame_name);
204  }
205  }
206 
207  pinocchio::DataTpl<Scalar>* pinocchio;
208  MatrixXs Arr_Ru;
209  boost::shared_ptr<ImpulseDataAbstractTpl<Scalar> > impulse;
210  using Base::activation;
211  using Base::cost;
212  using Base::Lu;
213  using Base::Luu;
214  using Base::Lx;
215  using Base::Lxu;
216  using Base::Lxx;
217  using Base::r;
218  using Base::Ru;
219  using Base::Rx;
220  using Base::shared;
221 };
222 
223 } // namespace crocoddyl
224 
225 /* --- Details -------------------------------------------------------------- */
226 /* --- Details -------------------------------------------------------------- */
227 /* --- Details -------------------------------------------------------------- */
228 #include "crocoddyl/multibody/costs/impulse-cop-position.hxx"
229 
230 #endif // CROCODDYL_MULTIBODY_COSTS_IMPULSE_COP_POSITION_HPP_
Abstract class for cost models.
Definition: cost-base.hpp:47
virtual void calcDiff(const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the derivatives of the impulse CoP cost.
boost::shared_ptr< ImpulseDataAbstractTpl< Scalar > > impulse
impulse force
virtual void set_referenceImpl(const std::type_info &ti, const void *pv)
Return the frame CoP support.
boost::shared_ptr< ActivationModelAbstract > activation_
Activation model.
Definition: cost-base.hpp:194
virtual boost::shared_ptr< CostDataAbstract > createData(DataCollectorAbstract *const data)
Create the impulse CoP cost data.
virtual void get_referenceImpl(const std::type_info &ti, void *pv) const
Modify the frame CoP support.
Define a stack of impulse models.
Definition: fwd.hpp:239
virtual void calc(const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the impulse CoP cost.
std::size_t nu_
Control dimension.
Definition: cost-base.hpp:195
boost::shared_ptr< StateAbstract > state_
State description.
Definition: cost-base.hpp:193
CostModelImpulseCoPPositionTpl(boost::shared_ptr< StateMultibody > state, boost::shared_ptr< ActivationModelAbstract > activation, const FrameCoPSupport &cop_support)
Initialize the impulse CoP cost model.
Define a center of pressure cost function.