crocoddyl  1.9.0
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 
87  virtual void calc(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
88 
96  virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
97  const Eigen::Ref<const VectorXs>& u);
98 
108  virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
109 
113  virtual boost::shared_ptr<ResidualDataAbstract> createData(DataCollectorAbstract* const data);
114 
120  virtual void print(std::ostream& os) const;
121 
122  protected:
123  using Base::nu_;
124  using Base::state_;
125  using Base::unone_;
126  using Base::v_dependent_;
127 
128  private:
129  typename StateMultibody::PinocchioModel pin_model_;
130 };
131 
132 template <typename _Scalar>
133 struct ResidualDataContactControlGravTpl : public ResidualDataAbstractTpl<_Scalar> {
134  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
135 
136  typedef _Scalar Scalar;
137  typedef MathBaseTpl<Scalar> MathBase;
138  typedef ResidualDataAbstractTpl<Scalar> Base;
139  typedef StateMultibodyTpl<Scalar> StateMultibody;
140  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
141  typedef pinocchio::DataTpl<Scalar> PinocchioData;
142 
143  template <template <typename Scalar> class Model>
144  ResidualDataContactControlGravTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
145  : Base(model, data) {
146  StateMultibody* sm = static_cast<StateMultibody*>(model->get_state().get());
147  pinocchio = PinocchioData(*(sm->get_pinocchio().get()));
148 
149  // Check that proper shared data has been passed
150  DataCollectorActMultibodyInContactTpl<Scalar>* d =
151  dynamic_cast<DataCollectorActMultibodyInContactTpl<Scalar>*>(shared);
152  if (d == NULL) {
153  throw_pretty(
154  "Invalid argument: the shared data should be derived from "
155  "DataCollectorActMultibodyInContactTpl");
156  }
157  // Avoids data casting at runtime
158  // pinocchio = d->pinocchio;
159  fext = d->contacts->fext;
160  actuation = d->actuation;
161  }
162 
163  PinocchioData pinocchio;
164  boost::shared_ptr<ActuationDataAbstractTpl<Scalar> > actuation;
165  pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> > fext;
166  using Base::r;
167  using Base::Ru;
168  using Base::Rx;
169  using Base::shared;
170 };
171 
172 } // namespace crocoddyl
173 
174 /* --- Details -------------------------------------------------------------- */
175 /* --- Details -------------------------------------------------------------- */
176 /* --- Details -------------------------------------------------------------- */
177 #include "crocoddyl/multibody/residuals/contact-control-gravity.hxx"
178 
179 #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 contact-control-gravity 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.