crocoddyl  1.5.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
impulse-wrench-cone.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_IMPULSE_WRENCH_CONE_HPP_
10 #define CROCODDYL_MULTIBODY_COSTS_IMPULSE_WRENCH_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/wrench-cone.hpp"
20 #include "crocoddyl/core/utils/exception.hpp"
21 
22 namespace crocoddyl {
23 
24 template <typename _Scalar>
26  public:
27  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
28 
29  typedef _Scalar Scalar;
41  typedef typename MathBase::Vector6s Vector6s;
42  typedef typename MathBase::VectorXs VectorXs;
43  typedef typename MathBase::MatrixXs MatrixXs;
44  typedef typename MathBase::MatrixX3s MatrixX3s;
45  typedef typename MathBase::MatrixX6s MatrixX6s;
46 
47  CostModelImpulseWrenchConeTpl(boost::shared_ptr<StateMultibody> state,
48  boost::shared_ptr<ActivationModelAbstract> activation, const FrameWrenchCone& fref);
49  CostModelImpulseWrenchConeTpl(boost::shared_ptr<StateMultibody> state, const FrameWrenchCone& fref);
51 
52  virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
53  const Eigen::Ref<const VectorXs>& u);
54  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
55  const Eigen::Ref<const VectorXs>& u);
56  virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract* const data);
57 
58  protected:
59  virtual void set_referenceImpl(const std::type_info& ti, const void* pv);
60  virtual void get_referenceImpl(const std::type_info& ti, void* pv) const;
61 
62  using Base::activation_;
63  using Base::state_;
64  // using Base::unone_;
65 
66  private:
67  FrameWrenchCone fref_;
68 };
69 
70 template <typename _Scalar>
72  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
73 
74  typedef _Scalar Scalar;
80  typedef typename MathBase::VectorXs VectorXs;
81  typedef typename MathBase::MatrixXs MatrixXs;
82  typedef typename MathBase::Matrix6xs Matrix6xs;
83 
84  template <template <typename Scalar> class Model>
85  CostDataImpulseWrenchConeTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
86  : Base(model, data), Arr_Rx(model->get_activation()->get_nr(), model->get_state()->get_ndx()) {
87  Arr_Rx.setZero();
88 
89  // Check that proper shared data has been passed
91  if (d == NULL) {
92  throw_pretty("Invalid argument: the shared data should be derived from DataCollectorImpulse");
93  }
94 
95  // Avoids data casting at runtime
96  FrameWrenchCone fref = model->template get_reference<FrameWrenchCone>();
97  std::string frame_name = model->get_state()->get_pinocchio()->frames[fref.id].name;
98  bool found_impulse = false;
99  for (typename ImpulseModelMultiple::ImpulseDataContainer::iterator it = d->impulses->impulses.begin();
100  it != d->impulses->impulses.end(); ++it) {
101  if (it->second->frame == fref.id) {
102  ImpulseData3DTpl<Scalar>* d3d = dynamic_cast<ImpulseData3DTpl<Scalar>*>(it->second.get());
103  if (d3d != NULL) {
104  found_impulse = true;
105  impulse = it->second;
106  throw_pretty("Domain error: there isn't defined at least a 6d contact for " + frame_name);
107  }
108  ImpulseData6DTpl<Scalar>* d6d = dynamic_cast<ImpulseData6DTpl<Scalar>*>(it->second.get());
109  if (d6d != NULL) {
110  found_impulse = true;
111  impulse = it->second;
112  break;
113  }
114  throw_pretty("Domain error: there isn't defined at least a 6d impulse for " + frame_name);
115  break;
116  }
117  }
118  if (!found_impulse) {
119  throw_pretty("Domain error: there isn't defined impulse data for " + frame_name);
120  }
121  }
122 
123  boost::shared_ptr<ImpulseDataAbstractTpl<Scalar> > impulse;
124  MatrixXs Arr_Rx;
125  using Base::activation;
126  using Base::cost;
127  using Base::Lx;
128  using Base::Lxx;
129  using Base::r;
130  using Base::Rx;
131  using Base::shared;
132 };
133 
134 } // namespace crocoddyl
135 
136 /* --- Details -------------------------------------------------------------- */
137 /* --- Details -------------------------------------------------------------- */
138 /* --- Details -------------------------------------------------------------- */
139 #include "crocoddyl/multibody/costs/impulse-wrench-cone.hxx"
140 
141 #endif // CROCODDYL_MULTIBODY_COSTS_IMPULSE_WRENCH_CONE_HPP_
Define a stack of impulse models.
Definition: fwd.hpp:254