Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Problems trying to switch from default datatypes to floats #1162

Open
will-gerard opened this issue Aug 29, 2023 · 10 comments
Open

Problems trying to switch from default datatypes to floats #1162

will-gerard opened this issue Aug 29, 2023 · 10 comments

Comments

@will-gerard
Copy link

will-gerard commented Aug 29, 2023

Thanks for all your work creating and open sourcing such a great trajopt library. I have gone through the tutorials and have been experimenting with it, and have been finding it to work very smoothly. I am able to get a DDP solve working correctly for a Kuka arm, both in python via the bindings and in the c++ directly. However I also want to test with floats, rather than with doubles, and when I try to switch to the templated versions of the functions and pass float as the template parameter, I am running into issues. I saw from this github issue that for parsing a URDF only doubles are supported, but that they can then be cast to floats, which is what I am doing for the robot model I create, and this part seems to work:

ModelTpl<double> model_double;
pinocchio::urdf::buildModel(urdf_filename, model_double);
pinocchio::ModelTpl<float> model = model_double.cast<float>();

However, I am unable to create a state multibody object using the float robot model, and I am not sure why. What I was doing before, and which works correctly, is:

boost::shared_ptr<crocoddyl::StateMultibody> state = 
    boost::make_shared<crocoddyl::StateMultibody>(
	    boost::make_shared<pinocchio::ModelTpl<double>>(robot_model));

And so what I want to do now is something like:

boost::shared_ptr<crocoddyl::StateMultibodyTpl<float>> state =
    boost::make_shared<crocoddyl::StateMultibodyTpl<float>>(
        boost::make_shared<pinocchio::ModelTpl<float>>(model));

But this results in the following errors:

/home/a2rlab/anaconda3/envs/crocoddyl/lib/pkgconfig/../../include/crocoddyl/multibody/states/multibody.hxx:64:6:   required from here
/home/a2rlab/anaconda3/envs/crocoddyl/lib/pkgconfig/../../include/pinocchio/multibody/liegroup/special-orthogonal.hpp:74:45: error: no matching function for call to ‘if_then_else(pinocchio::internal::ComparisonOperators, const Scalar&, double, float, pinocchio::internal::if_then_else_impl<float, float, float, float>::ReturnType)’
   74 |                                 if_then_else(internal::GT, tr, Scalar(2) - 1e-2,
      |                                 ~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
   75 |                                              asin((R(1,0) - R(0,1)) / Scalar(2)), // then
      |                                              ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
   76 |                                              if_then_else(internal::GE, R (1, 0), Scalar(0),
      |                                              ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
   77 |                                                           acos(tr/Scalar(2)), // then
      |                                                           ~~~~~~~~~~~~~~~~~~~~~~~~~~~
   78 |                                                           -acos(tr/Scalar(2))
      |                                                           ~~~~~~~~~~~~~~~~~~~
   79 |                                                           )
      |                                                           ~
   80 |                                              )
      |                                              ~
In file included from /home/a2rlab/anaconda3/envs/crocoddyl/lib/pkgconfig/../../include/pinocchio/math/quaternion.hpp:16,
                 from /home/a2rlab/anaconda3/envs/crocoddyl/lib/pkgconfig/../../include/pinocchio/spatial/se3-tpl.hpp:12,
                 from /home/a2rlab/anaconda3/envs/crocoddyl/lib/pkgconfig/../../include/pinocchio/spatial/se3.hpp:44,
                 from /home/a2rlab/anaconda3/envs/crocoddyl/lib/pkgconfig/../../include/pinocchio/multibody/model.hpp:10,
                 from /home/a2rlab/anaconda3/envs/crocoddyl/lib/pkgconfig/../../include/pinocchio/parsers/urdf.hpp:9,
                 from test_croc.cpp:1:
