crocoddyl  1.5.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
contact-impulse.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2020-, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_MULTIBODY_COSTS_CONTACT_IMPULSE_HPP_
10 #define CROCODDYL_MULTIBODY_COSTS_CONTACT_IMPULSE_HPP_
11 
12 #include "crocoddyl/multibody/fwd.hpp"
13 #include "crocoddyl/multibody/cost-base.hpp"
14 #include "crocoddyl/multibody/impulse-base.hpp"
15 #include "crocoddyl/multibody/impulses/impulse-3d.hpp"
16 #include "crocoddyl/multibody/impulses/impulse-6d.hpp"
17 #include "crocoddyl/multibody/data/impulses.hpp"
18 #include "crocoddyl/multibody/frames.hpp"
19 #include "crocoddyl/core/utils/exception.hpp"
20 #include "crocoddyl/core/utils/deprecate.hpp"
21 
22 namespace crocoddyl {
23 
44 template <typename _Scalar>
46  public:
47  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
48 
49  typedef _Scalar Scalar;
59  typedef typename MathBase::Vector6s Vector6s;
60  typedef typename MathBase::VectorXs VectorXs;
61  typedef typename MathBase::MatrixXs MatrixXs;
62 
72  CostModelContactImpulseTpl(boost::shared_ptr<StateMultibody> state,
73  boost::shared_ptr<ActivationModelAbstract> activation, const FrameForce& fref);
74 
86  CostModelContactImpulseTpl(boost::shared_ptr<StateMultibody> state, const FrameForce& fref, const std::size_t& nr);
87 
98  CostModelContactImpulseTpl(boost::shared_ptr<StateMultibody> state, const FrameForce& fref);
99  virtual ~CostModelContactImpulseTpl();
100 
110  virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
111  const Eigen::Ref<const VectorXs>& u);
112 
122  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
123  const Eigen::Ref<const VectorXs>& u);
124 
134  virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract* const data);
135 
136  DEPRECATED("Used set_reference<FrameForceTpl<Scalar> >()", void set_fref(const FrameForce& fref));
137  DEPRECATED("Used get_reference<FrameForceTpl<Scalar> >()", const FrameForce& get_fref() const);
138 
139  protected:
144  virtual void set_referenceImpl(const std::type_info& ti, const void* pv);
145 
150  virtual void get_referenceImpl(const std::type_info& ti, void* pv) const;
151 
152  using Base::activation_;
153  using Base::nu_;
154  using Base::state_;
155  using Base::unone_;
156 
157  protected:
158  FrameForce fref_;
159 };
161 
162 template <typename _Scalar>
164  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
165 
166  typedef _Scalar Scalar;
172  typedef typename MathBase::VectorXs VectorXs;
173  typedef typename MathBase::MatrixXs MatrixXs;
174  typedef typename MathBase::Matrix6xs Matrix6xs;
175 
176  template <template <typename Scalar> class Model>
177  CostDataContactImpulseTpl(Model<Scalar>* const model, DataCollectorAbstract* const data) : Base(model, data) {
178  impulse_type = ImpulseUndefined;
179 
180  // Check that proper shared data has been passed
182  if (d == NULL) {
183  throw_pretty("Invalid argument: the shared data should be derived from DataCollectorImpulse");
184  }
185 
186  // Avoids data casting at runtime
187  FrameForce fref = model->template get_reference<FrameForce>();
188  std::string frame_name = model->get_state()->get_pinocchio()->frames[fref.id].name;
189  bool found_impulse = false;
190  for (typename ImpulseModelMultiple::ImpulseDataContainer::iterator it = d->impulses->impulses.begin();
191  it != d->impulses->impulses.end(); ++it) {
192  if (it->second->frame == fref.id) {
193  ImpulseData3DTpl<Scalar>* d3d = dynamic_cast<ImpulseData3DTpl<Scalar>*>(it->second.get());
194  if (d3d != NULL) {
195  impulse_type = Impulse3D;
196  if (model->get_activation()->get_nr() != 3) {
197  throw_pretty("Domain error: nr isn't defined as 3 in the activation model for the 3d impulse in " +
198  frame_name);
199  }
200  found_impulse = true;
201  impulse = it->second;
202  break;
203  }
204  ImpulseData6DTpl<Scalar>* d6d = dynamic_cast<ImpulseData6DTpl<Scalar>*>(it->second.get());
205  if (d6d != NULL) {
206  impulse_type = Impulse6D;
207  if (model->get_activation()->get_nr() != 6) {
208  throw_pretty("Domain error: nr isn't defined as 6 in the activation model for the 3d impulse in " +
209  frame_name);
210  }
211  found_impulse = true;
212  impulse = it->second;
213  break;
214  }
215  throw_pretty("Domain error: there isn't defined at least a 3d impulse for " + frame_name);
216  break;
217  }
218  }
219  if (!found_impulse) {
220  throw_pretty("Domain error: there isn't defined impulse data for " + frame_name);
221  }
222  }
223 
224  boost::shared_ptr<ImpulseDataAbstractTpl<Scalar> > impulse;
225  ImpulseType impulse_type;
226  using Base::activation;
227  using Base::cost;
228  using Base::Lu;
229  using Base::Luu;
230  using Base::Lx;
231  using Base::Lxu;
232  using Base::Lxx;
233  using Base::r;
234  using Base::Ru;
235  using Base::Rx;
236  using Base::shared;
237 };
238 
239 } // namespace crocoddyl
240 
241 /* --- Details -------------------------------------------------------------- */
242 /* --- Details -------------------------------------------------------------- */
243 /* --- Details -------------------------------------------------------------- */
244 #include "crocoddyl/multibody/costs/contact-impulse.hxx"
245 
246 #endif // CROCODDYL_MULTIBODY_COSTS_CONTACT_IMPULSE_HPP_
virtual boost::shared_ptr< CostDataAbstract > createData(DataCollectorAbstract *const data)
Create the contact impulse cost data.
Define a contact impulse cost function.
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 contact impulse cost.
virtual void get_referenceImpl(const std::type_info &ti, void *pv) const
Modify the reference spatial contact impulse in the contact coordinates .
Define a stack of impulse models.
Definition: fwd.hpp:254
virtual void set_referenceImpl(const std::type_info &ti, const void *pv)
Return the reference spatial contact impulse in the contact coordinates .
virtual void calc(const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the contact impulse cost.
CostModelContactImpulseTpl(boost::shared_ptr< StateMultibody > state, boost::shared_ptr< ActivationModelAbstract > activation, const FrameForce &fref)
Initialize the contact impulse model.