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

[Minor] Update name in Data of oMof to oMf + in frames algo get directly the...

[Minor] Update name in Data of oMof to oMf + in frames algo get directly the value of the parent joint when the relative placement of the frame wrt parent joint is Identity
parent c5669d1c
......@@ -82,8 +82,16 @@ namespace se3
{
for (Model::FrameIndex i=0; i < (Model::FrameIndex) model.nFrames; ++i)
{
const Model::JointIndex & parent = model.frames[i].parent;
data.oMof[i] = (data.oMi[parent] * model.frames[i].placement);
const Frame & frame = model.frames[i];
const Model::JointIndex & parent = frame.parent;
if (frame.placement == SE3::Identity())
{
data.oMf[i] = data.oMi[parent];
}
else
{
data.oMf[i] = (data.oMi[parent] * frame.placement);
}
}
}
......@@ -108,7 +116,7 @@ namespace se3
assert( J.cols() == data.J.cols() );
const Model::JointIndex & parent = model.frames[frame_id].parent;
const SE3 & oMframe = data.oMof[frame_id];
const SE3 & oMframe = data.oMf[frame_id];
const Frame & frame = model.frames[frame_id];
const int colRef = nv(model.joints[parent])+idx_v(model.joints[parent])-1;
......@@ -117,8 +125,8 @@ namespace se3
const SE3::Vector3 lever(data.oMi[parent].rotation() * (frame.placement.translation()));
getJacobian<localFrame>(model, data, parent, J);
if (frame.type == JOINT)
if (frame.placement == SE3::Identity())
{
// do nothing
}
......
......@@ -423,7 +423,7 @@ namespace se3
Eigen::VectorXd nle;
/// \brief Vector of absolute operationnel frame placements (wrt the world).
std::vector<SE3> oMof;
std::vector<SE3> oMf;
/// \brief Vector of sub-tree composite rigid body inertias, i.e. the apparent inertia of the subtree supported by the joint.
std::vector<Inertia> Ycrb;
......
......@@ -287,7 +287,7 @@ namespace se3
,liMi((std::size_t)ref.nbody)
,tau(ref.nv)
,nle(ref.nv)
,oMof((std::size_t)ref.nFrames)
,oMf((std::size_t)ref.nFrames)
,Ycrb((std::size_t)ref.nbody)
,M(ref.nv,ref.nv)
,ddq(ref.nv)
......
......@@ -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,"frames absolute placement (wrt world)")
.ADD_DATA_PROPERTY(std::vector<SE3>,oMf,"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,"frames absolute placement (wrt world)")
IMPL_DATA_PROPERTY(std::vector<SE3>,oMf,"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")
......
......@@ -49,7 +49,7 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
q.middleRows<4> (3).normalize();
framesForwardKinematics(model, data, q);
BOOST_CHECK(data.oMof[model.getFrameId(frame_name)].isApprox(data.oMi[parent_idx]*framePlacement));
BOOST_CHECK(data.oMf[model.getFrameId(frame_name)].isApprox(data.oMi[parent_idx]*framePlacement));
}
......
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