Skip to content
GitLab
Commit 684581fb authored by jcarpent's avatar jcarpent
Browse files

[Algo] Add check of data consistency wrt model

parent 4693e059
Hide whitespace changes
Inline Side-by-side
src/algorithm/aba.hxx
View file @ 684581fb
//
// Copyright (c) 2016 CNRS
// Copyright (c) 2016-2017 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -19,6 +19,7 @@
#define __se3_aba_hxx__
#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/algorithm/check.hpp"
/// @cond DEV
......@@ -193,6 +194,10 @@ namespace se3
const Eigen::VectorXd & v,
const Eigen::VectorXd & tau)
{
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model.");
#endif
data.v[0].setZero();
data.a[0] = -model.gravity;
data.u = tau;
......
src/algorithm/center-of-mass.hxx
View file @ 684581fb
//
// Copyright (c) 2015 CNRS
// Copyright (c) 2015-2017 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -18,6 +18,8 @@
#ifndef __se3_center_of_mass_hxx__
#define __se3_center_of_mass_hxx__
#include "pinocchio/algorithm/check.hpp"
/// @cond DEV
namespace se3
......@@ -29,6 +31,10 @@ namespace se3
const bool computeSubtreeComs,
const bool updateKinematics)
{
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model.");
#endif
data.mass[0] = 0;
data.com[0].setZero ();
......@@ -74,6 +80,9 @@ namespace se3
const bool computeSubtreeComs,
const bool updateKinematics)
{
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model.");
#endif
using namespace se3;
data.mass[0] = 0;
......@@ -131,6 +140,9 @@ namespace se3
const bool computeSubtreeComs,
const bool updateKinematics)
{
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model.");
#endif
using namespace se3;
data.mass[0] = 0;
......@@ -187,8 +199,11 @@ namespace se3
}
inline const SE3::Vector3 &
getComFromCrba(const Model &, Data & data)
getComFromCrba(const Model & model, Data & data)
{
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model.");
#endif
return data.com[0] = data.liMi[1].act(data.Ycrb[1].lever());
}
......@@ -269,6 +284,9 @@ namespace se3
const bool computeSubtreeComs,
const bool updateKinematics)
{
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model.");
#endif
data.com[0].setZero ();
data.mass[0] = 0;
......@@ -302,6 +320,9 @@ namespace se3
inline const Data::Matrix3x &
getJacobianComFromCrba(const Model & model, Data & data)
{
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model.");
#endif
const SE3 & oM1 = data.liMi[1];
// As the 6 first rows of M*a are a wrench, we just need to multiply by the
......
src/algorithm/cholesky.hxx
View file @ 684581fb
//
// Copyright (c) 2015-2016 CNRS
// Copyright (c) 2015-2017 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -18,6 +18,8 @@
#ifndef __se3_cholesky_hxx__
#define __se3_cholesky_hxx__
#include "pinocchio/algorithm/check.hpp"
/// @cond DEV
namespace se3
......@@ -43,6 +45,10 @@ namespace se3
* end
*/
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model.");
#endif
Eigen::MatrixXd & M = data.M;
Eigen::MatrixXd & U = data.U;
Eigen::VectorXd & D = data.D;
......@@ -72,6 +78,9 @@ namespace se3
Eigen::MatrixBase<Mat> & v)
{
assert(v.rows() == model.nv);
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model.");
#endif
const Eigen::MatrixXd & U = data.U;
const std::vector<int> & nvt = data.nvSubtree_fromRow;
......@@ -89,6 +98,9 @@ namespace se3
Eigen::MatrixBase<Mat> & v)
{
assert(v.rows() == model.nv);
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model.");
#endif
const Eigen::MatrixXd & U = data.U;
const std::vector<int> & nvt = data.nvSubtree_fromRow;
......@@ -109,6 +121,9 @@ namespace se3
/* We search y s.t. v = U y.
* For any k, v_k = y_k + U_{k,k+1:} y_{k+1:} */
assert(v.rows() == model.nv);
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model.");
#endif
const Eigen::MatrixXd & U = data.U;
const std::vector<int> & nvt = data.nvSubtree_fromRow;
......@@ -126,6 +141,9 @@ namespace se3
/* We search y s.t. v = U' y.
* For any k, v_k = y_k + sum_{m \in parent{k}} U(m,k) v(k). */
assert(v.rows() == model.nv);
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model.");
#endif
const Eigen::MatrixXd & U = data.U;
const std::vector<int> & nvt = data.nvSubtree_fromRow;
......@@ -143,7 +161,7 @@ namespace se3
const Eigen::MatrixBase<Mat> & v)
{
assert(v.rows() == model.nv);
const Eigen::MatrixXd & M = data.M;
const std::vector<int> & nvt = data.nvSubtree_fromRow;
Mat res(model.nv);
......@@ -174,6 +192,9 @@ namespace se3
Eigen::MatrixBase<Mat> & v,
const bool usingCholesky)
{
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model.");
#endif
if(usingCholesky) return internal::UDUtv(model,data,v);
else return v = internal::Mv(model,data,v);
}
......@@ -183,6 +204,9 @@ namespace se3
const Data & data ,
Eigen::MatrixBase<Mat> & v)
{
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model.");
#endif
Uiv(model,data,v);
for(int k=0;k<model.nv;++k) v.row(k) /= data.D[k];
return Utiv(model,data,v);
......
src/algorithm/compute-all-terms.hpp
View file @ 684581fb
//
// Copyright (c) 2015-2016 CNRS
// Copyright (c) 2015-2017 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -23,6 +23,7 @@
#include "pinocchio/spatial/act-on-set.hpp"
#include "pinocchio/algorithm/center-of-mass.hpp"
#include "pinocchio/algorithm/energy.hpp"
#include "pinocchio/algorithm/check.hpp"
namespace se3
{
......@@ -203,6 +204,9 @@ namespace se3
const Eigen::VectorXd & q,
const Eigen::VectorXd & v)
{
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model.");
#endif
data.v[0].setZero();
data.a[0].setZero();
data.a_gf[0] = -model.gravity;
......
src/algorithm/copy.hpp
View file @ 684581fb
......@@ -19,6 +19,7 @@
#define __se3_copy_hpp__
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/algorithm/check.hpp"
namespace se3
{
......
src/algorithm/crba.hxx
View file @ 684581fb
//
// Copyright (c) 2015-2016 CNRS
// Copyright (c) 2015-2017 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -21,6 +21,7 @@
#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/spatial/act-on-set.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/check.hpp"
/// @cond DEV
......@@ -102,6 +103,10 @@ namespace se3
crba(const Model & model, Data& data,
const Eigen::VectorXd & q)
{
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model.");
#endif
for( Model::JointIndex i=1;i<(Model::JointIndex)(model.njoints);++i )
{
CrbaForwardStep::run(model.joints[i],data.joints[i],
......@@ -203,6 +208,9 @@ namespace se3
const Eigen::VectorXd & q,
const Eigen::VectorXd & v)
{
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model.");
#endif
typedef Eigen::Block <Data::Matrix6x,3,-1> Block3x;
forwardKinematics(model, data, q);
......
src/algorithm/dynamics.hpp
View file @ 684581fb
//
// Copyright (c) 2016 CNRS
// Copyright (c) 2016-2017 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -22,8 +22,10 @@
#include "pinocchio/algorithm/compute-all-terms.hpp"
#include "pinocchio/algorithm/cholesky.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/check.hpp"
#include <Eigen/Cholesky>
namespace se3
{
......@@ -61,6 +63,9 @@ namespace se3
assert(tau.size() == model.nv);
assert(J.cols() == model.nv);
assert(J.rows() == gamma.size());
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model.");
#endif
Eigen::VectorXd & a = data.ddq;
Eigen::VectorXd & lambda_c = data.lambda_c;
......@@ -125,6 +130,9 @@ namespace se3
assert(q.size() == model.nq);
assert(v_before.size() == model.nv);
assert(J.cols() == model.nv);
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model.");
#endif
Eigen::VectorXd & impulse_c = data.impulse_c;
Eigen::VectorXd & dq_after = data.dq_after;
......
src/algorithm/energy.hpp
View file @ 684581fb
//
// Copyright (c) 2016 CNRS
// Copyright (c) 2016-2017 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -20,6 +20,7 @@
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/check.hpp"
namespace se3 {
......@@ -71,6 +72,10 @@ namespace se3
const Eigen::VectorXd & v,
const bool update_kinematics)
{
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model.");
#endif
data.kinetic_energy = 0.;
if (update_kinematics)
......@@ -89,6 +94,10 @@ namespace se3
const Eigen::VectorXd & q,
const bool update_kinematics)
{
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model.");
#endif
data.potential_energy = 0.;
const Motion::ConstLinear_t & g = model.gravity.linear();
SE3::Vector3 com_global;
......
src/algorithm/frames.hpp
View file @ 684581fb
//
// Copyright (c) 2015-2016 CNRS
// Copyright (c) 2015-2017 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -22,6 +22,7 @@
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/check.hpp"
namespace se3
{
......@@ -84,6 +85,10 @@ namespace se3
Data & data
)
{
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model.");
#endif
// The following for loop starts by index 1 because the first frame is fixed
// and corresponds to the universe.s
for (Model::FrameIndex i=1; i < (Model::FrameIndex) model.nframes; ++i)
......@@ -102,6 +107,10 @@ namespace se3
const Eigen::VectorXd & q
)
{
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model.");
#endif
forwardKinematics(model, data, q);
framesForwardKinematics(model, data);
}
......@@ -116,6 +125,9 @@ namespace se3
{
assert(J.cols() == model.nv);
assert(data.J.cols() == model.nv);
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model.");
#endif
const Model::JointIndex & parent = model.frames[frame_id].parent;
const SE3 & oMframe = data.oMf[frame_id];
......
src/algorithm/geometry.hxx
View file @ 684581fb
//
// Copyright (c) 2015-2016 CNRS
// Copyright (c) 2015-2017 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -26,22 +26,30 @@ namespace se3
/* --- GEOMETRY PLACEMENTS -------------------------------------------------------- */
/* --- GEOMETRY PLACEMENTS -------------------------------------------------------- */
inline void updateGeometryPlacements(const Model & model,
Data & data,
const GeometryModel & geomModel,
GeometryData & geomData,
const Eigen::VectorXd & q
)
Data & data,
const GeometryModel & geomModel,
GeometryData & geomData,
const Eigen::VectorXd & q
)
{
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model.");
#endif
forwardKinematics(model, data, q);
updateGeometryPlacements(model, data, geomModel, geomData);
}
inline void updateGeometryPlacements(const Model &,
const Data & data,
const GeometryModel & geomModel,
GeometryData & geomData
)
inline void updateGeometryPlacements(const Model & model,
const Data & data,
const GeometryModel & geomModel,
GeometryData & geomData
)
{
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model.");
#endif
for (GeomIndex i=0; i < (GeomIndex) geomModel.ngeoms; ++i)
{
const Model::JointIndex & joint = geomModel.geometryObjects[i].parentJoint;
......@@ -108,6 +116,10 @@ namespace se3
const bool stopAtFirstCollision
)
{
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model.");
#endif
updateGeometryPlacements (model, data, geomModel, geomData, q);
return computeCollisions(geomModel,geomData, stopAtFirstCollision);
......@@ -185,6 +197,9 @@ namespace se3
const Eigen::VectorXd & q
)
{
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model.");
#endif
updateGeometryPlacements (model, data, geomModel, geomData, q);
return computeDistances<ComputeShortest>(geomModel,geomData);
}
......
src/algorithm/jacobian.hxx
View file @ 684581fb
//
// Copyright (c) 2015 CNRS
// Copyright (c) 2015-2017 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -19,6 +19,7 @@
#define __se3_jacobian_hxx__
#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/algorithm/check.hpp"
/// @cond DEV
......@@ -59,6 +60,10 @@ namespace se3
computeJacobians(const Model & model, Data & data,
const Eigen::VectorXd & q)
{
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model.");
#endif
for( Model::JointIndex i=1; i< (Model::JointIndex) model.njoints;++i )
{
JacobiansForwardStep::run(model.joints[i],data.joints[i],
......@@ -79,6 +84,9 @@ namespace se3
{
assert( J.rows() == data.J.rows() );
assert( J.cols() == data.J.cols() );
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model.");
#endif
const SE3 & oMjoint = data.oMi[jointId];
int colRef = nv(model.joints[jointId])+idx_v(model.joints[jointId])-1;
......@@ -124,6 +132,10 @@ namespace se3
const Eigen::VectorXd & q,
const Model::JointIndex jointId)
{
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model.");
#endif
data.iMf[jointId].setIdentity();
for( Model::JointIndex i=jointId;i>0;i=model.parents[i] )
{
......
src/algorithm/kinematics.hxx
View file @ 684581fb
//
// Copyright (c) 2016 CNRS
// Copyright (c) 2016-2017 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -19,6 +19,7 @@
#define __se3_kinematics_hxx__
#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/algorithm/check.hpp"
namespace se3
{
......@@ -44,6 +45,10 @@ namespace se3
inline void emptyForwardPass(const Model & model,
Data & data)
{
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model.");
#endif
for (Model::JointIndex i=1; i < (Model::JointIndex) model.njoints; ++i)
{
emptyForwardStep::run(model.joints[i],
......@@ -55,6 +60,10 @@ namespace se3
inline void updateGlobalPlacements(const Model & model, Data & data)
{
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model.");
#endif
for (Model::JointIndex i=1; i < (Model::JointIndex) model.njoints; ++i)
{
const Model::JointIndex & parent = model.parents[i];
......@@ -103,6 +112,9 @@ namespace se3
const Eigen::VectorXd & q)
{
assert(q.size() == model.nq && "The configuration vector is not of right size");
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model.");
#endif
for (Model::JointIndex i=1; i < (Model::JointIndex) model.njoints; ++i)
{
......@@ -156,6 +168,9 @@ namespace se3
{
assert(q.size() == model.nq && "The configuration vector is not of right size");
assert(v.size() == model.nv && "The velocity vector is not of right size");
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model.");
#endif
data.v[0].setZero();
......@@ -216,6 +231,9 @@ namespace se3
assert(q.size() == model.nq && "The configuration vector is not of right size");
assert(v.size() == model.nv && "The velocity vector is not of right size");
assert(a.size() == model.nv && "The acceleration vector is not of right size");
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model.");
#endif
data.v[0].setZero();
data.a[0].setZero();
......
src/algorithm/rnea.hxx
View file @ 684581fb
//
// Copyright (c) 2015-2016 CNRS
// Copyright (c) 2015-2017 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -21,6 +21,7 @@
/// @cond DEV
#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/algorithm/check.hpp"
namespace se3
{
......@@ -90,6 +91,10 @@ namespace se3
const Eigen::VectorXd & v,
const Eigen::VectorXd & a)
{
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model.");
#endif
data.v[0].setZero();
data.a_gf[0] = -model.gravity;
......@@ -116,6 +121,9 @@ namespace se3
const container::aligned_vector<Force> & fext)
{
assert(fext.size() == model.joints.size());
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model.");
#endif
data.v[0].setZero();
data.a_gf[0] = -model.gravity;
......@@ -199,6 +207,10 @@ namespace se3
const Eigen::VectorXd & q,
const Eigen::VectorXd & v)
{
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model.");
#endif
data.v[0].setZero ();
data.a_gf[0] = -model.gravity;
......
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