From 98a5132cd015709449a8141dc74c30b4fd38483d Mon Sep 17 00:00:00 2001
From: Valenza Florian <fvalenza@laas.fr>
Date: Mon, 20 Jun 2016 17:51:44 +0200
Subject: [PATCH] [C++][Python] Added an enum type of a Frame.
---
src/multibody/model.hpp | 3 ++-
src/multibody/model.hxx | 5 +++--
src/python/frame.hpp | 10 +++++++++-
src/spatial/frame.hpp | 19 ++++++++++++++++---
4 files changed, 30 insertions(+), 7 deletions(-)
diff --git a/src/multibody/model.hpp b/src/multibody/model.hpp
index 20bfbd457..988542aef 100644
--- a/src/multibody/model.hpp
+++ b/src/multibody/model.hpp
@@ -353,10 +353,11 @@ namespace se3
/// \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.
///
- bool addFrame(const std::string & name, const JointIndex parent, const SE3 & placement);
+ bool addFrame(const std::string & name, const JointIndex parent, const SE3 & placement, const FrameType type = OP_POINT);
};
diff --git a/src/multibody/model.hxx b/src/multibody/model.hxx
index d431bb50a..afb44446f 100644
--- a/src/multibody/model.hxx
+++ b/src/multibody/model.hxx
@@ -139,6 +139,7 @@ namespace se3
const Inertia & iYf = Y.se3Action(bodyPlacement);
inertias[parent] += iYf;
+ addFrame((bodyName!="")?bodyName:random(8), parent, bodyPlacement, BODY);
bodyParents.push_back(parent);
bodyPlacements.push_back(bodyPlacement);
bodyNames.push_back( (bodyName!="")?bodyName:random(8));
@@ -257,10 +258,10 @@ namespace se3
}
}
- inline bool Model::addFrame ( const std::string & name, JointIndex index, const SE3 & placement)
+ inline bool Model::addFrame ( const std::string & name, JointIndex index, const SE3 & placement, const FrameType type)
{
if( !existFrame(name) )
- return addFrame(Frame(name, index, placement));
+ return addFrame(Frame(name, index, placement, type));
else
return false;
}
diff --git a/src/python/frame.hpp b/src/python/frame.hpp
index 8cdeecfa6..9cc825ba0 100644
--- a/src/python/frame.hpp
+++ b/src/python/frame.hpp
@@ -51,7 +51,7 @@ namespace se3
void visit(PyClass& cl) const
{
cl
- .def(bp::init<const std::string&,const JointIndex, const SE3_fx&> ((bp::arg("name (string)"),bp::arg("parent (index)"), bp::arg("SE3 placement")),
+ .def(bp::init< const std::string&,const JointIndex, const SE3_fx&,FrameType> ((bp::arg("name (string)"),bp::arg("parent (index)"), bp::arg("SE3 placement"), bp::arg("type (FrameType)")),
"Initialize from name, parent id and placement wrt parent joint."))
.def_readwrite("name", &Frame::name, "name of the frame")
@@ -60,6 +60,7 @@ namespace se3
&FramePythonVisitor::getPlacementWrtParentJoint,
&FramePythonVisitor::setPlacementWrtParentJoint,
"placement in the parent joint local frame")
+ .def_readwrite("type", &Frame::type, "type of the frame")
;
}
@@ -69,6 +70,13 @@ namespace se3
static void expose()
{
+ bp::enum_<FrameType>("FrameType")
+ .value("OP_POINT",OP_POINT)
+ .value("FIXED_JOINT",FIXED_JOINT)
+ .value("BODY",BODY)
+ .value("SENSOR",SENSOR)
+ ;
+
bp::class_<Frame>("Frame",
"A Plucker coordinate frame related to a parent joint inside a kinematic tree.\n\n",
bp::no_init
diff --git a/src/spatial/frame.hpp b/src/spatial/frame.hpp
index c917b4611..9ec0fd1cf 100644
--- a/src/spatial/frame.hpp
+++ b/src/spatial/frame.hpp
@@ -29,6 +29,13 @@
namespace se3
{
+ enum FrameType
+ {
+ OP_POINT,
+ FIXED_JOINT,
+ BODY,
+ SENSOR
+ };
///
/// \brief A Plucker coordinate frame attached to a parent joint inside a kinematic tree
///
@@ -36,7 +43,7 @@ namespace se3
{
typedef se3::JointIndex JointIndex;
- Frame() : name(random(8)), parent(), placement() {} // needed by EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION
+ Frame() : name(random(8)), parent(), placement(), type(){} // needed by EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION
///
/// \brief Default constructor of a Frame
@@ -44,11 +51,13 @@ namespace se3
/// \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):
+ Frame(const std::string & name, const JointIndex parent, const SE3 & frame_placement, const FrameType type ):
name(name)
, parent(parent)
, placement(frame_placement)
+ , type(type)
{}
///
@@ -59,7 +68,8 @@ namespace se3
bool operator == (const Frame & other) const
{
return name == other.name && parent == other.parent
- && placement == other.placement ;
+ && placement == other.placement
+ && type == other.type ;
}
/// \brief Name of the frame.
@@ -70,6 +80,9 @@ namespace se3
/// \brief Placement of the frame wrt the parent joint.
SE3 placement;
+
+ /// \brief Type of the frame
+ FrameType type;
}; // struct Frame
--
GitLab