crocoddyl  1.8.1
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
contact-control-gravity.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2020-2021, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_CONTROL_GRAVITY_HPP_
10 #define CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_CONTROL_GRAVITY_HPP_
11 
12 #include "crocoddyl/core/residual-base.hpp"
13 #include "crocoddyl/multibody/states/multibody.hpp"
14 #include "crocoddyl/multibody/data/contacts.hpp"
15 #include "crocoddyl/core/utils/exception.hpp"
16 
17 namespace crocoddyl {
18 
34 template <typename _Scalar>
35 class ResidualModelContactControlGravTpl : public ResidualModelAbstractTpl<_Scalar> {
36  public:
37  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
38 
39  typedef _Scalar Scalar;
40  typedef MathBaseTpl<Scalar> MathBase;
41  typedef ResidualModelAbstractTpl<Scalar> Base;
42  typedef ResidualDataContactControlGravTpl<Scalar> Data;
43  typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
44  typedef StateMultibodyTpl<Scalar> StateMultibody;
45  typedef ActuationModelAbstractTpl<Scalar> ActuationModelAbstract;
46  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
47  typedef typename MathBase::VectorXs VectorXs;
48  typedef typename MathBase::MatrixXs MatrixXs;
49 
56  ResidualModelContactControlGravTpl(boost::shared_ptr<StateMultibody> state, const std::size_t nu);
57 
65  explicit ResidualModelContactControlGravTpl(boost::shared_ptr<StateMultibody> state);
67 
75  virtual void calc(const boost::shared_ptr<ResidualDataAbstract> &data, const Eigen::Ref<const VectorXs> &x,
76  const Eigen::Ref<const VectorXs> &u);
77 
85  virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract> &data, const Eigen::Ref<const VectorXs> &x,
86  const Eigen::Ref<const VectorXs> &u);
87 
88  virtual boost::shared_ptr<ResidualDataAbstract> createData(DataCollectorAbstract *const data);
89 
95  virtual void print(std::ostream &os) const;
96 
97  protected:
98  using Base::nu_;
99  using Base::state_;
100  using Base::unone_;
101  using Base::v_dependent_;
102 
103  private:
104  typename StateMultibody::PinocchioModel pin_model_;
105 };
106 
107 template <typename _Scalar>
108 struct ResidualDataContactControlGravTpl : public ResidualDataAbstractTpl<_Scalar> {
109  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
110 
111  typedef _Scalar Scalar;
112  typedef MathBaseTpl<Scalar> MathBase;
113  typedef ResidualDataAbstractTpl<Scalar> Base;
114  typedef StateMultibodyTpl<Scalar> StateMultibody;
115  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
116  typedef pinocchio::DataTpl<Scalar> PinocchioData;
117 
118  template <template <typename Scalar> class Model>
119  ResidualDataContactControlGravTpl(Model<Scalar> *const model, DataCollectorAbstract *const data)
120  : Base(model, data) {
121  StateMultibody *sm = static_cast<StateMultibody *>(model->get_state().get());
122  pinocchio = PinocchioData(*(sm->get_pinocchio().get()));
123 
124  // Check that proper shared data has been passed
125  DataCollectorActMultibodyInContactTpl<Scalar> *d =
126  dynamic_cast<DataCollectorActMultibodyInContactTpl<Scalar> *>(shared);
127  if (d == NULL) {
128  throw_pretty(
129  "Invalid argument: the shared data should be derived from "
130  "DataCollectorActMultibodyInContactTpl");
131  }
132  // Avoids data casting at runtime
133  // pinocchio = d->pinocchio;
134  fext = d->contacts->fext;
135  actuation = d->actuation;
136  }
137 
138  PinocchioData pinocchio;
139  boost::shared_ptr<ActuationDataAbstractTpl<Scalar> > actuation;
140  pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> > fext;
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/residuals/contact-control-gravity.hxx"
153 
154 #endif // CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_CONTROL_GRAVITY_HPP_
pinocchio::container::aligned_vector< pinocchio::ForceTpl< Scalar > > fext
External spatial forces.
virtual boost::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data)
Create the residual data.
virtual void print(std::ostream &os) const
Print relevant information of the contact-control-grav residual.
boost::shared_ptr< ActuationDataAbstractTpl< Scalar > > actuation
Actuation data.
virtual void calc(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the contact control gravity contact residual.
std::size_t nu_
Control dimension.
ResidualModelContactControlGravTpl(boost::shared_ptr< StateMultibody > state, const std::size_t nu)
Initialize the contact control gravity contact residual model.
virtual void calcDiff(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the Jacobians of the contact control gravity contact residual.
boost::shared_ptr< StateAbstract > state_
State description.
VectorXs unone_
No control vector.
bool v_dependent_
Label that indicates if the residual function depends on v.