Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/joint-planar.cpp	Lines:	71	71	100.0 %
Date:	2024-04-26 13:14:21	Branches:	288	576	50.0 %


//
// Copyright (c) 2015-2020 CNRS INRIA
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//

#include "pinocchio/math/fwd.hpp"
#include "pinocchio/multibody/joint/joints.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"

#include <boost/test/unit_test.hpp>
#include <iostream>

using namespace pinocchio;

template<typename D>
void addJointAndBody(Model & model,
                     const JointModelBase<D> & jmodel,
                     const Model::JointIndex parent_id,
                     const SE3 & joint_placement,
                     const std::string & joint_name,
                     const Inertia & Y)
{
  Model::JointIndex idx;

  idx = model.addJoint(parent_id,jmodel,joint_placement,joint_name);
  model.appendBodyToJoint(idx,Y);
}

BOOST_AUTO_TEST_SUITE(JointPlanar)

BOOST_AUTO_TEST_CASE(spatial)
{
  SE3 M(SE3::Random());
  Motion v(Motion::Random());

  MotionPlanar mp(1.,2.,3.);
  Motion mp_dense(mp);

  BOOST_CHECK(M.act(mp).isApprox(M.act(mp_dense)));
  BOOST_CHECK(M.actInv(mp).isApprox(M.actInv(mp_dense)));

  BOOST_CHECK(v.cross(mp).isApprox(v.cross(mp_dense)));
}

