Go to the documentation of this file.
18 int main(
int argc,
const char ** argv)
20 using namespace Eigen;
25 const int NBT = 1000 * 100;
28 std::cout <<
"(the time score in debug mode is not relevant) " << std::endl;
41 const std::string ff_option = argv[2];
42 if (ff_option ==
"-no-ff")
54 const std::string RA =
"RARM_LINK6";
56 const std::string LA =
"LARM_LINK6";
58 const std::string RF =
"RLEG_LINK6";
60 const std::string LF =
"LLEG_LINK6";
82 contact_models_6D.push_back(ci_RF_6D);
83 contact_data_6D.push_back(cd_RF_6D);
88 contact_models_6D6D.push_back(ci_RF_6D);
89 contact_models_6D6D.push_back(ci_LF_6D);
91 contact_data_6D6D.push_back(cd_RF_6D);
92 contact_data_6D6D.push_back(cd_LF_6D);
96 std::cout <<
"nq = " <<
model.nq << std::endl;
97 std::cout <<
"nv = " <<
model.nv << std::endl;
98 std::cout <<
"--" << std::endl;
101 VectorXd qmax = Eigen::VectorXd::Ones(
model.nq);
108 for (
size_t i = 0;
i < NBT; ++
i)
111 qdots[
i] = Eigen::VectorXd::Random(
model.nv);
112 qddots[
i] = Eigen::VectorXd::Random(
model.nv);
113 taus[
i] = Eigen::VectorXd::Random(
model.nv);
121 std::cout <<
"ABA derivatives= \t\t\t";
122 timer.
toc(std::cout, NBT);
124 double total_time = 0;
129 model,
data,
qs[_smooth], qdots[_smooth], taus[_smooth], contact_models_empty,
135 std::cout <<
"constraintDynamicsDerivs {} = \t\t" << (total_time / NBT) << std::endl;
142 model,
data,
qs[_smooth], qdots[_smooth], taus[_smooth], contact_models_6D, contact_data_6D);
147 std::cout <<
"constraintDynamicsDerivs {6D} = \t\t" << (total_time / NBT) << std::endl;
154 model,
data,
qs[_smooth], qdots[_smooth], taus[_smooth], contact_models_6D6D,
160 std::cout <<
"constraintDynamicsDerivs {6D,6D} = \t" << (total_time / NBT) << std::endl;
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)
void humanoidRandom(ModelTpl< Scalar, Options, JointCollectionTpl > &model, bool usingFF=true)
Create a humanoid kinematic tree with 6-DOF limbs and random joint placements.
JointModelFreeFlyerTpl< Scalar > JointModelFreeFlyer
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.
@ CONTACT_6D
Point contact model.
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.
ModelTpl< Scalar, Options, JointCollectionTpl > & buildModel(const std::string &filename, const typename ModelTpl< Scalar, Options, JointCollectionTpl >::JointModel &rootJoint, const std::string &rootJointName, ModelTpl< Scalar, Options, JointCollectionTpl > &model, const bool verbose=false)
Build the model from a URDF file with a particular joint as root of the model tree inside the model g...
VectorXd constraintDynamics(const Model &model, Data &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &tau, const Model::JointIndex id)
#define PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(T)
int main(int argc, const char **argv)
bp::tuple computeABADerivatives(const context::Model &model, context::Data &data, const context::VectorXs &q, const context::VectorXs &v, const context::VectorXs &tau)
Main pinocchio namespace.
#define PINOCCHIO_MODEL_DIR
pinocchio
Author(s):
autogenerated on Wed Dec 25 2024 03:41:19