Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	benchmark/bipedal_timings.cpp	Lines:	0	354	0.0 %
Date:	2024-02-13 11:12:33	Branches:	0	996	0.0 %


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2023, LAAS-CNRS, CTU, INRIA, 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/srdf.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/contact-fwddyn.hpp"
#include "crocoddyl/multibody/actuations/floating-base.hpp"
#include "crocoddyl/multibody/actuations/full.hpp"
#include "crocoddyl/multibody/contacts/contact-3d.hpp"
#include "crocoddyl/multibody/contacts/contact-6d.hpp"
#include "crocoddyl/multibody/contacts/multiple-contacts.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;

  pinocchio::urdf::buildModel(EXAMPLE_ROBOT_DATA_MODEL_DIR
                              "/talos_data/robots/talos_reduced.urdf",
                              pinocchio::JointModelFreeFlyer(), model);
  model.lowerPositionLimit.head<7>().array() = -1;
  model.upperPositionLimit.head<7>().array() = 1.;

  pinocchio::srdf::loadReferenceConfigurations(
      model, EXAMPLE_ROBOT_DATA_MODEL_DIR "/talos_data/srdf/talos.srdf", false);
  const std::string RF = "leg_right_6_joint";
  const std::string LF = "leg_left_6_joint";

  /*************************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::ActuationModelFloatingBase> actuation =
      boost::make_shared<crocoddyl::ActuationModelFloatingBase>(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("arm_right_7_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::ContactModelMultiple> contact_models =
      boost::make_shared<crocoddyl::ContactModelMultiple>(state,
                                                          actuation->get_nu());

  boost::shared_ptr<crocoddyl::ContactModelAbstract> support_contact_model6D =
      boost::make_shared<crocoddyl::ContactModel6D>(
          state, model.getFrameId(RF), pinocchio::SE3::Identity(),
          pinocchio::LOCAL_WORLD_ALIGNED, actuation->get_nu(),
          Eigen::Vector2d(0., 50.));
  contact_models->addContact(
      model.frames[model.getFrameId(RF)].name + "_contact",
      support_contact_model6D);

  boost::shared_ptr<crocoddyl::ContactModelAbstract> support_contact_model3D =
      boost::make_shared<crocoddyl::ContactModel3D>(
          state, model.getFrameId(LF), Eigen::Vector3d::Zero(),
          pinocchio::LOCAL_WORLD_ALIGNED, actuation->get_nu(),
          Eigen::Vector2d(0., 50.));
  contact_models->addContact(
      model.frames[model.getFrameId(LF)].name + "_contact",
      support_contact_model3D);

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

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