Crocoddyl
 
Loading...
Searching...
No Matches
contact-control-gravity.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2020-2025, LAAS-CNRS, University of Edinburgh,
5// Heriot-Watt University
6// Copyright note valid unless otherwise stated in individual files.
7// All rights reserved.
9
10#ifndef CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_CONTROL_GRAVITY_HPP_
11#define CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_CONTROL_GRAVITY_HPP_
12
13#include "crocoddyl/core/residual-base.hpp"
14#include "crocoddyl/multibody/data/contacts.hpp"
15#include "crocoddyl/multibody/states/multibody.hpp"
16
17namespace crocoddyl {
18
38template <typename _Scalar>
40 : public ResidualModelAbstractTpl<_Scalar> {
41 public:
42 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
44
45 typedef _Scalar Scalar;
53 typedef typename MathBase::VectorXs VectorXs;
54 typedef typename MathBase::MatrixXs MatrixXs;
55
62 ResidualModelContactControlGravTpl(std::shared_ptr<StateMultibody> state,
63 const std::size_t nu);
64
73 std::shared_ptr<StateMultibody> state);
74 virtual ~ResidualModelContactControlGravTpl() = default;
75
83 virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
84 const Eigen::Ref<const VectorXs>& x,
85 const Eigen::Ref<const VectorXs>& u) override;
86
97 virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
98 const Eigen::Ref<const VectorXs>& x) override;
99
108 virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
109 const Eigen::Ref<const VectorXs>& x,
110 const Eigen::Ref<const VectorXs>& u) override;
111
123 virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
124 const Eigen::Ref<const VectorXs>& x) override;
125
129 virtual std::shared_ptr<ResidualDataAbstract> createData(
130 DataCollectorAbstract* const data) override;
131
142 template <typename NewScalar>
144
150 virtual void print(std::ostream& os) const override;
151
152 protected:
153 using Base::nu_;
154 using Base::state_;
155 using Base::v_dependent_;
156
157 private:
158 typename StateMultibody::PinocchioModel pin_model_;
159};
160
161template <typename _Scalar>
163 : public ResidualDataAbstractTpl<_Scalar> {
164 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
165
166 typedef _Scalar Scalar;
171 typedef pinocchio::DataTpl<Scalar> PinocchioData;
172
173 template <template <typename Scalar> class Model>
174 ResidualDataContactControlGravTpl(Model<Scalar>* const model,
175 DataCollectorAbstract* const data)
176 : Base(model, data) {
177 StateMultibody* sm = static_cast<StateMultibody*>(model->get_state().get());
178 pinocchio = PinocchioData(*(sm->get_pinocchio().get()));
179
180 // Check that proper shared data has been passed
183 if (d == NULL) {
184 throw_pretty(
185 "Invalid argument: the shared data should be derived from "
186 "DataCollectorActMultibodyInContactTpl");
187 }
188 // Avoids data casting at runtime
189 // pinocchio = d->pinocchio;
190 fext = d->contacts->fext;
191 actuation = d->actuation;
192 }
193 virtual ~ResidualDataContactControlGravTpl() = default;
194
195 PinocchioData pinocchio;
196 std::shared_ptr<ActuationDataAbstractTpl<Scalar> >
198 pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> >
200 using Base::r;
201 using Base::Ru;
202 using Base::Rx;
203 using Base::shared;
204};
205
206} // namespace crocoddyl
207
208/* --- Details -------------------------------------------------------------- */
209/* --- Details -------------------------------------------------------------- */
210/* --- Details -------------------------------------------------------------- */
211#include "crocoddyl/multibody/residuals/contact-control-gravity.hxx"
212
213CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
215CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(
217
218#endif // CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_CONTROL_GRAVITY_HPP_
Abstract class for the actuation-mapping model.
Abstract class for residual models.
std::shared_ptr< StateAbstract > state_
State description.
std::size_t nu_
Control dimension.
Control gravity residual under contact.
virtual void calc(const std::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
Compute the residual vector for nodes that depends only on the state.
ResidualModelContactControlGravTpl(std::shared_ptr< StateMultibody > state, const std::size_t nu)
Initialize the contact control gravity contact residual model.
virtual void calcDiff(const std::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) override
Compute the Jacobians of the contact control gravity contact residual.
virtual std::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data) override
Create the contact-control-gravity residual data.
ResidualModelContactControlGravTpl< NewScalar > cast() const
Cast the contact-control-gravity residual model to a different scalar type.
ResidualModelContactControlGravTpl(std::shared_ptr< StateMultibody > state)
Initialize the contact control gravity contact residual model.
virtual void calc(const std::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) override
Compute the contact control gravity contact residual.
virtual void print(std::ostream &os) const override
Print relevant information of the contact-control-grav residual.
virtual void calcDiff(const std::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
Compute the Jacobian of the residual functions with respect to the state only.
State multibody representation.
Definition multibody.hpp:34
const std::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.
std::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.