diff --git a/bindings/python/multibody/joint/joint.hpp b/bindings/python/multibody/joint/joint.hpp
index 34957327b1a1cd3d97ca04331201580d6195246c..af5800b8e640c928ff5bb5754dcaed7942ece40a 100644
--- a/bindings/python/multibody/joint/joint.hpp
+++ b/bindings/python/multibody/joint/joint.hpp
@@ -22,6 +22,7 @@
#include <eigenpy/eigenpy.hpp>
#include "pinocchio/multibody/joint/joint.hpp"
+#include "pinocchio/bindings/python/utils/printable.hpp"
namespace se3
{
@@ -48,6 +49,7 @@ namespace se3
.add_property("nv",&JointModelPythonVisitor::getNv)
.def("setIndexes",&JointModel::setIndexes)
+ .def("shortname",&JointModel::shortname)
;
}
@@ -64,6 +66,7 @@ namespace se3
bp::no_init)
.def(bp::init<se3::JointModelVariant>())
.def(JointModelPythonVisitor())
+ .def(PrintableVisitor<JointModel>())
;
}
diff --git a/bindings/python/multibody/model.hpp b/bindings/python/multibody/model.hpp
index 8dbc21925f24b271295e0b65702bfae9da440ad3..7a55680488be8a9d6de068b0f6027f0aadae249b 100644
--- a/bindings/python/multibody/model.hpp
+++ b/bindings/python/multibody/model.hpp
@@ -27,6 +27,7 @@
#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/bindings/python/utils/eigen_container.hpp"
#include "pinocchio/bindings/python/utils/printable.hpp"
+#include "pinocchio/bindings/python/utils/copyable.hpp"
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(se3::Model)
@@ -214,7 +215,8 @@ namespace se3
"Articulated rigid body model (const)",
bp::no_init)
.def(ModelPythonVisitor())
- .def(PrintableVisitor<SE3>())
+ .def(PrintableVisitor<Model>())
+ .def(CopyableVisitor<Model>())
;
}
diff --git a/bindings/python/scripts/robot_wrapper.py b/bindings/python/scripts/robot_wrapper.py
index 0dc2271fba8d0ca75b91eb4f54566e5c025fb7ef..f4a52bd831ee1ddbe4a105bb90d5510a219f8336 100644
--- a/bindings/python/scripts/robot_wrapper.py
+++ b/bindings/python/scripts/robot_wrapper.py
@@ -58,12 +58,8 @@ class RobotWrapper(object):
self.q0 = utils.zero(self.nq)
def increment(self, q, dq):
- M = se3.SE3(se3.Quaternion(q[6, 0], q[3, 0], q[4, 0], q[5, 0]).matrix(), q[:3])
- dM = exp(dq[:6])
- M = M * dM
- q[:3] = M.translation
- q[3:7] = se3.Quaternion(M.rotation).coeffs()
- q[7:] += dq[6:]
+ q_next = se3.integrate(self.model,q,dq)
+ q[:] = q_next[:]
@property
def nq(self):
diff --git a/src/algorithm/frames.hpp b/src/algorithm/frames.hpp
index 825251898c5f8b31e964282e90a24ee741f83b6e..30201021d0fb95acb483519be0b1ff3ffd7851c3 100644
--- a/src/algorithm/frames.hpp
+++ b/src/algorithm/frames.hpp
@@ -84,7 +84,9 @@ namespace se3
Data & data
)
{
- for (Model::FrameIndex i=0; i < (Model::FrameIndex) model.nframes; ++i)
+ // The following for loop starts by index 1 because the first frame is fixed
+ // and corresponds to the universe.s
+ for (Model::FrameIndex i=1; i < (Model::FrameIndex) model.nframes; ++i)
{
const Frame & frame = model.frames[i];
const Model::JointIndex & parent = frame.parent;
diff --git a/src/algorithm/kinematics.hpp b/src/algorithm/kinematics.hpp
index 93c5d4442af019cd1dfb37436dcc6e1209a9aa95..0dee6bafc61415332c703a5168de83b84df74651 100644
--- a/src/algorithm/kinematics.hpp
+++ b/src/algorithm/kinematics.hpp
@@ -33,6 +33,18 @@ namespace se3
inline void emptyForwardPass(const Model & model,
Data & data);
+ ///
+ /// \brief Update the global placement of the joints oMi according to the relative
+ /// placements of the joints.
+ ///
+ /// \param[in] model The model structure of the rigid body system.
+ /// \param[in] data The data structure of the rigid body system.
+ ///
+ /// \remark This algorithm may be useful to call to update global joint placement
+ /// after calling se3::rnea, se3::aba, etc for example.
+ ///
+ inline void updateGlobalPlacements(const Model & model, Data & data);
+
///
/// \brief Update the joint placement according to the current joint configuration.
///
diff --git a/src/algorithm/kinematics.hxx b/src/algorithm/kinematics.hxx
index efb20a29fdb25e71fc3299af14b45aedab7742d1..2cd53faa95b3534ba49c43256ff9eaf0acb45663 100644
--- a/src/algorithm/kinematics.hxx
+++ b/src/algorithm/kinematics.hxx
@@ -53,6 +53,19 @@ namespace se3
}
}
+ inline void updateGlobalPlacements(const Model & model, Data & data)
+ {
+ for (Model::JointIndex i=1; i < (Model::JointIndex) model.njoints; ++i)
+ {
+ const Model::JointIndex & parent = model.parents[i];
+
+ if (parent>0)
+ data.oMi[i] = data.oMi[parent] * data.liMi[i];
+ else
+ data.oMi[i] = data.liMi[i];
+ }
+ }
+
struct ForwardKinematicZeroStep : public fusion::JointVisitor<ForwardKinematicZeroStep>
{
typedef boost::fusion::vector<const se3::Model &,
diff --git a/src/multibody/model.hxx b/src/multibody/model.hxx
index d425ff6332f118f1782361285b5bdf34734d9aa2..a3d730cd369362b7bf3121d44bc2f2f78ec79b01 100644
--- a/src/multibody/model.hxx
+++ b/src/multibody/model.hxx
@@ -291,6 +291,7 @@ namespace se3
f[0].setZero();
oMi[0].setIdentity();
liMi[0].setIdentity();
+ oMf[0].setIdentity();
}
inline void Data::computeLastChild (const Model & model)
diff --git a/src/spatial/fcl-pinocchio-conversions.hpp b/src/spatial/fcl-pinocchio-conversions.hpp
index 5276859b54ba967c9c7456c7301c7b9bc2bdd06f..907cd54c6f194ac988f2cbf41de481bac5189570 100644
--- a/src/spatial/fcl-pinocchio-conversions.hpp
+++ b/src/spatial/fcl-pinocchio-conversions.hpp
@@ -1,5 +1,5 @@
//
-// Copyright (c) 2015 CNRS
+// Copyright (c) 2015-2016 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
@@ -18,10 +18,8 @@
#ifndef __se3_fcl_convertion_hpp__
#define __se3_fcl_convertion_hpp__
-#include <Eigen/Geometry>
-
+#include <hpp/fcl/math/transform.h>
#include "pinocchio/spatial/se3.hpp"
-# include <hpp/fcl/math/transform.h>
namespace se3
{
@@ -45,27 +43,24 @@ namespace se3
inline fcl::Vec3f toFclVec3f(const Eigen::Vector3d & vec)
{
- return fcl::Vec3f( vec(0),vec(1), vec(2));
+ return fcl::Vec3f(vec(0),vec(1),vec(2));
}
inline Eigen::Vector3d toVector3d(const fcl::Vec3f & vec)
{
- Eigen::Vector3d res;
- res << vec[0],vec[1], vec[2];
- return res;
+ return Eigen::Vector3d(vec[0],vec[1],vec[2]);
}
- inline fcl::Transform3f toFclTransform3f(const se3::SE3 & m)
+ inline fcl::Transform3f toFclTransform3f(const SE3 & m)
{
return fcl::Transform3f(toFclMatrix3f(m.rotation()), toFclVec3f(m.translation()));
}
- inline se3::SE3 toPinocchioSE3(const fcl::Transform3f & tf)
+ inline SE3 toPinocchioSE3(const fcl::Transform3f & tf)
{
- return se3::SE3(toMatrix3d(tf.getRotation()), toVector3d(tf.getTranslation()));
+ return SE3(toMatrix3d(tf.getRotation()), toVector3d(tf.getTranslation()));
}
} // namespace se3
#endif // ifndef __se3_fcl_convertion_hpp__
-
diff --git a/src/spatial/se3.hpp b/src/spatial/se3.hpp
index a909915bee1f7fc9885466350c223edbb7d0ac36..10cf1a7ecd40ad75e44b2b6c6cb68de2e8a99b9d 100644
--- a/src/spatial/se3.hpp
+++ b/src/spatial/se3.hpp
@@ -277,8 +277,8 @@ namespace se3
return d.se3ActionInverse(*this);
}
- Vector3 act_impl (const Vector3& p) const { return rot*p+trans; }
- Vector3 actInv_impl(const Vector3& p) const { return rot.transpose()*(p-trans); }
+ Vector3 act_impl (const Vector3& p) const { return Vector3(rot*p+trans); }
+ Vector3 actInv_impl(const Vector3& p) const { return Vector3(rot.transpose()*(p-trans)); }
SE3Tpl act_impl (const SE3Tpl& m2) const { return SE3Tpl( rot*m2.rot,trans+rot*m2.trans);}
SE3Tpl actInv_impl (const SE3Tpl& m2) const { return SE3Tpl( rot.transpose()*m2.rot, rot.transpose()*(m2.trans-trans));}