Skip to content

Commit

Permalink
Merge pull request #1339 from nim65s/std
Browse files Browse the repository at this point in the history 
{boost -> std}::shared_ptr
  • Loading branch information
@cmastalli
cmastalli authored Jan 29, 2025
2 parents f559bb2 + 34a33e9 commit 5a540c9
Show file tree
Hide file tree
Showing 359 changed files with 4,133 additions and 4,238 deletions.
1 change: 1 addition & 0 deletions CHANGELOG.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).

## [Unreleased]

* :warning: BREAKING: replaced boost shared pointers by std ones in https://github.com/loco-3d/crocoddyl/pull/1339
* Changed return policy in std::vector of Eigen's vector and matrices to be compliant with Pinocchio in https://github.com/loco-3d/crocoddyl/pull/1338
* Prevent users to improperly setting residual references in https://github.com/loco-3d/crocoddyl/pull/1332
* Fixed the inequality constraints' feasibility computation by incorporating bounds into the calculation in https://github.com/loco-3d/crocoddyl/pull/1307
Expand Down
54 changes: 27 additions & 27 deletions benchmark/all_robots.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,7 @@ void print_benchmark(RobotEENames robot) {
unsigned int T = 1e3; // number of trials

// Building the running and terminal models
boost::shared_ptr<crocoddyl::ActionModelAbstract> runningModel, terminalModel;
std::shared_ptr<crocoddyl::ActionModelAbstract> runningModel, terminalModel;
if (robot.robot_name == "Talos_arm") {
crocoddyl::benchmark::build_arm_action_models(runningModel, terminalModel);
} else if (robot.robot_name == "Kinova_arm") {
Expand All @@ -46,62 +46,62 @@ void print_benchmark(RobotEENames robot) {
}

// Get the initial state
boost::shared_ptr<crocoddyl::StateMultibody> state =
boost::static_pointer_cast<crocoddyl::StateMultibody>(
std::shared_ptr<crocoddyl::StateMultibody> state =
std::static_pointer_cast<crocoddyl::StateMultibody>(
runningModel->get_state());
std::cout << "NQ: " << state->get_nq() << std::endl;
std::cout << "Number of nodes: " << N << std::endl << std::endl;

Eigen::VectorXd default_state(state->get_nq() + state->get_nv());
boost::shared_ptr<crocoddyl::IntegratedActionModelEulerTpl<double> > rm =
boost::static_pointer_cast<
std::shared_ptr<crocoddyl::IntegratedActionModelEulerTpl<double> > rm =
std::static_pointer_cast<
crocoddyl::IntegratedActionModelEulerTpl<double> >(runningModel);
if (robot.robot_name == "Talos_arm" || robot.robot_name == "Kinova_arm") {
boost::shared_ptr<
std::shared_ptr<
crocoddyl::DifferentialActionModelFreeFwdDynamicsTpl<double> >
dm = boost::static_pointer_cast<
dm = std::static_pointer_cast<
crocoddyl::DifferentialActionModelFreeFwdDynamicsTpl<double> >(
rm->get_differential());
default_state
<< dm->get_pinocchio().referenceConfigurations[robot.reference_conf],
Eigen::VectorXd::Zero(state->get_nv());
} else {
boost::shared_ptr<
std::shared_ptr<
crocoddyl::DifferentialActionModelContactFwdDynamicsTpl<double> >
dm = boost::static_pointer_cast<
dm = std::static_pointer_cast<
crocoddyl::DifferentialActionModelContactFwdDynamicsTpl<double> >(
rm->get_differential());
default_state
<< dm->get_pinocchio().referenceConfigurations[robot.reference_conf],
Eigen::VectorXd::Zero(state->get_nv());
}
Eigen::VectorXd x0(default_state);
std::vector<boost::shared_ptr<crocoddyl::ActionModelAbstract> > runningModels(
std::vector<std::shared_ptr<crocoddyl::ActionModelAbstract> > runningModels(
N, runningModel);
boost::shared_ptr<crocoddyl::ShootingProblem> problem =
boost::make_shared<crocoddyl::ShootingProblem>(x0, runningModels,
terminalModel);
std::shared_ptr<crocoddyl::ShootingProblem> problem =
std::make_shared<crocoddyl::ShootingProblem>(x0, runningModels,
terminalModel);
// Computing the warm-start
std::vector<Eigen::VectorXd> xs(N + 1, x0);
std::vector<Eigen::VectorXd> us(
N, Eigen::VectorXd::Zero(runningModel->get_nu()));
for (unsigned int i = 0; i < N; ++i) {
const boost::shared_ptr<crocoddyl::ActionModelAbstract>& model =
const std::shared_ptr<crocoddyl::ActionModelAbstract>& model =
problem->get_runningModels()[i];
const boost::shared_ptr<crocoddyl::ActionDataAbstract>& data =
const std::shared_ptr<crocoddyl::ActionDataAbstract>& data =
problem->get_runningDatas()[i];
model->quasiStatic(data, us[i], x0);
}
crocoddyl::SolverFDDP ddp(problem);
ddp.setCandidate(xs, us, false);
boost::shared_ptr<crocoddyl::ActionDataAbstract> runningData =
std::shared_ptr<crocoddyl::ActionDataAbstract> runningData =
runningModel->createData();

#ifdef CROCODDYL_WITH_CODEGEN
// Code generation of the running an terminal models
typedef CppAD::AD<CppAD::cg::CG<double> > ADScalar;
boost::shared_ptr<crocoddyl::ActionModelAbstractTpl<ADScalar> >
ad_runningModel, ad_terminalModel;
std::shared_ptr<crocoddyl::ActionModelAbstractTpl<ADScalar> > ad_runningModel,
ad_terminalModel;
if (robot.robot_name == "Talos_arm") {
crocoddyl::benchmark::build_arm_action_models(ad_runningModel,
ad_terminalModel);
Expand All @@ -113,26 +113,26 @@ void print_benchmark(RobotEENames robot) {
ad_terminalModel);
}

boost::shared_ptr<crocoddyl::ActionModelAbstract> cg_runningModel =
boost::make_shared<crocoddyl::ActionModelCodeGen>(
std::shared_ptr<crocoddyl::ActionModelAbstract> cg_runningModel =
std::make_shared<crocoddyl::ActionModelCodeGen>(
ad_runningModel, runningModel, robot.robot_name + "_running_cg");
boost::shared_ptr<crocoddyl::ActionModelAbstract> cg_terminalModel =
boost::make_shared<crocoddyl::ActionModelCodeGen>(
std::shared_ptr<crocoddyl::ActionModelAbstract> cg_terminalModel =
std::make_shared<crocoddyl::ActionModelCodeGen>(
ad_terminalModel, terminalModel, robot.robot_name + "_terminal_cg");

// Defining the shooting problem for both cases: with and without code
// generation
std::vector<boost::shared_ptr<crocoddyl::ActionModelAbstract> >
std::vector<std::shared_ptr<crocoddyl::ActionModelAbstract> >
cg_runningModels(N, cg_runningModel);
boost::shared_ptr<crocoddyl::ShootingProblem> cg_problem =
boost::make_shared<crocoddyl::ShootingProblem>(x0, cg_runningModels,
cg_terminalModel);
std::shared_ptr<crocoddyl::ShootingProblem> cg_problem =
std::make_shared<crocoddyl::ShootingProblem>(x0, cg_runningModels,
cg_terminalModel);

// Check that code-generated action model is the same as original.
/**************************************************************************/
crocoddyl::SolverFDDP cg_ddp(cg_problem);
cg_ddp.setCandidate(xs, us, false);
boost::shared_ptr<crocoddyl::ActionDataAbstract> cg_runningData =
std::shared_ptr<crocoddyl::ActionDataAbstract> cg_runningData =
cg_runningModel->createData();
Eigen::VectorXd x_rand = cg_runningModel->get_state()->rand();
Eigen::VectorXd u_rand = Eigen::VectorXd::Random(cg_runningModel->get_nu());
Expand Down
22 changes: 11 additions & 11 deletions benchmark/arm_manipulation_optctrl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,12 +22,12 @@ int main(int argc, char* argv[]) {
}

// Building the running and terminal models
boost::shared_ptr<crocoddyl::ActionModelAbstract> runningModel, terminalModel;
std::shared_ptr<crocoddyl::ActionModelAbstract> runningModel, terminalModel;
crocoddyl::benchmark::build_arm_action_models(runningModel, terminalModel);

// Get the initial state
boost::shared_ptr<crocoddyl::StateMultibody> state =
boost::static_pointer_cast<crocoddyl::StateMultibody>(
std::shared_ptr<crocoddyl::StateMultibody> state =
std::static_pointer_cast<crocoddyl::StateMultibody>(
runningModel->get_state());
std::cout << "NQ: " << state->get_nq() << std::endl;
std::cout << "Number of nodes: " << N << std::endl;
Expand All @@ -38,27 +38,27 @@ int main(int argc, char* argv[]) {

// For this optimal control problem, we define 100 knots (or running action
// models) plus a terminal knot
std::vector<boost::shared_ptr<crocoddyl::ActionModelAbstract> > runningModels(
std::vector<std::shared_ptr<crocoddyl::ActionModelAbstract> > runningModels(
N, runningModel);
boost::shared_ptr<crocoddyl::ShootingProblem> problem =
boost::make_shared<crocoddyl::ShootingProblem>(x0, runningModels,
terminalModel);
std::shared_ptr<crocoddyl::ShootingProblem> problem =
std::make_shared<crocoddyl::ShootingProblem>(x0, runningModels,
terminalModel);
std::vector<Eigen::VectorXd> xs(N + 1, x0);
std::vector<Eigen::VectorXd> us(
N, Eigen::VectorXd::Zero(runningModel->get_nu()));
for (unsigned int i = 0; i < N; ++i) {
const boost::shared_ptr<crocoddyl::ActionModelAbstract>& model =
const std::shared_ptr<crocoddyl::ActionModelAbstract>& model =
problem->get_runningModels()[i];
const boost::shared_ptr<crocoddyl::ActionDataAbstract>& data =
const std::shared_ptr<crocoddyl::ActionDataAbstract>& data =
problem->get_runningDatas()[i];
model->quasiStatic(data, us[i], x0);
}

// Formulating the optimal control problem
crocoddyl::SolverFDDP solver(problem);
if (CALLBACKS) {
std::vector<boost::shared_ptr<crocoddyl::CallbackAbstract> > cbs;
cbs.push_back(boost::make_shared<crocoddyl::CallbackVerbose>());
std::vector<std::shared_ptr<crocoddyl::CallbackAbstract> > cbs;
cbs.push_back(std::make_shared<crocoddyl::CallbackVerbose>());
solver.setCallbacks(cbs);
}

Expand Down
95 changes: 47 additions & 48 deletions benchmark/arm_manipulation_timings.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,104 +69,103 @@ int main(int argc, char* argv[]) {
crocoddyl::Timer timer;
std::cout << "NQ: " << model.nq << std::endl;

boost::shared_ptr<crocoddyl::StateMultibody> state =
boost::make_shared<crocoddyl::StateMultibody>(
boost::make_shared<pinocchio::Model>(model));
boost::shared_ptr<crocoddyl::ActuationModelFull> actuation =
boost::make_shared<crocoddyl::ActuationModelFull>(state);
std::shared_ptr<crocoddyl::StateMultibody> state =
std::make_shared<crocoddyl::StateMultibody>(
std::make_shared<pinocchio::Model>(model));
std::shared_ptr<crocoddyl::ActuationModelFull> actuation =
std::make_shared<crocoddyl::ActuationModelFull>(state);

Eigen::VectorXd q0 = Eigen::VectorXd::Random(state->get_nq());
Eigen::VectorXd x0(state->get_nx());
x0 << q0, Eigen::VectorXd::Random(state->get_nv());
Eigen::MatrixXd Jfirst(2 * model.nv, 2 * model.nv),
Jsecond(2 * model.nv, 2 * model.nv);

boost::shared_ptr<crocoddyl::CostModelAbstract> goalTrackingCost =
boost::make_shared<crocoddyl::CostModelResidual>(
state, boost::make_shared<crocoddyl::ResidualModelFramePlacement>(
std::shared_ptr<crocoddyl::CostModelAbstract> goalTrackingCost =
std::make_shared<crocoddyl::CostModelResidual>(
state, std::make_shared<crocoddyl::ResidualModelFramePlacement>(
state, model.getFrameId("gripper_left_joint"),
pinocchio::SE3(Eigen::Matrix3d::Identity(),
Eigen::Vector3d(.0, .0, .4)),
actuation->get_nu()));

boost::shared_ptr<crocoddyl::CostModelAbstract> xRegCost =
boost::make_shared<crocoddyl::CostModelResidual>(
state, boost::make_shared<crocoddyl::ResidualModelState>(
std::shared_ptr<crocoddyl::CostModelAbstract> xRegCost =
std::make_shared<crocoddyl::CostModelResidual>(
state, std::make_shared<crocoddyl::ResidualModelState>(
state, actuation->get_nu()));
boost::shared_ptr<crocoddyl::CostModelAbstract> uRegCost =
boost::make_shared<crocoddyl::CostModelResidual>(
state, boost::make_shared<crocoddyl::ResidualModelControl>(
std::shared_ptr<crocoddyl::CostModelAbstract> uRegCost =
std::make_shared<crocoddyl::CostModelResidual>(
state, std::make_shared<crocoddyl::ResidualModelControl>(
state, actuation->get_nu()));

boost::shared_ptr<crocoddyl::CostModelSum> runningCostModel =
boost::make_shared<crocoddyl::CostModelSum>(state, actuation->get_nu());
boost::shared_ptr<crocoddyl::CostModelSum> terminalCostModel =
boost::make_shared<crocoddyl::CostModelSum>(state, actuation->get_nu());
std::shared_ptr<crocoddyl::CostModelSum> runningCostModel =
std::make_shared<crocoddyl::CostModelSum>(state, actuation->get_nu());
std::shared_ptr<crocoddyl::CostModelSum> terminalCostModel =
std::make_shared<crocoddyl::CostModelSum>(state, actuation->get_nu());

runningCostModel->addCost("gripperPose", goalTrackingCost, 1);
runningCostModel->addCost("xReg", xRegCost, 1e-4);
runningCostModel->addCost("uReg", uRegCost, 1e-4);
terminalCostModel->addCost("gripperPose", goalTrackingCost, 1);

boost::shared_ptr<crocoddyl::DifferentialActionModelFreeFwdDynamics>
std::shared_ptr<crocoddyl::DifferentialActionModelFreeFwdDynamics>
runningDAM =
boost::make_shared<crocoddyl::DifferentialActionModelFreeFwdDynamics>(
std::make_shared<crocoddyl::DifferentialActionModelFreeFwdDynamics>(
state, actuation, runningCostModel);

boost::shared_ptr<crocoddyl::DifferentialActionModelFreeFwdDynamics>
std::shared_ptr<crocoddyl::DifferentialActionModelFreeFwdDynamics>
terminalDAM =
boost::make_shared<crocoddyl::DifferentialActionModelFreeFwdDynamics>(
std::make_shared<crocoddyl::DifferentialActionModelFreeFwdDynamics>(
state, actuation, terminalCostModel);

boost::shared_ptr<crocoddyl::ActionModelAbstract> runningModelWithEuler =
boost::make_shared<crocoddyl::IntegratedActionModelEuler>(runningDAM,
1e-3);
boost::shared_ptr<crocoddyl::ActionModelAbstract> runningModelWithRK4 =
boost::make_shared<crocoddyl::IntegratedActionModelRK>(
std::shared_ptr<crocoddyl::ActionModelAbstract> runningModelWithEuler =
std::make_shared<crocoddyl::IntegratedActionModelEuler>(runningDAM, 1e-3);
std::shared_ptr<crocoddyl::ActionModelAbstract> runningModelWithRK4 =
std::make_shared<crocoddyl::IntegratedActionModelRK>(
runningDAM, crocoddyl::RKType::four, 1e-3);
boost::shared_ptr<crocoddyl::ActionModelAbstract> terminalModel =
boost::make_shared<crocoddyl::IntegratedActionModelEuler>(terminalDAM,
1e-3);
std::shared_ptr<crocoddyl::ActionModelAbstract> terminalModel =
std::make_shared<crocoddyl::IntegratedActionModelEuler>(terminalDAM,
1e-3);

std::vector<boost::shared_ptr<crocoddyl::ActionModelAbstract> >
std::vector<std::shared_ptr<crocoddyl::ActionModelAbstract> >
runningModelsWithEuler(N, runningModelWithEuler);
std::vector<boost::shared_ptr<crocoddyl::ActionModelAbstract> >
std::vector<std::shared_ptr<crocoddyl::ActionModelAbstract> >
runningModelsWithRK4(N, runningModelWithRK4);

boost::shared_ptr<crocoddyl::ShootingProblem> problemWithEuler =
boost::make_shared<crocoddyl::ShootingProblem>(x0, runningModelsWithEuler,
terminalModel);
boost::shared_ptr<crocoddyl::ShootingProblem> problemWithRK4 =
boost::make_shared<crocoddyl::ShootingProblem>(x0, runningModelsWithRK4,
terminalModel);
std::shared_ptr<crocoddyl::ShootingProblem> problemWithEuler =
std::make_shared<crocoddyl::ShootingProblem>(x0, runningModelsWithEuler,
terminalModel);
std::shared_ptr<crocoddyl::ShootingProblem> problemWithRK4 =
std::make_shared<crocoddyl::ShootingProblem>(x0, runningModelsWithRK4,
terminalModel);
std::vector<Eigen::VectorXd> xs(N + 1, x0);

/***************************************************************/

boost::shared_ptr<crocoddyl::ActionDataAbstract> runningModelWithEuler_data =
std::shared_ptr<crocoddyl::ActionDataAbstract> runningModelWithEuler_data =
runningModelWithEuler->createData();
boost::shared_ptr<crocoddyl::ActionDataAbstract> runningModelWithRK4_data =
std::shared_ptr<crocoddyl::ActionDataAbstract> runningModelWithRK4_data =
runningModelWithRK4->createData();
boost::shared_ptr<crocoddyl::DifferentialActionDataAbstract> runningDAM_data =
std::shared_ptr<crocoddyl::DifferentialActionDataAbstract> runningDAM_data =
runningDAM->createData();
crocoddyl::DifferentialActionDataFreeFwdDynamics* d =
static_cast<crocoddyl::DifferentialActionDataFreeFwdDynamics*>(
runningDAM_data.get());
boost::shared_ptr<crocoddyl::ActuationDataAbstract> actuation_data =
std::shared_ptr<crocoddyl::ActuationDataAbstract> actuation_data =
actuation->createData();
boost::shared_ptr<crocoddyl::CostDataAbstract> goalTrackingCost_data =
std::shared_ptr<crocoddyl::CostDataAbstract> goalTrackingCost_data =
goalTrackingCost->createData(&d->multibody);
boost::shared_ptr<crocoddyl::CostDataAbstract> xRegCost_data =
std::shared_ptr<crocoddyl::CostDataAbstract> xRegCost_data =
xRegCost->createData(&d->multibody);
boost::shared_ptr<crocoddyl::CostDataAbstract> uRegCost_data =
std::shared_ptr<crocoddyl::CostDataAbstract> uRegCost_data =
uRegCost->createData(&d->multibody);

boost::shared_ptr<crocoddyl::CostDataSum> runningCostModel_data =
std::shared_ptr<crocoddyl::CostDataSum> runningCostModel_data =
runningCostModel->createData(&d->multibody);

boost::shared_ptr<crocoddyl::ActivationModelAbstract> activationQuad =
std::shared_ptr<crocoddyl::ActivationModelAbstract> activationQuad =
xRegCost->get_activation();
boost::shared_ptr<crocoddyl::ActivationDataAbstract> activationQuad_data =
std::shared_ptr<crocoddyl::ActivationDataAbstract> activationQuad_data =
activationQuad->createData();

/********************************************************************/
Expand Down
Loading

0 comments on commit 5a540c9

Please sign in to comment.