crocoddyl  1.4.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 
24 enum ImpulseType { Impulse3D, Impulse6D, Undefined };
25 
46 template <typename _Scalar>
48  public:
49  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
50 
51  typedef _Scalar Scalar;
61  typedef typename MathBase::Vector6s Vector6s;
62  typedef typename MathBase::VectorXs VectorXs;
63  typedef typename MathBase::MatrixXs MatrixXs;
64 
74  CostModelContactImpulseTpl(boost::shared_ptr<StateMultibody> state,
75  boost::shared_ptr<ActivationModelAbstract> activation, const FrameForce& fref);
76 
88  CostModelContactImpulseTpl(boost::shared_ptr<StateMultibody> state, const FrameForce& fref, const std::size_t& nr);
89 
100  CostModelContactImpulseTpl(boost::shared_ptr<StateMultibody> state, const FrameForce& fref);
101  virtual ~CostModelContactImpulseTpl();
102 
112  virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
113  const Eigen::Ref<const VectorXs>& u);
114 
124  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
125  const Eigen::Ref<const VectorXs>& u);
126 
136  virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract* const data);
137 
138  DEPRECATED("Used set_reference<FrameForceTpl<Scalar> >()", void set_fref(const FrameForce& fref));
139  DEPRECATED("Used get_reference<FrameForceTpl<Scalar> >()", const FrameForce& get_fref() const);
140 
141  protected:
146  virtual void set_referenceImpl(const std::type_info& ti, const void* pv);
147 
152  virtual void get_referenceImpl(const std::type_info& ti, void* pv) const;
153 
154  using Base::activation_;
155  using Base::nu_;
156  using Base::state_;
157  using Base::unone_;
158 
159  protected:
160  FrameForce fref_;
161 };
163 
164 template <typename _Scalar>
166  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
167 
168  typedef _Scalar Scalar;
174  typedef typename MathBase::VectorXs VectorXs;
175  typedef typename MathBase::MatrixXs MatrixXs;
176  typedef typename MathBase::Matrix6xs Matrix6xs;
177 
178  template <template <typename Scalar> class Model>
179  CostDataContactImpulseTpl(Model<Scalar>* const model, DataCollectorAbstract* const data) : Base(model, data) {
180  impulse_type = Undefined;
181 
182  // Check that proper shared data has been passed
184  if (d == NULL) {
185  throw_pretty("Invalid argument: the shared data should be derived from DataCollectorImpulse");
186  }
187 
188  // Avoids data casting at runtime
189  FrameForce fref = model->template get_reference<FrameForce>();
190  std::string frame_name = model->get_state()->get_pinocchio()->frames[fref.frame].name;
191  bool found_impulse = false;
192  for (typename ImpulseModelMultiple::ImpulseDataContainer::iterator it = d->impulses->impulses.begin();
193  it != d->impulses->impulses.end(); ++it) {
194  if (it->second->frame == fref.frame) {
195  ImpulseData3DTpl<Scalar>* d3d = dynamic_cast<ImpulseData3DTpl<Scalar>*>(it->second.get());
196  if (d3d != NULL) {
197  impulse_type = Impulse3D;
198  if (model->get_activation()->get_nr() != 3) {
199  throw_pretty("Domain error: nr isn't defined as 3 in the activation model for the 3d impulse in " +
200  frame_name);
201  }
202  found_impulse = true;
203  impulse = it->second;
204  break;
205  }
206  ImpulseData6DTpl<Scalar>* d6d = dynamic_cast<ImpulseData6DTpl<Scalar>*>(it->second.get());
207  if (d6d != NULL) {
208  impulse_type = Impulse6D;
209  if (model->get_activation()->get_nr() != 6) {
210  throw_pretty("Domain error: nr isn't defined as 6 in the activation model for the 3d impulse in " +
211  frame_name);
212  }
213  found_impulse = true;
214  impulse = it->second;
215  break;
216  }
217  throw_pretty("Domain error: there isn't defined at least a 3d impulse for " + frame_name);
218  break;
219  }
220  }
221  if (!found_impulse) {
222  throw_pretty("Domain error: there isn't defined impulse data for " + frame_name);
223  }
224  }
225 
226  boost::shared_ptr<ImpulseDataAbstractTpl<Scalar> > impulse;
227  ImpulseType impulse_type;
228  using Base::activation;
229  using Base::cost;
230  using Base::Lu;
231  using Base::Luu;
232  using Base::Lx;
233  using Base::Lxu;
234  using Base::Lxx;
235  using Base::r;
236  using Base::Ru;
237  using Base::Rx;
238  using Base::shared;
239 };
240 
241 } // namespace crocoddyl
242 
243 /* --- Details -------------------------------------------------------------- */
244 /* --- Details -------------------------------------------------------------- */
245 /* --- Details -------------------------------------------------------------- */
246 #include "crocoddyl/multibody/costs/contact-impulse.hxx"
247 
248 #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:229
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.