Verified Commit 85274b5b authored by Justin Carpentier's avatar Justin Carpentier
Browse files

test/frames-der: add tests for getFrameAccelerationDerivatives

parent e3fa2be2
......@@ -129,4 +129,261 @@ BOOST_AUTO_TEST_CASE(test_frames_derivatives_velocity)
}
BOOST_AUTO_TEST_CASE(test_kinematics_derivatives_acceleration)
{
using namespace Eigen;
using namespace pinocchio;
Model model;
buildModels::humanoidRandom(model);
const Model::JointIndex jointId = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
Frame frame("rand",jointId,0,SE3::Random(),OP_FRAME);
FrameIndex frameId = model.addFrame(frame);
BOOST_CHECK(model.getFrameId("rand") == frameId);
BOOST_CHECK(model.frames[frameId].parent == jointId);
Data data(model), data_ref(model);
model.lowerPositionLimit.head<3>().fill(-1.);
model.upperPositionLimit.head<3>().fill(1.);
VectorXd q = randomConfiguration(model);
VectorXd v(VectorXd::Random(model.nv));
VectorXd a(VectorXd::Random(model.nv));
computeForwardKinematicsDerivatives(model,data,q,v,a);
Data::Matrix6x v_partial_dq(6,model.nv); v_partial_dq.setZero();
Data::Matrix6x v_partial_dq_local(6,model.nv); v_partial_dq_local.setZero();
Data::Matrix6x v_partial_dq_local_world_aligned(6,model.nv); v_partial_dq_local_world_aligned.setZero();
Data::Matrix6x a_partial_dq(6,model.nv); a_partial_dq.setZero();
Data::Matrix6x a_partial_dq_local_world_aligned(6,model.nv); a_partial_dq_local_world_aligned.setZero();
Data::Matrix6x a_partial_dq_local(6,model.nv); a_partial_dq_local.setZero();
Data::Matrix6x a_partial_dv(6,model.nv); a_partial_dv.setZero();
Data::Matrix6x a_partial_dv_local_world_aligned(6,model.nv); a_partial_dv_local_world_aligned.setZero();
Data::Matrix6x a_partial_dv_local(6,model.nv); a_partial_dv_local.setZero();
Data::Matrix6x a_partial_da(6,model.nv); a_partial_da.setZero();
Data::Matrix6x a_partial_da_local_world_aligned(6,model.nv); a_partial_da_local_world_aligned.setZero();
Data::Matrix6x a_partial_da_local(6,model.nv); a_partial_da_local.setZero();
getFrameAccelerationDerivatives(model,data,frameId,WORLD,
v_partial_dq,
a_partial_dq,a_partial_dv,a_partial_da);
getFrameAccelerationDerivatives(model,data,frameId,LOCAL_WORLD_ALIGNED,
v_partial_dq_local_world_aligned,
a_partial_dq_local_world_aligned,
a_partial_dv_local_world_aligned,
a_partial_da_local_world_aligned);
getFrameAccelerationDerivatives(model,data,frameId,LOCAL,
v_partial_dq_local,
a_partial_dq_local,a_partial_dv_local,a_partial_da_local);
// Check v_partial_dq against getFrameVelocityDerivatives
{
Data data_v(model);
computeForwardKinematicsDerivatives(model,data_v,q,v,a);
Data::Matrix6x v_partial_dq_ref(6,model.nv); v_partial_dq_ref.setZero();
Data::Matrix6x v_partial_dq_ref_local_world_aligned(6,model.nv); v_partial_dq_ref_local_world_aligned.setZero();
Data::Matrix6x v_partial_dq_ref_local(6,model.nv); v_partial_dq_ref_local.setZero();
Data::Matrix6x v_partial_dv_ref(6,model.nv); v_partial_dv_ref.setZero();
Data::Matrix6x v_partial_dv_ref_local_world_aligned(6,model.nv); v_partial_dv_ref_local_world_aligned.setZero();
Data::Matrix6x v_partial_dv_ref_local(6,model.nv); v_partial_dv_ref_local.setZero();
getFrameVelocityDerivatives(model,data_v,frameId,WORLD,
v_partial_dq_ref,v_partial_dv_ref);
BOOST_CHECK(v_partial_dq.isApprox(v_partial_dq_ref));
BOOST_CHECK(a_partial_da.isApprox(v_partial_dv_ref));
getFrameVelocityDerivatives(model,data_v,frameId,LOCAL_WORLD_ALIGNED,
v_partial_dq_ref_local_world_aligned,v_partial_dv_ref_local_world_aligned);
BOOST_CHECK(v_partial_dq_local_world_aligned.isApprox(v_partial_dq_ref_local_world_aligned));
BOOST_CHECK(a_partial_da_local_world_aligned.isApprox(v_partial_dv_ref_local_world_aligned));
getFrameVelocityDerivatives(model,data_v,frameId,LOCAL,
v_partial_dq_ref_local,v_partial_dv_ref_local);
BOOST_CHECK(v_partial_dq_local.isApprox(v_partial_dq_ref_local));
BOOST_CHECK(a_partial_da_local.isApprox(v_partial_dv_ref_local));
}
Data::Matrix6x J_ref(6,model.nv); J_ref.setZero();
Data::Matrix6x J_ref_local(6,model.nv); J_ref_local.setZero();
Data::Matrix6x J_ref_local_world_aligned(6,model.nv); J_ref_local_world_aligned.setZero();
computeJointJacobians(model,data_ref,q);
getFrameJacobian(model,data_ref,frameId,WORLD,J_ref);
getFrameJacobian(model,data_ref,frameId,LOCAL_WORLD_ALIGNED,J_ref_local_world_aligned);
getFrameJacobian(model,data_ref,frameId,LOCAL,J_ref_local);
BOOST_CHECK(a_partial_da.isApprox(J_ref));
BOOST_CHECK(a_partial_da_local_world_aligned.isApprox(J_ref_local_world_aligned));
BOOST_CHECK(a_partial_da_local.isApprox(J_ref_local));
// Check against finite differences
Data::Matrix6x a_partial_da_fd(6,model.nv); a_partial_da_fd.setZero();
Data::Matrix6x a_partial_da_fd_local_world_aligned(6,model.nv); a_partial_da_fd_local_world_aligned.setZero();
Data::Matrix6x a_partial_da_fd_local(6,model.nv); a_partial_da_fd_local.setZero();
const double alpha = 1e-8;
Eigen::VectorXd v_plus(v), a_plus(a);
Data data_plus(model);
forwardKinematics(model,data_ref,q,v,a);
// dacc/da
Motion a0 = getFrameAcceleration(model,data,frameId,WORLD);
Motion a0_local_world_aligned = getFrameAcceleration(model,data,frameId,LOCAL_WORLD_ALIGNED);
Motion a0_local = getFrameAcceleration(model,data,frameId,LOCAL);
for(int k = 0; k < model.nv; ++k)
{
a_plus[k] += alpha;
forwardKinematics(model,data_plus,q,v,a_plus);
a_partial_da_fd.col(k) = (getFrameAcceleration(model,data_plus,frameId,WORLD) - a0).toVector()/alpha;
a_partial_da_fd_local_world_aligned.col(k) = (getFrameAcceleration(model,data_plus,frameId,LOCAL_WORLD_ALIGNED) - a0_local_world_aligned).toVector()/alpha;
a_partial_da_fd_local.col(k) = (getFrameAcceleration(model,data_plus,frameId,LOCAL) - a0_local).toVector()/alpha;
a_plus[k] -= alpha;
}
BOOST_CHECK(a_partial_da.isApprox(a_partial_da_fd,sqrt(alpha)));
BOOST_CHECK(a_partial_da_local_world_aligned.isApprox(a_partial_da_fd_local_world_aligned,sqrt(alpha)));
BOOST_CHECK(a_partial_da_local.isApprox(a_partial_da_fd_local,sqrt(alpha)));
// dacc/dv
Data::Matrix6x a_partial_dv_fd(6,model.nv); a_partial_dv_fd.setZero();
Data::Matrix6x a_partial_dv_fd_local_world_aligned(6,model.nv); a_partial_dv_fd_local_world_aligned.setZero();
Data::Matrix6x a_partial_dv_fd_local(6,model.nv); a_partial_dv_fd_local.setZero();
for(int k = 0; k < model.nv; ++k)
{
v_plus[k] += alpha;
forwardKinematics(model,data_plus,q,v_plus,a);
a_partial_dv_fd.col(k) = (getFrameAcceleration(model,data_plus,frameId,WORLD) - a0).toVector()/alpha; a_partial_dv_fd_local_world_aligned.col(k) = (getFrameAcceleration(model,data_plus,frameId,LOCAL_WORLD_ALIGNED) - a0_local_world_aligned).toVector()/alpha;
a_partial_dv_fd_local.col(k) = (getFrameAcceleration(model,data_plus,frameId,LOCAL) - a0_local).toVector()/alpha;
v_plus[k] -= alpha;
}
BOOST_CHECK(a_partial_dv.isApprox(a_partial_dv_fd,sqrt(alpha)));
BOOST_CHECK(a_partial_dv_local_world_aligned.isApprox(a_partial_dv_fd_local_world_aligned,sqrt(alpha)));
BOOST_CHECK(a_partial_dv_local.isApprox(a_partial_dv_fd_local,sqrt(alpha)));
// dacc/dq
a_partial_dq.setZero();
a_partial_dv.setZero();
a_partial_da.setZero();
a_partial_dq_local_world_aligned.setZero();
a_partial_dv_local_world_aligned.setZero();
a_partial_da_local_world_aligned.setZero();
a_partial_dq_local.setZero();
a_partial_dv_local.setZero();
a_partial_da_local.setZero();
Data::Matrix6x a_partial_dq_fd(6,model.nv); a_partial_dq_fd.setZero();
Data::Matrix6x a_partial_dq_fd_local_world_aligned(6,model.nv); a_partial_dq_fd_local_world_aligned.setZero();
Data::Matrix6x a_partial_dq_fd_local(6,model.nv); a_partial_dq_fd_local.setZero();
computeForwardKinematicsDerivatives(model,data,q,v,a);
getFrameAccelerationDerivatives(model,data,frameId,WORLD,
v_partial_dq,
a_partial_dq,a_partial_dv,a_partial_da);
getFrameAccelerationDerivatives(model,data,frameId,LOCAL_WORLD_ALIGNED,
v_partial_dq_local_world_aligned,
a_partial_dq_local_world_aligned,
a_partial_dv_local_world_aligned,
a_partial_da_local_world_aligned);
getFrameAccelerationDerivatives(model,data,frameId,LOCAL,
v_partial_dq_local,
a_partial_dq_local,a_partial_dv_local,a_partial_da_local);
Eigen::VectorXd q_plus(q), v_eps(Eigen::VectorXd::Zero(model.nv));
forwardKinematics(model,data_ref,q,v,a);
updateFramePlacements(model,data_ref);
a0 = getFrameAcceleration(model,data,frameId,WORLD);
a0_local_world_aligned = getFrameAcceleration(model,data,frameId,LOCAL_WORLD_ALIGNED);
a0_local = getFrameAcceleration(model,data,frameId,LOCAL);
for(int k = 0; k < model.nv; ++k)
{
v_eps[k] += alpha;
q_plus = integrate(model,q,v_eps);
forwardKinematics(model,data_plus,q_plus,v,a);
updateFramePlacements(model,data_plus);
a_partial_dq_fd.col(k) = (getFrameAcceleration(model,data_plus,frameId,WORLD) - a0).toVector()/alpha;
Motion a_plus_local_world_aligned = getFrameAcceleration(model,data_plus,frameId,LOCAL_WORLD_ALIGNED);
const SE3::Vector3 trans = data_plus.oMf[frameId].translation() - data_ref.oMf[frameId].translation();
a_plus_local_world_aligned.linear() -= a_plus_local_world_aligned.angular().cross(trans);
a_partial_dq_fd_local_world_aligned.col(k) = (a_plus_local_world_aligned - a0_local_world_aligned).toVector()/alpha;
a_partial_dq_fd_local.col(k) = (getFrameAcceleration(model,data_plus,frameId,LOCAL) - a0_local).toVector()/alpha;
v_eps[k] -= alpha;
}
BOOST_CHECK(a_partial_dq.isApprox(a_partial_dq_fd,sqrt(alpha)));
BOOST_CHECK(a_partial_dq_local_world_aligned.isApprox(a_partial_dq_fd_local_world_aligned,sqrt(alpha)));
BOOST_CHECK(a_partial_dq_local.isApprox(a_partial_dq_fd_local,sqrt(alpha)));
// Test other signatures
Data::Matrix6x v_partial_dq_other(6,model.nv); v_partial_dq_other.setZero();
Data::Matrix6x v_partial_dq_local_other(6,model.nv); v_partial_dq_local_other.setZero();
Data::Matrix6x v_partial_dq_local_world_aligned_other(6,model.nv); v_partial_dq_local_world_aligned_other.setZero();
Data::Matrix6x v_partial_dv_other(6,model.nv); v_partial_dv_other.setZero();
Data::Matrix6x v_partial_dv_local_other(6,model.nv); v_partial_dv_local_other.setZero();
Data::Matrix6x v_partial_dv_local_world_aligned_other(6,model.nv); v_partial_dv_local_world_aligned_other.setZero();
Data::Matrix6x a_partial_dq_other(6,model.nv); a_partial_dq_other.setZero();
Data::Matrix6x a_partial_dq_local_world_aligned_other(6,model.nv); a_partial_dq_local_world_aligned_other.setZero();
Data::Matrix6x a_partial_dq_local_other(6,model.nv); a_partial_dq_local_other.setZero();
Data::Matrix6x a_partial_dv_other(6,model.nv); a_partial_dv_other.setZero();
Data::Matrix6x a_partial_dv_local_world_aligned_other(6,model.nv); a_partial_dv_local_world_aligned_other.setZero();
Data::Matrix6x a_partial_dv_local_other(6,model.nv); a_partial_dv_local_other.setZero();
Data::Matrix6x a_partial_da_other(6,model.nv); a_partial_da_other.setZero();
Data::Matrix6x a_partial_da_local_world_aligned_other(6,model.nv); a_partial_da_local_world_aligned_other.setZero();
Data::Matrix6x a_partial_da_local_other(6,model.nv); a_partial_da_local_other.setZero();
getFrameAccelerationDerivatives(model,data,frameId,WORLD,
v_partial_dq_other,
v_partial_dv_other,
a_partial_dq_other,
a_partial_dv_other,
a_partial_da_other);
BOOST_CHECK(v_partial_dq_other.isApprox(v_partial_dq));
BOOST_CHECK(v_partial_dv_other.isApprox(a_partial_da));
BOOST_CHECK(a_partial_dq_other.isApprox(a_partial_dq));
BOOST_CHECK(a_partial_dv_other.isApprox(a_partial_dv));
BOOST_CHECK(a_partial_da_other.isApprox(a_partial_da));
getFrameAccelerationDerivatives(model,data,frameId,LOCAL_WORLD_ALIGNED,
v_partial_dq_local_world_aligned_other,
v_partial_dv_local_world_aligned_other,
a_partial_dq_local_world_aligned_other,
a_partial_dv_local_world_aligned_other,
a_partial_da_local_world_aligned_other);
BOOST_CHECK(v_partial_dq_local_world_aligned_other.isApprox(v_partial_dq_local_world_aligned));
BOOST_CHECK(v_partial_dv_local_world_aligned_other.isApprox(a_partial_da_local_world_aligned));
BOOST_CHECK(a_partial_dq_local_world_aligned_other.isApprox(a_partial_dq_local_world_aligned));
BOOST_CHECK(a_partial_dv_local_world_aligned_other.isApprox(a_partial_dv_local_world_aligned));
BOOST_CHECK(a_partial_da_local_world_aligned_other.isApprox(a_partial_da_local_world_aligned));
getFrameAccelerationDerivatives(model,data,frameId,LOCAL,
v_partial_dq_local_other,
v_partial_dv_local_other,
a_partial_dq_local_other,
a_partial_dv_local_other,
a_partial_da_local_other);
BOOST_CHECK(v_partial_dq_local_other.isApprox(v_partial_dq_local));
BOOST_CHECK(v_partial_dv_local_other.isApprox(a_partial_da_local));
BOOST_CHECK(a_partial_dq_local_other.isApprox(a_partial_dq_local));
BOOST_CHECK(a_partial_dv_local_other.isApprox(a_partial_dv_local));
BOOST_CHECK(a_partial_da_local_other.isApprox(a_partial_da_local));
}
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