Verified Commit c57f79a3 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

test/der: minor clean

parent 1d1a5d47
......@@ -84,8 +84,6 @@ BOOST_AUTO_TEST_CASE(test_frames_derivatives_velocity)
Eigen::VectorXd v_plus(v);
Data data_plus(model);
forwardKinematics(model,data_ref,q,v);
SE3 oMi_rot(SE3::Identity());
oMi_rot.rotation() = data_ref.oMi[jointId].rotation();
Motion v0 = getFrameVelocity(model,data,frameId,WORLD);
Motion v0_local_world_aligned = getFrameVelocity(model,data,frameId,LOCAL_WORLD_ALIGNED);
Motion v0_local = getFrameVelocity(model,data,frameId,LOCAL);
......@@ -116,9 +114,6 @@ BOOST_AUTO_TEST_CASE(test_frames_derivatives_velocity)
forwardKinematics(model,data_plus,q_plus,v);
updateFramePlacements(model,data_plus);
SE3 oMi_plus_rot = data_plus.oMi[jointId];
oMi_plus_rot.translation().setZero();
Motion v_plus_local_world_aligned = getFrameVelocity(model,data_plus,frameId,LOCAL_WORLD_ALIGNED);
SE3::Vector3 trans = data_plus.oMf[frameId].translation() - data_ref.oMf[frameId].translation();
v_plus_local_world_aligned.linear() -= v_plus_local_world_aligned.angular().cross(trans);
......
......@@ -159,14 +159,6 @@ BOOST_AUTO_TEST_CASE(test_kinematics_derivatives_velocity)
BOOST_CHECK(partial_dq_local_world_aligned.isApprox(partial_dq_fd_local_world_aligned,sqrt(alpha)));
BOOST_CHECK(partial_dq_local.isApprox(partial_dq_fd_local,sqrt(alpha)));
// computeJointJacobiansTimeVariation(model,data_ref,q,v);
// Data::Matrix6x dJ_ref(6,model.nv); dJ_ref.setZero();
// getJointJacobianTimeVariation<WORLD>(model,data_ref,jointId,dJ_ref);
// BOOST_CHECK(partial_dq.isApprox(dJ_ref));
//
// std::cout << "partial_dq\n" << partial_dq << std::endl;
// std::cout << "dJ_ref\n" << dJ_ref << std::endl;
}
BOOST_AUTO_TEST_CASE(test_kinematics_derivatives_acceleration)
......@@ -187,8 +179,8 @@ BOOST_AUTO_TEST_CASE(test_kinematics_derivatives_acceleration)
computeForwardKinematicsDerivatives(model,data,q,v,a);
const Model::JointIndex jointId = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
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();
......@@ -327,7 +319,6 @@ BOOST_AUTO_TEST_CASE(test_kinematics_derivatives_acceleration)
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();
// a.setZero();
computeForwardKinematicsDerivatives(model,data,q,v,a);
getJointAccelerationDerivatives(model,data,jointId,WORLD,
......@@ -371,8 +362,6 @@ BOOST_AUTO_TEST_CASE(test_kinematics_derivatives_acceleration)
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)));
std::cout << "a_partial_dq_local_world_aligned:\n" << a_partial_dq_local_world_aligned << std::endl;
std::cout << "a_partial_dq_fd_local_world_aligned:\n" << a_partial_dq_fd_local_world_aligned << std::endl;
BOOST_CHECK(a_partial_dq_local.isApprox(a_partial_dq_fd_local,sqrt(alpha)));
}
......
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