Commit df159ccf authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #297 from jcarpent/devel

Update the Frame the part + expose new methods
parents e7971d08 958307b6
......@@ -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; }
......
......@@ -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)
......
......@@ -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();
......
......@@ -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.
///
......
......@@ -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);
......
......@@ -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
......
......@@ -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);
......
......@@ -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:
......
......@@ -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; }
......
......@@ -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));
}
......
......@@ -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;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment