-
Notifications
You must be signed in to change notification settings - Fork 383
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
[pinocchio3] commit for Preview of Pinocchio3
- Loading branch information
Showing
330 changed files
with
23,914 additions
and
3,548 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -4,4 +4,4 @@ Xcode* | |
| *.pyc | ||
| coverage* | ||
| .travis | ||
| .vscode* | ||
| .vscode* | ||
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,159 @@ | ||
| // | ||
| // Copyright (c) 2019-2020 LAAS-CNRS INRIA | ||
| // | ||
|
|
||
| #include "pinocchio/algorithm/joint-configuration.hpp" | ||
| #include "pinocchio/algorithm/constrained-dynamics.hpp" | ||
| #include "pinocchio/algorithm/aba-derivatives.hpp" | ||
| #include "pinocchio/algorithm/contact-dynamics.hpp" | ||
| #include "pinocchio/algorithm/constrained-dynamics-derivatives.hpp" | ||
| #include "pinocchio/algorithm/cholesky.hpp" | ||
| #include "pinocchio/parsers/urdf.hpp" | ||
| #include "pinocchio/parsers/sample-models.hpp" | ||
|
|
||
| #include <iostream> | ||
|
|
||
| #include "pinocchio/utils/timer.hpp" | ||
|
|
||
| int main(int argc, const char ** argv) | ||
| { | ||
| using namespace Eigen; | ||
| using namespace pinocchio; | ||
|
|
||
| PinocchioTicToc timer(PinocchioTicToc::US); | ||
| #ifdef NDEBUG | ||
| const int NBT = 1000*100; | ||
| #else | ||
| const int NBT = 1; | ||
| std::cout << "(the time score in debug mode is not relevant) " << std::endl; | ||
| #endif | ||
|
|
||
| // Build model | ||
| Model model; | ||
|
|
||
| std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf"); | ||
| if(argc>1) filename = argv[1]; | ||
| bool with_ff = true; | ||
|
|
||
| if(argc>2) | ||
| { | ||
| const std::string ff_option = argv[2]; | ||
| if(ff_option == "-no-ff") | ||
| with_ff = false; | ||
| } | ||
|
|
||
| if( filename == "HS") | ||
| buildModels::humanoidRandom(model,true); | ||
| else | ||
| if(with_ff) | ||
| pinocchio::urdf::buildModel(filename,JointModelFreeFlyer(),model); | ||
| // pinocchio::urdf::buildModel(filename,JointModelRX(),model); | ||
| else | ||
| pinocchio::urdf::buildModel(filename,model); | ||
|
|
||
| const std::string RA = "RARM_LINK6"; | ||
| const JointIndex RA_id = model.frames[model.getFrameId(RA)].parent; | ||
| const std::string LA = "LARM_LINK6"; | ||
| const JointIndex LA_id = model.frames[model.getFrameId(LA)].parent; | ||
| const std::string RF = "RLEG_LINK6"; | ||
| const JointIndex RF_id = model.frames[model.getFrameId(RF)].parent; | ||
| const std::string LF = "LLEG_LINK6"; | ||
| const JointIndex LF_id = model.frames[model.getFrameId(LF)].parent; | ||
|
|
||
| RigidConstraintModel ci_RF_6D(CONTACT_6D,RF_id,LOCAL); | ||
| RigidConstraintData cd_RF_6D(ci_RF_6D); | ||
| //RigidConstraintModel ci_RF_3D(CONTACT_3D,model.getJointId(RF),WORLD); | ||
|
|
||
| RigidConstraintModel ci_LF_6D(CONTACT_6D,LF_id,LOCAL); | ||
| RigidConstraintData cd_LF_6D(ci_LF_6D); | ||
| // RigidConstraintModel ci_LF_3D(CONTACT_3D,model.getJointId(LF),WORLD); | ||
|
|
||
| //RigidConstraintModel ci_RA_3D(CONTACT_3D,model.getJointId(RA),WORLD); | ||
| //RigidConstraintModel ci_LA_3D(CONTACT_3D,model.getJointId(LA),WORLD); | ||
|
|
||
| // Define contact infos structure | ||
| const PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_empty; | ||
| PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data_empty; | ||
|
|
||
| cholesky::ContactCholeskyDecomposition contact_chol_empty(model,contact_models_empty); | ||
|
|
||
| PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_6D; | ||
| PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data_6D; | ||
| contact_models_6D.push_back(ci_RF_6D); | ||
| contact_data_6D.push_back(cd_RF_6D); | ||
|
|
||
| cholesky::ContactCholeskyDecomposition contact_chol_6D(model,contact_models_6D); | ||
|
|
||
| PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_6D6D; | ||
| contact_models_6D6D.push_back(ci_RF_6D); | ||
| contact_models_6D6D.push_back(ci_LF_6D); | ||
| PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data_6D6D; | ||
| contact_data_6D6D.push_back(cd_RF_6D); | ||
| contact_data_6D6D.push_back(cd_LF_6D); | ||
|
|
||
| cholesky::ContactCholeskyDecomposition contact_chol_6D6D(model,contact_models_6D6D); | ||
|
|
||
| std::cout << "nq = " << model.nq << std::endl; | ||
| std::cout << "nv = " << model.nv << std::endl; | ||
| std::cout << "--" << std::endl; | ||
|
|
||
| Data data(model); | ||
| VectorXd qmax = Eigen::VectorXd::Ones(model.nq); | ||
|
|
||
| PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(VectorXd) qs(NBT); | ||
| PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(VectorXd) qdots(NBT); | ||
| PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(VectorXd) qddots(NBT); | ||
| PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(VectorXd) taus(NBT); | ||
|
|
||
| for(size_t i=0;i<NBT;++i) | ||
| { | ||
| qs[i] = randomConfiguration(model,-qmax,qmax); | ||
| qdots[i] = Eigen::VectorXd::Random(model.nv); | ||
| qddots[i] = Eigen::VectorXd::Random(model.nv); | ||
| taus[i] = Eigen::VectorXd::Random(model.nv); | ||
| } | ||
|
|
||
| timer.tic(); | ||
| SMOOTH(NBT) | ||
| { | ||
| computeABADerivatives(model,data,qs[_smooth],qdots[_smooth],taus[_smooth]); | ||
| } | ||
| std::cout << "ABA derivatives= \t\t\t"; timer.toc(std::cout,NBT); | ||
|
|
||
|
|
||
| double total_time = 0; | ||
| initConstraintDynamics(model,data,contact_models_empty); | ||
| SMOOTH(NBT) | ||
| { | ||
| constraintDynamics(model,data,qs[_smooth],qdots[_smooth],taus[_smooth],contact_models_empty,contact_data_empty); | ||
| timer.tic(); | ||
| computeConstraintDynamicsDerivatives(model, data, | ||
| contact_models_empty, contact_data_empty); | ||
| total_time += timer.toc(timer.DEFAULT_UNIT); | ||
| } | ||
| std::cout << "constraintDynamicsDerivs {} = \t\t" << (total_time/NBT)<<std::endl; | ||
|
|
||
| total_time = 0; | ||
| initConstraintDynamics(model,data,contact_models_6D); | ||
| SMOOTH(NBT) | ||
| { | ||
| constraintDynamics(model,data,qs[_smooth],qdots[_smooth],taus[_smooth],contact_models_6D,contact_data_6D); | ||
| timer.tic(); | ||
| computeConstraintDynamicsDerivatives(model,data,contact_models_6D,contact_data_6D); | ||
| total_time += timer.toc(timer.DEFAULT_UNIT); | ||
| } | ||
| std::cout << "constraintDynamicsDerivs {6D} = \t\t" << (total_time/NBT)<<std::endl; | ||
|
|
||
| total_time = 0; | ||
| initConstraintDynamics(model,data,contact_models_6D6D); | ||
| SMOOTH(NBT) | ||
| { | ||
| constraintDynamics(model,data,qs[_smooth],qdots[_smooth],taus[_smooth],contact_models_6D6D,contact_data_6D6D); | ||
| timer.tic(); | ||
| computeConstraintDynamicsDerivatives(model,data, contact_models_6D6D, contact_data_6D6D); | ||
| total_time += timer.toc(timer.DEFAULT_UNIT); | ||
| } | ||
| std::cout << "constraintDynamicsDerivs {6D,6D} = \t" << (total_time/NBT)<<std::endl; | ||
| return 0; | ||
| } | ||
|
|
Oops, something went wrong.