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