crocoddyl  1.7.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/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/core/utils/exception.hpp"
21 #include "crocoddyl/core/utils/deprecate.hpp"
22 
23 namespace crocoddyl {
24 
43 template <typename _Scalar>
45  public:
46  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
47 
48  typedef _Scalar Scalar;
57  typedef typename MathBase::VectorXs VectorXs;
58  typedef typename MathBase::MatrixXs MatrixXs;
59 
69  CostModelContactImpulseTpl(boost::shared_ptr<StateMultibody> state,
70  boost::shared_ptr<ActivationModelAbstract> activation, const FrameForce& fref);
71 
82  CostModelContactImpulseTpl(boost::shared_ptr<StateMultibody> state, const FrameForce& fref, const std::size_t nr);
83 
93  CostModelContactImpulseTpl(boost::shared_ptr<StateMultibody> state, const FrameForce& fref);
94  virtual ~CostModelContactImpulseTpl();
95 
105  virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
106  const Eigen::Ref<const VectorXs>& u);
107 
117  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
118  const Eigen::Ref<const VectorXs>& u);
119 
126  virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract* const data);
127 
128  DEPRECATED("Used set_reference<FrameForceTpl<Scalar> >()", void set_fref(const FrameForce& fref));
129  DEPRECATED("Used get_reference<FrameForceTpl<Scalar> >()", const FrameForce& get_fref() const);
130 
131  protected:
135  virtual void set_referenceImpl(const std::type_info& ti, const void* pv);
136 
140  virtual void get_referenceImpl(const std::type_info& ti, void* pv) const;
141 
142  using Base::activation_;
143  using Base::nu_;
144  using Base::state_;
145  using Base::unone_;
146 
147  protected:
148  FrameForce fref_;
149 };
150 
151 template <typename _Scalar>
153  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
154 
155  typedef _Scalar Scalar;
162 
163  template <template <typename Scalar> class Model>
164  CostDataContactImpulseTpl(Model<Scalar>* const model, DataCollectorAbstract* const data) : Base(model, data) {
165  impulse_type = ImpulseUndefined;
166 
167  // Check that proper shared data has been passed
169  if (d == NULL) {
170  throw_pretty("Invalid argument: the shared data should be derived from DataCollectorImpulse");
171  }
172 
173  // Avoids data casting at runtime
174  FrameForce fref = model->template get_reference<FrameForce>();
175  const boost::shared_ptr<StateMultibody>& state = boost::static_pointer_cast<StateMultibody>(model->get_state());
176  std::string frame_name = state->get_pinocchio()->frames[fref.id].name;
177  bool found_impulse = false;
178  for (typename ImpulseModelMultiple::ImpulseDataContainer::iterator it = d->impulses->impulses.begin();
179  it != d->impulses->impulses.end(); ++it) {
180  if (it->second->frame == fref.id) {
181  ImpulseData3DTpl<Scalar>* d3d = dynamic_cast<ImpulseData3DTpl<Scalar>*>(it->second.get());
182  if (d3d != NULL) {
183  impulse_type = Impulse3D;
184  if (model->get_activation()->get_nr() != 3) {
185  throw_pretty("Domain error: nr isn't defined as 3 in the activation model for the 3d impulse in " +
186  frame_name);
187  }
188  found_impulse = true;
189  impulse = it->second;
190  break;
191  }
192  ImpulseData6DTpl<Scalar>* d6d = dynamic_cast<ImpulseData6DTpl<Scalar>*>(it->second.get());
193  if (d6d != NULL) {
194  impulse_type = Impulse6D;
195  if (model->get_activation()->get_nr() != 6) {
196  throw_pretty("Domain error: nr isn't defined as 6 in the activation model for the 3d impulse in " +
197  frame_name);
198  }
199  found_impulse = true;
200  impulse = it->second;
201  break;
202  }
203  throw_pretty("Domain error: there isn't defined at least a 3d impulse for " + frame_name);
204  break;
205  }
206  }
207  if (!found_impulse) {
208  throw_pretty("Domain error: there isn't defined impulse data for " + frame_name);
209  }
210  }
211 
212  boost::shared_ptr<ImpulseDataAbstractTpl<Scalar> > impulse;
213  ImpulseType impulse_type;
214  using Base::activation;
215  using Base::cost;
216  using Base::Lu;
217  using Base::Luu;
218  using Base::Lx;
219  using Base::Lxu;
220  using Base::Lxx;
221  using Base::r;
222  using Base::Ru;
223  using Base::Rx;
224  using Base::shared;
225 };
226 
227 } // namespace crocoddyl
228 
229 /* --- Details -------------------------------------------------------------- */
230 /* --- Details -------------------------------------------------------------- */
231 /* --- Details -------------------------------------------------------------- */
232 #include "crocoddyl/multibody/costs/contact-impulse.hxx"
233 
234 #endif // CROCODDYL_MULTIBODY_COSTS_CONTACT_IMPULSE_HPP_
Abstract class for cost models.
Definition: cost-base.hpp:47
virtual boost::shared_ptr< CostDataAbstract > createData(DataCollectorAbstract *const data)
Create the contact impulse cost data.
Define a contact impulse cost function.
boost::shared_ptr< ActivationModelAbstract > activation_
Activation model.
Definition: cost-base.hpp:194
State multibody representation.
Definition: fwd.hpp:215
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 impulse .
Define a stack of impulse models.
Definition: fwd.hpp:253
virtual void set_referenceImpl(const std::type_info &ti, const void *pv)
Return the reference spatial impulse .
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.
VectorXs unone_
No control vector.
Definition: cost-base.hpp:196
FrameForce fref_
Reference spatial impulse .
std::size_t nu_
Control dimension.
Definition: cost-base.hpp:195
boost::shared_ptr< StateAbstract > state_
State description.
Definition: cost-base.hpp:193
CostModelContactImpulseTpl(boost::shared_ptr< StateMultibody > state, boost::shared_ptr< ActivationModelAbstract > activation, const FrameForce &fref)
Initialize the contact impulse cost model.