crocoddyl  1.5.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
impulse-friction-cone.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2020, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_MULTIBODY_COSTS_IMPULSE_FRICTION_CONE_HPP_
10 #define CROCODDYL_MULTIBODY_COSTS_IMPULSE_FRICTION_CONE_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/multibody/friction-cone.hpp"
20 #include "crocoddyl/core/utils/exception.hpp"
21 #include "crocoddyl/core/utils/deprecate.hpp"
22 
23 namespace crocoddyl {
24 
25 template <typename _Scalar>
27  public:
28  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
29 
30  typedef _Scalar Scalar;
42  typedef typename MathBase::Vector6s Vector6s;
43  typedef typename MathBase::VectorXs VectorXs;
44  typedef typename MathBase::MatrixXs MatrixXs;
45  typedef typename MathBase::MatrixX3s MatrixX3s;
46 
47  CostModelImpulseFrictionConeTpl(boost::shared_ptr<StateMultibody> state,
48  boost::shared_ptr<ActivationModelAbstract> activation,
49  const FrameFrictionCone& fref);
50  CostModelImpulseFrictionConeTpl(boost::shared_ptr<StateMultibody> state, const FrameFrictionCone& fref);
52 
53  virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
54  const Eigen::Ref<const VectorXs>& u);
55  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
56  const Eigen::Ref<const VectorXs>& u);
57  virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract* const data);
58 
59  DEPRECATED("Use set_reference<FrameFrictionConeTpl<Scalar> >()", void set_fref(const FrameFrictionCone& fref));
60  DEPRECATED("Use get_reference<FrameFrictionConeTpl<Scalar> >()", const FrameFrictionCone& get_fref() const);
61 
62  protected:
63  virtual void set_referenceImpl(const std::type_info& ti, const void* pv);
64  virtual void get_referenceImpl(const std::type_info& ti, void* pv) const;
65 
66  using Base::activation_;
67  using Base::state_;
68  // using Base::unone_;
69 
70  private:
71  FrameFrictionCone fref_;
72 };
73 
74 template <typename _Scalar>
76  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
77 
78  typedef _Scalar Scalar;
84  typedef typename MathBase::VectorXs VectorXs;
85  typedef typename MathBase::MatrixXs MatrixXs;
86  typedef typename MathBase::Matrix6xs Matrix6xs;
87 
88  template <template <typename Scalar> class Model>
89  CostDataImpulseFrictionConeTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
90  : Base(model, data), more_than_3_constraints(false) {
91  // Check that proper shared data has been passed
93  if (d == NULL) {
94  throw_pretty("Invalid argument: the shared data should be derived from DataCollectorImpulse");
95  }
96 
97  // Avoids data casting at runtime
98  FrameFrictionCone fref = model->template get_reference<FrameFrictionCone>();
99  std::string frame_name = model->get_state()->get_pinocchio()->frames[fref.id].name;
100  bool found_impulse = false;
101  for (typename ImpulseModelMultiple::ImpulseDataContainer::iterator it = d->impulses->impulses.begin();
102  it != d->impulses->impulses.end(); ++it) {
103  if (it->second->frame == fref.id) {
104  ImpulseData3DTpl<Scalar>* d3d = dynamic_cast<ImpulseData3DTpl<Scalar>*>(it->second.get());
105  if (d3d != NULL) {
106  found_impulse = true;
107  impulse = it->second;
108  break;
109  }
110  ImpulseData6DTpl<Scalar>* d6d = dynamic_cast<ImpulseData6DTpl<Scalar>*>(it->second.get());
111  if (d6d != NULL) {
112  more_than_3_constraints = true;
113  found_impulse = true;
114  impulse = it->second;
115  break;
116  }
117  throw_pretty("Domain error: there isn't defined at least a 3d impulse for " + frame_name);
118  break;
119  }
120  }
121  if (!found_impulse) {
122  throw_pretty("Domain error: there isn't defined impulse data for " + frame_name);
123  }
124  }
125 
126  boost::shared_ptr<ImpulseDataAbstractTpl<Scalar> > impulse;
127  bool more_than_3_constraints;
128  using Base::activation;
129  using Base::cost;
130  using Base::Lx;
131  using Base::Lxx;
132  using Base::r;
133  using Base::Rx;
134  using Base::shared;
135 };
136 
137 } // namespace crocoddyl
138 
139 /* --- Details -------------------------------------------------------------- */
140 /* --- Details -------------------------------------------------------------- */
141 /* --- Details -------------------------------------------------------------- */
142 #include "crocoddyl/multibody/costs/impulse-friction-cone.hxx"
143 
144 #endif // CROCODDYL_MULTIBODY_COSTS_IMPULSE_FRICTION_CONE_HPP_
Define a stack of impulse models.
Definition: fwd.hpp:254