Commit c447ba03 authored by jcarpent's avatar jcarpent
Browse files

[C++] Add algo to compute the kinetic energy

parent da3244da
......@@ -138,6 +138,7 @@ SET(${PROJECT_NAME}_ALGORITHM_HEADERS
algorithm/center-of-mass.hpp
algorithm/non-linear-effects.hpp
algorithm/joint-limits.hpp
algorithm/energy.hpp
)
SET(${PROJECT_NAME}_SIMULATION_HEADERS
......
//
// Copyright (c) 2016 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
#ifndef __se3_energy_hpp__
#define __se3_energy_hpp__
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
namespace se3 {
inline double
kineticEnergy(const Model & model,
Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const bool update_kinematics = true);
}
/* --- Details -------------------------------------------------------------------- */
namespace se3
{
inline double
kineticEnergy(const Model & model,
Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const bool update_kinematics)
{
data.kinetic_energy = 0.;
if (update_kinematics)
kinematics(model,data,q,v);
for(Model::Index i=1;i<(Model::Index)(model.nbody);++i)
data.kinetic_energy += model.inertias[i].vtiv(data.v[i]);
data.kinetic_energy *= .5;
return data.kinetic_energy;
}
}
#endif // __se3_energy_hpp__
//
// Copyright (c) 2015 CNRS
// Copyright (c) 2015-2016 CNRS
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
// This file is part of Pinocchio
......@@ -154,6 +154,8 @@ namespace se3
Eigen::VectorXd lowerPositionLimit; // limit for joint lower position
Eigen::VectorXd upperPositionLimit; // limit for joint upper position
double kinetic_energy; // kinetic energy of the model
Data( const Model& ref );
private:
......
......@@ -86,3 +86,4 @@ ENDIF(LUA5_1_FOUND)
#ADD_UNIT_TEST(variant eigen3)
ADD_UNIT_TEST(joints eigen3)
ADD_UNIT_TEST(compute-all-terms eigen3)
ADD_UNIT_TEST(energy eigen3)
//
// Copyright (c) 2016 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
#include "pinocchio/algorithm/energy.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/multibody/parser/sample-models.hpp"
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE ComTest
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
#include "pinocchio/tools/matrix-comparison.hpp"
#include <boost/test/floating_point_comparison.hpp>
BOOST_AUTO_TEST_SUITE(EnergyTest)
BOOST_AUTO_TEST_CASE(test_kinetic_energy)
{
using namespace Eigen;
using namespace se3;
se3::Model model;
se3::buildModels::humanoidSimple(model);
se3::Data data(model);
VectorXd q = VectorXd::Zero(model.nq);
VectorXd v = VectorXd::Ones(model.nv);
VectorXd a = VectorXd::Ones(model.nv);
data.M.fill(0); crba(model,data,q);
data.M.triangularView<Eigen::StrictlyLower>()
= data.M.transpose().triangularView<Eigen::StrictlyLower>();
double kinetic_energy_ref = 0.5*v.transpose() * data.M * v;
double kinetic_energy = kineticEnergy(model, data, q, v);
BOOST_CHECK_SMALL(kinetic_energy_ref - kinetic_energy, 1e-12);
}
BOOST_AUTO_TEST_SUITE_END()
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment