Go to the documentation of this file.
5 #ifndef __pinocchio_autodiff_code_generator_algo_hpp__
6 #define __pinocchio_autodiff_code_generator_algo_hpp__
8 #include <casadi/core/casadi_types.hpp>
9 #include <casadi/core/code_generator.hpp>
23 template<
typename _Scalar>
50 typedef ::casadi::Function
ADFun;
92 std::ifstream file((
libname +
".so").c_str());
98 std::string compile_command =
99 "clang -fPIC -shared -Ofast -DNDEBUG " +
filename +
".c -o " +
libname +
".so";
102 compile_command +=
" -isysroot "
103 "/Applications/Xcode.app/Contents/Developer/Platforms/MacOSX.platform/"
104 "Developer/SDKs/MacOSX.sdk";
107 const int flag = system(compile_command.c_str());
110 std::cerr <<
"Compilation failed" << std::endl;
114 void loadLib(
const bool generate_if_not_exist =
true)
116 if (!
existLib() && generate_if_not_exist)
153 template<
typename _Scalar>
174 const std::string &
filename =
"casadi_aba",
175 const std::string &
libname =
"libcasadi_cg_aba",
176 const std::string &
fun_name =
"eval_f")
197 q_ad = Eigen::Map<ADConfigVectorType>(
198 static_cast<std::vector<ADScalar>
>(
cs_q).
data(),
model.nq, 1);
199 v_int_ad = Eigen::Map<ADConfigVectorType>(
201 v_ad = Eigen::Map<ADTangentVectorType>(
202 static_cast<std::vector<ADScalar>
>(
cs_v).
data(),
model.nv, 1);
203 tau_ad = Eigen::Map<ADTangentVectorType>(
206 Eigen::Map<TangentVectorType>(
v_int_vec.data(),
model.nv, 1).setZero();
233 template<
typename ConfigVectorType1,
typename TangentVectorType1,
typename TangentVectorType2>
235 const Eigen::MatrixBase<ConfigVectorType1> &
q,
236 const Eigen::MatrixBase<TangentVectorType1> &
v,
237 const Eigen::MatrixBase<TangentVectorType2> &
tau)
243 ddq = Eigen::Map<TangentVectorType>(
247 template<
typename ConfigVectorType1,
typename TangentVectorType1,
typename TangentVectorType2>
249 const Eigen::MatrixBase<ConfigVectorType1> &
q,
250 const Eigen::MatrixBase<TangentVectorType1> &
v,
251 const Eigen::MatrixBase<TangentVectorType2> &
tau)
258 ddq_dq = Eigen::Map<MatrixXs>(
260 ddq_dv = Eigen::Map<MatrixXs>(
295 template<
typename _Scalar>
317 const std::string &
filename =
"casadi_abaDerivs",
318 const std::string &
libname =
"libcasadi_cg_abaDerivs",
319 const std::string &
fun_name =
"eval_f")
336 q_ad = Eigen::Map<ADConfigVectorType>(
337 static_cast<std::vector<ADScalar>
>(
cs_q).
data(),
model.nq, 1);
338 v_ad = Eigen::Map<ADTangentVectorType>(
339 static_cast<std::vector<ADScalar>
>(
cs_v).
data(),
model.nv, 1);
340 tau_ad = Eigen::Map<ADTangentVectorType>(
355 ad_data.
Minv.template triangularView<Eigen::StrictlyLower>() =
356 ad_data.
Minv.transpose().template triangularView<Eigen::StrictlyLower>();
367 template<
typename ConfigVectorType1,
typename TangentVectorType1,
typename TangentVectorType2>
369 const Eigen::MatrixBase<ConfigVectorType1> &
q,
370 const Eigen::MatrixBase<TangentVectorType1> &
v,
371 const Eigen::MatrixBase<TangentVectorType2> &
tau)
378 ddq = Eigen::Map<TangentVectorType>(
380 ddq_dq = Eigen::Map<MatrixXs>(
382 ddq_dv = Eigen::Map<MatrixXs>(
415 template<
typename _Scalar>
444 typedef typename std::vector<ADConstraintModel, ADConstraintModelAllocator>
448 typedef typename std::vector<ADConstraintData, ADConstraintDataAllocator>
453 Eigen::DenseIndex num_total_constraints = 0;
454 for (
typename ConstraintModelVector::const_iterator
it =
contact_models.begin();
458 it->size() > 0,
"The dimension of the constraint must be positive");
459 num_total_constraints +=
it->size();
461 return num_total_constraints;
467 const std::string &
filename =
"casadi_contactDyn",
468 const std::string &
libname =
"libcasadi_cg_contactDyn",
469 const std::string &
fun_name =
"eval_f")
500 q_ad = Eigen::Map<ADConfigVectorType>(
501 static_cast<std::vector<ADScalar>
>(
cs_q).
data(),
model.nq, 1);
502 v_int_ad = Eigen::Map<ADConfigVectorType>(
504 v_ad = Eigen::Map<ADTangentVectorType>(
505 static_cast<std::vector<ADScalar>
>(
cs_v).
data(),
model.nv, 1);
506 tau_ad = Eigen::Map<ADTangentVectorType>(
509 Eigen::Map<TangentVectorType>(
v_int_vec.data(),
model.nv, 1).setZero();
549 template<
typename ConfigVectorType1,
typename TangentVectorType1,
typename TangentVectorType2>
551 const Eigen::MatrixBase<ConfigVectorType1> &
q,
552 const Eigen::MatrixBase<TangentVectorType1> &
v,
553 const Eigen::MatrixBase<TangentVectorType2> &
tau)
559 ddq = Eigen::Map<TangentVectorType>(
561 lambda_c = Eigen::Map<TangentVectorType>(
565 template<
typename ConfigVectorType1,
typename TangentVectorType1,
typename TangentVectorType2>
567 const Eigen::MatrixBase<ConfigVectorType1> &
q,
568 const Eigen::MatrixBase<TangentVectorType1> &
v,
569 const Eigen::MatrixBase<TangentVectorType2> &
tau)
575 ddq_dq = Eigen::Map<MatrixXs>(
577 ddq_dv = Eigen::Map<MatrixXs>(
611 Eigen::DenseIndex
nc;
623 template<
typename _Scalar>
652 typedef typename std::vector<ADConstraintModel, ADConstraintModelAllocator>
656 typedef typename std::vector<ADConstraintData, ADConstraintDataAllocator>
661 Eigen::DenseIndex num_total_constraints = 0;
662 for (
typename ConstraintModelVector::const_iterator
it =
contact_models.begin();
666 it->size() > 0,
"The dimension of the constraint must be positive");
667 num_total_constraints +=
it->size();
669 return num_total_constraints;
675 const std::string &
filename =
"casadi_contactDynDerivs",
676 const std::string &
libname =
"libcasadi_cg_contactDynDerivs",
677 const std::string &
fun_name =
"eval_f")
705 q_ad = Eigen::Map<ADConfigVectorType>(
706 static_cast<std::vector<ADScalar>
>(
cs_q).
data(),
model.nq, 1);
707 v_ad = Eigen::Map<ADTangentVectorType>(
708 static_cast<std::vector<ADScalar>
>(
cs_v).
data(),
model.nv, 1);
709 tau_ad = Eigen::Map<ADTangentVectorType>(
749 template<
typename ConfigVectorType1,
typename TangentVectorType1,
typename TangentVectorType2>
751 const Eigen::MatrixBase<ConfigVectorType1> &
q,
752 const Eigen::MatrixBase<TangentVectorType1> &
v,
753 const Eigen::MatrixBase<TangentVectorType2> &
tau)
759 ddq = Eigen::Map<TangentVectorType>(
761 lambda_c = Eigen::Map<TangentVectorType>(
763 ddq_dq = Eigen::Map<MatrixXs>(
765 ddq_dv = Eigen::Map<MatrixXs>(
797 Eigen::DenseIndex
nc;
813 #endif // ifndef __pinocchio_autodiff_code_generator_algo_hpp__
std::vector< ConstraintModel, ConstraintModelAllocator > ConstraintModelVector
Base::ADTangentVectorType ADTangentVectorType
AutoDiffAlgoBase< _Scalar > Base
ADTangentVectorType tau_ad
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, Options > MatrixXs
RowMatrixXs ddq_dv
Partial derivative of the joint acceleration vector with respect to the joint velocity.
ADConfigVectorType q_int_ad
TangentVectorType ddq
The joint accelerations computed from ABA.
std::vector< Scalar > q_vec
void evalFunction(const Eigen::MatrixBase< ConfigVectorType1 > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &tau)
Base::ADSVector ADSVector
bool build_jacobian
Options to build or not the Jacobian of he function.
ADTangentVectorType v_int_ad
Base::RowMatrixXs RowMatrixXs
std::vector< Scalar > tau_vec
std::vector< Scalar > q_vec
AutoDiffConstraintDynamics(const Model &model, const ConstraintModelVector &contact_models, const std::string &filename="casadi_contactDyn", const std::string &libname="libcasadi_cg_contactDyn", const std::string &fun_name="eval_f")
TangentVectorType lambda_c
#define PINOCCHIO_CHECK_INPUT_ARGUMENT(...)
Macro to check an assert-like condition and throw a std::invalid_argument exception (with a message) ...
Base::TangentVectorType TangentVectorType
Eigen::aligned_allocator< ConstraintModel > ConstraintModelAllocator
void computeConstraintDynamicsDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const std::vector< RigidConstraintModelTpl< Scalar, Options >, ConstraintModelAllocator > &contact_models, std::vector< RigidConstraintDataTpl< Scalar, Options >, ConstraintDataAllocator > &contact_data, const ProximalSettingsTpl< Scalar > &settings, const Eigen::MatrixBase< MatrixType1 > &ddq_partial_dq, const Eigen::MatrixBase< MatrixType2 > &ddq_partial_dv, const Eigen::MatrixBase< MatrixType3 > &ddq_partial_dtau, const Eigen::MatrixBase< MatrixType4 > &lambda_partial_dq, const Eigen::MatrixBase< MatrixType5 > &lambda_partial_dv, const Eigen::MatrixBase< MatrixType6 > &lambda_partial_dtau)
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor|Options > RowMatrixXs
Eigen::aligned_allocator< ADConstraintData > ADConstraintDataAllocator
virtual void buildMap()
build the mapping Y = f(X)
bool build_forward
Options to generate or not the source code for the evaluation function.
std::vector< DMMatrix > fun_output_derivs
virtual void buildMap()
build the mapping Y = f(X)
Base::ADSVector ADSVector
ADModel::TangentVectorType ADTangentVectorType
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space.
void sym(const Eigen::MatrixBase< MatrixDerived > &eig_mat, std::string const &name)
casadi_int getFunOperationCount() const
int nq(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNqVisitor to get the dimension of the joint configuration space.
std::vector< Scalar > q_vec
virtual void buildMap()
build the mapping Y = f(X)
AutoDiffAlgoBase< _Scalar > Base
AutoDiffAlgoBase< _Scalar > Base
Eigen::aligned_allocator< ConstraintData > ConstraintDataAllocator
std::vector< Scalar > v_int_vec
VectorXs lambda_c
Lagrange Multipliers corresponding to the contact forces in pinocchio::forwardDynamics.
Eigen::aligned_allocator< ConstraintData > ConstraintDataAllocator
void evalFunction(const Eigen::MatrixBase< ConfigVectorType1 > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &tau)
Eigen::aligned_allocator< ConstraintModel > ConstraintModelAllocator
std::vector< Scalar > v_vec
pinocchio::RigidConstraintModelTpl< Scalar, Base::Options > ConstraintModel
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & aba(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &tau, const Convention convention=Convention::LOCAL)
The Articulated-Body algorithm. It computes the forward dynamics, aka the joint accelerations given t...
std::vector< Scalar > v_int_vec
AutoDiffConstraintDynamicsDerivatives(const Model &model, const ConstraintModelVector &contact_models, const std::string &filename="casadi_contactDynDerivs", const std::string &libname="libcasadi_cg_contactDynDerivs", const std::string &fun_name="eval_f")
void loadLib(const bool generate_if_not_exist=true)
std::vector< ConstraintModel, ConstraintModelAllocator > ConstraintModelVector
pinocchio::RigidConstraintModelTpl< Scalar, Base::Options > ConstraintModel
NewScalar cast(const Scalar &value)
std::vector< ConstraintData, ConstraintDataAllocator > ConstraintDataVector
void evalJacobian(const Eigen::MatrixBase< ConfigVectorType1 > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &tau)
static Eigen::DenseIndex constraintDim(const ConstraintModelVector &contact_models)
ADTangentVectorType tau_ad
Eigen::aligned_allocator< ADConstraintData > ADConstraintDataAllocator
Model::ConfigVectorType ConfigVectorType
Base::RowMatrixXs RowMatrixXs
pinocchio::DataTpl< ADScalar, Options > ADData
RowMatrixXs ddq_dtau
Partial derivative of the joint acceleration vector with respect to the joint torques.
Base::ADConfigVectorType ADConfigVectorType
::casadi::SXVector ADSVector
AutoDiffABADerivatives(const Model &model, const std::string &filename="casadi_abaDerivs", const std::string &libname="libcasadi_cg_abaDerivs", const std::string &fun_name="eval_f")
Eigen::Matrix< Scalar, Eigen::Dynamic, 1, Options > VectorXs
std::vector< Scalar > tau_vec
ADConstraintDataVector ad_contact_datas
std::vector< ADConstraintModel, ADConstraintModelAllocator > ADConstraintModelVector
Base::RowMatrixXs RowMatrixXs
void integrate(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v, const Eigen::MatrixBase< ReturnType > &qout)
Integrate a configuration vector for the specified model for a tangent vector during one unit time.
ADModel::ConfigVectorType ADConfigVectorType
Base::ADSVector ADSVector
void evalFunction(const Eigen::MatrixBase< ConfigVectorType1 > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &tau)
VectorXs TangentVectorType
Dense vectorized version of a joint tangent vector (e.g. velocity, acceleration, etc)....
virtual ~AutoDiffAlgoBase()
pinocchio::RigidConstraintDataTpl< Scalar, Base::Options > ConstraintData
::casadi::CodeGenerator cg_generated
pinocchio::RigidConstraintDataTpl< Scalar, Base::Options > ConstraintData
RowMatrixXs Minv
The inverse of the joint space inertia matrix (a square matrix of dim model.nv).
Base::ADConfigVectorType ADConfigVectorType
Base::ADSVector ADSVector
MatrixXs dlambda_dq
Partial derivatives of the constraints forces with respect to the joint configuration,...
casadi_int fun_operation_count
void initConstraintDynamics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const std::vector< RigidConstraintModelTpl< Scalar, Options >, Allocator > &contact_models)
Init the forward dynamics data according to the contact information contained in contact_models.
std::vector< ADConstraintData, ADConstraintDataAllocator > ADConstraintDataVector
Eigen::aligned_allocator< ADConstraintModel > ADConstraintModelAllocator
pinocchio::RigidConstraintDataTpl< ADScalar, Base::Options > ADConstraintData
virtual ~AutoDiffABADerivatives()
Eigen::aligned_allocator< ADConstraintModel > ADConstraintModelAllocator
virtual void buildMap()=0
build the mapping Y = f(X)
std::vector< Scalar > tau_vec
Base::ADTangentVectorType ADTangentVectorType
casadi_int getFunDerivsOperationCount() const
virtual ~AutoDiffConstraintDynamicsDerivatives()
AutoDiffABA(const Model &model, const std::string &filename="casadi_aba", const std::string &libname="libcasadi_cg_aba", const std::string &fun_name="eval_f")
void jacobian(const ConstraintModelTpl< Scalar, Options, ConstraintCollectionTpl > &cmodel, ConstraintDataTpl< Scalar, Options, ConstraintCollectionTpl > &cdata, const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< JacobianMatrix > &jacobian_matrix)
std::vector< Scalar > v_vec
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
ADConstraintModelVector ad_contact_models
std::vector< Scalar > tau_vec
Model::TangentVectorType TangentVectorType
Base::ADConfigVectorType ADConfigVectorType
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
virtual void buildMap()
build the mapping Y = f(X)
std::vector< Scalar > v_vec
ADConstraintModelVector ad_contact_models
pinocchio::ModelTpl< Scalar, Options > Model
std::vector< ADConstraintData, ADConstraintDataAllocator > ADConstraintDataVector
pinocchio::RigidConstraintModelTpl< ADScalar, Base::Options > ADConstraintModel
void computeABADerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &tau, const Eigen::MatrixBase< MatrixType1 > &aba_partial_dq, const Eigen::MatrixBase< MatrixType2 > &aba_partial_dv, const Eigen::MatrixBase< MatrixType3 > &aba_partial_dtau)
The derivatives of the Articulated-Body algorithm.
Base::RowMatrixXs RowMatrixXs
Base::ADTangentVectorType ADTangentVectorType
RowMatrixXs ddq_dq
Partial derivative of the joint acceleration vector with respect to the joint configuration.
void copy(::casadi::Matrix< Scalar > const &src, Eigen::MatrixBase< MT > &dst)
casadi_int fun_derivs_operation_count
ADTangentVectorType v_int_ad
void evalJacobian(const Eigen::MatrixBase< ConfigVectorType1 > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &tau)
int nv
Dimension of the velocity vector space.
AutoDiffAlgoBase< _Scalar > Base
static Eigen::DenseIndex constraintDim(const ConstraintModelVector &contact_models)
std::vector< DMMatrix > fun_output
std::vector< ADConstraintModel, ADConstraintModelAllocator > ADConstraintModelVector
pinocchio::ModelTpl< ADScalar, Options > ADModel
ADTangentVectorType tau_ad
std::vector< ConstraintData, ConstraintDataAllocator > ConstraintDataVector
std::vector< Scalar > v_vec
std::vector< Scalar > q_vec
ADConfigVectorType q_int_ad
pinocchio::RigidConstraintDataTpl< ADScalar, Base::Options > ADConstraintData
Base::TangentVectorType TangentVectorType
VectorXs ConfigVectorType
Dense vectorized version of a joint configuration vector.
ADTangentVectorType tau_ad
Base::ADTangentVectorType ADTangentVectorType
pinocchio::RigidConstraintModelTpl< ADScalar, Base::Options > ADConstraintModel
pinocchio::DataTpl< Scalar, Options > Data
Base::TangentVectorType TangentVectorType
::casadi::DMVector DMVector
Data::RowMatrixXs RowMatrixXs
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & constraintDynamics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &tau, const std::vector< RigidConstraintModelTpl< Scalar, Options >, ConstraintModelAllocator > &contact_models, std::vector< RigidConstraintDataTpl< Scalar, Options >, ConstraintDataAllocator > &contact_datas, ProximalSettingsTpl< Scalar > &settings)
Computes the forward dynamics with contact constraints according to a given list of contact informati...
void evalFunction(const Eigen::MatrixBase< ConfigVectorType1 > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &tau)
AutoDiffAlgoBase(const Model &model, const std::string &filename, const std::string &libname, const std::string &fun_name)
Base::ADConfigVectorType ADConfigVectorType
int nq
Dimension of the configuration vector representation.
Base::TangentVectorType TangentVectorType
JointCollectionTpl & model
Main pinocchio namespace.
ADConstraintDataVector ad_contact_datas
virtual ~AutoDiffConstraintDynamics()
pinocchio
Author(s):
autogenerated on Sun Dec 22 2024 03:41:06