crocoddyl  1.5.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
impulse-cop-position.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2020, University of Duisburg-Essen, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_MULTIBODY_COSTS_IMPULSE_COP_POSITION_HPP_
10 #define CROCODDYL_MULTIBODY_COSTS_IMPULSE_COP_POSITION_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/data/multibody.hpp"
20 #include "crocoddyl/core/activations/quadratic-barrier.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;
41  typedef typename MathBase::Vector2s Vector2s;
42  typedef typename MathBase::Vector3s Vector3s;
43  typedef typename MathBase::VectorXs VectorXs;
44  typedef typename MathBase::MatrixXs MatrixXs;
45  typedef typename MathBase::MatrixX3s MatrixX3s;
46  typedef Eigen::Matrix<Scalar, 4, 6> Matrix46;
47 
48  CostModelImpulseCoPPositionTpl(boost::shared_ptr<StateMultibody> state,
49  boost::shared_ptr<ActivationModelAbstract> activation,
50  const FrameCoPSupport& cop_support);
51 
52  CostModelImpulseCoPPositionTpl(boost::shared_ptr<StateMultibody> state, const FrameCoPSupport& cop_support);
54 
55  virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
56  const Eigen::Ref<const VectorXs>& u);
57 
58  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
59  const Eigen::Ref<const VectorXs>& u);
60 
61  virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract* const data);
62 
63  protected:
64  virtual void set_referenceImpl(const std::type_info& ti, const void* pv);
65  virtual void get_referenceImpl(const std::type_info& ti, void* pv) const;
66 
67  using Base::activation_;
68  using Base::nu_;
69  using Base::state_;
70  // using Base::unone_;
71 
72  private:
73  FrameCoPSupport cop_support_;
74 };
75 
76 template <typename _Scalar>
78  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
79 
80  typedef _Scalar Scalar;
86  typedef typename MathBase::Vector3s Vector3s;
87  typedef typename MathBase::VectorXs VectorXs;
88  typedef typename MathBase::MatrixXs MatrixXs;
89  typedef typename MathBase::Matrix3s Matrix3s;
90  typedef typename MathBase::Matrix6xs Matrix6xs;
91  typedef typename MathBase::Matrix6s Matrix6s;
92  typedef typename MathBase::Vector6s Vector6s;
93 
94  template <template <typename Scalar> class Model>
95  CostDataImpulseCoPPositionTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
96  : Base(model, data), Arr_Ru(model->get_activation()->get_nr(), model->get_state()->get_nv()) {
97  Arr_Ru.setZero();
98 
99  // Check that proper shared data has been passed
101  if (d == NULL) {
102  throw_pretty("Invalid argument: the shared data should be derived from DataCollectorImpulse");
103  }
104 
105  // Get the active 6d impulse (avoids data casting at runtime)
106  FrameCoPSupport cop_support = model->template get_reference<FrameCoPSupport>();
107  std::string frame_name = model->get_state()->get_pinocchio()->frames[cop_support.get_id()].name;
108  bool found_impulse = false;
109  for (typename ImpulseModelMultiple::ImpulseDataContainer::iterator it = d->impulses->impulses.begin();
110  it != d->impulses->impulses.end(); ++it) {
111  if (it->second->frame == cop_support.get_id()) {
112  ImpulseData3DTpl<Scalar>* d3d = dynamic_cast<ImpulseData3DTpl<Scalar>*>(it->second.get());
113  if (d3d != NULL) {
114  throw_pretty("Domain error: a 6d impulse model is required in " + frame_name +
115  "in order to compute the CoP");
116  break;
117  }
118  ImpulseData6DTpl<Scalar>* d6d = dynamic_cast<ImpulseData6DTpl<Scalar>*>(it->second.get());
119  if (d6d != NULL) {
120  found_impulse = true;
121  impulse = it->second;
122  break;
123  }
124  }
125  }
126  if (!found_impulse) {
127  throw_pretty("Domain error: there isn't defined impulse data for " + frame_name);
128  }
129  }
130 
131  pinocchio::DataTpl<Scalar>* pinocchio;
132  MatrixXs Arr_Ru;
133  boost::shared_ptr<ImpulseDataAbstractTpl<Scalar> > impulse;
134  using Base::activation;
135  using Base::cost;
136  using Base::Lu;
137  using Base::Luu;
138  using Base::Lx;
139  using Base::Lxu;
140  using Base::Lxx;
141  using Base::r;
142  using Base::Ru;
143  using Base::Rx;
144  using Base::shared;
145 };
146 
147 } // namespace crocoddyl
148 
149 /* --- Details -------------------------------------------------------------- */
150 /* --- Details -------------------------------------------------------------- */
151 /* --- Details -------------------------------------------------------------- */
152 #include "crocoddyl/multibody/costs/impulse-cop-position.hxx"
153 
154 #endif // CROCODDYL_MULTIBODY_COSTS_IMPULSE_COP_POSITION_HPP_
boost::shared_ptr< ImpulseDataAbstractTpl< Scalar > > impulse
impulse force
Define a stack of impulse models.
Definition: fwd.hpp:254