From a632858dd7a79002ba128470cbe975336afa127c Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Wed, 20 Aug 2014 09:26:00 +0200
Subject: [PATCH] First test with variant?
---
CMakeLists.txt | 3 ++
src/multibody/joint.hpp | 96 +++++++++++++++++++++++++++++++++++++++--
unittest/rnea.cpp | 24 +----------
unittest/variant.cpp | 40 +++++++++++++++++
4 files changed, 137 insertions(+), 26 deletions(-)
create mode 100644 unittest/variant.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 367d52393..cdcaae9cb 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -71,6 +71,9 @@ PKG_CONFIG_USE_DEPENDENCY(tspatial eigenpy)
ADD_EXECUTABLE(rnea EXCLUDE_FROM_ALL unittest/rnea.cpp)
PKG_CONFIG_USE_DEPENDENCY(rnea eigenpy)
+ADD_EXECUTABLE(variant EXCLUDE_FROM_ALL unittest/variant.cpp)
+PKG_CONFIG_USE_DEPENDENCY(variant eigenpy)
+
SETUP_PROJECT_FINALIZE()
SETUP_PROJECT_CPACK()
diff --git a/src/multibody/joint.hpp b/src/multibody/joint.hpp
index 540befd1a..74f07ce99 100644
--- a/src/multibody/joint.hpp
+++ b/src/multibody/joint.hpp
@@ -37,7 +37,7 @@ namespace se3
{
typedef typename traits<Joint>::JointData JointData;
- JointData createData() const { return static_cast<Joint*>(this)->createData(); }
+ JointData createData() const { return static_cast<const Joint*>(this)->createData(); }
void calc( JointData& data,
const Eigen::VectorXd & qs,
const Eigen::VectorXd & vs,
@@ -51,11 +51,17 @@ namespace se3
};
+ struct JointDataRX;
+ struct JointModelRX;
+
+ struct JointDataFreeFlyer;
+ struct JointModelFreeFlyer;
+
+
+
/* --- REVOLUTE X --------------------------------------------------------- */
/* --- REVOLUTE X --------------------------------------------------------- */
/* --- REVOLUTE X --------------------------------------------------------- */
- struct JointDataRX;
- struct JointModelRX;
template<>
struct traits<JointDataRX>
@@ -93,6 +99,8 @@ namespace se3
JointDataRX() : M(1)
{
S << 0,0,0,1,0,0;
+ M.translation(SE3::Vector3::Zero());
+ v.linear(Motion::Vector3::Zero());
}
};
@@ -142,6 +150,88 @@ namespace se3
};
+
+ /* --- REVOLUTE FF -------------------------------------------------------- */
+ /* --- REVOLUTE FF -------------------------------------------------------- */
+ /* --- REVOLUTE FF -------------------------------------------------------- */
+
+ template<>
+ struct traits<JointDataFreeFlyer>
+ {
+ struct BiasZero {};
+ friend const Motion & operator+ ( const Motion& v, const BiasZero&) { return v; }
+
+ typedef Eigen::Matrix<double,6,6> Matrix6;
+ typedef Eigen::Quaternion<double> Quaternion;
+ typedef Eigen::Matrix<double,3,1> Vector3;
+
+ typedef Matrix6 Constraint_t;
+ typedef SE3 Transformation_t;
+ typedef Motion Velocity_t;
+ typedef BiasZero Bias_t;
+ typedef JointModelFreeFlyer JointModel;
+ typedef Eigen::Matrix<double,6,1> JointMotion_t;
+ };
+
+ struct JointDataFreeFlyer : public JointDataBase<JointDataFreeFlyer>
+ {
+ typedef typename traits<JointDataFreeFlyer>::Constraint_t Constraint_t;
+ typedef typename traits<JointDataFreeFlyer>::Transformation_t Transformation_t;
+ typedef typename traits<JointDataFreeFlyer>::Velocity_t Velocity_t;
+ typedef typename traits<JointDataFreeFlyer>::Bias_t Bias_t;
+ typedef typename traits<JointDataFreeFlyer>::JointMotion_t JointMotion_t;
+ typedef typename traits<JointDataFreeFlyer>::JointModel JointModel;
+
+ typedef Eigen::Matrix<double,6,6> Matrix6;
+ typedef Eigen::Quaternion<double> Quaternion;
+ typedef Eigen::Matrix<double,3,1> Vector3;
+
+ Constraint_t S;
+ Transformation_t M;
+ Velocity_t v;
+ Bias_t c;
+ JointMotion_t qdd;
+
+ Quaternion rotation;
+
+ JointDataFreeFlyer() : M(1)
+ {
+ S = Eigen::Matrix<double,6,6>::Identity();
+ }
+ };
+
+ template<>
+ struct traits<JointModelFreeFlyer>
+ {
+ typedef JointDataFreeFlyer JointData;
+ };
+
+ struct JointModelFreeFlyer : public JointModelBase<JointModelFreeFlyer>
+ {
+ typedef traits<JointModelFreeFlyer>::JointData JointData;
+
+ static const int nq = 7;
+ static const int nv = 6;
+ int idx_q,idx_v;
+
+ JointModelFreeFlyer() : idx_q(-1),idx_v(-1) {} // Default constructor for std::vector
+ JointModelFreeFlyer( int index_q,int index_v ) : idx_q(index_q),idx_v(index_v) {}
+
+ JointData createData() const { return JointData(); }
+ void calc( JointData& data,
+ const Eigen::VectorXd & qs,
+ const Eigen::VectorXd & vs,
+ const Eigen::VectorXd & as ) const
+ {
+ Eigen::VectorXd::ConstFixedSegmentReturnType<nq>::Type q = qs.segment<nq>(idx_q);
+ data.v = vs .segment<nv>(idx_v);
+ data.qdd = as.segment<nv>(idx_v);
+
+ JointData::Quaternion quat(Eigen::Matrix<double,4,1>(q.tail(4)));
+ data.M = SE3(quat.matrix(),q.head<3>());
+ }
+ };
+
} // namespace se3
#endif // ifndef __se3_joint_hpp__
diff --git a/unittest/rnea.cpp b/unittest/rnea.cpp
index 8546f4eeb..b6fa5546d 100644
--- a/unittest/rnea.cpp
+++ b/unittest/rnea.cpp
@@ -114,34 +114,12 @@ int main()
{
for( int i=1;i<model.nbody;++i )
{
- // JointModelRX & jmodel = model.joints[i];
- // JointDataRX & jdata = data.joints[i];
- // jmodel.calc(jdata,q,v,a);
-
- // const Model::Index & parent = model.parents[i];
- // const SE3 & liMi = data.liMi[i] = model.jointPlacements[i]*jdata.M;
-
- // if(parent>0) data.oMi[i] = data.oMi[parent]*liMi;
- // else data.oMi[i] = liMi;
-
- // data.v[i] = jdata.v;
- // if(parent>0) data.v[i] += liMi.actInv(data.v[parent]);
-
- // data.a[i] = Motion(jdata.S*jdata.qdd) + jdata.c + data.v[i].cross(jdata.v);
- // if(parent>0) data.a[i] += liMi.actInv(data.a[parent]);
-
- // data.f[i] = model.inertias[i]*data.a[i] + model.inertias[i].vxiv(data.v[i]); // -f_ext
rneaForwardStep(model,data,model.joints[i],data.joints[i],i,q,v,a);
}
for( int i=model.nbody-1;i>0;--i )
{
- const Model::Index & parent = model.parents[i];
- JointModelRX & jmodel = model.joints[i];
-
- data.tau.segment(jmodel.idx_v,jmodel.nv) = data.joints[i].S.transpose()*data.f[i].toVector();
- if(parent>0) data.f[parent] += data.liMi[i].act(data.f[i]);
- //rneaBackwardStep(model,data,model.joints[i],data.joints[i],i);
+ rneaBackwardStep(model,data,model.joints[i],data.joints[i],i);
}
}
timer.toc(std::cout,1000);
diff --git a/unittest/variant.cpp b/unittest/variant.cpp
new file mode 100644
index 000000000..cdc7e6009
--- /dev/null
+++ b/unittest/variant.cpp
@@ -0,0 +1,40 @@
+#include "pinocchio/spatial/fwd.hpp"
+#include "pinocchio/spatial/se3.hpp"
+#include "pinocchio/multibody/joint.hpp"
+#include "pinocchio/multibody/model.hpp"
+
+#include <iostream>
+
+#include "pinocchio/tools/timer.hpp"
+
+#include <boost/variant.hpp>
+
+
+using namespace se3;
+typedef boost::variant< JointModelRX,JointModelFreeFlyer> JointModel;
+typedef boost::variant< JointDataRX,JointDataFreeFlyer> JointData;
+
+
+class CreateJointData: public boost::static_visitor<JointData>
+{
+public:
+ template<typename D>
+ JointData operator()(const se3::JointModelBase<D> & jmodel) const
+ { return JointData(jmodel.createData()); }
+
+ static JointData run( const JointModel & jmodel)
+ { return boost::apply_visitor( CreateJointData(), jmodel ); }
+};
+
+
+int main()
+{
+ using namespace Eigen;
+
+
+ JointModel jmodel = JointModelRX(0,0);
+ const JointData & jdata = CreateJointData::run(jmodel); //x boost::apply_visitor( CreateJointData(), jmodel );
+
+
+ se3::Model model;
+}
--
GitLab