crocoddyl  1.6.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
impulse-friction-cone.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-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/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/friction-cone.hpp"
21 #include "crocoddyl/core/utils/exception.hpp"
22 #include "crocoddyl/core/utils/deprecate.hpp"
23 
24 namespace crocoddyl {
25 
48 template <typename _Scalar>
50  public:
51  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
52 
53  typedef _Scalar Scalar;
62  typedef typename MathBase::VectorXs VectorXs;
63  typedef typename MathBase::MatrixXs MatrixXs;
64  typedef typename MathBase::MatrixX3s MatrixX3s;
65 
73  CostModelImpulseFrictionConeTpl(boost::shared_ptr<StateMultibody> state,
74  boost::shared_ptr<ActivationModelAbstract> activation,
75  const FrameFrictionCone& fref);
76 
85  CostModelImpulseFrictionConeTpl(boost::shared_ptr<StateMultibody> state, const FrameFrictionCone& fref);
87 
95  virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
96  const Eigen::Ref<const VectorXs>& u);
97 
105  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
106  const Eigen::Ref<const VectorXs>& u);
107 
111  virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract* const data);
112 
113  DEPRECATED("Use set_reference<FrameFrictionConeTpl<Scalar> >()", void set_fref(const FrameFrictionCone& fref));
114  DEPRECATED("Use get_reference<FrameFrictionConeTpl<Scalar> >()", const FrameFrictionCone& get_fref() const);
115 
116  protected:
120  virtual void set_referenceImpl(const std::type_info& ti, const void* pv);
121 
125  virtual void get_referenceImpl(const std::type_info& ti, void* pv) const;
126 
127  using Base::activation_;
128  using Base::state_;
129 
130  private:
131  FrameFrictionCone fref_;
132 };
133 
134 template <typename _Scalar>
136  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
137 
138  typedef _Scalar Scalar;
145 
146  template <template <typename Scalar> class Model>
147  CostDataImpulseFrictionConeTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
148  : Base(model, data), more_than_3_constraints(false) {
149  // Check that proper shared data has been passed
151  if (d == NULL) {
152  throw_pretty("Invalid argument: the shared data should be derived from DataCollectorImpulse");
153  }
154 
155  // Avoids data casting at runtime
156  FrameFrictionCone fref = model->template get_reference<FrameFrictionCone>();
157  const boost::shared_ptr<StateMultibody>& state = boost::static_pointer_cast<StateMultibody>(model->get_state());
158  std::string frame_name = state->get_pinocchio()->frames[fref.id].name;
159  bool found_impulse = false;
160  for (typename ImpulseModelMultiple::ImpulseDataContainer::iterator it = d->impulses->impulses.begin();
161  it != d->impulses->impulses.end(); ++it) {
162  if (it->second->frame == fref.id) {
163  ImpulseData3DTpl<Scalar>* d3d = dynamic_cast<ImpulseData3DTpl<Scalar>*>(it->second.get());
164  if (d3d != NULL) {
165  found_impulse = true;
166  impulse = it->second;
167  break;
168  }
169  ImpulseData6DTpl<Scalar>* d6d = dynamic_cast<ImpulseData6DTpl<Scalar>*>(it->second.get());
170  if (d6d != NULL) {
171  more_than_3_constraints = true;
172  found_impulse = true;
173  impulse = it->second;
174  break;
175  }
176  throw_pretty("Domain error: there isn't defined at least a 3d impulse for " + frame_name);
177  break;
178  }
179  }
180  if (!found_impulse) {
181  throw_pretty("Domain error: there isn't defined impulse data for " + frame_name);
182  }
183  }
184 
185  boost::shared_ptr<ImpulseDataAbstractTpl<Scalar> > impulse;
186  bool more_than_3_constraints;
187  using Base::activation;
188  using Base::cost;
189  using Base::Lx;
190  using Base::Lxx;
191  using Base::r;
192  using Base::Rx;
193  using Base::shared;
194 };
195 
196 } // namespace crocoddyl
197 
198 /* --- Details -------------------------------------------------------------- */
199 /* --- Details -------------------------------------------------------------- */
200 /* --- Details -------------------------------------------------------------- */
201 #include "crocoddyl/multibody/costs/impulse-friction-cone.hxx"
202 
203 #endif // CROCODDYL_MULTIBODY_COSTS_IMPULSE_FRICTION_CONE_HPP_
Abstract class for cost models.
Definition: cost-base.hpp:47
virtual void get_referenceImpl(const std::type_info &ti, void *pv) const
Return the impulse friction cone reference.
boost::shared_ptr< ActivationModelAbstract > activation_
Activation model.
Definition: cost-base.hpp:194
virtual void calc(const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the impulse friction cone cost.
Define a stack of impulse models.
Definition: fwd.hpp:239
virtual void set_referenceImpl(const std::type_info &ti, const void *pv)
Modify the impulse friction cone reference.
CostModelImpulseFrictionConeTpl(boost::shared_ptr< StateMultibody > state, boost::shared_ptr< ActivationModelAbstract > activation, const FrameFrictionCone &fref)
Initialize the impulse friction cone cost model.
virtual boost::shared_ptr< CostDataAbstract > createData(DataCollectorAbstract *const data)
Create the impulse friction cone cost data.
boost::shared_ptr< StateAbstract > state_
State description.
Definition: cost-base.hpp:193
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 friction cone cost.