BOOST_AUTO_TEST_CASE(vsFreeFlyer)
{
  using namespace pinocchio;
  typedef SE3::Vector3 Vector3;
  typedef Eigen::Matrix <double, 6, 1> Vector6;
  typedef Eigen::Matrix <double, 4, 1> VectorPl;
  typedef Eigen::Matrix <double, 7, 1> VectorFF;
  typedef SE3::Matrix3 Matrix3;

  Model modelPlanar, modelFreeflyer;

  Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
  SE3 pos(1); pos.translation() = SE3::LinearType(1.,0.,0.);

  addJointAndBody(modelPlanar,JointModelPlanar(),0,SE3::Identity(),"planar",inertia);
  addJointAndBody(modelFreeflyer,JointModelFreeFlyer(),0,SE3::Identity(),"free-flyer",inertia);

  Data dataPlanar(modelPlanar);
  Data dataFreeFlyer(modelFreeflyer);

  VectorPl q; q << 1, 1, 0, 1; // Angle is PI /2;
  VectorFF qff; qff << 1, 1, 0, 0, 0, sqrt(2)/2, sqrt(2)/2 ;
  Eigen::VectorXd v = Eigen::VectorXd::Ones(modelPlanar.nv);
  Vector6 vff; vff << 1, 1, 0, 0, 0, 1;
  Eigen::VectorXd tauPlanar = Eigen::VectorXd::Ones(modelPlanar.nv);
  Eigen::VectorXd tauff = Eigen::VectorXd::Ones(modelFreeflyer.nv);
  Eigen::VectorXd aPlanar = Eigen::VectorXd::Ones(modelPlanar.nv);
  Eigen::VectorXd aff(vff);

  forwardKinematics(modelPlanar, dataPlanar, q, v);
  forwardKinematics(modelFreeflyer, dataFreeFlyer, qff, vff);

  computeAllTerms(modelPlanar, dataPlanar, q, v);
  computeAllTerms(modelFreeflyer, dataFreeFlyer, qff, vff);

  BOOST_CHECK(dataFreeFlyer.oMi[1].isApprox(dataPlanar.oMi[1]));
  BOOST_CHECK(dataFreeFlyer.liMi[1].isApprox(dataPlanar.liMi[1]));
  BOOST_CHECK(dataFreeFlyer.Ycrb[1].matrix().isApprox(dataPlanar.Ycrb[1].matrix()));
  BOOST_CHECK(dataFreeFlyer.f[1].toVector().isApprox(dataPlanar.f[1].toVector()));

  Eigen::VectorXd nle_expected_ff(3); nle_expected_ff << dataFreeFlyer.nle[0],
                                                         dataFreeFlyer.nle[1],
                                                         dataFreeFlyer.nle[5]
                                                         ;
  BOOST_CHECK(nle_expected_ff.isApprox(dataPlanar.nle));
  BOOST_CHECK(dataFreeFlyer.com[0].isApprox(dataPlanar.com[0]));

  // InverseDynamics == rnea
  tauPlanar = rnea(modelPlanar, dataPlanar, q, v, aPlanar);
  tauff = rnea(modelFreeflyer, dataFreeFlyer, qff, vff, aff);

  Vector3 tau_expected; tau_expected << tauff(0), tauff(1), tauff(5);
  BOOST_CHECK(tauPlanar.isApprox(tau_expected));

  // ForwardDynamics == aba
  Eigen::VectorXd aAbaPlanar = aba(modelPlanar,dataPlanar, q, v, tauPlanar);
  Eigen::VectorXd aAbaFreeFlyer = aba(modelFreeflyer,dataFreeFlyer, qff, vff, tauff);
  Vector3 a_expected; a_expected << aAbaFreeFlyer[0],
                                    aAbaFreeFlyer[1],
                                    aAbaFreeFlyer[5]
                                    ;
  BOOST_CHECK(aAbaPlanar.isApprox(a_expected));

  // crba
  crba(modelPlanar, dataPlanar,q);
  crba(modelFreeflyer, dataFreeFlyer, qff);

  Eigen::Matrix<double, 3, 3> M_expected;
  M_expected.block<2,2>(0,0) = dataFreeFlyer.M.block<2,2>(0,0);
  M_expected.block<1,2>(2,0) = dataFreeFlyer.M.block<1,2>(5,0);
  M_expected.block<2,1>(0,2) = dataFreeFlyer.M.col(5).head<2>();
  M_expected.block<1,1>(2,2) = dataFreeFlyer.M.col(5).tail<1>();

  BOOST_CHECK(dataPlanar.M.isApprox(M_expected));

  // Jacobian
  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian_planar;jacobian_planar.resize(6,3); jacobian_planar.setZero();
  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian_ff;jacobian_ff.resize(6,6);jacobian_ff.setZero();
  computeJointJacobians(modelPlanar, dataPlanar, q);
  computeJointJacobians(modelFreeflyer, dataFreeFlyer, qff);
  getJointJacobian(modelPlanar, dataPlanar, 1, LOCAL, jacobian_planar);
  getJointJacobian(modelFreeflyer, dataFreeFlyer, 1, LOCAL, jacobian_ff);

  Eigen::Matrix<double, 6, 3> jacobian_expected; jacobian_expected << jacobian_ff.col(0),
                                                                      jacobian_ff.col(1),
                                                                      jacobian_ff.col(5)
                                                                      ;

  BOOST_CHECK(jacobian_planar.isApprox(jacobian_expected));
}

