diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5ef5a85e083f1ba5443b4363cc15bf61aa4034c3..5c7593842cc466b774bc696852964d7eac354111 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -60,6 +60,11 @@ SET(${PROJECT_NAME}_HEADERS
algorithm/cholesky.hpp
algorithm/kinematics.hpp
algorithm/center-of-mass.hpp
+ python/python.hpp
+ python/se3.hpp
+ python/force.hpp
+ python/motion.hpp
+ python/inertia.hpp
)
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio")
@@ -69,6 +74,7 @@ MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/multibody/joint"
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/multibody/parser")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/tools")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/algorithm")
+MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/python")
FOREACH(header ${${PROJECT_NAME}_HEADERS})
GET_FILENAME_COMPONENT(headerName ${header} NAME)
@@ -89,6 +95,16 @@ ENDFOREACH(header)
# ----------------------------------------------------
# --- TARGETS ----------------------------------------
# ----------------------------------------------------
+MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/lib/python")
+
+ADD_LIBRARY(pynocchio SHARED src/python/module.cpp src/python/python.cpp)
+PKG_CONFIG_USE_DEPENDENCY(pynocchio eigenpy)
+SET_TARGET_PROPERTIES(pynocchio PROPERTIES PREFIX "")
+TARGET_LINK_LIBRARIES(pynocchio ${Boost_LIBRARIES} eigenpy)
+
+# ----------------------------------------------------
+# --- UNITTESTS --------------------------------------
+# ----------------------------------------------------
ADD_EXECUTABLE(tspatial EXCLUDE_FROM_ALL unittest/tspatial.cpp)
PKG_CONFIG_USE_DEPENDENCY(tspatial eigenpy)
diff --git a/src/multibody/joint/joint-revolute.hpp b/src/multibody/joint/joint-revolute.hpp
index 666a7d3a3265f7c3aaf4444783c914690e8df61a..18c3fc7aa268a285a23678490cb611eb9f60e200 100644
--- a/src/multibody/joint/joint-revolute.hpp
+++ b/src/multibody/joint/joint-revolute.hpp
@@ -50,7 +50,8 @@ namespace se3
operator Motion() const
{
- return Motion(Motion::Vector3::Zero(),typename revolute::CartesianVector3<axis>(w));
+ return Motion(Motion::Vector3::Zero(),
+ (Motion::Vector3)typename revolute::CartesianVector3<axis>(w));
}
}; // struct MotionRevolute
diff --git a/src/spatial/force.hpp b/src/spatial/force.hpp
index c2f28763decacf677136e6a92cf4436a60ba5a03..aa021e0cd44bf55df6da040678cb4be5bc94468d 100644
--- a/src/spatial/force.hpp
+++ b/src/spatial/force.hpp
@@ -24,9 +24,15 @@ namespace se3
public:
// Constructors
ForceTpl() : m_n(), m_f() {}
- ForceTpl(const Vector3 & f,const Vector3 & n) : m_n(n), m_f(f) {}
- ForceTpl(const Vector6 & v) : m_n(v.template segment<3>(ANGULAR)),
- m_f(v.template segment<3>(LINEAR)) {}
+ template<typename f_t,typename n_t>
+ ForceTpl(const Eigen::MatrixBase<f_t> & f,const Eigen::MatrixBase<n_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 S2,int O2>
+ ForceTpl(const ForceTpl<S2,O2> & clone) : m_n(clone.angular()), m_f(clone.linear()) {}
// initializers
static ForceTpl Zero() { return ForceTpl(Vector3::Zero(), Vector3::Zero()); }
@@ -44,6 +50,8 @@ namespace se3
// Getters
const Vector3 & linear() const { return m_f; }
const Vector3 & angular() const { return m_n; }
+ void linear(const Vector3 & f) { m_f = f; }
+ void angular(const Vector3 & n) { m_n = n; }
// Arithmetic operators
ForceTpl & operator=(const Vector6 & phi)
@@ -112,7 +120,7 @@ namespace se3
Vector3 m_n;
Vector3 m_f;
};
-
+
typedef ForceTpl<double> Force;
} // namespace se3
diff --git a/src/spatial/inertia.hpp b/src/spatial/inertia.hpp
index 73b4851985903948920adc60854aed6e3a68946c..4b5976abfea7ddfc5f57d6899e9afbec4f1eafb0 100644
--- a/src/spatial/inertia.hpp
+++ b/src/spatial/inertia.hpp
@@ -49,19 +49,24 @@ namespace se3
m=clone.m; c=clone.c; I=clone.I;
return *this;
}
+ template<typename S2,int O2>
+ InertiaTpl( const InertiaTpl<S2,O2> & clone )
+ : m(clone.mass()),
+ c(clone.lever()),
+ I(clone.inertia().matrix()) {}
// Initializers
static Inertia Zero()
{
return InertiaTpl(0.,
Vector3::Zero(),
- Matrix3::Zero());
+ Symmetric3::Zero());
}
static Inertia Identity()
{
return InertiaTpl(1.,
Vector3::Zero(),
- Matrix3::Identity());
+ Symmetric3::Identity());
}
static Inertia Random()
{
diff --git a/src/spatial/motion.hpp b/src/spatial/motion.hpp
index cd6d8cf1a13c2597146f1e3f45b395c6d3a9ee5f..3e2d02a5c114698d921821361bf35e2d58cdcc49 100644
--- a/src/spatial/motion.hpp
+++ b/src/spatial/motion.hpp
@@ -25,10 +25,15 @@ namespace se3
public:
// Constructors
MotionTpl() : m_w(), m_v() {}
- MotionTpl(const Vector3 & v, const Vector3 & w) : m_w(w), m_v(v) {}
- MotionTpl(const Vector6 & v) :
+ template<typename v1,typename v2>
+ MotionTpl(const Eigen::MatrixBase<v1> & v, const Eigen::MatrixBase<v2> & w)
+ : m_w(w), m_v(v) {}
+ template<typename v6>
+ MotionTpl(const Eigen::MatrixBase<v6> & v) :
m_w(v.template segment<3>(ANGULAR)),
m_v(v.template segment<3>(LINEAR)) {}
+ template<typename S2,int O2>
+ MotionTpl(const MotionTpl<S2,O2> & clone) : m_w(clone.angular()),m_v(clone.linear()) {}
static MotionTpl Zero() { return MotionTpl(Vector3::Zero(), Vector3::Zero()); }
static MotionTpl Random() { return MotionTpl(Vector3::Random(),Vector3::Random()); }
diff --git a/src/spatial/se3.hpp b/src/spatial/se3.hpp
index 54d00a16d4662e5da4a47623341543c2ad12d4b6..13915199d5b2a1f7bf53695f3491e6c86a0dd0e3 100644
--- a/src/spatial/se3.hpp
+++ b/src/spatial/se3.hpp
@@ -49,8 +49,13 @@ namespace se3
public:
// Constructors
SE3Tpl() : rot(), trans() {}
- SE3Tpl(const Matrix3 & R, const Vector3 & p) : rot(R), trans(p) {}
+ template<typename M3,typename v3>
+ SE3Tpl(const Eigen::MatrixBase<M3> & R, const Eigen::MatrixBase<v3> & p)
+ : rot(R), trans(p) {}
SE3Tpl(int) : rot(Matrix3::Identity()), trans(Vector3::Zero()) {}
+ template<typename S2, int O2>
+ SE3Tpl( const SE3Tpl<S2,O2> clone )
+ : rot(clone.rotation()),trans(clone.translation()) {}
const Matrix3 & rotation() const { return rot; }
const Vector3 & translation() const { return trans; }
diff --git a/src/spatial/skew.hpp b/src/spatial/skew.hpp
index 623174843b095bcce10f455c7029e7502bfe4b6f..73d9c5db450022c83890e64a36035331499a7741 100644
--- a/src/spatial/skew.hpp
+++ b/src/spatial/skew.hpp
@@ -1,3 +1,5 @@
+#ifndef __se3_skew_hpp__
+#define __se3_skew_hpp__
namespace se3
{
template <typename D>
@@ -26,3 +28,4 @@ namespace se3
return res;
}
} // namespace se3
+#endif // ifndef __se3_skew_hpp__
diff --git a/src/spatial/symmetric3.hpp b/src/spatial/symmetric3.hpp
index c2ec1282545c3ead91826189d4bde84f40f72f4a..80a65d789d24659e169914e0e16efe3d1cad4d48 100644
--- a/src/spatial/symmetric3.hpp
+++ b/src/spatial/symmetric3.hpp
@@ -33,14 +33,23 @@ namespace se3
public:
Symmetric3Tpl(): data() {}
- Symmetric3Tpl(const Matrix3 &I)
+ template<typename Sc,int N,int Opt>
+ explicit Symmetric3Tpl(const Eigen::Matrix<Sc,N,N,Opt> & I)
+ {
+ assert( (I.rows()==3)&&(I.cols()==3) );
+ assert( (I-I.transpose()).isMuchSmallerThan(I) );
+ data(0) = I(0,0);
+ data(1) = I(1,0); data(2) = I(1,1);
+ data(3) = I(2,0); data(4) = I(2,1); data(5) = I(2,2);
+ }
+ explicit Symmetric3Tpl(const Eigen::MatrixBase<Matrix3> &I)
{
assert( (I-I.transpose()).isMuchSmallerThan(I) );
data(0) = I(0,0);
data(1) = I(1,0); data(2) = I(1,1);
data(3) = I(2,0); data(4) = I(2,1); data(5) = I(2,2);
}
- Symmetric3Tpl(const Vector6 &I) : data(I) {}
+ explicit Symmetric3Tpl(const Vector6 &I) : data(I) {}
Symmetric3Tpl(const double & a0,const double & a1,const double & a2,
const double & a3,const double & a4,const double & a5)
{ data << a0,a1,a2,a3,a4,a5; }