diff --git a/bindings/python/model.hpp b/bindings/python/model.hpp
index d7a040821181bef3b0af82ed7943f68481c115d9..6e16e9686eca03af940726c8d487730b21d70cec 100644
--- a/bindings/python/model.hpp
+++ b/bindings/python/model.hpp
@@ -135,8 +135,11 @@ namespace se3
.def("getFrameParent", &ModelPythonVisitor::getFrameParent)
.def("getFramePlacement", &ModelPythonVisitor::getFramePlacement)
- .def("addFrame", &ModelPythonVisitor::addFrame)
- .add_property("frames", bp::make_function(&ModelPythonVisitor::operationalFrames, bp::return_internal_reference<>()) )
+ .def("addFrame",(bool (*)(ModelHandler&,const std::string &,const JointIndex, const SE3_fx &,const FrameType &)) &ModelPythonVisitor::addFrame,bp::args("name","parent_id","placement","type"),"Add a frame to the vector of frames. See also Frame for more details. Returns False if the frame already exists.")
+ .def("addFrame",(bool (*)(ModelHandler&,const Frame &)) &ModelPythonVisitor::addFrame,bp::args("frame"),"Add a frame to the vector of frames.")
+ .add_property("frames", bp::make_function(&ModelPythonVisitor::frames, bp::return_internal_reference<>()),"Vector of frames contained in the model.")
+ .def("existFrame",&ModelPythonVisitor::existFrame,bp::args("name"),"Returns true if the frame given by its name exists inside the Model.")
+ .def("getFrameId",&ModelPythonVisitor::getFrameId,bp::args("name"),"Returns the index of the frame given by its name. If the frame is not in the frames vector, it returns the current size of the frames vector.")
.add_property("gravity",&ModelPythonVisitor::gravity,&ModelPythonVisitor::setGravity)
.def("BuildEmptyModel",&ModelPythonVisitor::maker_empty)
@@ -190,12 +193,19 @@ namespace se3
static Eigen::VectorXd upperPositionLimit(ModelHandler & m) {return m->upperPositionLimit;}
static Model::JointIndex getFrameParent( ModelHandler & m, const std::string & name ) { return m->getFrameParent(name); }
- static SE3 getFramePlacement( ModelHandler & m, const std::string & name ) { return m->getFramePlacement(name); }
- static void addFrame( ModelHandler & m, const std::string & frameName, const JointIndex parent, const SE3_fx & placementWrtParent )
+ static SE3 getFramePlacement(ModelHandler & m, const std::string & name) { return m->getFramePlacement(name); }
+
+ static bool addFrame(ModelHandler & m, const Frame & frame) { return m->addFrame(frame); }
+ static bool addFrame( ModelHandler & m, const std::string & frameName, const JointIndex parent, const SE3_fx & placementWrtParent, const FrameType & type)
{
- m->addFrame(frameName, parent, placementWrtParent);
+ return m->addFrame(frameName,parent,placementWrtParent,type);
}
- static std::vector<Frame> & operationalFrames (ModelHandler & m ) { return m->frames;}
+ static Model::FrameIndex getFrameId(const ModelHandler & m, const std::string & frame_name)
+ { return m->getFrameId(frame_name); }
+ static bool existFrame(const ModelHandler & m, const std::string & frame_name)
+ { return m->existFrame(frame_name); }
+
+ static std::vector<Frame> & frames (ModelHandler & m ) { return m->frames;}
static Motion gravity( ModelHandler & m ) { return m->gravity; }
static void setGravity( ModelHandler & m,const Motion_fx & g ) { m->gravity = g; }
diff --git a/bindings/python/se3.hpp b/bindings/python/se3.hpp
index 58ca49ed83c6a7ff81aa34f21d3c5629f81ea5ee..f20c264da8d0e558af5770383c4bc4d903142533 100644
--- a/bindings/python/se3.hpp
+++ b/bindings/python/se3.hpp
@@ -44,6 +44,7 @@ namespace se3
: public boost::python::def_visitor< SE3PythonVisitor<SE3> >
{
typedef typename eigenpy::UnalignedEquivalent<SE3>::type SE3_fx;
+ typedef typename SE3::Scalar Scalar;
typedef typename SE3::Matrix3 Matrix3;
typedef typename SE3::Matrix6 Matrix6;
typedef typename SE3::Matrix4 Matrix4;
@@ -87,6 +88,12 @@ namespace se3
.def("actInv_point", &SE3PythonVisitor::actInv_point)
.def("act_se3", &SE3PythonVisitor::act_se3)
.def("actInv_se3", &SE3PythonVisitor::actInv_se3)
+
+ .def("isApprox",(bool (SE3_fx::*)(const SE3_fx & other, const Scalar & prec)) &SE3_fx::isApprox,bp::args("other","prec"),"Returns true if *this is approximately equal to other, within the precision given by prec.")
+ .def("isApprox",(bool (SE3_fx::*)(const SE3_fx & other)) &SE3_fx::isApprox,bp::args("other"),"Returns true if *this is approximately equal to other.")
+
+ .def("isIdentity",(bool (SE3_fx::*)(const Scalar & prec)) &SE3_fx::isIdentity,bp::args("prec"),"Returns true if *this is approximately equal to the identity placement, within the precision given by prec.")
+ .def("isIdentity",(bool (SE3_fx::*)(void)) &SE3_fx::isIdentity,"Returns true if *this is approximately equal to the identity placement.")
.def("__str__",&SE3PythonVisitor::toString)
.def("__invert__",&SE3_fx::inverse)
diff --git a/src/algorithm/frames.hpp b/src/algorithm/frames.hpp
index d53e736acf19dde50a85ea761af7808e7f1ebae7..30fbc7c1573126c17019ce3f99d92777360c4d59 100644
--- a/src/algorithm/frames.hpp
+++ b/src/algorithm/frames.hpp
@@ -27,10 +27,11 @@ namespace se3
{
/**
- * @brief Update the position of each extra frame
+ * @brief Updates the position of each frame contained in the model
+ *
+ * @param[in] model The kinematic model.
+ * @param data Data associated to model.
*
- * @param[in] model The kinematic model
- * @param data Data associated to model
* @warning One of the algorithms forwardKinematics should have been called first
*/
inline void framesForwardKinematics(const Model & model,
@@ -38,11 +39,12 @@ namespace se3
);
/**
- * @brief Compute Kinematics of full model, then the position of each operational frame
+ * @brief First calls the forwardKinematics on the model, then computes the placement of each frame.
+ * /sa se3::forwardKinematics
*
- * @param[in] model The kinematic model
- * @param data Data associated to model
- * @param[in] q Configuration vector
+ * @param[in] model The kinematic model.
+ * @param data Data associated to model.
+ * @param[in] q Configuration vector.
*/
inline void framesForwardKinematics(const Model & model,
Data & data,
@@ -50,19 +52,21 @@ namespace se3
);
/**
- * @brief Return the jacobian of the operational frame in the world frame or
- in the local frame depending on the template argument.
+ * @brief Returns the jacobian of the frame expresssed in the world frame or
+ in the local frame depending on the template argument.
+
+ * @remark Expressed in the local frame, the jacobian maps the joint velocity vector to the spatial velocity of the center of the frame, expressed in the frame coordinates system. Expressed in the global frame, the jacobian maps to the spatial velocity of the point coinciding with the center of the world and attached to the frame.
*
* @param[in] model The kinematic model
* @param[in] data Data associated to model
* @param[in] frame_id Id of the operational frame we want to compute the jacobian
- * @param J The filled Jacobian Matrix
+ * @param[out] J The Jacobian of the
*
- * @tparam localFrame Express the jacobian in the local or global frame
+ * @tparam local_frame If true, the jacobian is expressed in the local frame. Otherwise, the jacobian is expressed in the world frame.
*
- * @warning The function computeJacobians should have been called first
+ * @warning The function se3::computeJacobians should have been called first
*/
- template<bool localFrame>
+ template<bool local_frame>
inline void getFrameJacobian(const Model & model,
const Data& data,
const Model::FrameIndex frame_id,
@@ -84,14 +88,10 @@ namespace se3
{
const Frame & frame = model.frames[i];
const Model::JointIndex & parent = frame.parent;
- if (frame.placement == SE3::Identity())
- {
+ if (frame.placement.isIdentity())
data.oMf[i] = data.oMi[parent];
- }
else
- {
- data.oMf[i] = (data.oMi[parent] * frame.placement);
- }
+ data.oMf[i] = data.oMi[parent]*frame.placement;
}
}
@@ -106,14 +106,14 @@ namespace se3
- template<bool localFrame>
+ template<bool local_frame>
inline void getFrameJacobian(const Model & model,
const Data & data,
const Model::FrameIndex frame_id,
Data::Matrix6x & J)
{
- assert( J.rows() == data.J.rows() );
- assert( J.cols() == data.J.cols() );
+ assert(J.cols() == model.nv);
+ assert(data.J.cols() == model.nv);
const Model::JointIndex & parent = model.frames[frame_id].parent;
const SE3 & oMframe = data.oMf[frame_id];
@@ -122,19 +122,15 @@ namespace se3
const int colRef = nv(model.joints[parent])+idx_v(model.joints[parent])-1;
// Lever between the joint center and the frame center expressed in the global frame
- const SE3::Vector3 lever(data.oMi[parent].rotation() * (frame.placement.translation()));
+ const SE3::Vector3 lever(data.oMi[parent].rotation() * frame.placement.translation());
- getJacobian<localFrame>(model, data, parent, J);
+ getJacobian<local_frame>(model, data, parent, J);
- if (frame.placement == SE3::Identity())
- {
- // do nothing
- }
- else
+ if (!frame.placement.isIdentity())
{
for(int j=colRef;j>=0;j=data.parents_fromRow[(size_t) j])
{
- if(! localFrame )
+ if(!local_frame)
J.col(j).topRows<3>() -= lever.cross(J.col(j).bottomRows<3>());
else
J.col(j) = oMframe.actInv(Motion(data.J.col(j))).toVector();
diff --git a/src/multibody/frame.hpp b/src/multibody/frame.hpp
index 77ad2519b57057d9e6a78ab4083894d89c9ed9d2..6634f40cab536814685657cfc515593cc8c47ed6 100644
--- a/src/multibody/frame.hpp
+++ b/src/multibody/frame.hpp
@@ -24,19 +24,22 @@
#include "pinocchio/tools/string-generator.hpp"
#include <Eigen/StdVector>
-#include <iostream>
+#include <string>
namespace se3
{
-
+ ///
+ /// \brief Enum on the possible type of frame
+ ///
enum FrameType
{
- OP_FRAME,
- JOINT,
- FIXED_JOINT,
- BODY,
- SENSOR
+ OP_FRAME, // operational frame type
+ JOINT, // joint frame type
+ FIXED_JOINT, // fixed joint frame type
+ BODY, // body frame type
+ SENSOR // sensor frame type
};
+
///
/// \brief A Plucker coordinate frame attached to a parent joint inside a kinematic tree
///
@@ -44,26 +47,29 @@ namespace se3
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef se3::JointIndex JointIndex;
-
- Frame() : name(randomStringGenerator(8)), parent(), placement(), type(){} // needed by EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION
///
- /// \brief Default constructor of a Frame
+ /// \brief Default constructor of a frame.
+ ///
+ Frame() : name(randomStringGenerator(8)), parent(), placement(), type() {} // needed by std::vector
+
+ ///
+ /// \brief Builds a frame defined by its name, its joint parent id, its placement and its type.
///
/// \param[in] name Name of the frame.
/// \param[in] parent Index of the parent joint in the kinematic tree.
/// \param[in] placement Placement of the frame wrt the parent joint frame.
/// \param[in] type The type of the frame, see the enum FrameType
///
- Frame(const std::string & name, const JointIndex parent, const SE3 & frame_placement, const FrameType type ):
- name(name)
+ Frame(const std::string & name, const JointIndex parent, const SE3 & frame_placement, const FrameType type)
+ : name(name)
, parent(parent)
, placement(frame_placement)
, type(type)
{}
///
- /// \brief Compare the current Frame with another frame. Return true if all properties match.
+ /// \returns true if *this and other matches and have the same parent, name and type.
///
/// \param[in] other The frame to which the current frame is compared.
///
diff --git a/src/multibody/joint/joint-free-flyer.hpp b/src/multibody/joint/joint-free-flyer.hpp
index 2fc68d2cadbf2f38aa31dda0f27b92d2604bda24..36b67a07b6621c56755fb6d45cb0648644300021 100644
--- a/src/multibody/joint/joint-free-flyer.hpp
+++ b/src/multibody/joint/joint-free-flyer.hpp
@@ -207,11 +207,12 @@ namespace se3
template<typename V>
inline void forwardKinematics(Transformation_t & M, const Eigen::MatrixBase<V> & q_joint) const
{
+ using std::sqrt;
typedef Eigen::Map<const Motion_t::Quaternion_t> ConstQuaternionMap_t;
typename Eigen::MatrixBase<V>::template ConstFixedSegmentReturnType<NQ>::Type & q = q_joint.template segment<NQ> (idx_q ());
ConstQuaternionMap_t quat(q.template tail<4>().data());
- assert(std::fabs(quat.coeffs().norm() - 1.) <= 1e-14);
+ assert(std::fabs(quat.coeffs().norm()-1.) <= sqrt(Eigen::NumTraits<typename V::Scalar>::epsilon()));
M.rotation(quat.matrix());
M.translation(q_joint.template head<3>());
@@ -280,14 +281,11 @@ namespace se3
ConfigVector_t interpolate_impl(const Eigen::VectorXd & q0, const Eigen::VectorXd & q1, const double u) const
{
- typedef Eigen::Map<Motion_t::Quaternion_t> QuaternionMap_t;
- typedef Eigen::Map<const Motion_t::Quaternion_t> ConstQuaternionMap_t;
-
Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_0 = q0.segment<NQ> (idx_q ());
Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_1 = q1.segment<NQ> (idx_q ());
- if (u == 0) return q_0;
- else if( u == 1) return q_1;
+ if (u == 0.) return q_0;
+ else if( u == 1.) return q_1;
else
{
TangentVector_t nu(u*difference(q0, q1));
@@ -340,9 +338,6 @@ namespace se3
TangentVector_t difference_impl(const Eigen::VectorXd & q0, const Eigen::VectorXd & q1) const
{
- typedef Eigen::Map<const Motion_t::Quaternion_t> ConstQuaternionMap_t;
- using std::acos;
-
Transformation_t M0(Transformation_t::Identity()); forwardKinematics(M0, q0);
Transformation_t M1(Transformation_t::Identity()); forwardKinematics(M1, q1);
diff --git a/src/multibody/joint/joint-planar.hpp b/src/multibody/joint/joint-planar.hpp
index 2fdb986aef68bf3497140463526b5c19f779d81a..6faa4b70a131e7f3a082b555695c51cf6a52d408 100644
--- a/src/multibody/joint/joint-planar.hpp
+++ b/src/multibody/joint/joint-planar.hpp
@@ -466,18 +466,16 @@ namespace se3
{
Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_0 = q0.segment<NQ> (idx_q ());
Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_1 = q1.segment<NQ> (idx_q ());
- typedef Transformation_t::Matrix3 Matrix3;
Transformation_t M0(Transformation_t::Identity()); forwardKinematics(M0, q_0);
Transformation_t M1(Transformation_t::Identity()); forwardKinematics(M1, q_1);
- TangentVector_t res;
- Motion nu = se3::log6((M0.inverse()*M1));
+ Motion nu(se3::log6(M0.inverse()*M1)); // TODO: optimize implementation
+ TangentVector_t res;
res.head<2>() = nu.linear().head<2>();
res(2) = q_1(2) - q_0(2);
return res;
-
}
double distance_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
diff --git a/src/multibody/joint/joint-spherical.hpp b/src/multibody/joint/joint-spherical.hpp
index b258d0d2187bf72bebaee0ca1491e25654227aa0..2846518ed69a16daeca860d7174f17f631432be4 100644
--- a/src/multibody/joint/joint-spherical.hpp
+++ b/src/multibody/joint/joint-spherical.hpp
@@ -271,10 +271,11 @@ namespace se3
template<typename V>
inline void forwardKinematics(Transformation_t & M, const Eigen::MatrixBase<V> & q_joint) const
{
+ using std::sqrt;
typename Eigen::MatrixBase<V>::template ConstFixedSegmentReturnType<NQ>::Type & q = q_joint.template segment<NQ> (idx_q ());
ConstQuaternionMap_t quat(q.data());
- assert(std::fabs(quat.coeffs().norm() - 1.) <= 1e-14);
+ assert(std::fabs(quat.coeffs().norm()-1.) <= sqrt(Eigen::NumTraits<typename V::Scalar>::epsilon()));
M.rotation(quat.matrix());
M.translation().setZero();
@@ -385,9 +386,6 @@ namespace se3
TangentVector_t difference_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
{
- typedef Eigen::Map<const Motion_t::Quaternion_t> ConstQuaternionMap_t;
- using std::acos;
-
Transformation_t M0; forwardKinematics(M0, q0);
Transformation_t M1; forwardKinematics(M1, q1);
diff --git a/src/multibody/model.hpp b/src/multibody/model.hpp
index bc668c2747ab998202f1692f606776a85abe59fe..4fdebcb73411ed8dd177e53f3c25ea6bd0eee28d 100644
--- a/src/multibody/model.hpp
+++ b/src/multibody/model.hpp
@@ -27,6 +27,8 @@
#include "pinocchio/multibody/fwd.hpp"
#include "pinocchio/multibody/frame.hpp"
#include "pinocchio/multibody/joint/joint.hpp"
+#include "pinocchio/deprecated.hh"
+
#include <iostream>
#include <Eigen/Cholesky>
@@ -233,24 +235,25 @@ namespace se3
const std::string & getJointName(const JointIndex index) const;
///
- /// \brief Return the index of a frame given by its name.
+ /// \brief Returns the index of a frame given by its name.
+ /// \sa Model::existFrame to check if the frame exists or not.
///
- /// \warning If no frame is found, return the number of elements at time T.
+ /// \warning If no frame is found, returns the size of the vector of Model::frames.
/// This can lead to errors if the model is expanded after this method is called
- /// (for example to get the id of a parent frame)
+ /// (for example to get the id of a parent frame).
///
- /// \param[in] index Index of the frame.
+ /// \param[in] name Name of the frame.
///
/// \return Index of the frame.
///
FrameIndex getFrameId (const std::string & name) const;
///
- /// \brief Check if a frame given by its name exists.
+ /// \brief Checks if a frame given by its name exists.
///
/// \param[in] name Name of the frame.
///
- /// \return Return true if the frame exists.
+ /// \return Returns true if the frame exists.
///
bool existFrame (const std::string & name) const;
@@ -261,7 +264,7 @@ namespace se3
///
/// \return The name of the frame.
///
- const std::string & getFrameName (const FrameIndex index) const;
+ PINOCCHIO_DEPRECATED const std::string & getFrameName (const FrameIndex index) const;
///
/// \brief Get the index of the joint supporting the frame given by its name.
@@ -270,7 +273,7 @@ namespace se3
///
/// \return
///
- JointIndex getFrameParent(const std::string & name) const;
+ PINOCCHIO_DEPRECATED JointIndex getFrameParent(const std::string & name) const;
///
/// \brief Get the index of the joint supporting the frame given by its index.
@@ -279,7 +282,7 @@ namespace se3
///
/// \return
///
- JointIndex getFrameParent(const FrameIndex index) const;
+ PINOCCHIO_DEPRECATED JointIndex getFrameParent(const FrameIndex index) const;
///
/// \brief Get the type of the frame given by its index.
@@ -288,7 +291,7 @@ namespace se3
///
/// \return
///
- FrameType getFrameType(const std::string & name) const;
+ PINOCCHIO_DEPRECATED FrameType getFrameType(const std::string & name) const;
///
/// \brief Get the type of the frame given by its index.
@@ -297,7 +300,7 @@ namespace se3
///
/// \return
///
- FrameType getFrameType(const FrameIndex index) const;
+ PINOCCHIO_DEPRECATED FrameType getFrameType(const FrameIndex index) const;
///
/// \brief Return the relative placement between a frame and its supporting joint.
@@ -306,7 +309,7 @@ namespace se3
///
/// \return The frame placement regarding the supporing joint.
///
- const SE3 & getFramePlacement(const std::string & name) const;
+ PINOCCHIO_DEPRECATED const SE3 & getFramePlacement(const std::string & name) const;
///
/// \brief Return the relative placement between a frame and its supporting joint.
@@ -315,28 +318,28 @@ namespace se3
///
/// \return The frame placement regarding the supporing joint.
///
- const SE3 & getFramePlacement(const FrameIndex index) const;
+ PINOCCHIO_DEPRECATED const SE3 & getFramePlacement(const FrameIndex index) const;
///
- /// \brief Add a frame to the kinematic tree.
+ /// \brief Adds a frame to the kinematic tree.
///
/// \param[in] frame The frame to add to the kinematic tree.
///
- /// \return Return true if the frame has been successfully added.
+ /// \return Returns true if the frame has been successfully added.
///
bool addFrame(const Frame & frame);
///
- /// \brief Create and add a frame to the kinematic tree.
+ /// \brief Creates and adds a frame to the kinematic tree.
///
/// \param[in] name Name of the frame.
/// \param[in] parent Index of the supporting joint.
/// \param[in] placement Placement of the frame regarding to the joint frame.
/// \param[in] type The type of the frame
///
- /// \return Return true if the frame has been successfully added.
+ /// \return Returns true if the frame has been successfully added.
///
- bool addFrame(const std::string & name, const JointIndex parent, const SE3 & placement, const FrameType type = OP_FRAME);
+ PINOCCHIO_DEPRECATED bool addFrame(const std::string & name, const JointIndex parent, const SE3 & placement, const FrameType type = OP_FRAME);
protected:
diff --git a/src/spatial/se3.hpp b/src/spatial/se3.hpp
index 116b35ae89224b8010432dc88a615b1d14500472..a909915bee1f7fc9885466350c223edbb7d0ac36 100644
--- a/src/spatial/se3.hpp
+++ b/src/spatial/se3.hpp
@@ -46,7 +46,7 @@ namespace se3
* aMb (x) = aRb*x + aAB
* where aAB is the vector from origin A to origin B expressed in coordinates A.
*/
- template< class Derived>
+ template<class Derived>
class SE3Base
{
protected:
@@ -120,6 +120,14 @@ namespace se3
X.disp(os);
return os;
}
+
+ ///
+ /// \returns true if *this is approximately equal to the identity placement, within the precision given by prec.
+ ///
+ bool isIdentity(const typename traits<Derived>::Scalar & prec = Eigen::NumTraits<typename traits<Derived>::Scalar>::dummy_precision()) const
+ {
+ return derived().isIdentity(prec);
+ }
}; // class SE3Base
@@ -287,6 +295,11 @@ namespace se3
{
return rot.isApprox(m2.rot, prec) && trans.isApprox(m2.trans, prec);
}
+
+ bool isIdentity(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
+ {
+ return rot.isIdentity(prec) && trans.isZero(prec);
+ }
ConstAngular_t & rotation_impl() const { return rot; }
Angular_t & rotation_impl() { return rot; }
diff --git a/unittest/finite-differences.cpp b/unittest/finite-differences.cpp
index c361be5681fea9390acf3597bca49c0002ac737f..23fb794562d399c1e1d00f5f8622cbc7cd6e208b 100644
--- a/unittest/finite-differences.cpp
+++ b/unittest/finite-differences.cpp
@@ -30,6 +30,7 @@
using namespace se3;
using namespace Eigen;
+template<bool local>
Data::Matrix6x finiteDiffJacobian(const Model & model, Data & data, const Eigen::VectorXd & q, const Model::JointIndex joint_id)
{
Data::Matrix6x res(6,model.nv); res.setZero();
@@ -52,7 +53,11 @@ Data::Matrix6x finiteDiffJacobian(const Model & model, Data & data, const Eigen:
forwardKinematics(model,data,q_integrate);
const SE3 & oMi = data.oMi[joint_id];
- res.col(k) = oMi_ref.act(log6(oMi_ref.inverse()*oMi)).toVector();
+ if (local)
+ res.col(k) = log6(oMi_ref.inverse()*oMi).toVector();
+ else
+ res.col(k) = oMi_ref.act(log6(oMi_ref.inverse()*oMi)).toVector();
+
res.col(k) /= eps;
v_integrate[k] = 0.;
@@ -162,10 +167,13 @@ BOOST_AUTO_TEST_CASE (test_jacobian_vs_finit_diff)
Model::Index idx = model.existJointName("rarm2")?model.getJointId("rarm2"):(Model::Index)(model.njoint-1);
Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);
+
getJacobian<false>(model,data,idx,Jrh);
-
- Data::Matrix6x Jrh_finite_diff = finiteDiffJacobian(model,data,q,idx);
+ Data::Matrix6x Jrh_finite_diff = finiteDiffJacobian<false>(model,data,q,idx);
+ BOOST_CHECK(Jrh_finite_diff.isApprox(Jrh,fd_increment.maxCoeff()*1e1));
+ getJacobian<true>(model,data,idx,Jrh);
+ Jrh_finite_diff = finiteDiffJacobian<true>(model,data,q,idx);
BOOST_CHECK(Jrh_finite_diff.isApprox(Jrh,fd_increment.maxCoeff()*1e1));
}
diff --git a/unittest/tspatial.cpp b/unittest/tspatial.cpp
index f34173c9f370ccbb8cbb516465fc44c0009d3c80..b64e7ef4e3ea2521f8575f6899d4e2b87efefa0c 100644
--- a/unittest/tspatial.cpp
+++ b/unittest/tspatial.cpp
@@ -33,7 +33,7 @@ BOOST_AUTO_TEST_SUITE ( tspatialTest)
BOOST_AUTO_TEST_CASE ( test_SE3 )
{
-using namespace se3;
+ using namespace se3;
typedef Eigen::Matrix<double,4,4> Matrix4;
typedef SE3::Matrix6 Matrix6;
typedef SE3::Vector3 Vector3;
@@ -62,7 +62,6 @@ using namespace se3;
Vector3 Mip = (aMb.inverse()*p4).head(3);
BOOST_CHECK(amb.actInv(p).isApprox(Mip, 1e-12));
-
// Test action matrix
Matrix6 aXb = amb;
Matrix6 bXc = bmc;
@@ -71,6 +70,13 @@ using namespace se3;
Matrix6 bXa = amb.inverse();
BOOST_CHECK(bXa.isApprox(aXb.inverse(), 1e-12));
+
+ // Test isIdentity
+ SE3 identity = SE3::Identity();
+ BOOST_CHECK(identity.isIdentity());
+
+ // Test isApprox
+ BOOST_CHECK(identity.isApprox(identity));
}
BOOST_AUTO_TEST_CASE ( test_Motion )
@@ -210,7 +216,6 @@ BOOST_AUTO_TEST_CASE ( test_Inertia )
{
using namespace se3;
typedef Inertia::Matrix6 Matrix6;
- typedef Inertia::Matrix3 Matrix3;
Inertia aI = Inertia::Random();
Matrix6 matI = aI;