BOOST_AUTO_TEST_SUITE_END()


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2015-2020 CNRS INRIA
3			// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4			//
5
6			#include "pinocchio/math/fwd.hpp"
7			#include "pinocchio/multibody/joint/joints.hpp"
8			#include "pinocchio/algorithm/rnea.hpp"
9			#include "pinocchio/algorithm/aba.hpp"
10			#include "pinocchio/algorithm/crba.hpp"
11			#include "pinocchio/algorithm/jacobian.hpp"
12			#include "pinocchio/algorithm/compute-all-terms.hpp"
13
14			#include <boost/test/unit_test.hpp>
15			#include <iostream>
16
17			using namespace pinocchio;
18
19			template<typename D>
20		4	void addJointAndBody(Model & model,
21			const JointModelBase<D> & jmodel,
22			const Model::JointIndex parent_id,
23			const SE3 & joint_placement,
24			const std::string & joint_name,
25			const Inertia & Y)
26			{
27			Model::JointIndex idx;
28
29	✓✗	4	idx = model.addJoint(parent_id,jmodel,joint_placement,joint_name);
30	✓✗	4	model.appendBodyToJoint(idx,Y);
31		4	}
32
33			BOOST_AUTO_TEST_SUITE(JointPlanar)
34
35	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(spatial)
36			{
37	✓✗	2	SE3 M(SE3::Random());
38	✓✗	2	Motion v(Motion::Random());
39
40	✓✗	2	MotionPlanar mp(1.,2.,3.);
41	✓✗	2	Motion mp_dense(mp);
42
43	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(M.act(mp).isApprox(M.act(mp_dense)));
44	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(M.actInv(mp).isApprox(M.actInv(mp_dense)));
45
46	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(v.cross(mp).isApprox(v.cross(mp_dense)));
47		2	}
48
49	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(vsFreeFlyer)
50			{
51			using namespace pinocchio;
52			typedef SE3::Vector3 Vector3;
53			typedef Eigen::Matrix <double, 6, 1> Vector6;
54			typedef Eigen::Matrix <double, 4, 1> VectorPl;
55			typedef Eigen::Matrix <double, 7, 1> VectorFF;
56			typedef SE3::Matrix3 Matrix3;
57
58	✓✗✓✗	4	Model modelPlanar, modelFreeflyer;
59
60	✓✗✓✗ ✓✗✓✗	2	Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
61	✓✗✓✗ ✓✗	2	SE3 pos(1); pos.translation() = SE3::LinearType(1.,0.,0.);
62
63	✓✗✓✗ ✓✗✓✗	2	addJointAndBody(modelPlanar,JointModelPlanar(),0,SE3::Identity(),"planar",inertia);
64	✓✗✓✗ ✓✗✓✗	2	addJointAndBody(modelFreeflyer,JointModelFreeFlyer(),0,SE3::Identity(),"free-flyer",inertia);
65
66	✓✗	4	Data dataPlanar(modelPlanar);
67	✓✗	4	Data dataFreeFlyer(modelFreeflyer);
68
69	✓✗✓✗ ✓✗✓✗ ✓✗	2	VectorPl q; q << 1, 1, 0, 1; // Angle is PI /2;
70	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	2	VectorFF qff; qff << 1, 1, 0, 0, 0, sqrt(2)/2, sqrt(2)/2 ;
71	✓✗✓✗	4	Eigen::VectorXd v = Eigen::VectorXd::Ones(modelPlanar.nv);
72	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	2	Vector6 vff; vff << 1, 1, 0, 0, 0, 1;
73	✓✗✓✗	4	Eigen::VectorXd tauPlanar = Eigen::VectorXd::Ones(modelPlanar.nv);
74	✓✗✓✗	4	Eigen::VectorXd tauff = Eigen::VectorXd::Ones(modelFreeflyer.nv);
75	✓✗✓✗	4	Eigen::VectorXd aPlanar = Eigen::VectorXd::Ones(modelPlanar.nv);
76	✓✗	4	Eigen::VectorXd aff(vff);
77
78	✓✗	2	forwardKinematics(modelPlanar, dataPlanar, q, v);
79	✓✗	2	forwardKinematics(modelFreeflyer, dataFreeFlyer, qff, vff);
80
81	✓✗	2	computeAllTerms(modelPlanar, dataPlanar, q, v);
82	✓✗	2	computeAllTerms(modelFreeflyer, dataFreeFlyer, qff, vff);
83
84	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(dataFreeFlyer.oMi[1].isApprox(dataPlanar.oMi[1]));
85	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(dataFreeFlyer.liMi[1].isApprox(dataPlanar.liMi[1]));
86	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(dataFreeFlyer.Ycrb[1].matrix().isApprox(dataPlanar.Ycrb[1].matrix()));
87	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(dataFreeFlyer.f[1].toVector().isApprox(dataPlanar.f[1].toVector()));
88
89	✓✗✓✗ ✓✗	6	Eigen::VectorXd nle_expected_ff(3); nle_expected_ff << dataFreeFlyer.nle[0],
90	✓✗✓✗	2	dataFreeFlyer.nle[1],
91	✓✗✓✗	2	dataFreeFlyer.nle[5]
92			;
93	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(nle_expected_ff.isApprox(dataPlanar.nle));
94	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(dataFreeFlyer.com[0].isApprox(dataPlanar.com[0]));
95
96			// InverseDynamics == rnea
97	✓✗✓✗	2	tauPlanar = rnea(modelPlanar, dataPlanar, q, v, aPlanar);
98	✓✗✓✗	2	tauff = rnea(modelFreeflyer, dataFreeFlyer, qff, vff, aff);
99
100	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	2	Vector3 tau_expected; tau_expected << tauff(0), tauff(1), tauff(5);
101	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(tauPlanar.isApprox(tau_expected));
102
103			// ForwardDynamics == aba
104	✓✗✓✗	4	Eigen::VectorXd aAbaPlanar = aba(modelPlanar,dataPlanar, q, v, tauPlanar);
105	✓✗✓✗	4	Eigen::VectorXd aAbaFreeFlyer = aba(modelFreeflyer,dataFreeFlyer, qff, vff, tauff);
106	✓✗✓✗ ✓✗	4	Vector3 a_expected; a_expected << aAbaFreeFlyer[0],
107	✓✗✓✗	2	aAbaFreeFlyer[1],
108	✓✗✓✗	2	aAbaFreeFlyer[5]
109			;
110	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(aAbaPlanar.isApprox(a_expected));
111
112			// crba
113	✓✗	2	crba(modelPlanar, dataPlanar,q);
114	✓✗	2	crba(modelFreeflyer, dataFreeFlyer, qff);
115
116	✓✗	2	Eigen::Matrix<double, 3, 3> M_expected;
117	✓✗✓✗ ✓✗	2	M_expected.block<2,2>(0,0) = dataFreeFlyer.M.block<2,2>(0,0);
118	✓✗✓✗ ✓✗	2	M_expected.block<1,2>(2,0) = dataFreeFlyer.M.block<1,2>(5,0);
119	✓✗✓✗ ✓✗✓✗	2	M_expected.block<2,1>(0,2) = dataFreeFlyer.M.col(5).head<2>();
120	✓✗✓✗ ✓✗✓✗	2	M_expected.block<1,1>(2,2) = dataFreeFlyer.M.col(5).tail<1>();
121
122	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(dataPlanar.M.isApprox(M_expected));
123
124			// Jacobian
125	✓✗✓✗ ✓✗	4	Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian_planar;jacobian_planar.resize(6,3); jacobian_planar.setZero();
126	✓✗✓✗ ✓✗	4	Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian_ff;jacobian_ff.resize(6,6);jacobian_ff.setZero();
127	✓✗	2	computeJointJacobians(modelPlanar, dataPlanar, q);
128	✓✗	2	computeJointJacobians(modelFreeflyer, dataFreeFlyer, qff);
129	✓✗	2	getJointJacobian(modelPlanar, dataPlanar, 1, LOCAL, jacobian_planar);
130	✓✗	2	getJointJacobian(modelFreeflyer, dataFreeFlyer, 1, LOCAL, jacobian_ff);
131
132	✓✗✓✗ ✓✗	4	Eigen::Matrix<double, 6, 3> jacobian_expected; jacobian_expected << jacobian_ff.col(0),
133	✓✗✓✗	2	jacobian_ff.col(1),
134	✓✗✓✗	2	jacobian_ff.col(5)
135			;
136
137	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(jacobian_planar.isApprox(jacobian_expected));
138		2	}
139
140			BOOST_AUTO_TEST_SUITE_END()