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.
MatrixXs Rx
Jacobian of the residual vector with respect the state.
MatrixXs Ru
Jacobian of the residual vector with respect the control.
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.
VectorXs r
Residual vector.
DataCollectorAbstract * shared
Shared data allocated by the action model.