diff --git a/CMakeLists.txt b/CMakeLists.txt
index 827c626c0c21b834affe1c8659adaa331daaef68..e7ac94b0471a2b46b7819b4b918077efff412563 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -55,7 +55,7 @@ SET(${PROJECT_NAME}_HEADERS
tools/timer.hpp
algorithm/rnea.hpp
algorithm/crba.hpp
- algorithm/crba2.hpp
+ algorithm/jacobian.hpp
algorithm/cholesky.hpp
algorithm/kinematics.hpp
)
@@ -103,6 +103,9 @@ ADD_EXECUTABLE(crba EXCLUDE_FROM_ALL unittest/crba.cpp)
PKG_CONFIG_USE_DEPENDENCY(crba eigenpy)
PKG_CONFIG_USE_DEPENDENCY(crba urdfdom)
+ADD_EXECUTABLE(jacobian EXCLUDE_FROM_ALL unittest/jacobian.cpp)
+PKG_CONFIG_USE_DEPENDENCY(jacobian eigenpy)
+
ADD_EXECUTABLE(cholesky EXCLUDE_FROM_ALL unittest/cholesky.cpp)
PKG_CONFIG_USE_DEPENDENCY(cholesky eigenpy)
PKG_CONFIG_USE_DEPENDENCY(cholesky urdfdom)
diff --git a/src/algorithm/jacobian.hpp b/src/algorithm/jacobian.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..a3883002250fa39460a0d2b31c877bb49e049042
--- /dev/null
+++ b/src/algorithm/jacobian.hpp
@@ -0,0 +1,90 @@
+#ifndef __se3_jacobian_hpp__
+#define __se3_jacobian_hpp__
+
+#include "pinocchio/multibody/visitor.hpp"
+#include "pinocchio/multibody/model.hpp"
+#include <iostream>
+ #include <boost/utility/binary.hpp>
+
+namespace se3
+{
+ // inline const Eigen::MatrixXd&
+ // jacobian(const Model & model,
+ // Data& data,
+ // const Eigen::VectorXd & q);
+
+} // namespace se3
+
+/* --- Details -------------------------------------------------------------------- */
+namespace se3
+{
+ struct JacobianForwardStep : public fusion::JointVisitor<JacobianForwardStep>
+ {
+ typedef boost::fusion::vector< const se3::Model&,
+ se3::Data&,
+ const Eigen::VectorXd &
+ > ArgsType;
+
+ JOINT_VISITOR_INIT(JacobianForwardStep);
+
+ template<typename JointModel>
+ static void algo(const se3::JointModelBase<JointModel> & jmodel,
+ se3::JointDataBase<typename JointModel::JointData> & jdata,
+ const se3::Model& model,
+ se3::Data& data,
+ const Eigen::VectorXd & q)
+ {
+ using namespace Eigen;
+ using namespace se3;
+
+ const Model::Index & i = jmodel.id();
+ const Model::Index & parent = model.parents[i];
+
+ jmodel.calc(jdata.derived(),q);
+
+ data.liMi[i] = model.jointPlacements[i]*jdata.M();
+ if(parent>0) data.oMi[i] = data.oMi[parent]*data.liMi[i];
+ else data.oMi[i] = data.liMi[i];
+
+ // std::cout << data.oMi[i] << std::endl << std::endl;
+ // std::cout << data.oMi[i].toActionMatrix() << std::endl << std::endl;
+ // std::cout << data.oMi[i].act(jdata.S()) << std::endl << std::endl;
+ data.J.block(0,jmodel.idx_v(),6,jmodel.nv()) = data.oMi[i].act(jdata.S());
+ }
+
+ };
+
+
+ inline const Eigen::MatrixXd&
+ computeJacobian(const Model & model, Data& data,
+ const Eigen::VectorXd & q)
+ {
+ for( int i=1;i<model.nbody;++i )
+ {
+ JacobianForwardStep::run(model.joints[i],data.joints[i],
+ JacobianForwardStep::ArgsType(model,data,q));
+ }
+
+ return data.J;
+ }
+
+ template<bool localFrame>
+ void getJacobian(const Model & model, const Data& data,
+ Model::Index jointId, Eigen::MatrixXd & J)
+ {
+ assert( J.rows() == data.J.rows() );
+ assert( J.cols() == data.J.cols() );
+
+ const SE3 & oMjoint = data.oMi[jointId];
+ int colRef = nv(model.joints[jointId])+idx_v(model.joints[jointId])-1;
+ for(int j=colRef;j>=0;j=data.parents_fromRow[j])
+ {
+ if(! localFrame ) J.col(j) = data.J.col(j);
+ // else J.col(j) = oMi.actInv(Motion(data.J.col(j)));
+ }
+ }
+
+} // namespace se3
+
+#endif // ifndef __se3_jacobian_hpp__
+
diff --git a/src/multibody/joint/joint-base.hpp b/src/multibody/joint/joint-base.hpp
index 9293e218bf92af00f2e8fe0631c0a22f632fb0ec..fd9b2b104ef5834fa856e02e56f54eeae6621704 100644
--- a/src/multibody/joint/joint-base.hpp
+++ b/src/multibody/joint/joint-base.hpp
@@ -40,6 +40,12 @@ namespace se3
* JointInertia::block = J::Constraint::Transpose*F
*/
+ /* Jacobian operations
+ *
+ * internal::ActionReturn<Constraint>::Type
+ * Constraint::se3Action
+ */
+
#define SE3_JOINT_TYPEDEF \
typedef int Index; \
typedef typename traits<Joint>::JointData JointData; \
diff --git a/src/multibody/joint/joint-free-flyer.hpp b/src/multibody/joint/joint-free-flyer.hpp
index 977cdd93f1b877b59252c88a71462f8e617700e1..a6bcb95e0e241e648c4c9c4acb09db5de73781d4 100644
--- a/src/multibody/joint/joint-free-flyer.hpp
+++ b/src/multibody/joint/joint-free-flyer.hpp
@@ -21,6 +21,8 @@ namespace se3
struct ConstraintIdentity
{
+ SE3::Matrix6 se3Action(const SE3 & m) const { return m.toActionMatrix(); }
+
struct TransposeConst
{
Force::Vector6 operator* (const Force & phi)
@@ -38,7 +40,6 @@ namespace se3
};
-
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
Inertia::Matrix6 operator*( const Inertia& Y,const JointFreeFlyer::ConstraintIdentity & )
{
@@ -53,6 +54,12 @@ namespace se3
return F;
}
+ namespace internal
+ {
+ template<>
+ struct ActionReturn<JointFreeFlyer::ConstraintIdentity >
+ { typedef SE3::Matrix6 Type; };
+ }
template<>
diff --git a/src/multibody/joint/joint-revolute.hpp b/src/multibody/joint/joint-revolute.hpp
index 2e898d6995bda29b034fc4849ffd06a5ae55b2a3..666a7d3a3265f7c3aaf4444783c914690e8df61a 100644
--- a/src/multibody/joint/joint-revolute.hpp
+++ b/src/multibody/joint/joint-revolute.hpp
@@ -54,7 +54,9 @@ namespace se3
}
}; // struct MotionRevolute
- friend const MotionRevolute& operator+ (const MotionRevolute& m, const BiasZero&) { return m; }
+ friend const MotionRevolute& operator+ (const MotionRevolute& m, const BiasZero&)
+ { return m; }
+
friend Motion operator+( const MotionRevolute& m1, const Motion& m2)
{
return Motion( m2.linear(),m2.angular()+typename revolute::CartesianVector3<axis>(m1.w));
@@ -62,7 +64,16 @@ namespace se3
struct ConstraintRevolute
{
template<typename D>
- MotionRevolute operator*( const Eigen::MatrixBase<D> & v ) const { return MotionRevolute(v[0]); }
+ MotionRevolute operator*( const Eigen::MatrixBase<D> & v ) const
+ { return MotionRevolute(v[0]); }
+
+ Eigen::Matrix<double,6,1> se3Action(const SE3 & m) const
+ {
+ Eigen::Matrix<double,6,1> res;
+ res.head<3>() = m.translation().cross( m.rotation().col(0));
+ res.tail<3>() = m.rotation().col(0);
+ return res;
+ }
struct TransposeConst
{
@@ -85,6 +96,7 @@ namespace se3
};
TransposeConst transpose() const { return TransposeConst(*this); }
+
/* CRBA joint operators
* - ForceSet::Block = ForceSet
* - ForceSet operator* (Inertia Y,Constraint S)
@@ -230,6 +242,21 @@ namespace se3
return res;
}
+ namespace internal
+ {
+ // TODO: I am not able to write the next three lines as a template. Why?
+ template<>
+ struct ActionReturn<typename JointRevolute<0>::ConstraintRevolute >
+ { typedef Eigen::Matrix<double,6,1> Type; };
+ template<>
+ struct ActionReturn<typename JointRevolute<1>::ConstraintRevolute >
+ { typedef Eigen::Matrix<double,6,1> Type; };
+ template<>
+ struct ActionReturn<typename JointRevolute<2>::ConstraintRevolute >
+ { typedef Eigen::Matrix<double,6,1> Type; };
+ }
+
+
template<int axis>
struct traits< JointRevolute<axis> >
diff --git a/src/multibody/model.hpp b/src/multibody/model.hpp
index 34c64673133ac5fa21dc7d2e351d5a53f254b9cd..d6d3eb12e3730ae6f4a184dd2fb1ecca196e6c8b 100644
--- a/src/multibody/model.hpp
+++ b/src/multibody/model.hpp
@@ -89,6 +89,7 @@ namespace se3
std::vector<int> parents_fromRow; // First previous non-zero row in M (used in Cholesky)
std::vector<int> nvSubtree_fromRow; //
+ Eigen::MatrixXd J; // Jacobian of joint placement
Data( const Model& ref );
@@ -193,6 +194,7 @@ namespace se3
,tmp(ref.nv)
,parents_fromRow(ref.nv)
,nvSubtree_fromRow(ref.nv)
+ ,J(6,ref.nv)
{
for(int i=0;i<model.nbody;++i)
joints.push_back(CreateJointData::run(model.joints[i]));
@@ -205,6 +207,9 @@ namespace se3
/* Init for Cholesky */
U = Eigen::MatrixXd::Identity(ref.nv,ref.nv);
computeParents_fromRow(ref);
+
+ /* Init Jacobian */
+ J.fill(0);
}
void Data::computeLastChild(const Model& model)
diff --git a/unittest/jacobian.cpp b/unittest/jacobian.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..18fe497e4ac98f1cb51b6d2894be9b609628fd56
--- /dev/null
+++ b/unittest/jacobian.cpp
@@ -0,0 +1,90 @@
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/algorithm/jacobian.hpp"
+#include "pinocchio/algorithm/rnea.hpp"
+#include <iostream>
+#include <boost/utility/binary.hpp>
+
+#include "pinocchio/tools/timer.hpp"
+#include "pinocchio/multibody/parser/sample-models.hpp"
+
+
+void timings(const se3::Model & model, se3::Data& data, long flag)
+{
+ using namespace se3;
+ StackTicToc timer(StackTicToc::US);
+#ifdef NDEBUG
+ const int NBT = 1000*1000;
+#else
+ const int NBT = 1;
+#endif
+
+ bool verbose = flag & (flag-1) ; // True is two or more binaries of the flag are 1.
+ if(verbose) std::cout <<"--" << std::endl;
+ Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
+
+ if( flag >> 0 & 1 )
+ {
+ timer.tic();
+ SMOOTH(NBT)
+ {
+ computeJacobian(model,data,q);
+ }
+ if(verbose) std::cout << "Compute =\t";
+ timer.toc(std::cout,NBT);
+ }
+ if( flag >> 1 & 1 )
+ {
+ computeJacobian(model,data,q);
+ Model::Index idx = model.existBodyName("rarm6")?model.getBodyId("rarm6"):model.nbody-1;
+ Eigen::MatrixXd Jrh(6,model.nv); Jrh.fill(0);
+
+ timer.tic();
+ SMOOTH(NBT)
+ {
+ getJacobian<false>(model,data,idx,Jrh);
+ }
+ if(verbose) std::cout << "Copy =\t";
+ timer.toc(std::cout,NBT);
+ }
+}
+
+void assertValues(const se3::Model & model, se3::Data& data)
+{
+ using namespace Eigen;
+ using namespace se3;
+
+ VectorXd q = VectorXd::Zero(model.nq);
+ computeJacobian(model,data,q);
+
+ Model::Index idx = model.existBodyName("rarm2")?model.getBodyId("rarm2"):model.nbody-1;
+ MatrixXd Jrh(6,model.nv); Jrh.fill(0);
+ getJacobian<false>(model,data,idx,Jrh);
+
+ VectorXd qdot = VectorXd::Random(model.nv);
+ VectorXd qddot = VectorXd::Zero(model.nv);
+ rnea( model,data,q,qdot,qddot );
+
+ Motion v = data.oMi[idx].act( data.v[idx] );
+ assert( v.toVector().isApprox( Jrh*qdot ));
+
+ std::cout << "Jrh = [ " << Jrh << " ];" << std::endl;
+ std::cout << "J = [ " << data.J << " ];" << std::endl;
+}
+
+int main()
+{
+ using namespace Eigen;
+ using namespace se3;
+
+ se3::Model model;
+ se3::buildModels::humanoidSimple(model);
+ se3::Data data(model);
+
+#ifndef NDEBUG
+ assertValues(model,data);
+#else
+ timings(model,data,BOOST_BINARY(11));
+#endif
+
+ return 0;
+}