crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
contact-control-gravity.hpp
1
2// 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
17namespace crocoddyl {
18
34template <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
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
132template <typename _Scalar>
134 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
135
136 typedef _Scalar Scalar;
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
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_
Abstract class for the actuation-mapping model.
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 calc(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the residual vector for nodes that depends only on the state.
virtual boost::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data)
Create the contact-control-gravity residual data.
virtual void calcDiff(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the Jacobian of the residual functions with respect to the state only.
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.
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.