Commit f0ef7326 authored by Gabriele Buondonno's avatar Gabriele Buondonno
Browse files

[frames] placement, velocity and acceleration

parent f981cdaf
......@@ -25,7 +25,7 @@ namespace se3
{
/**
* @brief Updates the position of each frame contained in the model
* @brief Updates the placement of each frame contained in the model
*
* @param[in] model The kinematic model.
* @param data Data associated to model.
......@@ -48,14 +48,62 @@ namespace se3
const Eigen::VectorXd & q);
/**
* @brief Returns the jacobian of the frame expresssed either expressed in the LOCAL frame coordinate system or in the WORLD coordinate system,
* @brief Updates the placement of the given frame.
*
* @param[in] model The kinematic model.
* @param data Data associated to model.
* @param[in] frame_id Id of the operational Frame.
*
* @return A reference to the frame placement stored in data.oMf[frame_id]
*
* @warning One of the algorithms forwardKinematics should have been called first
*/
inline const SE3 & frameForwardKinematics(const Model & model,
Data & data,
const Model::FrameIndex frame_id);
/**
* @brief Returns the spatial velocity of the frame expressed in the LOCAL frame coordinate system.
* You must first call se3::forwardKinematics to update placement and velocity values in data structure.
*
* @param[in] model The kinematic model
* @param[in] data Data associated to model
* @param[in] frame_id Id of the operational Frame
* @param[out] frame_v The spatial velocity of the Frame expressed in the coordinates Frame.
*
* @warning Fist or second order forwardKinematics should have been called first
*/
void getFrameVelocity(const Model & model,
const Data & data,
const Model::FrameIndex frame_id,
Motion & frame_v);
/**
* @brief Returns the spatial acceleration of the frame expressed in the LOCAL frame coordinate system.
* You must first call se3::forwardKinematics to update placement values in data structure.
*
* @param[in] model The kinematic model
* @param[in] data Data associated to model
* @param[in] frame_id Id of the operational Frame
* @param[out] frame_a The spatial acceleration of the Frame expressed in the coordinates Frame.
*
* @warning Second order forwardKinematics should have been called first
*/
void getFrameAcceleration(const Model & model,
const Data & data,
const Model::FrameIndex frame_id,
Motion & frame_a);
/**
* @brief Returns the jacobian of the frame expressed either in the LOCAL frame coordinate system or in the WORLD coordinate system,
* depending on the value of the template parameter rf.
* You must first call se3::computeJointJacobians followed by se3::framesForwardKinematics to update placement values in data structure.
*
* @tparam rf Reference frame in which the columns of the Jacobian are expressed.
*
* @remark Similarly to se3::getJointJacobian with LOCAL or WORLD templated parameters, if rf == LOCAL, this function returns the Jacobian of the frame expressed
* in the local coordinates of the frame, or if rl == WORDL, it returns the Jacobian expressed of the point coincident with the origin
* in the local coordinates of the frame, or if rl == WORLD, it returns the Jacobian expressed of the point coincident with the origin
* and expressed in a coordinate system aligned with the WORLD.
*
* @param[in] model The kinematic model
......@@ -63,7 +111,7 @@ namespace se3
* @param[in] frame_id Id of the operational Frame
* @param[out] J The Jacobian of the Frame expressed in the coordinates Frame.
*
* @warning The function se3::computeJointJacobians and se3::framesForwardKinematics should have been called first.
* @warning The functions se3::computeJointJacobians and se3::framesForwardKinematics should have been called first.
*/
template<ReferenceFrame rf>
void getFrameJacobian(const Model & model,
......
......@@ -53,6 +53,43 @@ namespace se3
forwardKinematics(model, data, q);
framesForwardKinematics(model, data);
}
inline const SE3 & frameForwardKinematics(const Model & model,
Data & data,
const Model::FrameIndex frame_id)
{
const Frame & frame = model.frames[frame_id];
const Model::JointIndex & parent = frame.parent;
if (frame.placement.isIdentity())
data.oMf[frame_id] = data.oMi[parent];
else
data.oMf[frame_id] = data.oMi[parent]*frame.placement;
return data.oMf[frame_id];
}
void getFrameVelocity(const Model & model,
const Data & data,
const Model::FrameIndex frame_id,
Motion & frame_v)
{
assert(model.check(data) && "data is not consistent with model.");
const Frame & frame = model.frames[frame_id];
const Model::JointIndex & parent = frame.parent;
frame_v = frame.placement.actInv(data.v[parent]);
}
void getFrameAcceleration(const Model & model,
const Data & data,
const Model::FrameIndex frame_id,
Motion & frame_a)
{
assert(model.check(data) && "data is not consistent with model.");
const Frame & frame = model.frames[frame_id];
const Model::JointIndex & parent = frame.parent;
frame_a = frame.placement.actInv(data.a[parent]);
}
template<ReferenceFrame rf>
inline void getFrameJacobian(const Model & model,
......
......@@ -60,6 +60,82 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
}
BOOST_AUTO_TEST_CASE ( test_single_kinematics )
{
using namespace Eigen;
using namespace se3;
se3::Model model;
se3::buildModels::humanoidSimple(model);
Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
const SE3 & framePlacement = SE3::Random();
model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
Model::FrameIndex frame_idx = model.getFrameId(frame_name);
se3::Data data(model);
se3::Data data_ref(model);
VectorXd q = VectorXd::Ones(model.nq);
q.middleRows<4> (3).normalize();
forwardKinematics(model, data, q);
frameForwardKinematics(model, data, frame_idx);
framesForwardKinematics(model, data_ref, q);
BOOST_CHECK(data.oMf[frame_idx].isApprox(data_ref.oMf[frame_idx]));
}
BOOST_AUTO_TEST_CASE ( test_velocity )
{
using namespace Eigen;
using namespace se3;
se3::Model model;
se3::buildModels::humanoidSimple(model);
Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
const SE3 & framePlacement = SE3::Random();
model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
Model::FrameIndex frame_idx = model.getFrameId(frame_name);
se3::Data data(model);
VectorXd q = VectorXd::Ones(model.nq);
q.middleRows<4> (3).normalize();
VectorXd v = VectorXd::Ones(model.nv);
forwardKinematics(model, data, q, v);
Motion vf;
getFrameVelocity(model, data, frame_idx, vf);
BOOST_CHECK(vf.isApprox(framePlacement.actInv(data.v[parent_idx])));
}
BOOST_AUTO_TEST_CASE ( test_acceleration )
{
using namespace Eigen;
using namespace se3;
se3::Model model;
se3::buildModels::humanoidSimple(model);
Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
const SE3 & framePlacement = SE3::Random();
model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
Model::FrameIndex frame_idx = model.getFrameId(frame_name);
se3::Data data(model);
VectorXd q = VectorXd::Ones(model.nq);
q.middleRows<4> (3).normalize();
VectorXd v = VectorXd::Ones(model.nv);
VectorXd a = VectorXd::Ones(model.nv);
forwardKinematics(model, data, q, v, a);
Motion af;
getFrameAcceleration(model, data, frame_idx, af);
BOOST_CHECK(af.isApprox(framePlacement.actInv(data.a[parent_idx])));
}
BOOST_AUTO_TEST_CASE ( test_jacobian )
{
......@@ -86,7 +162,7 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
BOOST_CHECK(frame.placement.isApprox_impl(framePlacement));
Data::Matrix6x Jjj(6,model.nv); Jjj.fill(0);
Data::Matrix6x Jff(6,model.nv); Jff.fill(0);
getFrameJacobian(model,data,idx,Jff);
getFrameJacobian<LOCAL>(model,data,idx,Jff);
getJointJacobian<LOCAL>(model, data_ref, parent_idx, Jjj);
Motion nu_frame = Motion(Jff*q_dot);
......
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