Open will-gerard opened 1 year ago
Hi @will-gerard,
This seems to be a bug in Pinocchio. Could you try this https://github.com/stack-of-tasks/pinocchio/pull/2047? Please report to us if this PR fixes this issue.
Thanks
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!
Please share the entire code in test_croc.cpp
. I need more details.
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.
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>
?
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
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?
@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?
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.
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!
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:
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:
And so what I want to do now is something like:
But this results in the following errors:
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:
This compiles and runs successfully. However, when I try to change the template parameter to float, I get the following errors:
And also:
And also:
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.