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

[frames] Add optional reference frame to getFrameAcceleration

parent 1b932680
......@@ -101,14 +101,15 @@ namespace pinocchio
const ReferenceFrame rf = ReferenceFrame::LOCAL);
/**
* @brief Returns the spatial acceleration of the frame expressed in the LOCAL frame coordinate system.
* You must first call pinocchio::forwardKinematics to update placement values in data structure.
* @brief Returns the spatial acceleration of the Frame expressed in the desired reference frame.
* 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 spatial acceleration of the Frame expressed in the coordinates Frame.
* @return The spatial acceleration of the Frame expressed in the desired reference frame.
*
* @warning Second order forwardKinematics should have been called first
*/
......@@ -116,7 +117,8 @@ namespace pinocchio
inline MotionTpl<Scalar, Options>
getFrameAcceleration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id);
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,
......
......@@ -94,15 +94,29 @@ namespace pinocchio
inline MotionTpl<Scalar, Options>
getFrameAcceleration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id)
const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id,
const ReferenceFrame rf)
{
assert(model.check(data) && "data is not consistent with model.");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::Motion Motion;
const typename Model::Frame & frame = model.frames[frame_id];
const typename Model::JointIndex & parent = frame.parent;
return frame.placement.actInv(data.a[parent]);
const typename Model::SE3 & oMi = data.oMi[frame.parent];
const typename Model::Motion & a = data.a[frame.parent];
switch(rf)
{
case ReferenceFrame::LOCAL:
return frame.placement.actInv(a);
case ReferenceFrame::WORLD:
return oMi.act(a);
case ReferenceFrame::LOCAL_WORLD_ALIGNED:
return Motion(oMi.rotation() * (a.linear() + a.angular().cross(frame.placement.translation())),
oMi.rotation() * a.angular());
default:
throw std::invalid_argument("Bad reference frame.");
}
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xLike>
......
......@@ -186,6 +186,16 @@ BOOST_AUTO_TEST_CASE ( test_acceleration )
Motion af = getFrameAcceleration(model, data, frame_idx);
BOOST_CHECK(af.isApprox(framePlacement.actInv(data.a[parent_idx])));
pinocchio::Data data_ref(model);
forwardKinematics(model, data_ref, q, v, a);
updateFramePlacements(model, data_ref);
Motion a_ref = getFrameAcceleration(model, data_ref, frame_idx);
BOOST_CHECK(a_ref.isApprox(getFrameAcceleration(model,data,frame_idx)));
BOOST_CHECK(a_ref.isApprox(getFrameAcceleration(model,data,frame_idx,ReferenceFrame::LOCAL)));
BOOST_CHECK(data_ref.oMf[frame_idx].act(a_ref).isApprox(getFrameAcceleration(model,data,frame_idx,ReferenceFrame::WORLD)));
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_get_frame_jacobian )
......
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