constraint-dynamics-derivatives.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2019-2020 INRIA
3 //
4 
7 
13 
15 
16 #include <casadi/casadi.hpp>
17 
18 #include <iostream>
19 #include <boost/test/unit_test.hpp>
20 #include <boost/utility/binary.hpp>
21 
22 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
23 
24 BOOST_AUTO_TEST_CASE(test_constraintDynamicsDerivatives_casadi_algo)
25 {
26  typedef double Scalar;
29  typedef typename Model::ConfigVectorType ConfigVector;
30  typedef typename Model::TangentVectorType TangentVector;
31 
32  const Scalar prec = Eigen::NumTraits<Scalar>::dummy_precision();
33 
34  Model model;
36  model.lowerPositionLimit.head<3>().fill(-1.);
37  model.upperPositionLimit.head<3>().fill(1.);
39 
40  const std::string RF = "rleg6_joint";
41  const Model::JointIndex RF_id = model.getJointId(RF);
42  const std::string LF = "lleg6_joint";
43  const Model::JointIndex LF_id = model.getJointId(LF);
44 
45  // Contact models and data
48 
51  ci_RF.joint1_placement.setRandom();
52  contact_models.push_back(ci_RF);
54 
57  ci_LF.joint1_placement.setRandom();
58  contact_models.push_back(ci_LF);
60  const double mu0 = 0.;
61  ConfigVector q(model.nq);
63  TangentVector v(TangentVector::Random(model.nv));
64  TangentVector tau(TangentVector::Random(model.nv));
65 
70 
71  ad_casadi.initLib();
72  ad_casadi.loadLib();
73 
74  ad_casadi.evalFunction(q, v, tau);
75  BOOST_CHECK(ad_casadi.ddq.isApprox(data.ddq, 1e1 * prec));
76  BOOST_CHECK(ad_casadi.lambda_c.isApprox(data.lambda_c, 1e1 * prec));
77  BOOST_CHECK(ad_casadi.ddq_dq.isApprox(data.ddq_dq, 1e1 * prec));
78  BOOST_CHECK(ad_casadi.ddq_dv.isApprox(data.ddq_dv, 1e1 * prec));
79  BOOST_CHECK(ad_casadi.ddq_dtau.isApprox(data.ddq_dtau, 1e1 * prec));
80  BOOST_CHECK(ad_casadi.dlambda_dq.isApprox(data.dlambda_dq, 1e1 * prec));
81  BOOST_CHECK(ad_casadi.dlambda_dv.isApprox(data.dlambda_dv, 1e1 * prec));
82  BOOST_CHECK(ad_casadi.dlambda_dtau.isApprox(data.dlambda_dtau, 1e1 * prec));
83 }
84 
85 BOOST_AUTO_TEST_SUITE_END()
pinocchio.casadi::AutoDiffConstraintDynamicsDerivatives::evalFunction
void evalFunction(const Eigen::MatrixBase< ConfigVectorType1 > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &tau)
Definition: casadi-algo.hpp:750
pinocchio::DataTpl
Definition: context/generic.hpp:25
pinocchio.casadi::AutoDiffConstraintDynamicsDerivatives::lambda_c
TangentVectorType lambda_c
Definition: casadi-algo.hpp:777
pinocchio::computeConstraintDynamicsDerivatives
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)
contact-cholesky.contact_data
list contact_data
Definition: contact-cholesky.py:33
rnea-derivatives.hpp
setup.data
data
Definition: cmake/cython/setup.in.py:48
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_constraintDynamicsDerivatives_casadi_algo)
Definition: constraint-dynamics-derivatives.cpp:24
pinocchio::python::Scalar
context::Scalar Scalar
Definition: admm-solver.cpp:29
pinocchio::buildModels::humanoidRandom
void humanoidRandom(ModelTpl< Scalar, Options, JointCollectionTpl > &model, bool usingFF=true)
Create a humanoid kinematic tree with 6-DOF limbs and random joint placements.
pinocchio.casadi::AutoDiffAlgoBase::loadLib
void loadLib(const bool generate_if_not_exist=true)
Definition: casadi-algo.hpp:114
pinocchio::python::context::Model
ModelTpl< Scalar, Options > Model
Definition: bindings/python/context/generic.hpp:63
pinocchio::randomConfiguration
void randomConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &lowerLimits, const Eigen::MatrixBase< ConfigVectorIn2 > &upperLimits, const Eigen::MatrixBase< ReturnType > &qout)
Generate a configuration vector uniformly sampled among provided limits.
Definition: joint-configuration.hpp:315
pinocchio.casadi::AutoDiffConstraintDynamicsDerivatives::ddq_dtau
RowMatrixXs ddq_dtau
Definition: casadi-algo.hpp:778
pinocchio::CONTACT_6D
@ CONTACT_6D
Point contact model.
Definition: algorithm/contact-info.hpp:22
rnea.hpp
aba-derivatives.hpp
casadi-algo.hpp
aba.hpp
anymal-simulation.model
model
Definition: anymal-simulation.py:8
pinocchio::LOCAL_WORLD_ALIGNED
@ LOCAL_WORLD_ALIGNED
Definition: multibody/fwd.hpp:52
forward-dynamics-derivatives.tau
tau
Definition: forward-dynamics-derivatives.py:25
joint-configuration.hpp
pinocchio.casadi::AutoDiffConstraintDynamicsDerivatives::dlambda_dtau
RowMatrixXs dlambda_dtau
Definition: casadi-algo.hpp:778
pinocchio::RigidConstraintModelTpl
Definition: algorithm/constraints/fwd.hpp:14
pinocchio::python::context::Data
DataTpl< Scalar, Options > Data
Definition: bindings/python/context/generic.hpp:64
pinocchio.casadi::AutoDiffConstraintDynamicsDerivatives::dlambda_dv
RowMatrixXs dlambda_dv
Definition: casadi-algo.hpp:778
pinocchio.casadi::AutoDiffAlgoBase::initLib
void initLib()
Definition: casadi-algo.hpp:75
pinocchio::RigidConstraintDataTpl
Definition: algorithm/constraints/fwd.hpp:16
pinocchio.casadi::AutoDiffConstraintDynamicsDerivatives::dlambda_dq
RowMatrixXs dlambda_dq
Definition: casadi-algo.hpp:778
cartpole.v
v
Definition: cartpole.py:153
pinocchio::initConstraintDynamics
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.
q
q
PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR
#define PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(T)
Definition: container/aligned-vector.hpp:13
pinocchio.casadi::AutoDiffConstraintDynamicsDerivatives::ddq_dq
RowMatrixXs ddq_dq
Definition: casadi-algo.hpp:778
fill
fill
pinocchio.casadi::AutoDiffConstraintDynamicsDerivatives::ddq_dv
RowMatrixXs ddq_dv
Definition: casadi-algo.hpp:778
pinocchio.casadi::AutoDiffConstraintDynamicsDerivatives
Definition: casadi-algo.hpp:624
pinocchio.casadi::AutoDiffConstraintDynamicsDerivatives::ddq
TangentVectorType ddq
Definition: casadi-algo.hpp:777
pinocchio::JointIndex
Index JointIndex
Definition: multibody/fwd.hpp:26
casadi.hpp
contact-cholesky.contact_models
list contact_models
Definition: contact-cholesky.py:22
sample-models.hpp
pinocchio::ModelTpl
Definition: context/generic.hpp:20
pinocchio::CONTACT_3D
@ CONTACT_3D
Definition: algorithm/contact-info.hpp:21
pinocchio::constraintDynamics
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...


pinocchio
Author(s):
autogenerated on Sun Dec 22 2024 03:41:08