Commit 4142668c authored by Rohan Budhiraja's avatar Rohan Budhiraja
Browse files

[unittest/frames] add unittest for framejacobiantimederivative

parent f8c5cee4
......@@ -23,12 +23,20 @@
#include "pinocchio/spatial/act-on-set.hpp"
#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/utils/timer.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include <iostream>
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
template<typename Derived>
inline bool isFinite(const Eigen::MatrixBase<Derived> & x)
{
return ((x - x).array() == (x - x).array()).all();
}
BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
BOOST_AUTO_TEST_CASE ( test_kinematics )
......@@ -96,5 +104,111 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
BOOST_CHECK(Jff.isApprox(Jjj));
}
BOOST_AUTO_TEST_CASE ( test_frame_jacobian_time_variation )
{
using namespace Eigen;
using namespace se3;
se3::Model model;
se3::buildModels::humanoidSimple(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));
se3::Data data(model);
se3::Data data_ref(model);
VectorXd q = randomConfiguration(model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq) );
VectorXd v = VectorXd::Random(model.nv);
VectorXd a = VectorXd::Random(model.nv);
computeJointJacobiansTimeVariation(model,data,q,v);
forwardKinematics(model,data_ref,q,v,a);
framesForwardKinematics(model,data_ref);
framesForwardKinematics(model,data);
BOOST_CHECK(isFinite(data.dJ));
Model::Index idx = model.getFrameId(frame_name);
const Frame & frame = model.frames[idx];
BOOST_CHECK(frame.placement.isApprox_impl(framePlacement));
Data::Matrix6x J(6,model.nv); J.fill(0.);
Data::Matrix6x dJ(6,model.nv); dJ.fill(0.);
// Regarding to the world origin
getFrameJacobian<WORLD>(model,data,idx,J);
getFrameJacobianTimeVariation<WORLD>(model,data,idx,dJ);
Motion v_idx(J*v);
const Motion & v_ref_local = frame.placement.actInv(data_ref.v[parent_idx]);
const Motion & v_ref = data_ref.oMf[idx].act(v_ref_local);
BOOST_CHECK(v_idx.isApprox(v_ref));
Motion a_idx(J*a + dJ*v);
const Motion & a_ref_local = frame.placement.actInv(data_ref.a[parent_idx]);
const Motion & a_ref = data_ref.oMf[idx].act(a_ref_local);
BOOST_CHECK(a_idx.isApprox(a_ref));
J.fill(0.); dJ.fill(0.);
// Regarding to the local frame
getFrameJacobian<LOCAL>(model,data,idx,J);
getFrameJacobianTimeVariation<LOCAL>(model,data,idx,dJ);
v_idx = (Motion::Vector6)(J*v);
BOOST_CHECK(v_idx.isApprox(v_ref_local));
a_idx = (Motion::Vector6)(J*a + dJ*v);
BOOST_CHECK(a_idx.isApprox(a_ref_local));
// compare to finite differencies
{
Data data_ref(model), data_ref_plus(model);
const double alpha = 1e-8;
Eigen::VectorXd q_plus(model.nq);
q_plus = integrate(model,q,alpha*v);
//data_ref
Data::Matrix6x J_ref_world(6,model.nv), J_ref_local(6,model.nv);
J_ref_world.fill(0.); J_ref_local.fill(0.);
computeJointJacobians(model,data_ref,q);
framesForwardKinematics(model,data_ref);
const SE3 & oMf_q = data_ref.oMf[idx];
getFrameJacobian<WORLD>(model,data_ref,idx,J_ref_world);
getFrameJacobian<LOCAL>(model,data_ref,idx,J_ref_local);
//data_ref_plus
Data::Matrix6x J_ref_plus_world(6,model.nv), J_ref_plus_local(6,model.nv);
J_ref_plus_world.fill(0.); J_ref_plus_local.fill(0.);
computeJointJacobians(model,data_ref_plus,q_plus);
framesForwardKinematics(model,data_ref_plus);
const SE3 & oMf_qplus = data_ref_plus.oMf[idx];
getFrameJacobian<WORLD>(model,data_ref_plus,idx,J_ref_plus_world);
getFrameJacobian<LOCAL>(model,data_ref_plus,idx,J_ref_plus_local);
//Move J_ref_plus_local to reference frame
J_ref_plus_local = (oMf_q.inverse()*oMf_qplus).toActionMatrix()*(J_ref_plus_local);
Data::Matrix6x dJ_ref_world(6,model.nv), dJ_ref_local(6,model.nv);
dJ_ref_world.fill(0.); dJ_ref_local.fill(0.);
dJ_ref_world = (J_ref_plus_world - J_ref_world)/alpha;
dJ_ref_local = (J_ref_plus_local - J_ref_local)/alpha;
//data
computeJointJacobiansTimeVariation(model,data,q,v);
forwardKinematics(model,data,q,v);
framesForwardKinematics(model,data);
Data::Matrix6x dJ_world(6,model.nv), dJ_local(6,model.nv);
dJ_world.fill(0.); dJ_local.fill(0.);
getFrameJacobianTimeVariation<WORLD>(model,data,idx,dJ_world);
getFrameJacobianTimeVariation<LOCAL>(model,data,idx,dJ_local);
BOOST_CHECK(dJ_world.isApprox(dJ_ref_world,sqrt(alpha)));
BOOST_CHECK(dJ_local.isApprox(dJ_ref_local,sqrt(alpha)));
}
}
BOOST_AUTO_TEST_SUITE_END ()
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