Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	tests/control/test_control_admittance.cpp	Lines:	26	26	100.0 %
Date:	2023-03-13 12:09:37	Branches:	100	200	50.0 %


/*
 * Copyright 2019,
 * Noëlie Ramuzat,
 *
 *
 */

#include <iostream>
#include <sot/core/debug.hh>

#ifndef WIN32
#include <unistd.h>
#endif

using namespace std;

#include <dynamic-graph/entity.h>
#include <dynamic-graph/factory.h>

#include <sot/core/admittance-control-op-point.hh>
#include <sstream>

using namespace dynamicgraph;
using namespace dynamicgraph::sot;

#define BOOST_TEST_MODULE debug - control - admittance

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

BOOST_AUTO_TEST_CASE(control_admittance) {
  sot::core::AdmittanceControlOpPoint *aControlAdm =
      new sot::core::AdmittanceControlOpPoint("acontrol_admittance");

  std::istringstream Kp("[6](10.0,10.0,10.0,10.0,10.0,10.0)");
  std::istringstream Kd("[6](0.0,0.0,0.0,0.0,0.0,0.0)");
  aControlAdm->m_KpSIN.set(Kp);
  aControlAdm->m_KdSIN.set(Kd);
  std::istringstream dqSaturation("[6](10.0,10.0,10.0,10.0,10.0,10.0)");
  aControlAdm->m_dqSaturationSIN.set(dqSaturation);
  std::istringstream w_forceDes("[6](100.0,0.0,0.0,0.0,0.0,0.0)");
  aControlAdm->m_w_forceDesSIN.set(w_forceDes);
  std::istringstream force("[6](10.0,0.0,10.0,0.0,0.0,0.0)");
  aControlAdm->m_forceSIN.set(force);
  MatrixHomogeneous opPose;
  opPose.translation() << 0.3, 0.0, 0.0;
  opPose.linear() << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0;
  aControlAdm->m_opPoseSIN = opPose;
  MatrixHomogeneous sensorPose;
  sensorPose.translation() << 0.3, 0.0, 0.0;
  sensorPose.linear() << 0.0, 0.0, -1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 0.0;
  aControlAdm->m_sensorPoseSIN = sensorPose;
  aControlAdm->init(0.001);

  aControlAdm->m_dqSOUT.recompute(0);
  {
    dynamicgraph::Vector expected(6);
    expected << 1.1, 0.0, -0.109, 0.0, 0.03, 0.0;
    BOOST_CHECK(aControlAdm->m_dqSOUT(0).isApprox(expected));
  }
}


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			/*
2			* Copyright 2019,
3			* Noëlie Ramuzat,
4			*
5			*
6			*/
7
8			#include <iostream>
9			#include <sot/core/debug.hh>
10
11			#ifndef WIN32
12			#include <unistd.h>
13			#endif
14
15			using namespace std;
16
17			#include <dynamic-graph/entity.h>
18			#include <dynamic-graph/factory.h>
19
20			#include <sot/core/admittance-control-op-point.hh>
21			#include <sstream>
22
23			using namespace dynamicgraph;
24			using namespace dynamicgraph::sot;
25
26			#define BOOST_TEST_MODULE debug - control - admittance
27
28			#include <boost/test/output_test_stream.hpp>
29			#include <boost/test/unit_test.hpp>
30
31	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(control_admittance) {
32			sot::core::AdmittanceControlOpPoint *aControlAdm =
33	✓✗✓✗ ✓✗	2	new sot::core::AdmittanceControlOpPoint("acontrol_admittance");
34
35	✓✗✓✗	6	std::istringstream Kp("[6](10.0,10.0,10.0,10.0,10.0,10.0)");
36	✓✗✓✗	6	std::istringstream Kd("[6](0.0,0.0,0.0,0.0,0.0,0.0)");
37	✓✗	2	aControlAdm->m_KpSIN.set(Kp);
38	✓✗	2	aControlAdm->m_KdSIN.set(Kd);
39	✓✗✓✗	6	std::istringstream dqSaturation("[6](10.0,10.0,10.0,10.0,10.0,10.0)");
40	✓✗	2	aControlAdm->m_dqSaturationSIN.set(dqSaturation);
41	✓✗✓✗	6	std::istringstream w_forceDes("[6](100.0,0.0,0.0,0.0,0.0,0.0)");
42	✓✗	2	aControlAdm->m_w_forceDesSIN.set(w_forceDes);
43	✓✗✓✗	6	std::istringstream force("[6](10.0,0.0,10.0,0.0,0.0,0.0)");
44	✓✗	2	aControlAdm->m_forceSIN.set(force);
45	✓✗	2	MatrixHomogeneous opPose;
46	✓✗✓✗ ✓✗✓✗	2	opPose.translation() << 0.3, 0.0, 0.0;
47	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	2	opPose.linear() << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0;
48	✓✗	2	aControlAdm->m_opPoseSIN = opPose;
49	✓✗	2	MatrixHomogeneous sensorPose;
50	✓✗✓✗ ✓✗✓✗	2	sensorPose.translation() << 0.3, 0.0, 0.0;
51	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	2	sensorPose.linear() << 0.0, 0.0, -1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 0.0;
52	✓✗	2	aControlAdm->m_sensorPoseSIN = sensorPose;
53	✓✗	2	aControlAdm->init(0.001);
54
55	✓✗	2	aControlAdm->m_dqSOUT.recompute(0);
56			{
57	✓✗	4	dynamicgraph::Vector expected(6);
58	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	2	expected << 1.1, 0.0, -0.109, 0.0, 0.03, 0.0;
59	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(aControlAdm->m_dqSOUT(0).isApprox(expected));
60			}
61		2	}