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

[kinematics] Remove ReferenceFrame:: prefix

parent 72f7da1e
......@@ -98,7 +98,7 @@ namespace pinocchio
getFrameVelocity(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);
const ReferenceFrame rf = LOCAL);
/**
* @brief Returns the spatial acceleration of the Frame expressed in the desired reference frame.
......@@ -118,7 +118,7 @@ namespace pinocchio
getFrameAcceleration(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);
const ReferenceFrame rf = LOCAL);
/**
* @brief Returns the "classical" acceleration of the Frame expressed in the desired reference frame.
......@@ -139,7 +139,7 @@ namespace pinocchio
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);
const ReferenceFrame rf = LOCAL);
/**
* @brief Returns the jacobian of the frame expressed either expressed in the LOCAL frame coordinate system or in the WORLD coordinate system,
......
......@@ -78,11 +78,11 @@ namespace pinocchio
const typename Model::Motion & v = data.v[frame.parent];
switch(rf)
{
case ReferenceFrame::LOCAL:
case LOCAL:
return frame.placement.actInv(v);
case ReferenceFrame::WORLD:
case WORLD:
return oMi.act(v);
case ReferenceFrame::LOCAL_WORLD_ALIGNED:
case LOCAL_WORLD_ALIGNED:
return Motion(oMi.rotation() * (v.linear() + v.angular().cross(frame.placement.translation())),
oMi.rotation() * v.angular());
default:
......@@ -107,11 +107,11 @@ namespace pinocchio
const typename Model::Motion & a = data.a[frame.parent];
switch(rf)
{
case ReferenceFrame::LOCAL:
case LOCAL:
return frame.placement.actInv(a);
case ReferenceFrame::WORLD:
case WORLD:
return oMi.act(a);
case ReferenceFrame::LOCAL_WORLD_ALIGNED:
case LOCAL_WORLD_ALIGNED:
return Motion(oMi.rotation() * (a.linear() + a.angular().cross(frame.placement.translation())),
oMi.rotation() * a.angular());
default:
......
......@@ -98,7 +98,7 @@ namespace pinocchio
getVelocity(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const JointIndex jointId,
const ReferenceFrame rf = ReferenceFrame::LOCAL);
const ReferenceFrame rf = LOCAL);
/**
* @brief Returns the spatial acceleration of the joint expressed in the desired reference frame.
......@@ -118,7 +118,7 @@ namespace pinocchio
getAcceleration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const JointIndex jointId,
const ReferenceFrame rf = ReferenceFrame::LOCAL);
const ReferenceFrame rf = LOCAL);
/**
* @brief Returns the "classical" acceleration of the joint expressed in the desired reference frame.
......@@ -139,7 +139,7 @@ namespace pinocchio
getClassicalAcceleration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const JointIndex jointId,
const ReferenceFrame rf = ReferenceFrame::LOCAL);
const ReferenceFrame rf = LOCAL);
} // namespace pinocchio
......
......@@ -230,11 +230,11 @@ namespace pinocchio
PINOCCHIO_UNUSED_VARIABLE(model);
switch(rf)
{
case ReferenceFrame::LOCAL:
case LOCAL:
return data.v[jointId];
case ReferenceFrame::WORLD:
case WORLD:
return data.oMi[jointId].act(data.v[jointId]);
case ReferenceFrame::LOCAL_WORLD_ALIGNED:
case LOCAL_WORLD_ALIGNED:
return MotionTpl<Scalar, Options>(data.oMi[jointId].rotation() * data.v[jointId].linear(), data.oMi[jointId].rotation() * data.v[jointId].angular());
default:
throw std::invalid_argument("Bad reference frame.");
......@@ -252,11 +252,11 @@ namespace pinocchio
PINOCCHIO_UNUSED_VARIABLE(model);
switch(rf)
{
case ReferenceFrame::LOCAL:
case LOCAL:
return data.a[jointId];
case ReferenceFrame::WORLD:
case WORLD:
return data.oMi[jointId].act(data.a[jointId]);
case ReferenceFrame::LOCAL_WORLD_ALIGNED:
case LOCAL_WORLD_ALIGNED:
return MotionTpl<Scalar, Options>(data.oMi[jointId].rotation() * data.a[jointId].linear(), data.oMi[jointId].rotation() * data.a[jointId].angular());
default:
throw std::invalid_argument("Bad reference frame.");
......
......@@ -158,9 +158,9 @@ BOOST_AUTO_TEST_CASE ( test_velocity )
Motion v_ref = getFrameVelocity(model, data_ref, frame_idx);
BOOST_CHECK(v_ref.isApprox(getFrameVelocity(model,data,frame_idx)));
BOOST_CHECK(v_ref.isApprox(getFrameVelocity(model,data,frame_idx,ReferenceFrame::LOCAL)));
BOOST_CHECK(data_ref.oMf[frame_idx].act(v_ref).isApprox(getFrameVelocity(model,data,frame_idx,ReferenceFrame::WORLD)));
BOOST_CHECK(SE3(data_ref.oMf[frame_idx].rotation(), Eigen::Vector3d::Zero()).act(v_ref).isApprox(getFrameVelocity(model,data,frame_idx,ReferenceFrame::LOCAL_WORLD_ALIGNED)));
BOOST_CHECK(v_ref.isApprox(getFrameVelocity(model,data,frame_idx,LOCAL)));
BOOST_CHECK(data_ref.oMf[frame_idx].act(v_ref).isApprox(getFrameVelocity(model,data,frame_idx,WORLD)));
BOOST_CHECK(SE3(data_ref.oMf[frame_idx].rotation(), Eigen::Vector3d::Zero()).act(v_ref).isApprox(getFrameVelocity(model,data,frame_idx,LOCAL_WORLD_ALIGNED)));
}
BOOST_AUTO_TEST_CASE ( test_acceleration )
......@@ -193,9 +193,9 @@ BOOST_AUTO_TEST_CASE ( test_acceleration )
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_CHECK(a_ref.isApprox(getFrameAcceleration(model,data,frame_idx,LOCAL)));
BOOST_CHECK(data_ref.oMf[frame_idx].act(a_ref).isApprox(getFrameAcceleration(model,data,frame_idx,WORLD)));
BOOST_CHECK(SE3(data_ref.oMf[frame_idx].rotation(), Eigen::Vector3d::Zero()).act(a_ref).isApprox(getFrameAcceleration(model,data,frame_idx,LOCAL_WORLD_ALIGNED)));
}
BOOST_AUTO_TEST_CASE ( test_classic_acceleration )
......@@ -243,21 +243,21 @@ BOOST_AUTO_TEST_CASE ( test_classic_acceleration )
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)));
BOOST_CHECK(acc_classical_local_ref.isApprox(getFrameClassicalAcceleration(model,data,frame_idx,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)));
BOOST_CHECK(acc_classical_world_ref.isApprox(getFrameClassicalAcceleration(model,data,frame_idx,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_CHECK(acc_classical_aligned_ref.isApprox(getFrameClassicalAcceleration(model,data,frame_idx,LOCAL_WORLD_ALIGNED)));
}
BOOST_AUTO_TEST_CASE(test_frame_getters)
......@@ -311,21 +311,21 @@ BOOST_AUTO_TEST_CASE(test_frame_getters)
// Check output velocity
BOOST_CHECK(v_local.isApprox(getFrameVelocity(model,data,frameId)));
BOOST_CHECK(v_local.isApprox(getFrameVelocity(model,data,frameId,ReferenceFrame::LOCAL)));
BOOST_CHECK(v_world.isApprox(getFrameVelocity(model,data,frameId,ReferenceFrame::WORLD)));
BOOST_CHECK(v_align.isApprox(getFrameVelocity(model,data,frameId,ReferenceFrame::LOCAL_WORLD_ALIGNED)));
BOOST_CHECK(v_local.isApprox(getFrameVelocity(model,data,frameId,LOCAL)));
BOOST_CHECK(v_world.isApprox(getFrameVelocity(model,data,frameId,WORLD)));
BOOST_CHECK(v_align.isApprox(getFrameVelocity(model,data,frameId,LOCAL_WORLD_ALIGNED)));
// Check output acceleration (all zero)
BOOST_CHECK(getFrameAcceleration(model,data,frameId).isZero());
BOOST_CHECK(getFrameAcceleration(model,data,frameId,ReferenceFrame::LOCAL).isZero());
BOOST_CHECK(getFrameAcceleration(model,data,frameId,ReferenceFrame::WORLD).isZero());
BOOST_CHECK(getFrameAcceleration(model,data,frameId,ReferenceFrame::LOCAL_WORLD_ALIGNED).isZero());
BOOST_CHECK(getFrameAcceleration(model,data,frameId,LOCAL).isZero());
BOOST_CHECK(getFrameAcceleration(model,data,frameId,WORLD).isZero());
BOOST_CHECK(getFrameAcceleration(model,data,frameId,LOCAL_WORLD_ALIGNED).isZero());
// Check output classical acceleration
BOOST_CHECK(ac_local.isApprox(getFrameClassicalAcceleration(model,data,frameId)));
BOOST_CHECK(ac_local.isApprox(getFrameClassicalAcceleration(model,data,frameId,ReferenceFrame::LOCAL)));
BOOST_CHECK(ac_world.isApprox(getFrameClassicalAcceleration(model,data,frameId,ReferenceFrame::WORLD)));
BOOST_CHECK(ac_align.isApprox(getFrameClassicalAcceleration(model,data,frameId,ReferenceFrame::LOCAL_WORLD_ALIGNED)));
BOOST_CHECK(ac_local.isApprox(getFrameClassicalAcceleration(model,data,frameId,LOCAL)));
BOOST_CHECK(ac_world.isApprox(getFrameClassicalAcceleration(model,data,frameId,WORLD)));
BOOST_CHECK(ac_align.isApprox(getFrameClassicalAcceleration(model,data,frameId,LOCAL_WORLD_ALIGNED)));
}
BOOST_AUTO_TEST_CASE ( test_get_frame_jacobian )
......
......@@ -126,9 +126,9 @@ BOOST_AUTO_TEST_CASE(test_get_velocity)
for(Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
{
BOOST_CHECK(data.v[i].isApprox(getVelocity(model,data,i)));
BOOST_CHECK(data.v[i].isApprox(getVelocity(model,data,i,ReferenceFrame::LOCAL)));
BOOST_CHECK(data.oMi[i].act(data.v[i]).isApprox(getVelocity(model,data,i,ReferenceFrame::WORLD)));
BOOST_CHECK(SE3(data.oMi[i].rotation(), Eigen::Vector3d::Zero()).act(data.v[i]).isApprox(getVelocity(model,data,i,ReferenceFrame::LOCAL_WORLD_ALIGNED)));
BOOST_CHECK(data.v[i].isApprox(getVelocity(model,data,i,LOCAL)));
BOOST_CHECK(data.oMi[i].act(data.v[i]).isApprox(getVelocity(model,data,i,WORLD)));
BOOST_CHECK(SE3(data.oMi[i].rotation(), Eigen::Vector3d::Zero()).act(data.v[i]).isApprox(getVelocity(model,data,i,LOCAL_WORLD_ALIGNED)));
}
}
......@@ -153,9 +153,9 @@ BOOST_AUTO_TEST_CASE(test_get_acceleration)
for(Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
{
BOOST_CHECK(data.a[i].isApprox(getAcceleration(model,data,i)));
BOOST_CHECK(data.a[i].isApprox(getAcceleration(model,data,i,ReferenceFrame::LOCAL)));
BOOST_CHECK(data.oMi[i].act(data.a[i]).isApprox(getAcceleration(model,data,i,ReferenceFrame::WORLD)));
BOOST_CHECK(SE3(data.oMi[i].rotation(), Eigen::Vector3d::Zero()).act(data.a[i]).isApprox(getAcceleration(model,data,i,ReferenceFrame::LOCAL_WORLD_ALIGNED)));
BOOST_CHECK(data.a[i].isApprox(getAcceleration(model,data,i,LOCAL)));
BOOST_CHECK(data.oMi[i].act(data.a[i]).isApprox(getAcceleration(model,data,i,WORLD)));
BOOST_CHECK(SE3(data.oMi[i].rotation(), Eigen::Vector3d::Zero()).act(data.a[i]).isApprox(getAcceleration(model,data,i,LOCAL_WORLD_ALIGNED)));
}
}
......@@ -189,21 +189,21 @@ BOOST_AUTO_TEST_CASE(test_get_classical_acceleration)
acc_classical_local.linear() = linear;
BOOST_CHECK(acc_classical_local.isApprox(getClassicalAcceleration(model,data,i)));
BOOST_CHECK(acc_classical_local.isApprox(getClassicalAcceleration(model,data,i,ReferenceFrame::LOCAL)));
BOOST_CHECK(acc_classical_local.isApprox(getClassicalAcceleration(model,data,i,LOCAL)));
Motion vel_world = T.act(vel);
Motion acc_classical_world = T.act(acc);
linear = acc_classical_world.linear() + vel_world.angular().cross(vel_world.linear());
acc_classical_world.linear() = linear;
BOOST_CHECK(acc_classical_world.isApprox(getClassicalAcceleration(model,data,i,ReferenceFrame::WORLD)));
BOOST_CHECK(acc_classical_world.isApprox(getClassicalAcceleration(model,data,i,WORLD)));
Motion vel_aligned = SE3(data.oMi[i].rotation(), Eigen::Vector3d::Zero()).act(vel);
Motion acc_classical_aligned = SE3(data.oMi[i].rotation(), Eigen::Vector3d::Zero()).act(acc);
linear = acc_classical_aligned.linear() + vel_aligned.angular().cross(vel_aligned.linear());
acc_classical_aligned.linear() = linear;
BOOST_CHECK(acc_classical_aligned.isApprox(getClassicalAcceleration(model,data,i,ReferenceFrame::LOCAL_WORLD_ALIGNED)));
BOOST_CHECK(acc_classical_aligned.isApprox(getClassicalAcceleration(model,data,i,LOCAL_WORLD_ALIGNED)));
}
}
......@@ -259,21 +259,21 @@ BOOST_AUTO_TEST_CASE(test_kinematic_getters)
// Check output velocity
BOOST_CHECK(v_local.isApprox(getVelocity(model,data,jointId)));
BOOST_CHECK(v_local.isApprox(getVelocity(model,data,jointId,ReferenceFrame::LOCAL)));
BOOST_CHECK(v_world.isApprox(getVelocity(model,data,jointId,ReferenceFrame::WORLD)));
BOOST_CHECK(v_align.isApprox(getVelocity(model,data,jointId,ReferenceFrame::LOCAL_WORLD_ALIGNED)));
BOOST_CHECK(v_local.isApprox(getVelocity(model,data,jointId,LOCAL)));
BOOST_CHECK(v_world.isApprox(getVelocity(model,data,jointId,WORLD)));
BOOST_CHECK(v_align.isApprox(getVelocity(model,data,jointId,LOCAL_WORLD_ALIGNED)));
// Check output acceleration (all zero)
BOOST_CHECK(getAcceleration(model,data,jointId).isZero());
BOOST_CHECK(getAcceleration(model,data,jointId,ReferenceFrame::LOCAL).isZero());
BOOST_CHECK(getAcceleration(model,data,jointId,ReferenceFrame::WORLD).isZero());
BOOST_CHECK(getAcceleration(model,data,jointId,ReferenceFrame::LOCAL_WORLD_ALIGNED).isZero());
BOOST_CHECK(getAcceleration(model,data,jointId,LOCAL).isZero());
BOOST_CHECK(getAcceleration(model,data,jointId,WORLD).isZero());
BOOST_CHECK(getAcceleration(model,data,jointId,LOCAL_WORLD_ALIGNED).isZero());
// Check output classical
BOOST_CHECK(ac_local.isApprox(getClassicalAcceleration(model,data,jointId)));
BOOST_CHECK(ac_local.isApprox(getClassicalAcceleration(model,data,jointId,ReferenceFrame::LOCAL)));
BOOST_CHECK(ac_world.isApprox(getClassicalAcceleration(model,data,jointId,ReferenceFrame::WORLD)));
BOOST_CHECK(ac_align.isApprox(getClassicalAcceleration(model,data,jointId,ReferenceFrame::LOCAL_WORLD_ALIGNED)));
BOOST_CHECK(ac_local.isApprox(getClassicalAcceleration(model,data,jointId,LOCAL)));
BOOST_CHECK(ac_world.isApprox(getClassicalAcceleration(model,data,jointId,WORLD)));
BOOST_CHECK(ac_align.isApprox(getClassicalAcceleration(model,data,jointId,LOCAL_WORLD_ALIGNED)));
}
BOOST_AUTO_TEST_SUITE_END()
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