Skip to content
Snippets Groups Projects
Commit c52c492f authored by jcarpent's avatar jcarpent
Browse files

[C++][Bug Fixed] Rework op frames 

There were some bugs in the algo + rename framesForwardKinematic in
framesForwardKinematics (follow the API)
parent 4d726e47
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/algorithm/operational-frames.hpp
+ 53
− 60
View file @ c52c492f
......@@ -28,8 +28,6 @@
namespace se3
{
/**
* @brief Update the position of each extra frame
*
......@@ -37,9 +35,9 @@ namespace se3
* @param data Data associated to model
* @warning One of the algorithms forwardKinematics should have been called first
*/
inline void framesForwardKinematic(const Model & model,
Data & data
);
inline void framesForwardKinematics(const Model & model,
Data & data
);
/**
* @brief Compute Kinematics of full model, then the position of each operational frame
......@@ -48,10 +46,10 @@ namespace se3
* @param data Data associated to model
* @param[in] q Configuration vector
*/
inline void framesForwardKinematic(const Model & model,
Data & data,
const Eigen::VectorXd & q
);
inline void framesForwardKinematics(const Model & model,
Data & data,
const Eigen::VectorXd & q
);
/**
* @brief Return the jacobian of the extra frame in the world frame or
......@@ -68,72 +66,67 @@ namespace se3
*/
template<bool localFrame>
inline void getFrameJacobian(const Model & model,
const Data& data,
Model::FrameIndex frame_id,
Data::Matrix6x & J
);
const Data& data,
const Model::FrameIndex frame_id,
Data::Matrix6x & J
);
} // namespace se3
} // namespace se3
/* --- Details -------------------------------------------------------------------- */
namespace se3
{
inline void framesForwardKinematic(const Model & model,
Data & data
)
{
using namespace se3;
for (Model::FrameIndex i=0; i < (Model::FrameIndex) model.nOperationalFrames; ++i)
inline void framesForwardKinematics(const Model & model,
Data & data
)
{
const Model::JointIndex & parent = model.operational_frames[i].parent_id;
data.oMof[i] = (data.oMi[parent] * model.operational_frames[i].framePlacement);
for (Model::FrameIndex i=0; i < (Model::FrameIndex) model.nOperationalFrames; ++i)
{
const Model::JointIndex & parent = model.operational_frames[i].parent_id;
data.oMof[i] = (data.oMi[parent] * model.operational_frames[i].framePlacement);
}
}
}
inline void framesForwardKinematic(const Model & model,
Data & data,
const Eigen::VectorXd & q
)
{
using namespace se3;
forwardKinematics(model, data, q);
framesForwardKinematic(model, data);
}
template<bool localFrame>
inline void getFrameJacobian(const Model & model, const Data& data,
Model::FrameIndex frame_id, Data::Matrix6x & J)
{
assert( J.rows() == data.J.rows() );
assert( J.cols() == data.J.cols() );
const Model::JointIndex & parent = model.operational_frames[frame_id].parent_id;
const SE3 & oMframe = data.oMof[frame_id];
int colRef = nv(model.joints[parent])+idx_v(model.joints[parent])-1;
SE3::Vector3 lever(data.oMi[parent].rotation() * (model.operational_frames[frame_id].framePlacement.translation()));
if (!localFrame) getJacobian<localFrame>(model, data, parent, J);
for(int j=colRef;j>=0;j=data.parents_fromRow[(Model::Index)j])
inline void framesForwardKinematics(const Model & model,
Data & data,
const Eigen::VectorXd & q
)
{
forwardKinematics(model, data, q);
framesForwardKinematics(model, data);
}
template<bool localFrame>
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() );
const Model::JointIndex & parent = model.operational_frames[frame_id].parent_id;
const SE3 & oMframe = data.oMof[frame_id];
const Frame & frame = model.operational_frames[frame_id];
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.framePlacement.translation()));
getJacobian<localFrame>(model, data, parent, J);
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>());
}
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
......
src/python/algorithms.hpp
+ 4
− 4
View file @ c52c492f
......@@ -150,11 +150,11 @@ namespace se3
static void frames_fk_0_proxy(const ModelHandler& model,
DataHandler & data,
const VectorXd_fx & q
)
DataHandler & data,
const VectorXd_fx & q
)
{
framesForwardKinematic( *model,*data,q );
framesForwardKinematics( *model,*data,q );
}
static void fk_2_proxy(const ModelHandler& model,
......
unittest/operational-frames.cpp
+ 2
− 2
View file @ c52c492f
......@@ -47,7 +47,7 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
VectorXd q = VectorXd::Ones(model.nq);
q.middleRows<4> (3).normalize();
framesForwardKinematic(model, data, q);
framesForwardKinematics(model, data, q);
BOOST_CHECK(data.oMof[model.getFrameId(frame_name)].isApprox(data.oMi[parent_idx]*framePlacement));
......@@ -71,7 +71,7 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
VectorXd q = VectorXd::Ones(model.nq);
q.middleRows<4> (3).normalize();
VectorXd q_dot = VectorXd::Ones(model.nv);
framesForwardKinematic(model, data, q);
framesForwardKinematics(model, data, q);
computeJacobians(model,data,q);
......
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