Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	benchmark/arm_manipulation_timings.cpp	Lines:	0	343	0.0 %
Date:	2024-02-13 11:12:33	Branches:	0	958	0.0 %


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2023, LAAS-CNRS, University of Edinburgh,
//                          Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#include <example-robot-data/path.hpp>
#include <pinocchio/algorithm/model.hpp>
#include <pinocchio/parsers/urdf.hpp>

#include "crocoddyl/core/costs/cost-sum.hpp"
#include "crocoddyl/core/costs/residual.hpp"
#include "crocoddyl/core/integrator/euler.hpp"
#include "crocoddyl/core/integrator/rk.hpp"
#include "crocoddyl/core/mathbase.hpp"
#include "crocoddyl/core/optctrl/shooting.hpp"
#include "crocoddyl/core/residuals/control.hpp"
#include "crocoddyl/core/utils/timer.hpp"
#include "crocoddyl/multibody/actions/free-fwddyn.hpp"
#include "crocoddyl/multibody/actuations/full.hpp"
#include "crocoddyl/multibody/residuals/frame-placement.hpp"
#include "crocoddyl/multibody/residuals/state.hpp"
#include "crocoddyl/multibody/states/multibody.hpp"

#define SMOOTH(s) for (size_t _smooth = 0; _smooth < s; ++_smooth)

#define STDDEV(vec) \
  std::sqrt(((vec - vec.mean())).square().sum() / ((double)vec.size() - 1))
#define AVG(vec) (vec.mean())

void printStatistics(std::string name, Eigen::ArrayXd duration) {
  std::cout << "  " << std::left << std::setw(42) << name << std::left
            << std::setw(15) << AVG(duration) << std::left << std::setw(15)
            << STDDEV(duration) << std::left << std::setw(15)
            << duration.maxCoeff() << std::left << std::setw(15)
            << duration.minCoeff() << std::endl;
}

