diff --git a/CMakeLists.txt b/CMakeLists.txt
index 2dce72f8bf35fb0b52947f53b98ece5f4e3b92d8..704c6ce8cbe9a3482d79a5497cc951d86657a105 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -76,6 +76,7 @@ SET(HEADERS
python/inertia.hpp
python/model.hpp
python/data.hpp
+ python/algorithms.hpp
)
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio")
diff --git a/python/test_model.py b/python/test_model.py
index 1f750db0170ce6c49f1cc4128a049f965e32efe3..493e5c91c80ece2ed25564d4fa3dc7bed9f0ac27 100644
--- a/python/test_model.py
+++ b/python/test_model.py
@@ -19,3 +19,14 @@ assert( isapprox(model.gravity.np,np.matrix('0; 0; -9.81; 0; 0; 0')) )
data = model.createData()
+
+q = zero(model.nq)
+qdot = zero(model.nv)
+qddot = zero(model.nv)
+for i in range(model.nbody): data.a[i] = se3.Motion.Zero()
+
+se3.rnea(model,data,q,qdot,qddot)
+for i in range(model.nbody):
+ assert( isapprox(data.v[i].np,zero(6)) )
+assert( isapprox(data.a[0].np,-model.gravity.np) )
+assert( isapprox(data.f[-1],model.inertias[-1]*data.a[-1]) )
diff --git a/src/multibody/model.hpp b/src/multibody/model.hpp
index 5196c4f8e1e04565c7810d1fcfa39e7d99d28e77..cc094f4981bf2a6583c5c95bdfc0a31ea2c19a0c 100644
--- a/src/multibody/model.hpp
+++ b/src/multibody/model.hpp
@@ -65,7 +65,10 @@ namespace se3
class Data
{
public:
+ typedef Eigen::Matrix<double,6,Eigen::Dynamic> Matrix6x;
+ typedef Eigen::Matrix<double,3,Eigen::Dynamic> Matrix3x;
+ public:
const Model& model;
JointDataVector joints;
std::vector<Motion> a; // Body acceleration
@@ -78,7 +81,6 @@ namespace se3
std::vector<Inertia> Ycrb; // Inertia of the sub-tree composit rigid body
Eigen::MatrixXd M; // Joint Inertia
- typedef Eigen::Matrix<double,6,Eigen::Dynamic> Matrix6x;
std::vector<Matrix6x> Fcrb; // Spatial forces set, used in CRBA
std::vector<Model::Index> lastChild; // Index of the last child (for CRBA)
diff --git a/src/python/data.hpp b/src/python/data.hpp
index a8e78a202ac480bec331b9857e0ddf4abb55a1e2..b287917e44fe44d541c7e818d904d1654a4df419 100644
--- a/src/python/data.hpp
+++ b/src/python/data.hpp
@@ -18,6 +18,8 @@ namespace se3
struct DataPythonVisitor
: public boost::python::def_visitor< DataPythonVisitor >
{
+ typedef Data::Matrix6x Matrix6x;
+ typedef Data::Matrix3x Matrix3x;
public:
@@ -27,31 +29,80 @@ namespace se3
return boost::python::incref(boost::python::object(DataHandler(ptr)).ptr());
}
+#define ADD_DATA_PROPERTY(TYPE,NAME,DOC) \
+ add_property(#NAME, \
+ bp::make_function(&DataPythonVisitor::NAME, \
+ bp::return_internal_reference<>()), \
+ DOC)
+ //add_property(#NAME,&DataPythonVisitor::NAME,DOC)
+ //bp::make_function(&ModelPythonVisitor::inertias,
+ // bp::return_internal_reference<>()) )
+#define IMPL_DATA_PROPERTY(TYPE,NAME,DOC) \
+ static TYPE & NAME( DataHandler & d ) { return d->NAME; }
+
+
/* --- Exposing C++ API to python through the handler ----------------- */
template<class PyClass>
void visit(PyClass& cl) const
{
cl
+ .ADD_DATA_PROPERTY(std::vector<Motion>,a,"Body acceleration")
+ .ADD_DATA_PROPERTY(std::vector<Motion>,v,"Body velocity")
+ .ADD_DATA_PROPERTY(std::vector<Force>,f,"Body force")
+ .ADD_DATA_PROPERTY(std::vector<SE3>,oMi,"Body absolute placement (wrt world)")
+ .ADD_DATA_PROPERTY(std::vector<SE3>,liMi,"Body relative placement (wrt parent)")
+ .ADD_DATA_PROPERTY(Eigen::VectorXd,tau,"Joint forces")
+ .ADD_DATA_PROPERTY(std::vector<Inertia>,Ycrb,"Inertia of the sub-tree composit rigid body")
+ .ADD_DATA_PROPERTY(Eigen::MatrixXd,M,"Joint Inertia")
+ .ADD_DATA_PROPERTY(std::vector<Matrix6x>,Fcrb,"Spatial forces set, used in CRBA")
+ .ADD_DATA_PROPERTY(std::vector<Model::Index>,lastChild,"Index of the last child (for CRBA)")
+ .ADD_DATA_PROPERTY(std::vector<int>,nvSubtree,"Dimension of the subtree motion space (for CRBA)")
+ .ADD_DATA_PROPERTY(Eigen::MatrixXd,U,"Joint Inertia square root (upper triangle)")
+ .ADD_DATA_PROPERTY(Eigen::VectorXd,D,"Diagonal of UDUT inertia decomposition")
+ .ADD_DATA_PROPERTY(std::vector<int>,parents_fromRow,"First previous non-zero row in M (used in Cholesky)")
+ .ADD_DATA_PROPERTY(std::vector<int>,nvSubtree_fromRow,"")
+ .ADD_DATA_PROPERTY(Eigen::MatrixXd,J,"Jacobian of joint placement")
+ .ADD_DATA_PROPERTY(std::vector<SE3>,iMf,"Body placement wrt to algorithm end effector.")
+ .ADD_DATA_PROPERTY(std::vector<Eigen::Vector3d>,com,"Subtree com position.")
+ .ADD_DATA_PROPERTY(std::vector<double>,mass,"Subtree total mass.")
+ .ADD_DATA_PROPERTY(Matrix3x,Jcom,"Jacobian of center of mass.")
;
}
-
- /* --- Expose --------------------------------------------------------- */
- static void expose()
+ IMPL_DATA_PROPERTY(std::vector<Motion>,a,"Body acceleration")
+ IMPL_DATA_PROPERTY(std::vector<Motion>,v,"Body velocity")
+ IMPL_DATA_PROPERTY(std::vector<Force>,f,"Body force")
+ IMPL_DATA_PROPERTY(std::vector<SE3>,oMi,"Body absolute placement (wrt world)")
+ IMPL_DATA_PROPERTY(std::vector<SE3>,liMi,"Body relative placement (wrt parent)")
+ IMPL_DATA_PROPERTY(Eigen::VectorXd,tau,"Joint forces")
+ IMPL_DATA_PROPERTY(std::vector<Inertia>,Ycrb,"Inertia of the sub-tree composit rigid body")
+ IMPL_DATA_PROPERTY(Eigen::MatrixXd,M,"Joint Inertia")
+ IMPL_DATA_PROPERTY(std::vector<Matrix6x>,Fcrb,"Spatial forces set, used in CRBA")
+ IMPL_DATA_PROPERTY(std::vector<Model::Index>,lastChild,"Index of the last child (for CRBA)")
+ IMPL_DATA_PROPERTY(std::vector<int>,nvSubtree,"Dimension of the subtree motion space (for CRBA)")
+ IMPL_DATA_PROPERTY(Eigen::MatrixXd,U,"Joint Inertia square root (upper triangle)")
+ IMPL_DATA_PROPERTY(Eigen::VectorXd,D,"Diagonal of UDUT inertia decomposition")
+ IMPL_DATA_PROPERTY(std::vector<int>,parents_fromRow,"First previous non-zero row in M (used in Cholesky)")
+ IMPL_DATA_PROPERTY(std::vector<int>,nvSubtree_fromRow,"")
+ IMPL_DATA_PROPERTY(Eigen::MatrixXd,J,"Jacobian of joint placement")
+ IMPL_DATA_PROPERTY(std::vector<SE3>,iMf,"Body placement wrt to algorithm end effector.")
+ IMPL_DATA_PROPERTY(std::vector<Eigen::Vector3d>,com,"Subtree com position.")
+ IMPL_DATA_PROPERTY(std::vector<double>,mass,"Subtree total mass.")
+ IMPL_DATA_PROPERTY(Matrix3x,Jcom,"Jacobian of center of mass.")
+
+ /* --- Expose --------------------------------------------------------- */
+ static void expose()
{
bp::class_<DataHandler>("Data",
- "Articulated rigid body data (const)",
- bp::no_init)
+ "Articulated rigid body data (const)",
+ bp::no_init)
.def(DataPythonVisitor());
bp::to_python_converter< DataHandler::SmartPtr_t,DataPythonVisitor >();
}
-
};
-
-
}} // namespace se3::python
#endif // ifndef __se3_python_data_hpp__
diff --git a/src/python/eigen_container.hpp b/src/python/eigen_container.hpp
index 296d2c3bbc67ba34647248cb3a0f3f8907fb0152..39948bdcd5d927b717decc46d460449993ee6016 100644
--- a/src/python/eigen_container.hpp
+++ b/src/python/eigen_container.hpp
@@ -35,12 +35,22 @@ namespace se3
}
static EigenObject getItem( const stdVectorAligned & Ys,int i)
- { return Ys[i]; }
+ {
+ assert( Ys.size()<INT_MAX );
+ if( i<0 ) i = int(Ys.size())+i;
+ assert( (i>=0) && (i<int(Ys.size())) );
+ return Ys[i];
+ }
static void setItem( stdVectorAligned & Ys,
int i,const EigenObject_fx & Y)
- { Ys[i] = Y; }
- static int length( const stdVectorAligned & Ys )
+ {
+ assert( Ys.size()<INT_MAX );
+ if( i<0 ) i = int(Ys.size())+i;
+ assert( (i>=0) && (i<int(Ys.size())) );
+ Ys[i] = Y;
+ }
+ static typename stdVectorAligned::size_type length( const stdVectorAligned & Ys )
{ return Ys.size(); }
static void expose(const std::string & className)
diff --git a/src/python/module.cpp b/src/python/module.cpp
index b621f2a6095664c083eb01ae6afd2b4870cf115e..690ba9c0f57c6b799913fd5df66887d6b89c9b48 100644
--- a/src/python/module.cpp
+++ b/src/python/module.cpp
@@ -24,5 +24,6 @@ BOOST_PYTHON_MODULE(libpinocchio_pywrap)
se3::python::exposeInertia();
se3::python::exposeModel();
+ se3::python::exposeAlgorithms();
}
diff --git a/src/python/python.cpp b/src/python/python.cpp
index 8545b61715d80864bc8111e46cbdfdc896620455..8554689a5717a53802026813a7eb2802856ca36f 100644
--- a/src/python/python.cpp
+++ b/src/python/python.cpp
@@ -6,6 +6,7 @@
#include "pinocchio/python/model.hpp"
#include "pinocchio/python/data.hpp"
+#include "pinocchio/python/algorithms.hpp"
namespace se3
{
@@ -36,5 +37,8 @@ namespace se3
ModelPythonVisitor::expose();
DataPythonVisitor::expose();
}
-
+ void exposeAlgorithms()
+ {
+ AlgorithmsPythonVisitor::expose();
+ }
}} // namespace se3::python
diff --git a/src/python/python.hpp b/src/python/python.hpp
index dc00aa8126d01e58d3e0b9052288b9d7e2d8a818..f79d68dce48f30a37e011aa892d5c4d7b8fd3aaf 100644
--- a/src/python/python.hpp
+++ b/src/python/python.hpp
@@ -11,6 +11,7 @@ namespace se3
void exposeInertia();
void exposeModel();
+ void exposeAlgorithms();
}} // namespace se3::python
diff --git a/src/python/utils.py b/src/python/utils.py
index 0b2152f0e8b437d5a9d054461165d6d1bfaa1bd3..8f41245a2b22fe94a66e034748308871b237946d 100644
--- a/src/python/utils.py
+++ b/src/python/utils.py
@@ -10,6 +10,8 @@ def skew(p):
return np.matrix([ [ 0,-z,y ], [ z,0,-x ], [ -y,x,0 ] ],np.double)
def isapprox(a,b,epsilon=1e-6):
+ if "np" in a.__class__.__dict__: a = a.np
+ if "np" in b.__class__.__dict__: b = b.np
if issubclass(a.__class__,np.ndarray) and issubclass(b.__class__,np.ndarray):
return np.allclose(a,b,epsilon)
else:
diff --git a/src/spatial/force.hpp b/src/spatial/force.hpp
index aa021e0cd44bf55df6da040678cb4be5bc94468d..c119f80180e917abbb7343a92d54ffbe57f99992 100644
--- a/src/spatial/force.hpp
+++ b/src/spatial/force.hpp
@@ -24,13 +24,17 @@ namespace se3
public:
// Constructors
ForceTpl() : m_n(), m_f() {}
- template<typename f_t,typename n_t>
- ForceTpl(const Eigen::MatrixBase<f_t> & f,const Eigen::MatrixBase<n_t> & n)
+ template<typename f3_t,typename n3_t>
+ ForceTpl(const Eigen::MatrixBase<f3_t> & f,const Eigen::MatrixBase<n3_t> & n)
: m_n(n), m_f(f) {}
- template<typename f_t>
- ForceTpl(const Eigen::MatrixBase<f_t> & v)
- : m_n(v.template segment<3>(ANGULAR)),
- m_f(v.template segment<3>(LINEAR)) {}
+ template<typename f6>
+ ForceTpl(const Eigen::MatrixBase<f6> & f)
+ : m_n(f.template segment<3>(ANGULAR)),
+ m_f(f.template segment<3>(LINEAR))
+ {
+ EIGEN_STATIC_ASSERT_VECTOR_ONLY(f6);
+ assert( f.size() == 6 );
+ }
template<typename S2,int O2>
ForceTpl(const ForceTpl<S2,O2> & clone) : m_n(clone.angular()), m_f(clone.linear()) {}
diff --git a/src/spatial/motion.hpp b/src/spatial/motion.hpp
index 3e2d02a5c114698d921821361bf35e2d58cdcc49..ac6d763fdfae4bdbdfc905ccf10783ebb0662fe4 100644
--- a/src/spatial/motion.hpp
+++ b/src/spatial/motion.hpp
@@ -31,7 +31,11 @@ namespace se3
template<typename v6>
MotionTpl(const Eigen::MatrixBase<v6> & v) :
m_w(v.template segment<3>(ANGULAR)),
- m_v(v.template segment<3>(LINEAR)) {}
+ m_v(v.template segment<3>(LINEAR))
+ {
+ EIGEN_STATIC_ASSERT_VECTOR_ONLY(v6);
+ assert( v.size() == 6 );
+ }
template<typename S2,int O2>
MotionTpl(const MotionTpl<S2,O2> & clone) : m_w(clone.angular()),m_v(clone.linear()) {}
diff --git a/src/spatial/se3.hpp b/src/spatial/se3.hpp
index 13915199d5b2a1f7bf53695f3491e6c86a0dd0e3..74184364b33d802ea52f9148591334979f73e8b2 100644
--- a/src/spatial/se3.hpp
+++ b/src/spatial/se3.hpp
@@ -8,6 +8,7 @@
namespace se3
{
+ /* Type returned by the "se3Action" and "se3ActionInverse" functions. */
namespace internal
{
template<typename D>
@@ -130,7 +131,8 @@ namespace se3
operator Matrix4() const { return toHomogeneousMatrix(); }
operator Matrix6() const { return toActionMatrix(); }
SE3Tpl operator*(const SE3Tpl & m2) const { return this->act(m2); }
- friend std::ostream & operator << (std::ostream & os,const SE3Tpl & X) { X.disp(os); return os; }
+ friend std::ostream & operator << (std::ostream & os,const SE3Tpl & X)
+ { X.disp(os); return os; }
public:
private: