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>
36  public:
37  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
38 
39  typedef _Scalar Scalar;
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>
109  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
110 
111  typedef _Scalar Scalar;
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
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_
Abstract class for residual models.
boost::shared_ptr< StateAbstract > state_
State description.
std::size_t nu_
Control dimension.
VectorXs unone_
No control vector.
bool v_dependent_
Label that indicates if the residual function depends on v.
Control gravity residual under contact.
virtual void print(std::ostream &os) const
Print relevant information of the contact-control-grav residual.
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.
ResidualModelContactControlGravTpl(boost::shared_ptr< StateMultibody > state)
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.
virtual boost::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data)
Create the residual data.
ResidualModelContactControlGravTpl(boost::shared_ptr< StateMultibody > state, const std::size_t nu)
Initialize the contact control gravity contact residual model.
State multibody representation.
Definition: multibody.hpp:31
const boost::shared_ptr< PinocchioModel > & get_pinocchio() const
Return the Pinocchio model (i.e., model of the rigid body system)
MatrixXs Ru
Jacobian of the residual vector with respect the control.
MatrixXs Rx
Jacobian of the residual vector with respect the state.
DataCollectorAbstract * shared
Shared data allocated by the action model.
boost::shared_ptr< ActuationDataAbstractTpl< Scalar > > actuation
Actuation data.
pinocchio::container::aligned_vector< pinocchio::ForceTpl< Scalar > > fext
External spatial forces.
DataCollectorAbstract * shared
Shared data allocated by the action model.