int main(int argc, char* argv[]) {
  unsigned int N = 100;  // number of nodes
  unsigned int T = 5e4;  // number of trials
  if (argc > 1) {
    T = atoi(argv[1]);
  }

  /**************************DOUBLE**********************/
  /**************************DOUBLE**********************/
  /**************************DOUBLE**********************/
  pinocchio::Model model_full, model;
  pinocchio::urdf::buildModel(EXAMPLE_ROBOT_DATA_MODEL_DIR
                              "/talos_data/robots/talos_left_arm.urdf",
                              model_full);
  std::vector<pinocchio::JointIndex> locked_joints;
  locked_joints.reserve(3);

  locked_joints.push_back(5);
  locked_joints.push_back(6);
  locked_joints.push_back(7);

  pinocchio::buildReducedModel(model_full, locked_joints,
                               Eigen::VectorXd::Zero(model_full.nq), model);

  /*************************PINOCCHIO MODEL**************/

  /************************* SETUP ***********************/
  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);

  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>(
                     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>(
                     state, actuation->get_nu()));
  boost::shared_ptr<crocoddyl::CostModelAbstract> uRegCost =
      boost::make_shared<crocoddyl::CostModelResidual>(
          state, boost::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());

  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>
      runningDAM =
          boost::make_shared<crocoddyl::DifferentialActionModelFreeFwdDynamics>(
              state, actuation, runningCostModel);

  boost::shared_ptr<crocoddyl::DifferentialActionModelFreeFwdDynamics>
      terminalDAM =
          boost::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>(
          runningDAM, crocoddyl::RKType::four, 1e-3);
  boost::shared_ptr<crocoddyl::ActionModelAbstract> terminalModel =
      boost::make_shared<crocoddyl::IntegratedActionModelEuler>(terminalDAM,
                                                                1e-3);

  std::vector<boost::shared_ptr<crocoddyl::ActionModelAbstract> >
      runningModelsWithEuler(N, runningModelWithEuler);
  std::vector<boost::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::vector<Eigen::VectorXd> xs(N + 1, x0);

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

  boost::shared_ptr<crocoddyl::ActionDataAbstract> runningModelWithEuler_data =
      runningModelWithEuler->createData();
  boost::shared_ptr<crocoddyl::ActionDataAbstract> runningModelWithRK4_data =
      runningModelWithRK4->createData();
  boost::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 =
      actuation->createData();
  boost::shared_ptr<crocoddyl::CostDataAbstract> goalTrackingCost_data =
      goalTrackingCost->createData(&d->multibody);
  boost::shared_ptr<crocoddyl::CostDataAbstract> xRegCost_data =
      xRegCost->createData(&d->multibody);
  boost::shared_ptr<crocoddyl::CostDataAbstract> uRegCost_data =
      uRegCost->createData(&d->multibody);

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

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

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

  Eigen::ArrayXd duration(T);

  std::vector<Eigen::VectorXd> x0s;
  std::vector<Eigen::VectorXd> u0s;
  PINOCCHIO_ALIGNED_STD_VECTOR(Eigen::VectorXd) x1s;  // (T, state->rand());
  PINOCCHIO_ALIGNED_STD_VECTOR(Eigen::VectorXd) x2s;  // (T, state->rand());
  PINOCCHIO_ALIGNED_STD_VECTOR(Eigen::VectorXd) us;   // (T, state->rand());
  PINOCCHIO_ALIGNED_STD_VECTOR(Eigen::VectorXd)
  dxs(T, Eigen::VectorXd::Zero(2 * model.nv));

  for (size_t i = 0; i < T; ++i) {
    x1s.push_back(state->rand());
    x2s.push_back(state->rand());
    us.push_back(Eigen::VectorXd(actuation->get_nu()));
  }
  for (size_t i = 0; i < N; ++i) {
    x0s.push_back(state->rand());
    u0s.push_back(Eigen::VectorXd(actuation->get_nu()));
  }
  x0s.push_back(state->rand());

  /*********************State**********************************/
  std::cout << std::left << std::setw(42) << "Function call"
            << "  " << std::left << std::setw(15) << "AVG (us)" << std::left
            << std::setw(15) << "STDDEV (us)" << std::left << std::setw(15)
            << "MAX (us)" << std::left << std::setw(15) << "MIN (us)"
            << std::endl;

  duration.setZero();
  SMOOTH(T) {
    timer.reset();
    pinocchio::difference(model, x1s[_smooth].head(model.nq),
                          x2s[_smooth].head(model.nq),
                          dxs[_smooth].head(model.nv));
    duration[_smooth] = timer.get_us_duration();
  }
  std::cout << "pinocchio" << std::endl;
  printStatistics("difference", duration);

  duration.setZero();
  SMOOTH(T) {
    timer.reset();
    pinocchio::integrate(model, x1s[_smooth].head(model.nq),
                         dxs[_smooth].head(model.nv),
                         x2s[_smooth].head(model.nq));
    duration[_smooth] = timer.get_us_duration();
  }
  printStatistics("integrate", duration);

  duration.setZero();
  SMOOTH(T) {
    timer.reset();
    pinocchio::dIntegrate(
        model, x1s[_smooth].head(model.nq), dxs[_smooth].head(model.nv),
        Jsecond.bottomLeftCorner(model.nv, model.nv), pinocchio::ARG1);
    duration[_smooth] = timer.get_us_duration();
  }
  printStatistics("dIntegrate ARG1", duration);

  duration.setZero();
  SMOOTH(T) {
    timer.reset();
    pinocchio::dIntegrate(
        model, x1s[_smooth].head(model.nq), dxs[_smooth].head(model.nv),
        Jsecond.bottomLeftCorner(model.nv, model.nv), pinocchio::ARG0);
    duration[_smooth] = timer.get_us_duration();
  }
  printStatistics("dIntegrate ARG0", duration);

  duration.setZero();
  Eigen::MatrixXd Jin(Eigen::MatrixXd::Random(model.nv, 2 * model.nv));
  SMOOTH(T) {
    timer.reset();
    pinocchio::dIntegrateTransport(model, x1s[_smooth].head(model.nq),
                                   dxs[_smooth].head(model.nv), Jin,
                                   pinocchio::ARG0);
    duration[_smooth] = timer.get_us_duration();
  }
  printStatistics("dIntegrateTransport with aliasing", duration);

  duration.setZero();
  Jin = Eigen::MatrixXd::Random(model.nv, 2 * model.nv);
  Eigen::MatrixXd Jout(Eigen::MatrixXd::Random(model.nv, 2 * model.nv));
  SMOOTH(T) {
    timer.reset();
    pinocchio::dIntegrateTransport(model, x1s[_smooth].head(model.nq),
                                   dxs[_smooth].head(model.nv), Jin, Jout,
                                   pinocchio::ARG0);
    duration[_smooth] = timer.get_us_duration();
  }
  printStatistics("dIntegrateTransport w/o aliasing", duration);

  duration.setZero();
  SMOOTH(T) {
    timer.reset();
    state->diff(x1s[_smooth], x2s[_smooth], dxs[_smooth]);
    duration[_smooth] = timer.get_us_duration();
  }
  std::cout << "StateMultibody" << std::endl;
  printStatistics("diff", duration);

  duration.setZero();
  SMOOTH(T) {
    timer.reset();
    state->integrate(x1s[_smooth], dxs[_smooth], x2s[_smooth]);
    duration[_smooth] = timer.get_us_duration();
  }
  printStatistics("integrate", duration);

  duration.setZero();
  SMOOTH(T) {
    timer.reset();
    state->Jdiff(x1s[_smooth], x2s[_smooth], Jfirst, Jsecond, crocoddyl::both);
    duration[_smooth] = timer.get_us_duration();
  }
  printStatistics("Jdiff both", duration);

  duration.setZero();
  SMOOTH(T) {
    timer.reset();
    state->Jdiff(x1s[_smooth], x2s[_smooth], Jfirst, Jsecond, crocoddyl::first);
    duration[_smooth] = timer.get_us_duration();
  }
  printStatistics("Jdiff first", duration);

  duration.setZero();
  SMOOTH(T) {
    timer.reset();
    state->Jdiff(x1s[_smooth], x2s[_smooth], Jfirst, Jsecond,
                 crocoddyl::second);
    duration[_smooth] = timer.get_us_duration();
  }
  printStatistics("Jdiff second", duration);

  duration.setZero();
  SMOOTH(T) {
    timer.reset();
    state->Jintegrate(x1s[_smooth], dxs[_smooth], Jfirst, Jsecond,
                      crocoddyl::both);
    duration[_smooth] = timer.get_us_duration();
  }
  printStatistics("Jintegrate both", duration);

  duration.setZero();
  SMOOTH(T) {
    timer.reset();
    state->Jintegrate(x1s[_smooth], dxs[_smooth], Jfirst, Jsecond,
                      crocoddyl::first);
    duration[_smooth] = timer.get_us_duration();
  }
  printStatistics("Jintegrate first", duration);

  duration.setZero();
  SMOOTH(T) {
    timer.reset();
    state->Jintegrate(x1s[_smooth], dxs[_smooth], Jfirst, Jsecond,
                      crocoddyl::second);
    duration[_smooth] = timer.get_us_duration();
  }
  printStatistics("Jintegrate second", duration);

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

  duration.setZero();
  SMOOTH(T) {
    timer.reset();
    activationQuad->calc(activationQuad_data, dxs[_smooth]);
    duration[_smooth] = timer.get_us_duration();
  }
  std::cout << "ActivationModelQuad" << std::endl;
  printStatistics("calc", duration);

  duration.setZero();
  SMOOTH(T) {
    timer.reset();
    activationQuad->calcDiff(activationQuad_data, dxs[_smooth]);
    duration[_smooth] = timer.get_us_duration();
  }
  printStatistics("calcDiff", duration);

  /*************************************Actuation*******************/

  duration.setZero();
  SMOOTH(T) {
    timer.reset();
    actuation->calc(actuation_data, x1s[_smooth], us[_smooth]);
    duration[_smooth] = timer.get_us_duration();
  }
  std::cout << "ActuationModelFull" << std::endl;
  printStatistics("calc", duration);

  duration.setZero();
  SMOOTH(T) {
    timer.reset();
    actuation->calcDiff(actuation_data, x1s[_smooth], us[_smooth]);
    duration[_smooth] = timer.get_us_duration();
  }
  printStatistics("calcDiff", duration);

  /*******************************Cost****************************/
  duration.setZero();
  SMOOTH(T) {
    timer.reset();
    goalTrackingCost->calc(goalTrackingCost_data, x1s[_smooth], us[_smooth]);
    duration[_smooth] = timer.get_us_duration();
  }
  std::cout << "CostModelFramePlacement" << std::endl;
  printStatistics("calc", duration);

  duration.setZero();
  SMOOTH(T) {
    timer.reset();
    goalTrackingCost->calcDiff(goalTrackingCost_data, x1s[_smooth],
                               us[_smooth]);
    duration[_smooth] = timer.get_us_duration();
  }
  printStatistics("calcDiff", duration);

  duration.setZero();
  SMOOTH(T) {
    timer.reset();
    xRegCost->calc(xRegCost_data, x1s[_smooth], us[_smooth]);
    duration[_smooth] = timer.get_us_duration();
  }
  std::cout << "CostModelState" << std::endl;
  printStatistics("calc", duration);

  duration.setZero();
  SMOOTH(T) {
    timer.reset();
    xRegCost->calcDiff(xRegCost_data, x1s[_smooth], us[_smooth]);
    duration[_smooth] = timer.get_us_duration();
  }
  printStatistics("calcDiff", duration);

  duration.setZero();
  SMOOTH(T) {
    timer.reset();
    uRegCost->calc(uRegCost_data, x1s[_smooth], us[_smooth]);
    duration[_smooth] = timer.get_us_duration();
  }
  std::cout << "CostModelControl" << std::endl;
  printStatistics("calc", duration);

  duration.setZero();
  SMOOTH(T) {
    timer.reset();
    uRegCost->calcDiff(uRegCost_data, x1s[_smooth], us[_smooth]);
    duration[_smooth] = timer.get_us_duration();
  }
  printStatistics("calcDiff", duration);

  duration.setZero();
  SMOOTH(T) {
    timer.reset();
    runningCostModel->calc(runningCostModel_data, x1s[_smooth], us[_smooth]);
    duration[_smooth] = timer.get_us_duration();
  }
  std::cout << "CostModelSum" << std::endl;
  printStatistics("calc", duration);

  duration.setZero();
  SMOOTH(T) {
    timer.reset();
    runningCostModel->calcDiff(runningCostModel_data, x1s[_smooth],
                               us[_smooth]);
    duration[_smooth] = timer.get_us_duration();
  }
  printStatistics("calcDiff", duration);

  duration.setZero();
  SMOOTH(T) {
    timer.reset();
    runningDAM->calc(runningDAM_data, x1s[_smooth], us[_smooth]);
    duration[_smooth] = timer.get_us_duration();
  }
  std::cout << "FreeFwdDynamics" << std::endl;
  printStatistics("calc", duration);

  duration.setZero();
  SMOOTH(T) {
    timer.reset();
    runningDAM->calcDiff(runningDAM_data, x1s[_smooth], us[_smooth]);
    duration[_smooth] = timer.get_us_duration();
  }
  printStatistics("calcDiff", duration);

  duration.setZero();
  SMOOTH(T) {
    timer.reset();
    runningModelWithEuler->calc(runningModelWithEuler_data, x1s[_smooth],
                                us[_smooth]);
    duration[_smooth] = timer.get_us_duration();
  }
  std::cout << "FreeFwdDynamics+Euler" << std::endl;
  printStatistics("calc", duration);

  duration.setZero();
  SMOOTH(T) {
    timer.reset();
    runningModelWithEuler->calcDiff(runningModelWithEuler_data, x1s[_smooth],
                                    us[_smooth]);
    duration[_smooth] = timer.get_us_duration();
  }
  printStatistics("calcDiff", duration);

  duration.setZero();
  SMOOTH(T) {
    timer.reset();
    runningModelWithRK4->calc(runningModelWithRK4_data, x1s[_smooth],
                              us[_smooth]);
    duration[_smooth] = timer.get_us_duration();
  }
  std::cout << "FreeFwdDynamics+RK4" << std::endl;
  printStatistics("calc", duration);

  duration.setZero();
  SMOOTH(T) {
    timer.reset();
    runningModelWithRK4->calcDiff(runningModelWithRK4_data, x1s[_smooth],
                                  us[_smooth]);
    duration[_smooth] = timer.get_us_duration();
  }
  printStatistics("calcDiff", duration);

  duration = Eigen::ArrayXd(T / N);
  SMOOTH(T / N) {
    timer.reset();
    problemWithEuler->calc(x0s, u0s);
    duration[_smooth] = timer.get_us_duration();
  }
  std::cout << "Problem+Euler" << std::endl;
  printStatistics("calc", duration);

  duration.setZero();
  SMOOTH(T / N) {
    timer.reset();
    problemWithEuler->calcDiff(x0s, u0s);
    duration[_smooth] = timer.get_us_duration();
  }
  printStatistics("calcDiff", duration);

  duration.setZero();
  SMOOTH(T / N) {
    timer.reset();
    problemWithRK4->calc(x0s, u0s);
    duration[_smooth] = timer.get_us_duration();
  }
  std::cout << "Problem+RK4" << std::endl;
  printStatistics("calc", duration);

  duration.setZero();
  SMOOTH(T / N) {
    timer.reset();
    problemWithRK4->calcDiff(x0s, u0s);
    duration[_smooth] = timer.get_us_duration();
  }
  printStatistics("calcDiff", duration);
}


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019-2023, LAAS-CNRS, University of Edinburgh,
5			// Heriot-Watt University
6			// Copyright note valid unless otherwise stated in individual files.
7			// All rights reserved.
8			///////////////////////////////////////////////////////////////////////////////
9
10			#include <example-robot-data/path.hpp>
11			#include <pinocchio/algorithm/model.hpp>
12			#include <pinocchio/parsers/urdf.hpp>
13
14			#include "crocoddyl/core/costs/cost-sum.hpp"
15			#include "crocoddyl/core/costs/residual.hpp"
16			#include "crocoddyl/core/integrator/euler.hpp"
17			#include "crocoddyl/core/integrator/rk.hpp"
18			#include "crocoddyl/core/mathbase.hpp"
19			#include "crocoddyl/core/optctrl/shooting.hpp"
20			#include "crocoddyl/core/residuals/control.hpp"
21			#include "crocoddyl/core/utils/timer.hpp"
22			#include "crocoddyl/multibody/actions/free-fwddyn.hpp"
23			#include "crocoddyl/multibody/actuations/full.hpp"
24			#include "crocoddyl/multibody/residuals/frame-placement.hpp"
25			#include "crocoddyl/multibody/residuals/state.hpp"
26			#include "crocoddyl/multibody/states/multibody.hpp"
27
28			#define SMOOTH(s) for (size_t _smooth = 0; _smooth < s; ++_smooth)
29
30			#define STDDEV(vec) \
31			std::sqrt(((vec - vec.mean())).square().sum() / ((double)vec.size() - 1))
32			#define AVG(vec) (vec.mean())
33
34			void printStatistics(std::string name, Eigen::ArrayXd duration) {
35			std::cout << " " << std::left << std::setw(42) << name << std::left
36			<< std::setw(15) << AVG(duration) << std::left << std::setw(15)
37			<< STDDEV(duration) << std::left << std::setw(15)
38			<< duration.maxCoeff() << std::left << std::setw(15)
39			<< duration.minCoeff() << std::endl;
40			}
41
42			int main(int argc, char* argv[]) {
43			unsigned int N = 100; // number of nodes
44			unsigned int T = 5e4; // number of trials
45			if (argc > 1) {
46			T = atoi(argv[1]);
47			}
48
49			/************************DOUBLE********************/
50			/************************DOUBLE********************/
51			/************************DOUBLE********************/
52			pinocchio::Model model_full, model;
53			pinocchio::urdf::buildModel(EXAMPLE_ROBOT_DATA_MODEL_DIR
54			"/talos_data/robots/talos_left_arm.urdf",
55			model_full);
56			std::vector<pinocchio::JointIndex> locked_joints;
57			locked_joints.reserve(3);
58
59			locked_joints.push_back(5);
60			locked_joints.push_back(6);
61			locked_joints.push_back(7);
62
63			pinocchio::buildReducedModel(model_full, locked_joints,
64			Eigen::VectorXd::Zero(model_full.nq), model);
65
66			/***********************PINOCCHIO MODEL************/
67
68			/*********************** SETUP *********************/
69			crocoddyl::Timer timer;
70			std::cout << "NQ: " << model.nq << std::endl;
71
72			boost::shared_ptr<crocoddyl::StateMultibody> state =
73			boost::make_shared<crocoddyl::StateMultibody>(
74			boost::make_shared<pinocchio::Model>(model));
75			boost::shared_ptr<crocoddyl::ActuationModelFull> actuation =
76			boost::make_shared<crocoddyl::ActuationModelFull>(state);
77
78			Eigen::VectorXd q0 = Eigen::VectorXd::Random(state->get_nq());
79			Eigen::VectorXd x0(state->get_nx());
80			x0 << q0, Eigen::VectorXd::Random(state->get_nv());
81			Eigen::MatrixXd Jfirst(2 * model.nv, 2 * model.nv),
82			Jsecond(2 * model.nv, 2 * model.nv);
83
84			boost::shared_ptr<crocoddyl::CostModelAbstract> goalTrackingCost =
85			boost::make_shared<crocoddyl::CostModelResidual>(
86			state, boost::make_shared<crocoddyl::ResidualModelFramePlacement>(
87			state, model.getFrameId("gripper_left_joint"),
88			pinocchio::SE3(Eigen::Matrix3d::Identity(),
89			Eigen::Vector3d(.0, .0, .4)),
90			actuation->get_nu()));
91
92			boost::shared_ptr<crocoddyl::CostModelAbstract> xRegCost =
93			boost::make_shared<crocoddyl::CostModelResidual>(
94			state, boost::make_shared<crocoddyl::ResidualModelState>(
95			state, actuation->get_nu()));
96			boost::shared_ptr<crocoddyl::CostModelAbstract> uRegCost =
97			boost::make_shared<crocoddyl::CostModelResidual>(
98			state, boost::make_shared<crocoddyl::ResidualModelControl>(
99			state, actuation->get_nu()));
100
101			boost::shared_ptr<crocoddyl::CostModelSum> runningCostModel =
102			boost::make_shared<crocoddyl::CostModelSum>(state, actuation->get_nu());
103			boost::shared_ptr<crocoddyl::CostModelSum> terminalCostModel =
104			boost::make_shared<crocoddyl::CostModelSum>(state, actuation->get_nu());
105
106			runningCostModel->addCost("gripperPose", goalTrackingCost, 1);
107			runningCostModel->addCost("xReg", xRegCost, 1e-4);
108			runningCostModel->addCost("uReg", uRegCost, 1e-4);
109			terminalCostModel->addCost("gripperPose", goalTrackingCost, 1);
110
111			boost::shared_ptr<crocoddyl::DifferentialActionModelFreeFwdDynamics>
112			runningDAM =
113			boost::make_shared<crocoddyl::DifferentialActionModelFreeFwdDynamics>(
114			state, actuation, runningCostModel);
115
116			boost::shared_ptr<crocoddyl::DifferentialActionModelFreeFwdDynamics>
117			terminalDAM =
118			boost::make_shared<crocoddyl::DifferentialActionModelFreeFwdDynamics>(
119			state, actuation, terminalCostModel);
120
121			boost::shared_ptr<crocoddyl::ActionModelAbstract> runningModelWithEuler =
122			boost::make_shared<crocoddyl::IntegratedActionModelEuler>(runningDAM,
123			1e-3);
124			boost::shared_ptr<crocoddyl::ActionModelAbstract> runningModelWithRK4 =
125			boost::make_shared<crocoddyl::IntegratedActionModelRK>(
126			runningDAM, crocoddyl::RKType::four, 1e-3);
127			boost::shared_ptr<crocoddyl::ActionModelAbstract> terminalModel =
128			boost::make_shared<crocoddyl::IntegratedActionModelEuler>(terminalDAM,
129			1e-3);
130
131			std::vector<boost::shared_ptr<crocoddyl::ActionModelAbstract> >
132			runningModelsWithEuler(N, runningModelWithEuler);
133			std::vector<boost::shared_ptr<crocoddyl::ActionModelAbstract> >
134			runningModelsWithRK4(N, runningModelWithRK4);
135
136			boost::shared_ptr<crocoddyl::ShootingProblem> problemWithEuler =
137			boost::make_shared<crocoddyl::ShootingProblem>(x0, runningModelsWithEuler,
138			terminalModel);
139			boost::shared_ptr<crocoddyl::ShootingProblem> problemWithRK4 =
140			boost::make_shared<crocoddyl::ShootingProblem>(x0, runningModelsWithRK4,
141			terminalModel);
142			std::vector<Eigen::VectorXd> xs(N + 1, x0);
143
144			/***************************************************************/
145
146			boost::shared_ptr<crocoddyl::ActionDataAbstract> runningModelWithEuler_data =
147			runningModelWithEuler->createData();
148			boost::shared_ptr<crocoddyl::ActionDataAbstract> runningModelWithRK4_data =
149			runningModelWithRK4->createData();
150			boost::shared_ptr<crocoddyl::DifferentialActionDataAbstract> runningDAM_data =
151			runningDAM->createData();
152			crocoddyl::DifferentialActionDataFreeFwdDynamics* d =
153			static_cast<crocoddyl::DifferentialActionDataFreeFwdDynamics*>(
154			runningDAM_data.get());
155			boost::shared_ptr<crocoddyl::ActuationDataAbstract> actuation_data =
156			actuation->createData();
157			boost::shared_ptr<crocoddyl::CostDataAbstract> goalTrackingCost_data =
158			goalTrackingCost->createData(&d->multibody);
159			boost::shared_ptr<crocoddyl::CostDataAbstract> xRegCost_data =
160			xRegCost->createData(&d->multibody);
161			boost::shared_ptr<crocoddyl::CostDataAbstract> uRegCost_data =
162			uRegCost->createData(&d->multibody);
163
164			boost::shared_ptr<crocoddyl::CostDataSum> runningCostModel_data =
165			runningCostModel->createData(&d->multibody);
166
167			boost::shared_ptr<crocoddyl::ActivationModelAbstract> activationQuad =
168			xRegCost->get_activation();
169			boost::shared_ptr<crocoddyl::ActivationDataAbstract> activationQuad_data =
170			activationQuad->createData();
171
172			/********************************************************************/
173
174			Eigen::ArrayXd duration(T);
175
176			std::vector<Eigen::VectorXd> x0s;
177			std::vector<Eigen::VectorXd> u0s;
178			PINOCCHIO_ALIGNED_STD_VECTOR(Eigen::VectorXd) x1s; // (T, state->rand());
179			PINOCCHIO_ALIGNED_STD_VECTOR(Eigen::VectorXd) x2s; // (T, state->rand());
180			PINOCCHIO_ALIGNED_STD_VECTOR(Eigen::VectorXd) us; // (T, state->rand());
181			PINOCCHIO_ALIGNED_STD_VECTOR(Eigen::VectorXd)
182			dxs(T, Eigen::VectorXd::Zero(2 * model.nv));
183
184			for (size_t i = 0; i < T; ++i) {
185			x1s.push_back(state->rand());
186			x2s.push_back(state->rand());
187			us.push_back(Eigen::VectorXd(actuation->get_nu()));
188			}
189			for (size_t i = 0; i < N; ++i) {
190			x0s.push_back(state->rand());
191			u0s.push_back(Eigen::VectorXd(actuation->get_nu()));
192			}
193			x0s.push_back(state->rand());
194
195			/*******************State********************************/
196			std::cout << std::left << std::setw(42) << "Function call"
197			<< " " << std::left << std::setw(15) << "AVG (us)" << std::left
198			<< std::setw(15) << "STDDEV (us)" << std::left << std::setw(15)
199			<< "MAX (us)" << std::left << std::setw(15) << "MIN (us)"
200			<< std::endl;
201
202			duration.setZero();
203			SMOOTH(T) {
204			timer.reset();
205			pinocchio::difference(model, x1s[_smooth].head(model.nq),
206			x2s[_smooth].head(model.nq),
207			dxs[_smooth].head(model.nv));
208			duration[_smooth] = timer.get_us_duration();
209			}
210			std::cout << "pinocchio" << std::endl;
211			printStatistics("difference", duration);
212
213			duration.setZero();
214			SMOOTH(T) {
215			timer.reset();
216			pinocchio::integrate(model, x1s[_smooth].head(model.nq),
217			dxs[_smooth].head(model.nv),
218			x2s[_smooth].head(model.nq));
219			duration[_smooth] = timer.get_us_duration();
220			}
221			printStatistics("integrate", duration);
222
223			duration.setZero();
224			SMOOTH(T) {
225			timer.reset();
226			pinocchio::dIntegrate(
227			model, x1s[_smooth].head(model.nq), dxs[_smooth].head(model.nv),
228			Jsecond.bottomLeftCorner(model.nv, model.nv), pinocchio::ARG1);
229			duration[_smooth] = timer.get_us_duration();
230			}
231			printStatistics("dIntegrate ARG1", duration);
232
233			duration.setZero();
234			SMOOTH(T) {
235			timer.reset();
236			pinocchio::dIntegrate(
237			model, x1s[_smooth].head(model.nq), dxs[_smooth].head(model.nv),
238			Jsecond.bottomLeftCorner(model.nv, model.nv), pinocchio::ARG0);
239			duration[_smooth] = timer.get_us_duration();
240			}
241			printStatistics("dIntegrate ARG0", duration);
242
243			duration.setZero();
244			Eigen::MatrixXd Jin(Eigen::MatrixXd::Random(model.nv, 2 * model.nv));
245			SMOOTH(T) {
246			timer.reset();
247			pinocchio::dIntegrateTransport(model, x1s[_smooth].head(model.nq),
248			dxs[_smooth].head(model.nv), Jin,
249			pinocchio::ARG0);
250			duration[_smooth] = timer.get_us_duration();
251			}
252			printStatistics("dIntegrateTransport with aliasing", duration);
253
254			duration.setZero();
255			Jin = Eigen::MatrixXd::Random(model.nv, 2 * model.nv);
256			Eigen::MatrixXd Jout(Eigen::MatrixXd::Random(model.nv, 2 * model.nv));
257			SMOOTH(T) {
258			timer.reset();
259			pinocchio::dIntegrateTransport(model, x1s[_smooth].head(model.nq),
260			dxs[_smooth].head(model.nv), Jin, Jout,
261			pinocchio::ARG0);
262			duration[_smooth] = timer.get_us_duration();
263			}
264			printStatistics("dIntegrateTransport w/o aliasing", duration);
265
266			duration.setZero();
267			SMOOTH(T) {
268			timer.reset();
269			state->diff(x1s[_smooth], x2s[_smooth], dxs[_smooth]);
270			duration[_smooth] = timer.get_us_duration();
271			}
272			std::cout << "StateMultibody" << std::endl;
273			printStatistics("diff", duration);
274
275			duration.setZero();
276			SMOOTH(T) {
277			timer.reset();
278			state->integrate(x1s[_smooth], dxs[_smooth], x2s[_smooth]);
279			duration[_smooth] = timer.get_us_duration();
280			}
281			printStatistics("integrate", duration);
282
283			duration.setZero();
284			SMOOTH(T) {
285			timer.reset();
286			state->Jdiff(x1s[_smooth], x2s[_smooth], Jfirst, Jsecond, crocoddyl::both);
287			duration[_smooth] = timer.get_us_duration();
288			}
289			printStatistics("Jdiff both", duration);
290
291			duration.setZero();
292			SMOOTH(T) {
293			timer.reset();
294			state->Jdiff(x1s[_smooth], x2s[_smooth], Jfirst, Jsecond, crocoddyl::first);
295			duration[_smooth] = timer.get_us_duration();
296			}
297			printStatistics("Jdiff first", duration);
298
299			duration.setZero();
300			SMOOTH(T) {
301			timer.reset();
302			state->Jdiff(x1s[_smooth], x2s[_smooth], Jfirst, Jsecond,
303			crocoddyl::second);
304			duration[_smooth] = timer.get_us_duration();
305			}
306			printStatistics("Jdiff second", duration);
307
308			duration.setZero();
309			SMOOTH(T) {
310			timer.reset();
311			state->Jintegrate(x1s[_smooth], dxs[_smooth], Jfirst, Jsecond,
312			crocoddyl::both);
313			duration[_smooth] = timer.get_us_duration();
314			}
315			printStatistics("Jintegrate both", duration);
316
317			duration.setZero();
318			SMOOTH(T) {
319			timer.reset();
320			state->Jintegrate(x1s[_smooth], dxs[_smooth], Jfirst, Jsecond,
321			crocoddyl::first);
322			duration[_smooth] = timer.get_us_duration();
323			}
324			printStatistics("Jintegrate first", duration);
325
326			duration.setZero();
327			SMOOTH(T) {
328			timer.reset();
329			state->Jintegrate(x1s[_smooth], dxs[_smooth], Jfirst, Jsecond,
330			crocoddyl::second);
331			duration[_smooth] = timer.get_us_duration();
332			}
333			printStatistics("Jintegrate second", duration);
334
335			/**************************************************************/
336
337			duration.setZero();
338			SMOOTH(T) {
339			timer.reset();
340			activationQuad->calc(activationQuad_data, dxs[_smooth]);
341			duration[_smooth] = timer.get_us_duration();
342			}
343			std::cout << "ActivationModelQuad" << std::endl;
344			printStatistics("calc", duration);
345
346			duration.setZero();
347			SMOOTH(T) {
348			timer.reset();
349			activationQuad->calcDiff(activationQuad_data, dxs[_smooth]);
350			duration[_smooth] = timer.get_us_duration();
351			}
352			printStatistics("calcDiff", duration);
353
354			/***********************************Actuation*****************/
355
356			duration.setZero();
357			SMOOTH(T) {
358			timer.reset();
359			actuation->calc(actuation_data, x1s[_smooth], us[_smooth]);
360			duration[_smooth] = timer.get_us_duration();
361			}
362			std::cout << "ActuationModelFull" << std::endl;
363			printStatistics("calc", duration);
364
365			duration.setZero();
366			SMOOTH(T) {
367			timer.reset();
368			actuation->calcDiff(actuation_data, x1s[_smooth], us[_smooth]);
369			duration[_smooth] = timer.get_us_duration();
370			}
371			printStatistics("calcDiff", duration);
372
373			/*****************************Cost**************************/
374			duration.setZero();
375			SMOOTH(T) {
376			timer.reset();
377			goalTrackingCost->calc(goalTrackingCost_data, x1s[_smooth], us[_smooth]);
378			duration[_smooth] = timer.get_us_duration();
379			}
380			std::cout << "CostModelFramePlacement" << std::endl;
381			printStatistics("calc", duration);
382
383			duration.setZero();
384			SMOOTH(T) {
385			timer.reset();
386			goalTrackingCost->calcDiff(goalTrackingCost_data, x1s[_smooth],
387			us[_smooth]);
388			duration[_smooth] = timer.get_us_duration();
389			}
390			printStatistics("calcDiff", duration);
391
392			duration.setZero();
393			SMOOTH(T) {
394			timer.reset();
395			xRegCost->calc(xRegCost_data, x1s[_smooth], us[_smooth]);
396			duration[_smooth] = timer.get_us_duration();
397			}
398			std::cout << "CostModelState" << std::endl;
399			printStatistics("calc", duration);
400
401			duration.setZero();
402			SMOOTH(T) {
403			timer.reset();
404			xRegCost->calcDiff(xRegCost_data, x1s[_smooth], us[_smooth]);
405			duration[_smooth] = timer.get_us_duration();
406			}
407			printStatistics("calcDiff", duration);
408
409			duration.setZero();
410			SMOOTH(T) {
411			timer.reset();
412			uRegCost->calc(uRegCost_data, x1s[_smooth], us[_smooth]);
413			duration[_smooth] = timer.get_us_duration();
414			}
415			std::cout << "CostModelControl" << std::endl;
416			printStatistics("calc", duration);
417
418			duration.setZero();
419			SMOOTH(T) {
420			timer.reset();
421			uRegCost->calcDiff(uRegCost_data, x1s[_smooth], us[_smooth]);
422			duration[_smooth] = timer.get_us_duration();
423			}
424			printStatistics("calcDiff", duration);
425
426			duration.setZero();
427			SMOOTH(T) {
428			timer.reset();
429			runningCostModel->calc(runningCostModel_data, x1s[_smooth], us[_smooth]);
430			duration[_smooth] = timer.get_us_duration();
431			}
432			std::cout << "CostModelSum" << std::endl;
433			printStatistics("calc", duration);
434
435			duration.setZero();
436			SMOOTH(T) {
437			timer.reset();
438			runningCostModel->calcDiff(runningCostModel_data, x1s[_smooth],
439			us[_smooth]);
440			duration[_smooth] = timer.get_us_duration();
441			}
442			printStatistics("calcDiff", duration);
443
444			duration.setZero();
445			SMOOTH(T) {
446			timer.reset();
447			runningDAM->calc(runningDAM_data, x1s[_smooth], us[_smooth]);
448			duration[_smooth] = timer.get_us_duration();
449			}
450			std::cout << "FreeFwdDynamics" << std::endl;
451			printStatistics("calc", duration);
452
453			duration.setZero();
454			SMOOTH(T) {
455			timer.reset();
456			runningDAM->calcDiff(runningDAM_data, x1s[_smooth], us[_smooth]);
457			duration[_smooth] = timer.get_us_duration();
458			}
459			printStatistics("calcDiff", duration);
460
461			duration.setZero();
462			SMOOTH(T) {
463			timer.reset();
464			runningModelWithEuler->calc(runningModelWithEuler_data, x1s[_smooth],
465			us[_smooth]);
466			duration[_smooth] = timer.get_us_duration();
467			}
468			std::cout << "FreeFwdDynamics+Euler" << std::endl;
469			printStatistics("calc", duration);
470
471			duration.setZero();
472			SMOOTH(T) {
473			timer.reset();
474			runningModelWithEuler->calcDiff(runningModelWithEuler_data, x1s[_smooth],
475			us[_smooth]);
476			duration[_smooth] = timer.get_us_duration();
477			}
478			printStatistics("calcDiff", duration);
479
480			duration.setZero();
481			SMOOTH(T) {
482			timer.reset();
483			runningModelWithRK4->calc(runningModelWithRK4_data, x1s[_smooth],
484			us[_smooth]);
485			duration[_smooth] = timer.get_us_duration();
486			}
487			std::cout << "FreeFwdDynamics+RK4" << std::endl;
488			printStatistics("calc", duration);
489
490			duration.setZero();
491			SMOOTH(T) {
492			timer.reset();
493			runningModelWithRK4->calcDiff(runningModelWithRK4_data, x1s[_smooth],
494			us[_smooth]);
495			duration[_smooth] = timer.get_us_duration();
496			}
497			printStatistics("calcDiff", duration);
498
499			duration = Eigen::ArrayXd(T / N);
500			SMOOTH(T / N) {
501			timer.reset();
502			problemWithEuler->calc(x0s, u0s);
503			duration[_smooth] = timer.get_us_duration();
504			}
505			std::cout << "Problem+Euler" << std::endl;
506			printStatistics("calc", duration);
507
508			duration.setZero();
509			SMOOTH(T / N) {
510			timer.reset();
511			problemWithEuler->calcDiff(x0s, u0s);
512			duration[_smooth] = timer.get_us_duration();
513			}
514			printStatistics("calcDiff", duration);
515
516			duration.setZero();
517			SMOOTH(T / N) {
518			timer.reset();
519			problemWithRK4->calc(x0s, u0s);
520			duration[_smooth] = timer.get_us_duration();
521			}
522			std::cout << "Problem+RK4" << std::endl;
523			printStatistics("calc", duration);
524
525			duration.setZero();
526			SMOOTH(T / N) {
527			timer.reset();
528			problemWithRK4->calcDiff(x0s, u0s);
529			duration[_smooth] = timer.get_us_duration();
530			}
531			printStatistics("calcDiff", duration);
532			}