Commit cd056d01 authored by Valenza Florian's avatar Valenza Florian
Browse files

Renamed operational_frames to frames in Model

parent 10295e6c
......@@ -159,7 +159,7 @@ SET(${PROJECT_NAME}_ALGORITHM_HEADERS
algorithm/center-of-mass.hxx
algorithm/joint-configuration.hpp
algorithm/energy.hpp
algorithm/operational-frames.hpp
algorithm/frames.hpp
algorithm/compute-all-terms.hpp
)
......
......@@ -24,19 +24,19 @@ class TestFrameBindings(unittest.TestCase):
robot = RobotWrapper(romeo_model_path, hint_list, se3.JointModelFreeFlyer())
def test_type_get_set(self):
f = self.robot.model.operational_frames[1]
f = self.robot.model.frames[1]
self.assertTrue(f.type == se3.FrameType.JOINT)
f.type = se3.FrameType.BODY
self.assertTrue(f.type == se3.FrameType.BODY)
def test_name_get_set(self):
f = self.robot.model.operational_frames[1]
f = self.robot.model.frames[1]
self.assertTrue(f.name == 'LHipYaw')
f.name = 'new_hip_frame'
self.assertTrue(f.name == 'new_hip_frame')
def test_parent_get_set(self):
f = self.robot.model.operational_frames[1]
f = self.robot.model.frames[1]
self.assertTrue(f.parent == 2)
f.parent = 5
self.assertTrue(f.parent == 5)
......@@ -44,7 +44,7 @@ class TestFrameBindings(unittest.TestCase):
def test_placement_get_set(self):
m = se3.SE3(self.m3ones, np.array([0,0,0],np.double))
new_m = se3.SE3(rand([3,3]), rand(3))
f = self.robot.model.operational_frames[1]
f = self.robot.model.frames[1]
self.assertTrue(np.allclose(f.placement.homogeneous, m.homogeneous))
f.placement = new_m
self.assertTrue(np.allclose(f.placement.homogeneous, new_m.homogeneous))
......
......@@ -15,8 +15,8 @@
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
#ifndef __se3_operational_frames_hpp__
#define __se3_operational_frames_hpp__
#ifndef __se3_frames_hpp__
#define __se3_frames_hpp__
#include "pinocchio/multibody/model.hpp"
......@@ -80,10 +80,10 @@ namespace se3
Data & data
)
{
for (Model::FrameIndex i=0; i < (Model::FrameIndex) model.nOperationalFrames; ++i)
for (Model::FrameIndex i=0; i < (Model::FrameIndex) model.nFrames; ++i)
{
const Model::JointIndex & parent = model.operational_frames[i].parent;
data.oMof[i] = (data.oMi[parent] * model.operational_frames[i].placement);
const Model::JointIndex & parent = model.frames[i].parent;
data.oMof[i] = (data.oMi[parent] * model.frames[i].placement);
}
}
......@@ -107,9 +107,9 @@ namespace se3
assert( J.rows() == data.J.rows() );
assert( J.cols() == data.J.cols() );
const Model::JointIndex & parent = model.operational_frames[frame_id].parent;
const Model::JointIndex & parent = model.frames[frame_id].parent;
const SE3 & oMframe = data.oMof[frame_id];
const Frame & frame = model.operational_frames[frame_id];
const Frame & frame = model.frames[frame_id];
const int colRef = nv(model.joints[parent])+idx_v(model.joints[parent])-1;
......@@ -117,16 +117,24 @@ namespace se3
const SE3::Vector3 lever(data.oMi[parent].rotation() * (frame.placement.translation()));
getJacobian<localFrame>(model, data, parent, J);
for(int j=colRef;j>=0;j=data.parents_fromRow[(size_t) j])
if (frame.type == JOINT)
{
// do nothing
}
else
{
if(! localFrame )
J.col(j).topRows<3>() -= lever.cross(J.col(j).bottomRows<3>());
else
J.col(j) = oMframe.actInv(Motion(data.J.col(j))).toVector();
for(int j=colRef;j>=0;j=data.parents_fromRow[(size_t) j])
{
if(! localFrame )
J.col(j).topRows<3>() -= lever.cross(J.col(j).bottomRows<3>());
else
J.col(j) = oMframe.actInv(Motion(data.J.col(j))).toVector();
}
}
}
} // namespace se3
#endif // ifndef __se3_operational_frames_hpp__
#endif // ifndef __se3_frames_hpp__
......@@ -67,7 +67,7 @@ namespace se3
/// \brief Number of operational frames.
int nOperationalFrames;
int nFrames;
/// \brief Spatial inertias of the body <i> expressed in the supporting joint frame <i>.
std::vector<Inertia> inertias;
......@@ -95,7 +95,7 @@ namespace se3
Eigen::VectorXd upperPositionLimit;
/// \brief Vector of operational frames registered on the model.
std::vector<Frame> operational_frames;
std::vector<Frame> frames;
/// \brief Spatial gravity
Motion gravity;
......@@ -108,7 +108,7 @@ namespace se3
, nv(0)
, njoint(1)
, nbody(1)
, nOperationalFrames(0)
, nFrames(0)
, inertias(1)
, jointPlacements(1)
, joints(1)
......
......@@ -189,80 +189,80 @@ namespace se3
inline Model::FrameIndex Model::getFrameId ( const std::string & name ) const
{
std::vector<Frame>::const_iterator it = std::find_if( operational_frames.begin()
, operational_frames.end()
std::vector<Frame>::const_iterator it = std::find_if( frames.begin()
, frames.end()
, boost::bind(&Frame::name, _1) == name
);
return Model::FrameIndex(it - operational_frames.begin());
return Model::FrameIndex(it - frames.begin());
}
inline bool Model::existFrame ( const std::string & name ) const
{
return std::find_if( operational_frames.begin(), operational_frames.end(), boost::bind(&Frame::name, _1) == name) != operational_frames.end();
return std::find_if( frames.begin(), frames.end(), boost::bind(&Frame::name, _1) == name) != frames.end();
}
inline const std::string & Model::getFrameName ( const FrameIndex index ) const
{
return operational_frames[index].name;
return frames[index].name;
}
inline Model::JointIndex Model::getFrameParent( const std::string & name ) const
{
assert(existFrame(name) && "The Frame you requested does not exist");
std::vector<Frame>::const_iterator it = std::find_if( operational_frames.begin()
, operational_frames.end()
std::vector<Frame>::const_iterator it = std::find_if( frames.begin()
, frames.end()
, boost::bind(&Frame::name, _1) == name
);
std::vector<Frame>::iterator::difference_type it_diff = it - operational_frames.begin();
std::vector<Frame>::iterator::difference_type it_diff = it - frames.begin();
return getFrameParent(Model::JointIndex(it_diff));
}
inline Model::JointIndex Model::getFrameParent( const FrameIndex index ) const
{
return operational_frames[index].parent;
return frames[index].parent;
}
inline FrameType Model::getFrameType( const std::string & name ) const
{
assert(existFrame(name) && "The Frame you requested does not exist");
std::vector<Frame>::const_iterator it = std::find_if( operational_frames.begin()
, operational_frames.end()
std::vector<Frame>::const_iterator it = std::find_if( frames.begin()
, frames.end()
, boost::bind(&Frame::name, _1) == name
);
std::vector<Frame>::iterator::difference_type it_diff = it - operational_frames.begin();
std::vector<Frame>::iterator::difference_type it_diff = it - frames.begin();
return getFrameType(Model::JointIndex(it_diff));
}
inline FrameType Model::getFrameType( const FrameIndex index ) const
{
return operational_frames[index].type;
return frames[index].type;
}
inline const SE3 & Model::getFramePlacement( const std::string & name) const
{
assert(existFrame(name) && "The Frame you requested does not exist");
std::vector<Frame>::const_iterator it = std::find_if( operational_frames.begin()
, operational_frames.end()
std::vector<Frame>::const_iterator it = std::find_if( frames.begin()
, frames.end()
, boost::bind(&Frame::name, _1) == name
);
std::vector<Frame>::iterator::difference_type it_diff = it - operational_frames.begin();
std::vector<Frame>::iterator::difference_type it_diff = it - frames.begin();
return getFramePlacement(Model::Index(it_diff));
}
inline const SE3 & Model::getFramePlacement( const FrameIndex index ) const
{
return operational_frames[index].placement;
return frames[index].placement;
}
inline bool Model::addFrame ( const Frame & frame )
{
if( !existFrame(frame.name) )
{
operational_frames.push_back(frame);
nOperationalFrames++;
frames.push_back(frame);
nFrames++;
return true;
}
else
......@@ -291,7 +291,7 @@ namespace se3
,liMi((std::size_t)ref.nbody)
,tau(ref.nv)
,nle(ref.nv)
,oMof((std::size_t)ref.nOperationalFrames)
,oMof((std::size_t)ref.nFrames)
,Ycrb((std::size_t)ref.nbody)
,M(ref.nv,ref.nv)
,ddq(ref.nv)
......
......@@ -30,7 +30,7 @@
#include "pinocchio/algorithm/dynamics.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/operational-frames.hpp"
#include "pinocchio/algorithm/frames.hpp"
#include "pinocchio/algorithm/center-of-mass.hpp"
#include "pinocchio/algorithm/energy.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
......
......@@ -74,7 +74,7 @@ namespace se3
.ADD_DATA_PROPERTY(std::vector<Motion>,v,"Body velocity")
.ADD_DATA_PROPERTY(std::vector<Force>,f,"Body force")
.ADD_DATA_PROPERTY(std::vector<SE3>,oMi,"Body absolute placement (wrt world)")
.ADD_DATA_PROPERTY(std::vector<SE3>,oMof,"operational_frames absolute placement (wrt world)")
.ADD_DATA_PROPERTY(std::vector<SE3>,oMof,"frames absolute placement (wrt world)")
.ADD_DATA_PROPERTY(std::vector<SE3>,liMi,"Body relative placement (wrt parent)")
.ADD_DATA_PROPERTY_CONST(Eigen::VectorXd,tau,"Joint forces")
.ADD_DATA_PROPERTY_CONST(Eigen::VectorXd,nle,"Non Linear Effects")
......@@ -120,7 +120,7 @@ namespace se3
IMPL_DATA_PROPERTY(std::vector<Motion>,v,"Body velocity")
IMPL_DATA_PROPERTY(std::vector<Force>,f,"Body force")
IMPL_DATA_PROPERTY(std::vector<SE3>,oMi,"Body absolute placement (wrt world)")
IMPL_DATA_PROPERTY(std::vector<SE3>,oMof,"operational_frames absolute placement (wrt world)")
IMPL_DATA_PROPERTY(std::vector<SE3>,oMof,"frames absolute placement (wrt world)")
IMPL_DATA_PROPERTY(std::vector<SE3>,liMi,"Body relative placement (wrt parent)")
IMPL_DATA_PROPERTY_CONST(Eigen::VectorXd,tau,"Joint forces")
IMPL_DATA_PROPERTY_CONST(Eigen::VectorXd,nle,"Non Linear Effects")
......
......@@ -130,7 +130,7 @@ namespace se3
.def("getFrameParent", &ModelPythonVisitor::getFrameParent)
.def("getFramePlacement", &ModelPythonVisitor::getFramePlacement)
.def("addFrame", &ModelPythonVisitor::addFrame)
.add_property("operational_frames", bp::make_function(&ModelPythonVisitor::operationalFrames, bp::return_internal_reference<>()) )
.add_property("frames", bp::make_function(&ModelPythonVisitor::operationalFrames, bp::return_internal_reference<>()) )
.add_property("gravity",&ModelPythonVisitor::gravity,&ModelPythonVisitor::setGravity)
.def("BuildEmptyModel",&ModelPythonVisitor::maker_empty)
......@@ -180,7 +180,7 @@ namespace se3
{
m->addFrame(frameName, parent, placementWrtParent);
}
static std::vector<Frame> & operationalFrames (ModelHandler & m ) { return m->operational_frames;}
static std::vector<Frame> & operationalFrames (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; }
......
......@@ -88,6 +88,6 @@ ADD_UNIT_TEST(constraint eigen3)
ADD_UNIT_TEST(joints eigen3)
ADD_UNIT_TEST(compute-all-terms eigen3)
ADD_UNIT_TEST(energy eigen3)
ADD_UNIT_TEST(operational-frames eigen3)
ADD_UNIT_TEST(frames eigen3)
ADD_UNIT_TEST(joint-configurations eigen3)
ADD_UNIT_TEST(joint-accessor eigen3)
\ No newline at end of file
......@@ -17,7 +17,7 @@
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/operational-frames.hpp"
#include "pinocchio/algorithm/frames.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/spatial/act-on-set.hpp"
#include "pinocchio/multibody/parser/sample-models.hpp"
......@@ -26,11 +26,11 @@
#include <iostream>
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE OperationalFramesTest
#define BOOST_TEST_MODULE FramesTest
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
BOOST_AUTO_TEST_SUITE ( OperationalFramesTest)
BOOST_AUTO_TEST_SUITE ( FramesTest)
BOOST_AUTO_TEST_CASE ( test_kinematics )
{
......
......@@ -17,7 +17,7 @@
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/operational-frames.hpp"
#include "pinocchio/algorithm/frames.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"
......
Markdown is supported
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