Verified Commit cc2e1afc authored by Gabriele Buondonno's avatar Gabriele Buondonno Committed by Justin Carpentier
Browse files

[kinematics] Add getFrameClassicalAcceleration

parent 7e76fdb7
......@@ -120,6 +120,27 @@ namespace pinocchio
const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id,
const ReferenceFrame rf = ReferenceFrame::LOCAL);
/**
* @brief Returns the "classical" acceleration of the Frame expressed in the desired reference frame.
* This is different from the "spatial" acceleration in that centrifugal effects are accounted for.
* You must first call pinocchio::forwardKinematics to update placement, velocity and acceleration 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[in] rf Reference frame in which the acceleration is expressed.
*
* @return The classical acceleration of the Frame expressed in the desired reference frame.
*
* @warning Second order forwardKinematics should have been called first
*/
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
inline MotionTpl<Scalar, Options>
getFrameClassicalAcceleration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id,
const ReferenceFrame rf = ReferenceFrame::LOCAL);
/**
* @brief Returns the jacobian of the frame expressed either expressed in the LOCAL frame coordinate system or in the WORLD coordinate system,
* depending on the value of rf.
......
......@@ -118,7 +118,25 @@ namespace pinocchio
throw std::invalid_argument("Bad reference frame.");
}
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
inline MotionTpl<Scalar, Options>
getFrameClassicalAcceleration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id,
const ReferenceFrame rf)
{
assert(model.check(data) && "data is not consistent with model.");
typedef MotionTpl<Scalar, Options> Motion;
Motion vel = getFrameVelocity(model, data, frame_id, rf);
Motion acc = getFrameAcceleration(model, data, frame_id, rf);
acc.linear() += vel.angular().cross(vel.linear());
return acc;
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xLike>
inline void getFrameJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
......
......@@ -198,6 +198,68 @@ BOOST_AUTO_TEST_CASE ( test_acceleration )
BOOST_CHECK(SE3(data_ref.oMf[frame_idx].rotation(), Eigen::Vector3d::Zero()).act(a_ref).isApprox(getFrameAcceleration(model,data,frame_idx,ReferenceFrame::LOCAL_WORLD_ALIGNED)));
}
BOOST_AUTO_TEST_CASE ( test_classic_acceleration )
{
using namespace Eigen;
using namespace pinocchio;
pinocchio::Model model;
pinocchio::buildModels::humanoidRandom(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);
pinocchio::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 vel = framePlacement.actInv(data.v[parent_idx]);
Motion acc = framePlacement.actInv(data.a[parent_idx]);
Vector3d linear;
Motion acc_classical_local = acc;
linear = acc.linear() + vel.angular().cross(vel.linear());
acc_classical_local.linear() = linear;
Motion af = getFrameClassicalAcceleration(model, data, frame_idx);
BOOST_CHECK(af.isApprox(acc_classical_local));
pinocchio::Data data_ref(model);
forwardKinematics(model, data_ref, q, v, a);
updateFramePlacements(model, data_ref);
SE3 T_ref = data_ref.oMf[frame_idx];
Motion v_ref = getFrameVelocity(model, data_ref, frame_idx);
Motion a_ref = getFrameAcceleration(model, data_ref, frame_idx);
Motion acc_classical_local_ref = a_ref;
linear = a_ref.linear() + v_ref.angular().cross(v_ref.linear());
acc_classical_local_ref.linear() = linear;
BOOST_CHECK(acc_classical_local_ref.isApprox(getFrameClassicalAcceleration(model,data,frame_idx)));
BOOST_CHECK(acc_classical_local_ref.isApprox(getFrameClassicalAcceleration(model,data,frame_idx,ReferenceFrame::LOCAL)));
Motion vel_world_ref = T_ref.act(v_ref);
Motion acc_classical_world_ref = T_ref.act(a_ref);
linear = acc_classical_world_ref.linear() + vel_world_ref.angular().cross(vel_world_ref.linear());
acc_classical_world_ref.linear() = linear;
BOOST_CHECK(acc_classical_world_ref.isApprox(getFrameClassicalAcceleration(model,data,frame_idx,ReferenceFrame::WORLD)));
Motion vel_aligned_ref = SE3(T_ref.rotation(), Eigen::Vector3d::Zero()).act(v_ref);
Motion acc_classical_aligned_ref = SE3(T_ref.rotation(), Eigen::Vector3d::Zero()).act(a_ref);
linear = acc_classical_aligned_ref.linear() + vel_aligned_ref.angular().cross(vel_aligned_ref.linear());
acc_classical_aligned_ref.linear() = linear;
BOOST_CHECK(acc_classical_aligned_ref.isApprox(getFrameClassicalAcceleration(model,data,frame_idx,ReferenceFrame::LOCAL_WORLD_ALIGNED)));
}
BOOST_AUTO_TEST_CASE ( test_get_frame_jacobian )
{
using namespace Eigen;
......
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