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
...@@ -28,8 +28,6 @@ ...@@ -28,8 +28,6 @@
namespace se3 namespace se3
{ {
/** /**
* @brief Update the position of each extra frame * @brief Update the position of each extra frame
* *
...@@ -37,9 +35,9 @@ namespace se3 ...@@ -37,9 +35,9 @@ namespace se3
* @param data Data associated to model * @param data Data associated to model
* @warning One of the algorithms forwardKinematics should have been called first * @warning One of the algorithms forwardKinematics should have been called first
*/ */
inline void framesForwardKinematic(const Model & model, inline void framesForwardKinematics(const Model & model,
Data & data Data & data
); );
/** /**
* @brief Compute Kinematics of full model, then the position of each operational frame * @brief Compute Kinematics of full model, then the position of each operational frame
...@@ -48,10 +46,10 @@ namespace se3 ...@@ -48,10 +46,10 @@ namespace se3
* @param data Data associated to model * @param data Data associated to model
* @param[in] q Configuration vector * @param[in] q Configuration vector
*/ */
inline void framesForwardKinematic(const Model & model, inline void framesForwardKinematics(const Model & model,
Data & data, Data & data,
const Eigen::VectorXd & q const Eigen::VectorXd & q
); );
/** /**
* @brief Return the jacobian of the extra frame in the world frame or * @brief Return the jacobian of the extra frame in the world frame or
...@@ -68,72 +66,67 @@ namespace se3 ...@@ -68,72 +66,67 @@ namespace se3
*/ */
template<bool localFrame> template<bool localFrame>
inline void getFrameJacobian(const Model & model, inline void getFrameJacobian(const Model & model,
const Data& data, const Data& data,
Model::FrameIndex frame_id, const Model::FrameIndex frame_id,
Data::Matrix6x & J Data::Matrix6x & J
); );
} // namespace se3 } // namespace se3
/* --- Details -------------------------------------------------------------------- */ /* --- Details -------------------------------------------------------------------- */
namespace se3 namespace se3
{ {
inline void framesForwardKinematic(const Model & model, inline void framesForwardKinematics(const Model & model,
Data & data Data & data
) )
{
using namespace se3;
for (Model::FrameIndex i=0; i < (Model::FrameIndex) model.nOperationalFrames; ++i)
{ {
const Model::JointIndex & parent = model.operational_frames[i].parent_id; for (Model::FrameIndex i=0; i < (Model::FrameIndex) model.nOperationalFrames; ++i)
data.oMof[i] = (data.oMi[parent] * model.operational_frames[i].framePlacement); {
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; inline void framesForwardKinematics(const Model & model,
Data & data,
SE3::Vector3 lever(data.oMi[parent].rotation() * (model.operational_frames[frame_id].framePlacement.translation())); const Eigen::VectorXd & q
)
if (!localFrame) getJacobian<localFrame>(model, data, parent, J); {
for(int j=colRef;j>=0;j=data.parents_fromRow[(Model::Index)j]) 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 ) 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 else
{
J.col(j) = oMframe.actInv(Motion(data.J.col(j))).toVector(); J.col(j) = oMframe.actInv(Motion(data.J.col(j))).toVector();
}
} }
} }
} // namespace se3 } // namespace se3
......
...@@ -150,11 +150,11 @@ namespace se3 ...@@ -150,11 +150,11 @@ namespace se3
static void frames_fk_0_proxy(const ModelHandler& model, static void frames_fk_0_proxy(const ModelHandler& model,
DataHandler & data, DataHandler & data,
const VectorXd_fx & q const VectorXd_fx & q
) )
{ {
framesForwardKinematic( *model,*data,q ); framesForwardKinematics( *model,*data,q );
} }
static void fk_2_proxy(const ModelHandler& model, static void fk_2_proxy(const ModelHandler& model,
......
...@@ -47,7 +47,7 @@ BOOST_AUTO_TEST_CASE ( test_kinematics ) ...@@ -47,7 +47,7 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
VectorXd q = VectorXd::Ones(model.nq); VectorXd q = VectorXd::Ones(model.nq);
q.middleRows<4> (3).normalize(); 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)); BOOST_CHECK(data.oMof[model.getFrameId(frame_name)].isApprox(data.oMi[parent_idx]*framePlacement));
...@@ -71,7 +71,7 @@ BOOST_AUTO_TEST_CASE ( test_jacobian ) ...@@ -71,7 +71,7 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
VectorXd q = VectorXd::Ones(model.nq); VectorXd q = VectorXd::Ones(model.nq);
q.middleRows<4> (3).normalize(); q.middleRows<4> (3).normalize();
VectorXd q_dot = VectorXd::Ones(model.nv); VectorXd q_dot = VectorXd::Ones(model.nv);
framesForwardKinematic(model, data, q); framesForwardKinematics(model, data, q);
computeJacobians(model,data,q); computeJacobians(model,data,q);
......
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