diff --git a/CMakeLists.txt b/CMakeLists.txt
index e037da21d811c70279d50830dd9ec5a9b813cb8c..e9eae80f9ab3301d9eafb44565e88e8d618cee94 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -160,6 +160,7 @@ SET(${PROJECT_NAME}_SPATIAL_HEADERS
)
SET(${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS
+ multibody/joint/fwd.hpp
multibody/joint/joint-base.hpp
multibody/joint/joint-dense.hpp
multibody/joint/joint-revolute.hpp
diff --git a/bindings/python/joint.hpp b/bindings/python/joint.hpp
index 094bb7f02102f11fa6268a549bd5a0c2e6b9e90d..9014d720fdbbb2e9609aa0db29bc280911f6ccf6 100644
--- a/bindings/python/joint.hpp
+++ b/bindings/python/joint.hpp
@@ -20,7 +20,7 @@
#include <eigenpy/exception.hpp>
#include <eigenpy/eigenpy.hpp>
-#include "pinocchio/multibody/joint/joint-composite.hpp"
+#include "pinocchio/multibody/joint/joint.hpp"
namespace se3
{
diff --git a/src/multibody/fwd.hpp b/src/multibody/fwd.hpp
index 29da7ab5e6aa2e8c5e64cc87d75ac58f4d37fef6..aacff9a09f5c6218e38b09e5860e08d3fd10fc6c 100644
--- a/src/multibody/fwd.hpp
+++ b/src/multibody/fwd.hpp
@@ -33,8 +33,7 @@ namespace se3
struct Data;
struct GeometryModel;
struct GeometryData;
- struct JointModel;
- struct JointData;
+
// Forward declaration needed for Model::check
template<class D> struct AlgorithmCheckerBase;
diff --git a/src/multibody/joint/fwd.hpp b/src/multibody/joint/fwd.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..a3debcafac0111bb0d68c96898d5b1f72d6bcd2b
--- /dev/null
+++ b/src/multibody/joint/fwd.hpp
@@ -0,0 +1,33 @@
+//
+// Copyright (c) 2016 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef __se3_joint_fwd_hpp__
+#define __se3_joint_fwd_hpp__
+
+namespace se3
+{
+ enum { MAX_JOINT_NV = 6 };
+
+ struct JointModelComposite;
+ struct JointDataComposite;
+
+ struct JointModel;
+ struct JointData;
+
+}
+
+#endif // ifndef __se3_joint_fwd_hpp__
diff --git a/src/multibody/joint/joint-composite.hpp b/src/multibody/joint/joint-composite.hpp
index 40228586ff5118b44efa822dc25a3f050f7b6c5b..4ce66486a99014cc6707be8d0fa0613c7127c59d 100644
--- a/src/multibody/joint/joint-composite.hpp
+++ b/src/multibody/joint/joint-composite.hpp
@@ -19,13 +19,11 @@
#define __se3_joint_composite_hpp__
#include "pinocchio/assert.hpp"
-#include "pinocchio/multibody/joint/joint.hpp"
+#include "pinocchio/multibody/joint/joint-variant.hpp"
+#include "pinocchio/multibody/joint/joint-basic-visitors.hpp"
#include <Eigen/StdVector>
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::SE3)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Motion)
-
namespace se3
{
@@ -67,6 +65,7 @@ namespace se3
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef JointComposite Joint;
+ typedef JointDataVariant JointData;
typedef std::vector<JointData, Eigen::aligned_allocator<JointData> > JointDataVector;
SE3_JOINT_TYPEDEF;
@@ -74,7 +73,7 @@ namespace se3
int nq_composite,nv_composite;
Constraint_t S;
- std::vector<Transformation_t> ljMj;
+ std::vector<Transformation_t, Eigen::aligned_allocator<Transformation_t> > ljMj;
Transformation_t M;
Motion_t v;
Bias_t c;
@@ -110,8 +109,13 @@ namespace se3
typedef JointComposite Joint;
SE3_JOINT_TYPEDEF;
SE3_JOINT_USE_INDEXES;
+
+ typedef JointModelVariant JointModel;
typedef std::vector<JointModel, Eigen::aligned_allocator<JointModel> > JointModelVector;
+ typedef JointDataVariant JointData;
+ typedef std::vector<JointData, Eigen::aligned_allocator<JointData> > JointDataVector;
+
using JointModelBase<JointModelComposite>::id;
using JointModelBase<JointModelComposite>::setIndexes;
@@ -475,15 +479,16 @@ namespace se3
};
- inline std::ostream & operator << (std::ostream & os, const JointModelComposite & jmodel)
+ inline std::ostream & operator << (std::ostream & os, const JointModelComposite & jmodel)
+ {
+ typedef JointModelComposite::JointModelVector JointModelVector;
+ os << "JointModelComposite containing following models:\n" ;
+ for (JointModelVector::const_iterator i = jmodel.joints.begin(); i != jmodel.joints.end(); ++i)
{
- os << "JointModelComposite containing following models:\n" ;
- for (JointModelVector::const_iterator i = jmodel.joints.begin(); i != jmodel.joints.end(); ++i)
- {
- os << shortname(*i) << std::endl;
- }
- return os;
+ os << shortname(*i) << std::endl;
}
+ return os;
+ }
} // namespace se3
diff --git a/src/multibody/joint/joint-variant.hpp b/src/multibody/joint/joint-variant.hpp
index b1e2b43c278cd683229d7d33b618e4bdd9a67f26..6ea4354de4b190db3e7700f7e6152646d83f174d 100644
--- a/src/multibody/joint/joint-variant.hpp
+++ b/src/multibody/joint/joint-variant.hpp
@@ -18,7 +18,7 @@
#ifndef __se3_joint_variant_hpp__
#define __se3_joint_variant_hpp__
-#include "pinocchio/multibody/joint/joint-base.hpp"
+#include "pinocchio/multibody/joint/fwd.hpp"
#include "pinocchio/multibody/joint/joint-dense.hpp"
#include "pinocchio/multibody/joint/joint-free-flyer.hpp"
#include "pinocchio/multibody/joint/joint-planar.hpp"
@@ -36,12 +36,7 @@
namespace se3
{
- enum { MAX_JOINT_NV = 6 };
-
- struct JointComposite;
- struct JointModelComposite;
- struct JointDataComposite;
-
+
// The JointModelComposite contains several JointModel (which are JointModelVariant). Hence there is a circular
// dependency between JointModelComposite and JointModelVariant that can be resolved with the use of boost::recursive_variant
// For more details, see http://www.boost.org/doc/libs/1_58_0/doc/html/variant/tutorial.html#variant.tutorial.recursive
diff --git a/src/multibody/joint/joint.hpp b/src/multibody/joint/joint.hpp
index 34b8f2074ece0f9719556c9bec3234c805e89a94..f5f829301131c516cfcd5905920d0328f60389e2 100644
--- a/src/multibody/joint/joint.hpp
+++ b/src/multibody/joint/joint.hpp
@@ -150,12 +150,9 @@ namespace se3
void setIndexes(JointIndex id,int nq,int nv) { ::se3::setIndexes(*this,id, nq, nv); }
};
- typedef std::vector<JointData> JointDataVector;
- typedef std::vector<JointModel> JointModelVector;
+ typedef std::vector<JointData, Eigen::aligned_allocator<JointData> > JointDataVector;
+ typedef std::vector<JointModel, Eigen::aligned_allocator<JointModel> > JointModelVector;
} // namespace se3
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::JointModel)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::JointData)
-
#endif // ifndef __se3_joint_model_hpp__
diff --git a/src/multibody/model.hpp b/src/multibody/model.hpp
index a11b2218cb91df1c205c8b7b4499d5b9213c7721..591d75d73b4ff8b8d5be1e357306396ceb24bb33 100644
--- a/src/multibody/model.hpp
+++ b/src/multibody/model.hpp
@@ -26,7 +26,7 @@
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/multibody/fwd.hpp"
#include "pinocchio/multibody/frame.hpp"
-#include "pinocchio/multibody/joint/joint-composite.hpp"
+#include "pinocchio/multibody/joint/joint.hpp"
#include "pinocchio/deprecated.hh"
#include "pinocchio/tools/string-generator.hpp"