/home/a2rlab/anaconda3/envs/crocoddyl/lib/pkgconfig/../../include/pinocchio/utils/static-if.hpp:77:5: note: candidate: ‘template<class LhsType, class RhsType, class ThenType, class ElseType> typename pinocchio::internal::if_then_else_impl<LhsType, RhsType, ThenType, ElseType>::ReturnType pinocchio::internal::if_then_else(pinocchio::internal::ComparisonOperators, const LhsType&, const RhsType&, const ThenType&, const ElseType&)’
   77 |     if_then_else(const ComparisonOperators op,
      |     ^~~~~~~~~~~~
/home/a2rlab/anaconda3/envs/crocoddyl/lib/pkgconfig/../../include/pinocchio/utils/static-if.hpp:77:5: note:   template argument deduction/substitution failed:

...

home/a2rlab/anaconda3/envs/crocoddyl/lib/pkgconfig/../../include/crocoddyl/multibody/states/multibody.hxx:64:6:   required from here
/home/a2rlab/anaconda3/envs/crocoddyl/lib/pkgconfig/../../include/pinocchio/utils/static-if.hpp:77:5: error: invalid use of incomplete type ‘struct pinocchio::internal::if_then_else_impl<float, double, float, float>’

The output is large so I have truncated it and provided what seem like the most relevant snippets.

I have tried various versions of this, without success. This construction I included above is the same thing I see done in the arm.hpp benchmark here. And to be sure I was doing the same thing, I tried calling this build_arm_action_models function directly in an extremely simple program:

#include <iostream>
#define EXAMPLE_ROBOT_DATA_MODEL_DIR "/home/a2rlab/anaconda3/envs/crocoddyl/share/example-robot-data/robots"
#include "crocoddyl/benchmark/factory/arm.hpp"

int main() {
    boost::shared_ptr<crocoddyl::ActionModelAbstractTpl<double>> runningModel;
    boost::shared_ptr<crocoddyl::ActionModelAbstractTpl<double>> terminalModel;
    crocoddyl::benchmark::build_arm_action_models<double>(runningModel, terminalModel);

    if (runningModel && terminalModel) {
        std::cout << "Done" << std::endl;
    }
    return 0;
}

This compiles and runs successfully. However, when I try to change the template parameter to float, I get the following errors:

required from here
/usr/include/eigen3/Eigen/src/Core/MatrixBase.h:75:17: error: ‘coeff’ has not been declared in ‘Eigen::MatrixBase<Eigen::Product<Eigen::Product<Eigen::Transpose<const Eigen::Block<Eigen::Matrix<float, -1, -1>, -1, -1, false> >, Eigen::DiagonalWrapper<const Eigen::Block<Eigen::Matrix<double, -1, 1>, -1, 1, false> >, 1>, Eigen::Block<Eigen::Matrix<float, -1, -1>, -1, -1, false>, 0> >::Base’
   75 |     using Base::coeff;
      |                 ^~~~~
/usr/include/eigen3/Eigen/src/Core/MatrixBase.h:78:17: error: ‘eval’ has not been declared in ‘Eigen::MatrixBase<Eigen::Product<Eigen::Product<Eigen::Transpose<const Eigen::Block<Eigen::Matrix<float, -1, -1>, -1, -1, false> >, Eigen::DiagonalWrapper<const Eigen::Block<Eigen::Matrix<double, -1, 1>, -1, 1, false> >, 1>, Eigen::Block<Eigen::Matrix<float, -1, -1>, -1, -1, false>, 0> >::Base’
   78 |     using Base::eval;
      |                 ^~~~
/usr/include/eigen3/Eigen/src/Core/MatrixBase.h:79:25: error: ‘operator-’ has not been declared in ‘Eigen::MatrixBase<Eigen::Product<Eigen::Product<Eigen::Transpose<const Eigen::Block<Eigen::Matrix<float, -1, -1>, -1, -1, false> >, Eigen::DiagonalWrapper<const Eigen::Block<Eigen::Matrix<double, -1, 1>, -1, 1, false> >, 1>, Eigen::Block<Eigen::Matrix<float, -1, -1>, -1, -1, false>, 0> >::Base’
   79 |     using Base::operator-;
      |                         ^
/usr/include/eigen3/Eigen/src/Core/MatrixBase.h:82:25: error: ‘operator*=’ has not been declared in ‘Eigen::MatrixBase<Eigen::Product<Eigen::Product<Eigen::Transpose<const Eigen::Block<Eigen::Matrix<float, -1, -1>, -1, -1, false> >, Eigen::DiagonalWrapper<const Eigen::Block<Eigen::Matrix<double, -1, 1>, -1, 1, false> >, 1>, Eigen::Block<Eigen::Matrix<float, -1, -1>, -1, -1, false>, 0> >::Base’
   82 |     using Base::operator*=;
      |                         ^~
/usr/include/eigen3/Eigen/src/Core/MatrixBase.h:83:25: error: ‘operator/=’ has not been declared in ‘Eigen::MatrixBase<Eigen::Product<Eigen::Product<Eigen::Transpose<const Eigen::Block<Eigen::Matrix<float, -1, -1>, -1, -1, false> >, Eigen::DiagonalWrapper<const Eigen::Block<Eigen::Matrix<double, -1, 1>, -1, 1, false> >, 1>, Eigen::Block<Eigen::Matrix<float, -1, -1>, -1, -1, false>, 0> >::Base’
   83 |     using Base::operator/=;
      |                         ^~

And also:

/home/a2rlab/anaconda3/envs/crocoddyl/lib/pkgconfig/../../include/crocoddyl/multibody/states/multibody.hxx:64:6:   required from here
/home/a2rlab/anaconda3/envs/crocoddyl/lib/pkgconfig/../../include/pinocchio/multibody/liegroup/special-orthogonal.hpp:74:45: error: no matching function for call to ‘if_then_else(pinocchio::internal::ComparisonOperators, const Scalar&, double, float, pinocchio::internal::if_then_else_impl<float, float, float, float>::ReturnType)’
   74 |                                 if_then_else(internal::GT, tr, Scalar(2) - 1e-2,
      |                                 ~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
   75 |                                              asin((R(1,0) - R(0,1)) / Scalar(2)), // then
      |                                              ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
   76 |                                              if_then_else(internal::GE, R (1, 0), Scalar(0),
      |                                              ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
   77 |                                                           acos(tr/Scalar(2)), // then
      |                                                           ~~~~~~~~~~~~~~~~~~~~~~~~~~~
   78 |                                                           -acos(tr/Scalar(2))
      |                                                           ~~~~~~~~~~~~~~~~~~~
   79 |                                                           )
      |                                                           ~
   80 |                                              )
      |                                              ~
In file included from /home/a2rlab/anaconda3/envs/crocoddyl/lib/pkgconfig/../../include/pinocchio/math/quaternion.hpp:16,
                 from /home/a2rlab/anaconda3/envs/crocoddyl/lib/pkgconfig/../../include/pinocchio/spatial/se3-tpl.hpp:12,
                 from /home/a2rlab/anaconda3/envs/crocoddyl/lib/pkgconfig/../../include/pinocchio/spatial/se3.hpp:44,
                 from /home/a2rlab/anaconda3/envs/crocoddyl/lib/pkgconfig/../../include/pinocchio/multibody/model.hpp:10,
                 from /home/a2rlab/anaconda3/envs/crocoddyl/lib/pkgconfig/../../include/pinocchio/algorithm/model.hpp:8,
                 from crocoddyl/benchmark/factory/arm.hpp:13,
                 from test_arm_benchmark.cpp:5:
/home/a2rlab/anaconda3/envs/crocoddyl/lib/pkgconfig/../../include/pinocchio/utils/static-if.hpp:77:5: note: candidate: ‘template<class LhsType, class RhsType, class ThenType, class ElseType> typename pinocchio::internal::if_then_else_impl<LhsType, RhsType, ThenType, ElseType>::ReturnType pinocchio::internal::if_then_else(pinocchio::internal::ComparisonOperators, const LhsType&, const RhsType&, const ThenType&, const ElseType&)’
   77 |     if_then_else(const ComparisonOperators op,


And also:

/home/a2rlab/anaconda3/envs/crocoddyl/lib/pkgconfig/../../include/pinocchio/utils/static-if.hpp:77:5: error: invalid use of incomplete type ‘struct pinocchio::internal::if_then_else_impl<float, double, float, float>’
/home/a2rlab/anaconda3/envs/crocoddyl/lib/pkgconfig/../../include/pinocchio/utils/static-if.hpp:18:12: note: declaration of ‘struct pinocchio::internal::if_then_else_impl<float, double, float, float>’
   18 |     struct if_then_else_impl;

So there are more errors here, but at least many of them appear to be of the same form, and I am wondering if this is my issue. I am running on Ubuntu 22.04, and installed crocoddyl via conda. I have version 2.0.0 of Crocoddyl, version 2.6.18 of Pinocchio, and version 3.4.0 of Eigen. Any support or suggestions you can provide would be very much appreciated, and if there is any other additional information which would be helpful, please let me know! Also tagging @plancherb1 for visibility.

@cmastalli
Copy link
Member

Hi @will-gerard,

This seems to be a bug in Pinocchio.
Could you try this stack-of-tasks/pinocchio#2047?
Please report to us if this PR fixes this issue.

Thanks

@will-gerard
Copy link
Author

Thank you for the quick fix! Building pinocchio with this change does in fact fix the issue, now I am successfully able to instantiate my state multibody object with

	boost::shared_ptr<crocoddyl::StateMultibodyTpl<float>> state =
		boost::make_shared<crocoddyl::StateMultibodyTpl<float>>(
			boost::make_shared<pinocchio::ModelTpl<float>>(model));

However there appears to still be something strange going on when I try to use floats. I am trying to create several residual models to use as part of my cost function. These two both appear to be working correctly:

		boost::shared_ptr<crocoddyl::ResidualModelFrameTranslationTpl<float>> ee_residual =
			boost::make_shared<crocoddyl::ResidualModelFrameTranslationTpl<float>>(state, 
                                 model.getFrameId("iiwa_joint_7"), ee_pos_target);

			boost::shared_ptr<crocoddyl::ResidualModelControlTpl<float>> control_residual =
				boost::make_shared<crocoddyl::ResidualModelControlTpl<float>>(state, u_ref_t);

However, the residual model state of the exact same form causes issues:

		boost::shared_ptr<crocoddyl::ResidualModelStateTpl<float>> terminal_state_residual =
			boost::make_shared<crocoddyl::ResidualModelStateTpl<float>>(state, x_ref_t);

This leads to compilation errors like:

/usr/include/eigen3/Eigen/src/Core/Product.h:29:127: error: no type named 'ReturnType' in 'struct Eigen::ScalarBinaryOpTraits<float, double, Eigen::internal::scalar_product_op<float, double> >'
   29 | typedef typename ScalarBinaryOpTraits<typename traits<LhsCleaned>::Scalar, typename traits<RhsCleaned>::Scalar>::ReturnType Scalar;
      |                                                                                                                             ^~~~~~

In file included from /usr/include/eigen3/Eigen/Core:272,
                 from /usr/local/lib/pkgconfig/../../include/pinocchio/utils/cast.hpp:8,
                 from /usr/local/lib/pkgconfig/../../include/pinocchio/fwd.hpp:23,
                 from /usr/local/lib/pkgconfig/../../include/pinocchio/spatial/fwd.hpp:9,
                 from /usr/local/lib/pkgconfig/../../include/pinocchio/multibody/model.hpp:9,
                 from /usr/local/lib/pkgconfig/../../include/pinocchio/parsers/urdf.hpp:9,
                 from test_croc.cpp:1:
/usr/include/eigen3/Eigen/src/Core/DenseBase.h: In instantiation of 'class Eigen::DenseBase<Eigen::Product<Eigen::Transpose<const Eigen::Block<Eigen::Matrix<float, -1, -1>, -1, -1, false> >, Eigen::Block<Eigen::Matrix<double, -1, 1>, -1, 1, false>, 0> >':
/usr/include/eigen3/Eigen/src/Core/MatrixBase.h:48:34:   required from 'class Eigen::MatrixBase<Eigen::Product<Eigen::Transpose<const Eigen::Block<Eigen::Matrix<float, -1, -1>, -1, -1, false> >, Eigen::Block<Eigen::Matrix<double, -1, 1>, -1, 1, false>, 0> >'
/usr/include/eigen3/Eigen/src/Core/Product.h:120:7:   required from 'class Eigen::internal::dense_product_base<Eigen::Transpose<const Eigen::Block<Eigen::Matrix<float, -1, -1>, -1, -1, false> >, Eigen::Block<Eigen::Matrix<double, -1, 1>, -1, 1, false>, 0, 7>'
/usr/include/eigen3/Eigen/src/Core/Product.h:152:7:   required from 'class Eigen::ProductImpl<Eigen::Transpose<const Eigen::Block<Eigen::Matrix<float, -1, -1>, -1, -1, false> >, Eigen::Block<Eigen::Matrix<double, -1, 1>, -1, 1, false>, 0, Eigen::Dense>'
/usr/include/eigen3/Eigen/src/Core/Product.h:71:7:   required from 'class Eigen::Product<Eigen::Transpose<const Eigen::Block<Eigen::Matrix<float, -1, -1>, -1, -1, false> >, Eigen::Block<Eigen::Matrix<double, -1, 1>, -1, 1, false>, 0>'
/usr/local/lib/pkgconfig/../../include/crocoddyl/multibody/residuals/state.hxx:118:31:   required from 'void crocoddyl::ResidualModelStateTpl<Scalar>::calcCostDiff(const boost::shared_ptr<crocoddyl::CostDataAbstractTpl<double> >&, const boost::shared_ptr<crocoddyl::ResidualDataAbstractTpl<_Scalar> >&, const boost::shared_ptr<crocoddyl::ActivationDataAbstractTpl<double> >&, bool) [with _Scalar = float]'
/usr/local/lib/pkgconfig/../../include/crocoddyl/multibody/residuals/state.hxx:105:6:   required from here
/usr/include/eigen3/Eigen/src/Core/DenseBase.h:83:17: error: 'coeff' has not been declared in 'Eigen::Base<Eigen::Product<Eigen::Transpose<const Eigen::Block<Eigen::Matrix<float, -1, -1, 0, -1, -1>, -1, -1, false> >, Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false>, 0> >'
   83 |     using Base::coeff;
      |                 ^~~~~
/usr/include/eigen3/Eigen/src/Core/DenseBase.h:84:17: error: 'coeffByOuterInner' has not been declared in 'Eigen::Base<Eigen::Product<Eigen::Transpose<const Eigen::Block<Eigen::Matrix<float, -1, -1, 0, -1, -1>, -1, -1, false> >, Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false>, 0> >'
   84 |     using Base::coeffByOuterInner;
      |                 ^~~~~~~~~~~~~~~~~
/usr/include/eigen3/Eigen/src/Core/DenseBase.h:85:26: error: 'operator()' has not been declared in 'Eigen::Base<Eigen::Product<Eigen::Transpose<const Eigen::Block<Eigen::Matrix<float, -1, -1, 0, -1, -1>, -1, -1, false> >, Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false>, 0> >'
   85 |     using Base::operator();
      |                          ^

When I prototype in python this construction seems fine, i.e. this:

state_error_residual = crocoddyl.ResidualModelState(state_differential_model, x_ref_t)

Works correctly, and when I switch back to doubles from floats, this is also working:

		boost::shared_ptr<crocoddyl::ResidualModelStateTpl<double>> terminal_state_residual =
			boost::make_shared<crocoddyl::ResidualModelStateTpl<double>>(state, x_ref_t);

So it does seem like this issue is specific to the ResidualModelStateTpl object, with float specifically as the template parameter. This time the errors look like they are coming from Eigen - maybe there could be a similar cast fix I need to apply somewhere in that library? Thank you so much for your help with this!

@cmastalli
Copy link
Member

Please share the entire code in test_croc.cpp. I need more details.

@will-gerard
Copy link
Author

The full file looks like this at the moment:

#include "pinocchio/parsers/urdf.hpp"

#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include <Eigen/Dense>
#include <pinocchio/multibody/model.hpp>
#include <crocoddyl/core/actuation/squashing/smooth-sat.hpp>
#include <crocoddyl/core/costs/residual.hpp>
#include <crocoddyl/core/integrator/euler.hpp>
#include <crocoddyl/core/solvers/ddp.hpp>
#include <crocoddyl/core/utils/callbacks.hpp>
#include <crocoddyl/core/costs/cost-sum.hpp>

#include <crocoddyl/core/activations/weighted-quadratic.hpp>

#include <crocoddyl/multibody/states/multibody.hpp>
#include <crocoddyl/multibody/actuations/full.hpp>
#include <crocoddyl/multibody/residuals/state.hpp>
#include <crocoddyl/core/residuals/control.hpp>
#include <crocoddyl/multibody/actions/free-fwddyn.hpp>

#include <crocoddyl/multibody/residuals/frame-translation.hpp>

#include <iostream>

#include <experiment_helpers.cuh>
#include <settings.cuh>

// PINOCCHIO_MODEL_DIR is defined by the CMake but you can define your own directory here.
#ifndef PINOCCHIO_MODEL_DIR
#define PINOCCHIO_MODEL_DIR "path_to_the_model_dir"
#endif

#define KNOT_POINTS 32
#define DT .001
#define NUM_CONTROLS 7
#define NUM_STATES 14
#define EE_SIZE 6

#define EE_DIM_POS 3

#define QD_COST 0.1
#define Q_COST 0.1
#define R_COST 0.0001
#define EE_COST 0.5

int main(int argc, char **argv)
{
	using namespace pinocchio;

	// You should change here to set up your own URDF file or just pass it as an argument of this example.
	const std::string urdf_filename = (argc <= 1) ? PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/ur_description/urdf/ur5_robot.urdf") : argv[1];
	// Load the urdf model

	// NOTE: parsing the URDF only works with doubles, if we need to use floats we need to parse first and then cast
	// https://github.com/stack-of-tasks/pinocchio/issues/2037
	pinocchio::ModelTpl<double> model_double;
	// ModelTpl<float> model;
	pinocchio::urdf::buildModel(urdf_filename, model_double);
	// pinocchio::urdf::buildModel(urdf_filename, model);
	pinocchio::ModelTpl<float> model = model_double.cast<float>();

	// // Create data required by the algorithms
	DataTpl<float> data(model);

	// // Create the state and actuation models
	boost::shared_ptr<crocoddyl::StateMultibodyTpl<float>> state =
		boost::make_shared<crocoddyl::StateMultibodyTpl<float>>(
			boost::make_shared<pinocchio::ModelTpl<float>>(model));

	boost::shared_ptr<crocoddyl::ActuationModelFullTpl<float>> actuation =
		boost::make_shared<crocoddyl::ActuationModelFullTpl<float>>(state);

	// Sample a random configuration
	Eigen::VectorXf q = randomConfiguration(model);
	std::cout << "q: " << q.transpose() << std::endl;

	// Perform the forward kinematics over the kinematic tree
	forwardKinematics(model, data, q);

	// Print out the placement of each joint of the kinematic tree
	for (JointIndex joint_id = 0; joint_id < (JointIndex)model.njoints; ++joint_id)
		std::cout << std::setw(24) << std::left
				  << model.names[joint_id] << ": "
				  << std::fixed << std::setprecision(2)
				  << data.oMi[joint_id].translation().transpose()
				  << std::endl;

	
	char eePos_traj_file_name[100];
	char xu_traj_file_name[100];
	const uint32_t knot_points = KNOT_POINTS;
	int start_state = 0;
	int goal_state = 0;

	snprintf(eePos_traj_file_name, sizeof(eePos_traj_file_name), "testfiles/%d_%d_eepos.traj", start_state, goal_state);
	std::vector<std::vector<float>> eePos_traj2d = readCSVToVecVec<float>(eePos_traj_file_name);

	snprintf(xu_traj_file_name, sizeof(xu_traj_file_name), "testfiles/%d_%d_traj.csv", start_state, goal_state);
	std::vector<std::vector<float>> xu_traj2d = readCSVToVecVec<float>(xu_traj_file_name);

	if (eePos_traj2d.size() < knot_points)
	{
		std::cout << "precomputed traj length < knotpoints, not implemented\n";
		return 0;
	}

	std::vector<float> h_eePos_traj;
	for (const auto &vec : eePos_traj2d)
	{
		h_eePos_traj.insert(h_eePos_traj.end(), vec.begin(), vec.end());
	}
	std::vector<float> h_xu_traj;
	for (const auto &xu_vec : xu_traj2d)
	{
		h_xu_traj.insert(h_xu_traj.end(), xu_vec.begin(), xu_vec.end());
	}

	// Create the x0, assuming x0 is a vector of size 14
	Eigen::VectorXf x0(NUM_STATES);
	for (int i = 0; i < NUM_STATES; ++i)
	{
		x0(i) = h_xu_traj[i];
	}

	// // Initialize matrices Q and R for state and control tracking cost
	Eigen::VectorXf Q_vec(NUM_STATES);
	Q_vec.fill(Q_COST);
	
	Eigen::VectorXf R_vec(NUM_CONTROLS);
	R_vec.fill(R_COST);

	Eigen::VectorXf EE_penalty_vec(EE_DIM_POS);
	EE_penalty_vec.fill(EE_COST);
	
	boost::shared_ptr<crocoddyl::ActivationModelWeightedQuadTpl<float>> activation_model_state = 
		boost::make_shared<crocoddyl::ActivationModelWeightedQuadTpl<float>>(Q_vec);
	boost::shared_ptr<crocoddyl::ActivationModelWeightedQuadTpl<float>> activation_model_control = 
		boost::make_shared<crocoddyl::ActivationModelWeightedQuadTpl<float>>(R_vec);


	// Initialize the cost models and action models
	boost::shared_ptr<crocoddyl::CostModelSumTpl<float>> running_cost_model =
		boost::make_shared<crocoddyl::CostModelSumTpl<float>>(state);
	boost::shared_ptr<crocoddyl::CostModelSumTpl<float>> terminal_cost_model =
		boost::make_shared<crocoddyl::CostModelSumTpl<float>>(state);

	printf("initialized the cost models\n");

	for (int t = 0; t < KNOT_POINTS; ++t)
	{
		Eigen::VectorXf x_ref_t(NUM_STATES);
		Eigen::VectorXf u_ref_t(NUM_CONTROLS);
		Eigen::VectorXf ee_pos_target(EE_DIM_POS);
		// read into x_ref_t and u_ref_t from xu_traj2d
		for (int i = 0; i < NUM_STATES; ++i)
		{
			x_ref_t(i) = xu_traj2d[t][i];
			if (i < NUM_CONTROLS) {
				u_ref_t(i) = xu_traj2d[t][i + 14];
			}
			if (i < EE_DIM_POS) {
				ee_pos_target(i) = eePos_traj2d[t][i];
			}
		}


		boost::shared_ptr<crocoddyl::ActivationModelWeightedQuadTpl<float>> activation_ee = 
			boost::make_shared<crocoddyl::ActivationModelWeightedQuadTpl<float>>(EE_penalty_vec);

		boost::shared_ptr<crocoddyl::ResidualModelFrameTranslationTpl<float>> ee_residual =
			boost::make_shared<crocoddyl::ResidualModelFrameTranslationTpl<float>>(state, model.getFrameId("iiwa_joint_7"), ee_pos_target);
		printf("initialized state_residual\n");
		boost::shared_ptr<crocoddyl::CostModelResidualTpl<float>> goal_tracking_xyz_cost =
			boost::make_shared<crocoddyl::CostModelResidualTpl<float>>(state, activation_ee, ee_residual);
		printf("initialized state_cost\n");

		running_cost_model->addCost("goalTrack" + std::to_string(t), goal_tracking_xyz_cost, 1.);

		if (t < (KNOT_POINTS - 1)) {
			// Add control cost if this is not the final knot point
			boost::shared_ptr<crocoddyl::ResidualModelControlTpl<float>> control_residual =
				boost::make_shared<crocoddyl::ResidualModelControlTpl<float>>(state, u_ref_t);
			boost::shared_ptr<crocoddyl::CostModelResidualTpl<float>> control_cost =
				boost::make_shared<crocoddyl::CostModelResidualTpl<float>>(state, control_residual);

			running_cost_model->addCost("controlTrack" + std::to_string(t), control_cost, 0.001);

			printf("Added control cost for knot point %d\n", t);
		}
		else {
			// Add terminal cost if this is final knot point
			// printf("Added state cost for knot point %d\n", t);
			boost::shared_ptr<crocoddyl::ResidualModelStateTpl<float>> terminal_state_residual =
				boost::make_shared<crocoddyl::ResidualModelStateTpl<float>>(state, x_ref_t);

			// boost::shared_ptr<crocoddyl::ResidualModelStateTpl<float>> terminal_state_residual =
			// 	boost::make_shared<crocoddyl::ResidualModelStateTpl<float>>(state, x_ref_t);
			// boost::shared_ptr<crocoddyl::CostModelResidualTpl<float>> terminal_state_cost =
			// 	boost::make_shared<crocoddyl::CostModelResidualTpl<float>>(state, activation_model_state, terminal_state_residual);
			
			// terminal_cost_model->addCost("stateTrack" + std::to_string(t), terminal_state_cost, 10000.);

			// printf("Added terminal cost for knot point %d\n", t);
			// // print out the x_ref_t at this point so we know what the goal is
			// std::cout << "x_ref_t: " << x_ref_t << std::endl;
		}
	}

	// Create the action models
	// boost::shared_ptr<crocoddyl::IntegratedActionModelEulerTpl<float>> running_model =
	// 	boost::make_shared<crocoddyl::IntegratedActionModelEulerTpl<float>>(
	// 		boost::make_shared<crocoddyl::DifferentialActionModelFreeFwdDynamicsTpl<float>>(state, actuation, running_cost_model),
	// 		DT);
	// boost::shared_ptr<crocoddyl::IntegratedActionModelEulerTpl<float>> terminal_model =
	// 	boost::make_shared<crocoddyl::IntegratedActionModelEulerTpl<float>>(
	// 		boost::make_shared<crocoddyl::DifferentialActionModelFreeFwdDynamicsTpl<float>>(state, actuation, terminal_cost_model),
	// 		DT);

	// printf("initialized the action models\n");

	// // Create the problem
	// std::vector<boost::shared_ptr<crocoddyl::ActionModelAbstractTpl<float>>> running_models(
	// 	KNOT_POINTS, running_model);
	// crocoddyl::ShootingProblemTpl<float> problem(x0, running_models, terminal_model);
	// boost::shared_ptr<crocoddyl::ShootingProblem> problem_ptr = boost::make_shared<crocoddyl::ShootingProblem>(x0, running_models, terminal_model);


	// // Create the DDP solver
	// crocoddyl::SolverDDP ddp(problem_ptr);

	// // Set up callback
	// std::vector<boost::shared_ptr<crocoddyl::CallbackAbstract>> cbs;
	// cbs.push_back(boost::make_shared<crocoddyl::CallbackVerbose>());
	// ddp.setCallbacks(cbs);

	// // Solve the problem
	// ddp.solve();

	// //print the initial state x0 for reference
	// std::cout << "Initial state: " << x0 << std::endl;

	// Eigen::VectorXf final_state = ddp.get_xs().back();
    
    // // Print it
    // std::cout << "Final state: " << final_state << std::endl;

	// return 0;
}

Trying to compile this leads to the errors I mention above, but if I comment out the first line in the else statement, the

			boost::shared_ptr<crocoddyl::ResidualModelStateTpl<float>> terminal_state_residual =
				boost::make_shared<crocoddyl::ResidualModelStateTpl<float>>(state, x_ref_t);

Then it compiles successfully.

@cmastalli
Copy link
Member

I don't understand the issue yet, but I see you didn't forward Pinocchio first. Could you include it as the first line? i.e., your first code line should be #include <pinocchio/fwd.hpp>?

@will-gerard
Copy link
Author

I added that line, still encountering the same issue though it looks like. In case useful, I will attach the full error log here:
compiler_error_log.txt

@will-gerard
Copy link
Author

Issue no longer experienced after the fix in this PR, compiling Crocoddyl from the devel branch, including this change, fixed the initial issue. My full test with floats is now very close working, I have one other question, I hope I am not missing anything obvious. For completeness, here is my full example, it is very similar to what I shared above, with the file uncommented and one or two small tweaks:

#include <pinocchio/fwd.hpp>

#include "pinocchio/parsers/urdf.hpp"

#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include <Eigen/Dense>
#include <pinocchio/multibody/model.hpp>
#include <crocoddyl/core/actuation/squashing/smooth-sat.hpp>
#include <crocoddyl/core/costs/residual.hpp>
#include <crocoddyl/core/integrator/euler.hpp>
#include <crocoddyl/core/solvers/ddp.hpp>
#include <crocoddyl/core/utils/callbacks.hpp>
#include <crocoddyl/core/costs/cost-sum.hpp>

#include <crocoddyl/core/activations/weighted-quadratic.hpp>

#include <crocoddyl/multibody/states/multibody.hpp>
#include <crocoddyl/multibody/actuations/full.hpp>
#include <crocoddyl/multibody/residuals/state.hpp>
#include <crocoddyl/core/residuals/control.hpp>
#include <crocoddyl/multibody/actions/free-fwddyn.hpp>

#include <crocoddyl/multibody/residuals/frame-translation.hpp>

#include <iostream>

#include <experiment_helpers.cuh>
#include <settings.cuh>

// PINOCCHIO_MODEL_DIR is defined by the CMake but you can define your own directory here.
#ifndef PINOCCHIO_MODEL_DIR
#define PINOCCHIO_MODEL_DIR "path_to_the_model_dir"
#endif

#define KNOT_POINTS 32
#define DT .001
#define NUM_CONTROLS 7
#define NUM_STATES 14
#define EE_SIZE 6

#define EE_DIM_POS 3

#define QD_COST 0.1
#define Q_COST 0.1
#define R_COST 0.0001
#define EE_COST 0.5

int main(int argc, char **argv)
{
	using namespace pinocchio;

	// You should change here to set up your own URDF file or just pass it as an argument of this example.
	const std::string urdf_filename = (argc <= 1) ? PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/ur_description/urdf/ur5_robot.urdf") : argv[1];
	// Load the urdf model

	// NOTE: parsing the URDF only works with doubles, if we need to use floats we need to parse first and then cast
	// https://github.com/stack-of-tasks/pinocchio/issues/2037
	pinocchio::ModelTpl<double> model_double;
	// ModelTpl<float> model;
	pinocchio::urdf::buildModel(urdf_filename, model_double);
	// pinocchio::urdf::buildModel(urdf_filename, model);
	pinocchio::ModelTpl<float> model = model_double.cast<float>();

	// // Create data required by the algorithms
	DataTpl<float> data(model);

	// // Create the state and actuation models
	boost::shared_ptr<crocoddyl::StateMultibodyTpl<float>> state =
		boost::make_shared<crocoddyl::StateMultibodyTpl<float>>(
			boost::make_shared<pinocchio::ModelTpl<float>>(model));

	boost::shared_ptr<crocoddyl::ActuationModelFullTpl<float>> actuation =
		boost::make_shared<crocoddyl::ActuationModelFullTpl<float>>(state);

	// Sample a random configuration
	Eigen::VectorXf q = randomConfiguration(model);
	std::cout << "q: " << q.transpose() << std::endl;

	// Perform the forward kinematics over the kinematic tree
	forwardKinematics(model, data, q);

	// Print out the placement of each joint of the kinematic tree
	for (JointIndex joint_id = 0; joint_id < (JointIndex)model.njoints; ++joint_id)
		std::cout << std::setw(24) << std::left
				  << model.names[joint_id] << ": "
				  << std::fixed << std::setprecision(2)
				  << data.oMi[joint_id].translation().transpose()
				  << std::endl;

	
	char eePos_traj_file_name[100];
	char xu_traj_file_name[100];
	const uint32_t knot_points = KNOT_POINTS;
	int start_state = 0;
	int goal_state = 0;

	snprintf(eePos_traj_file_name, sizeof(eePos_traj_file_name), "testfiles/%d_%d_eepos.traj", start_state, goal_state);
	std::vector<std::vector<float>> eePos_traj2d = readCSVToVecVec<float>(eePos_traj_file_name);

	snprintf(xu_traj_file_name, sizeof(xu_traj_file_name), "testfiles/%d_%d_traj.csv", start_state, goal_state);
	std::vector<std::vector<float>> xu_traj2d = readCSVToVecVec<float>(xu_traj_file_name);

	if (eePos_traj2d.size() < knot_points)
	{
		std::cout << "precomputed traj length < knotpoints, not implemented\n";
		return 0;
	}

	std::vector<float> h_eePos_traj;
	for (const auto &vec : eePos_traj2d)
	{
		h_eePos_traj.insert(h_eePos_traj.end(), vec.begin(), vec.end());
	}
	std::vector<float> h_xu_traj;
	for (const auto &xu_vec : xu_traj2d)
	{
		h_xu_traj.insert(h_xu_traj.end(), xu_vec.begin(), xu_vec.end());
	}

	// Create the x0, assuming x0 is a vector of size 14
	Eigen::VectorXf x0(NUM_STATES);
	for (int i = 0; i < NUM_STATES; ++i)
	{
		x0(i) = h_xu_traj[i];
	}

	// // Initialize matrices Q and R for state and control tracking cost
	Eigen::VectorXf Q_vec(NUM_STATES);
	Q_vec.fill(Q_COST);
	
	Eigen::VectorXf R_vec(NUM_CONTROLS);
	R_vec.fill(R_COST);

	Eigen::VectorXf EE_penalty_vec(EE_DIM_POS);
	EE_penalty_vec.fill(EE_COST);
	
	boost::shared_ptr<crocoddyl::ActivationModelWeightedQuadTpl<float>> activation_model_state = 
		boost::make_shared<crocoddyl::ActivationModelWeightedQuadTpl<float>>(Q_vec);
	boost::shared_ptr<crocoddyl::ActivationModelWeightedQuadTpl<float>> activation_model_control = 
		boost::make_shared<crocoddyl::ActivationModelWeightedQuadTpl<float>>(R_vec);


	// Initialize the cost models and action models
	boost::shared_ptr<crocoddyl::CostModelSumTpl<float>> running_cost_model =
		boost::make_shared<crocoddyl::CostModelSumTpl<float>>(state);
	boost::shared_ptr<crocoddyl::CostModelSumTpl<float>> terminal_cost_model =
		boost::make_shared<crocoddyl::CostModelSumTpl<float>>(state);

	printf("initialized the cost models\n");

	for (int t = 0; t < KNOT_POINTS; ++t)
	{
		Eigen::VectorXf x_ref_t(NUM_STATES);
		Eigen::VectorXf u_ref_t(NUM_CONTROLS);
		Eigen::VectorXf ee_pos_target(EE_DIM_POS);
		// read into x_ref_t and u_ref_t from xu_traj2d
		for (int i = 0; i < NUM_STATES; ++i)
		{
			x_ref_t(i) = xu_traj2d[t][i];
			if (i < NUM_CONTROLS) {
				u_ref_t(i) = xu_traj2d[t][i + 14];
			}
			if (i < EE_DIM_POS) {
				ee_pos_target(i) = eePos_traj2d[t][i];
			}
		}


		boost::shared_ptr<crocoddyl::ActivationModelWeightedQuadTpl<float>> activation_ee = 
			boost::make_shared<crocoddyl::ActivationModelWeightedQuadTpl<float>>(EE_penalty_vec);

		boost::shared_ptr<crocoddyl::ResidualModelFrameTranslationTpl<float>> ee_residual =
			boost::make_shared<crocoddyl::ResidualModelFrameTranslationTpl<float>>(state, model.getFrameId("iiwa_joint_7"), ee_pos_target);
		printf("initialized state_residual\n");
		boost::shared_ptr<crocoddyl::CostModelResidualTpl<float>> goal_tracking_xyz_cost =
			boost::make_shared<crocoddyl::CostModelResidualTpl<float>>(state, activation_ee, ee_residual);
		printf("initialized state_cost\n");

		running_cost_model->addCost("goalTrack" + std::to_string(t), goal_tracking_xyz_cost, 1.);

		if (t < (KNOT_POINTS - 1)) {
			// Add control cost if this is not the final knot point
			boost::shared_ptr<crocoddyl::ResidualModelControlTpl<float>> control_residual =
				boost::make_shared<crocoddyl::ResidualModelControlTpl<float>>(state, u_ref_t);
			boost::shared_ptr<crocoddyl::CostModelResidualTpl<float>> control_cost =
				boost::make_shared<crocoddyl::CostModelResidualTpl<float>>(state, control_residual);

			running_cost_model->addCost("controlTrack" + std::to_string(t), control_cost, 0.001);

			printf("Added control cost for knot point %d\n", t);
		}
		else {
			// Add terminal cost if this is final knot point
			// printf("Added state cost for knot point %d\n", t);
			boost::shared_ptr<crocoddyl::ResidualModelStateTpl<float>> terminal_state_residual =
				boost::make_shared<crocoddyl::ResidualModelStateTpl<float>>(state, x_ref_t);
			boost::shared_ptr<crocoddyl::CostModelResidualTpl<float>> terminal_state_cost =
				boost::make_shared<crocoddyl::CostModelResidualTpl<float>>(state, activation_model_state, terminal_state_residual);
			
			terminal_cost_model->addCost("stateTrack" + std::to_string(t), terminal_state_cost, 10000.);

			printf("Added terminal cost for knot point %d\n", t);
			// print out the x_ref_t at this point so we know what the goal is
			std::cout << "x_ref_t: " << x_ref_t << std::endl;
		}
	}

	// Create the action models
	boost::shared_ptr<crocoddyl::IntegratedActionModelEulerTpl<float>> running_model =
		boost::make_shared<crocoddyl::IntegratedActionModelEulerTpl<float>>(
			boost::make_shared<crocoddyl::DifferentialActionModelFreeFwdDynamicsTpl<float>>(state, actuation, running_cost_model),
			DT);
	boost::shared_ptr<crocoddyl::IntegratedActionModelEulerTpl<float>> terminal_model =
		boost::make_shared<crocoddyl::IntegratedActionModelEulerTpl<float>>(
			boost::make_shared<crocoddyl::DifferentialActionModelFreeFwdDynamicsTpl<float>>(state, actuation, terminal_cost_model),
			DT);

	printf("initialized the action models\n");

	// // Create the problem
	std::vector<boost::shared_ptr<crocoddyl::ActionModelAbstractTpl<float>>> running_models(
		KNOT_POINTS, running_model);
	crocoddyl::ShootingProblemTpl<float> problem(x0, running_models, terminal_model);
	boost::shared_ptr<crocoddyl::ShootingProblemTpl<float>> problem_ptr = boost::make_shared<crocoddyl::ShootingProblemTpl<float>>(x0, running_models, terminal_model);


	// // Create the DDP solver
	crocoddyl::SolverDDP ddp(problem_ptr);

	// // Set up callback
	std::vector<boost::shared_ptr<crocoddyl::CallbackAbstract>> cbs;
	cbs.push_back(boost::make_shared<crocoddyl::CallbackVerbose>());
	ddp.setCallbacks(cbs);

	// // Solve the problem
	ddp.solve();

	//print the initial state x0 for reference
	std::cout << "Initial state: " << x0 << std::endl;

	Eigen::VectorXf final_state = ddp.get_xs().back();
    
    // Print it
    std::cout << "Final state: " << final_state << std::endl;

	return 0;
}

It all works correctly up until the creation of the solver itself. I pass in the pointer to the shooting problem, but encounter a type mismatch error, when I include this line:

	crocoddyl::SolverDDP ddp(problem_ptr);

The error trace is:

test_croc.cpp: In function 'int main(int, char**)':
test_croc.cpp:229:38: error: no matching function for call to 'crocoddyl::SolverDDP::SolverDDP(boost::shared_ptr<crocoddyl::ShootingProblemTpl<float> >&)'
  229 |  crocoddyl::SolverDDP ddp(problem_ptr);
      |                                      ^
In file included from test_croc.cpp:12:
/usr/local/lib/pkgconfig/../../include/crocoddyl/core/solvers/ddp.hpp:67:12: note: candidate: 'crocoddyl::SolverDDP::SolverDDP(boost::shared_ptr<crocoddyl::ShootingProblemTpl<double> >)'
   67 |   explicit SolverDDP(boost::shared_ptr<ShootingProblem> problem);
      |            ^~~~~~~~~
/usr/local/lib/pkgconfig/../../include/crocoddyl/core/solvers/ddp.hpp:67:57: note:   no known conversion for argument 1 from 'shared_ptr<ShootingProblemTpl<float>>' to 'shared_ptr<ShootingProblemTpl<double>>'
   67 |   explicit SolverDDP(boost::shared_ptr<ShootingProblem> problem);
      |                      ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~
/usr/local/lib/pkgconfig/../../include/crocoddyl/core/solvers/ddp.hpp:56:7: note: candidate: 'crocoddyl::SolverDDP::SolverDDP(const crocoddyl::SolverDDP&)'
   56 | class SolverDDP : public SolverAbstract {
      |       ^~~~~~~~~
/usr/local/lib/pkgconfig/../../include/crocoddyl/core/solvers/ddp.hpp:56:7: note:   no known conversion for argument 1 from 'boost::shared_ptr<crocoddyl::ShootingProblemTpl<float> >' to 'const crocoddyl::SolverDDP&'
In file included from /usr/include/eigen3/Eigen/Core:164,
                 from /usr/local/lib/pkgconfig/../../include/pinocchio/utils/cast.hpp:8,
                 from /usr/local/lib/pkgconfig/../../include/pinocchio/fwd.hpp:23,
                 from test_croc.cpp:1:
/usr/include/eigen3/Eigen/src/Core/AssignEvaluator.h: In instantiation of 'void Eigen::internal::call_assignment_no_alias(Dst&, const Src&, const Func&) [with Dst = Eigen::Matrix<float, -1, 1>; Src = Eigen::Matrix<double, -1, 1>; Func = Eigen::internal::assign_op<float, double>]':
/usr/include/eigen3/Eigen/src/Core/PlainObjectBase.h:797:41:   required from 'Derived& Eigen::PlainObjectBase<Derived>::_set_noalias(const Eigen::DenseBase<OtherDerived>&) [with OtherDerived = Eigen::Matrix<double, -1, 1>; Derived = Eigen::Matrix<float, -1, 1>]'
/usr/include/eigen3/Eigen/src/Core/PlainObjectBase.h:594:19:   required from 'Eigen::PlainObjectBase<Derived>::PlainObjectBase(const Eigen::DenseBase<OtherDerived>&) [with OtherDerived = Eigen::Matrix<double, -1, 1>; Derived = Eigen::Matrix<float, -1, 1>]'
/usr/include/eigen3/Eigen/src/Core/Matrix.h:423:29:   required from 'Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::Matrix(const Eigen::EigenBase<OtherDerived>&) [with OtherDerived = Eigen::Matrix<double, -1, 1>; _Scalar = float; int _Rows = -1; int _Cols = 1; int _Options = 0; int _MaxRows = -1; int _MaxCols = 1]'
test_croc.cpp:242:50:   required from here
/usr/include/eigen3/Eigen/src/Core/util/XprHelper.h:851:96: error: static assertion failed: YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY
  851 |   EIGEN_STATIC_ASSERT((Eigen::internal::has_ReturnType<ScalarBinaryOpTraits<LHS, RHS,BINOP> >::value), \
      |                       ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~
/usr/include/eigen3/Eigen/src/Core/util/StaticAssert.h:33:54: note: in definition of macro 'EIGEN_STATIC_ASSERT'
   33 |     #define EIGEN_STATIC_ASSERT(X,MSG) static_assert(X,#MSG);

Unlike with the other objects, I don't see a templated version of the solver classes in the docs, and indeed if I try to use a template parameter and declare this as crocoddyl::SolverDDPTpl<float> or crocoddyl::SolverDDP<float>, the compiler tells me test_croc.cpp:229:13: error: 'SolverDDPTpl' is not a member of 'crocoddyl'; did you mean 'SolverDDP'? or test_croc.cpp: In function 'int main(int, char**)': test_croc.cpp:229:2: error: 'crocoddyl::SolverDDP' is not a template 229 | crocoddyl::SolverDDP<float> ddp(problem_ptr); , respectively. Could you please explain what I am doing wrong? How can I instantiate the solver when the shooting problem datatype is float?

@cmastalli
Copy link
Member

@will-gerard -- As you said, our solvers are not templatized. To support codegen, we templatized the action models only. However, I could enable this feature ASAP. Is this critical for you now?

@will-gerard
Copy link
Author

I see, thanks for the explanation @cmastalli. We don't need this critically in order to keep going, we just thought it may increase performance. I'm trying to benchmark the solver running in an MPC loop and wanted to try to do what I can to make it as fast as possible. If you do make that update I will try with it, but if not I will go back to using doubles, not an issue.

@will-gerard
Copy link
Author

Thanks for all your help with this @cmastalli. More generally, I am using Crocoddyl with the IIWA manipulator, and so to get a sense of the performance I should be expecting, I have been referencing this paper. It is reported that on a 30 knot point problem the authors are able to achieve a 1kHz control frequency, which I haven't quite been able to replicate. I thought using floats might increase the performance somewhat, which motivated my original question on this thread.

When I warm start the solver with a good solution, and time with a monotonic timer around the call to ddp.solve:

    clock_gettime(CLOCK_MONOTONIC, &ddp_solve_start);
	ddp.solve(x_init, u_init, 100, false);
    clock_gettime(CLOCK_MONOTONIC, &ddp_solve_end);

it still seems to take close to 2 ms (~1.8), even if it only takes 1 ddp iteration. I am running on a 24 core processor at 3.4GHz, so a little slower than the one they are using in that paper, with multithreading enabled. I have not been able to get vectorization to work correctly yet, which I just asked about in 1166. But I'm curious if this is surprising to you, or if this is within the range of performance you would expect? Are there things I should be sure I'm doing in order to get the best performance I can? Thank you in advance for you time. If you would prefer to close this out and open a new issue for this, I am happy to, please let me know!

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
Development

No branches or pull requests

2 participants