Crocoddyl
 
Loading...
Searching...
No Matches
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_CONTROL_GRAVITY_HPP_
10#define CROCODDYL_MULTIBODY_RESIDUALS_CONTROL_GRAVITY_HPP_
11
12#include "crocoddyl/core/residual-base.hpp"
13#include "crocoddyl/core/utils/exception.hpp"
14#include "crocoddyl/multibody/data/multibody.hpp"
15#include "crocoddyl/multibody/states/multibody.hpp"
16
17namespace crocoddyl {
18
35template <typename _Scalar>
37 public:
38 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
39
40 typedef _Scalar Scalar;
47 typedef typename MathBase::VectorXs VectorXs;
48 typedef typename MathBase::MatrixXs MatrixXs;
49
56 ResidualModelControlGravTpl(std::shared_ptr<StateMultibody> state,
57 const std::size_t nu);
58
66 ResidualModelControlGravTpl(std::shared_ptr<StateMultibody> state);
68
76 virtual void calc(const std::shared_ptr<ResidualDataAbstract> &data,
77 const Eigen::Ref<const VectorXs> &x,
78 const Eigen::Ref<const VectorXs> &u);
79
84 virtual void calc(const std::shared_ptr<ResidualDataAbstract> &data,
85 const Eigen::Ref<const VectorXs> &x);
86
94 virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract> &data,
95 const Eigen::Ref<const VectorXs> &x,
96 const Eigen::Ref<const VectorXs> &u);
97
103 virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract> &data,
104 const Eigen::Ref<const VectorXs> &x);
105
106 virtual std::shared_ptr<ResidualDataAbstract> createData(
107 DataCollectorAbstract *const data);
108
114 virtual void print(std::ostream &os) const;
115
116 protected:
117 using Base::nu_;
118 using Base::state_;
119 using Base::v_dependent_;
120
121 private:
122 typename StateMultibody::PinocchioModel
123 pin_model_;
124};
125
126template <typename _Scalar>
128 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
129
130 typedef _Scalar Scalar;
135 typedef pinocchio::DataTpl<Scalar> PinocchioData;
136
137 template <template <typename Scalar> class Model>
138 ResidualDataControlGravTpl(Model<Scalar> *const model,
139 DataCollectorAbstract *const data)
140 : Base(model, data) {
141 // Check that proper shared data has been passed
144 if (d == NULL) {
145 throw_pretty(
146 "Invalid argument: the shared data should be derived from "
147 "DataCollectorActMultibodyTpl");
148 }
149 // Avoids data casting at runtime
150 StateMultibody *sm =
151 static_cast<StateMultibody *>(model->get_state().get());
152 pinocchio = PinocchioData(*(sm->get_pinocchio().get()));
153 actuation = d->actuation;
154 }
155
156 PinocchioData pinocchio;
157 std::shared_ptr<ActuationDataAbstractTpl<Scalar> >
159 using Base::r;
160 using Base::Ru;
161 using Base::Rx;
162 using Base::shared;
163};
164
165} // namespace crocoddyl
166
167/* --- Details -------------------------------------------------------------- */
168/* --- Details -------------------------------------------------------------- */
169/* --- Details -------------------------------------------------------------- */
170#include "crocoddyl/multibody/residuals/control-gravity.hxx"
171
172#endif // CROCODDYL_MULTIBODY_RESIDUALS_CONTROL_GRAVITY_HPP_
Abstract class for residual models.
std::shared_ptr< StateAbstract > state_
State description.
std::size_t nu_
Control dimension.
virtual void calcDiff(const std::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the Jacobians of the control gravity residual.
virtual void print(std::ostream &os) const
Print relevant information of the control-grav residual.
virtual void calcDiff(const std::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
virtual void calc(const std::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the control gravity residual.
ResidualModelControlGravTpl(std::shared_ptr< StateMultibody > state)
Initialize the control gravity residual model.
virtual std::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data)
Create the residual data.
virtual void calc(const std::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the residual vector for nodes that depends only on the state.
ResidualModelControlGravTpl(std::shared_ptr< StateMultibody > state, const std::size_t nu)
Initialize the control gravity residual model.
State multibody representation.
Definition multibody.hpp:35
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.
PinocchioData pinocchio
Pinocchio data.
std::shared_ptr< ActuationDataAbstractTpl< Scalar > > actuation
Actuation data.
DataCollectorAbstract * shared
Shared data allocated by the action model.