test_control_admittance.cpp
Go to the documentation of this file.
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 
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 BOOST_AUTO_TEST_CASE(control_admittance) {
33  new sot::core::AdmittanceControlOpPoint("acontrol_admittance");
34 
35  std::istringstream Kp("[6](10.0,10.0,10.0,10.0,10.0,10.0)");
36  std::istringstream Kd("[6](0.0,0.0,0.0,0.0,0.0,0.0)");
37  aControlAdm->m_KpSIN.set(Kp);
38  aControlAdm->m_KdSIN.set(Kd);
39  std::istringstream dqSaturation("[6](10.0,10.0,10.0,10.0,10.0,10.0)");
40  aControlAdm->m_dqSaturationSIN.set(dqSaturation);
41  std::istringstream w_forceDes("[6](100.0,0.0,0.0,0.0,0.0,0.0)");
42  aControlAdm->m_w_forceDesSIN.set(w_forceDes);
43  std::istringstream force("[6](10.0,0.0,10.0,0.0,0.0,0.0)");
44  aControlAdm->m_forceSIN.set(force);
45  MatrixHomogeneous opPose;
46  opPose.translation() << 0.3, 0.0, 0.0;
47  opPose.linear() << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0;
48  aControlAdm->m_opPoseSIN = opPose;
49  MatrixHomogeneous sensorPose;
50  sensorPose.translation() << 0.3, 0.0, 0.0;
51  sensorPose.linear() << 0.0, 0.0, -1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 0.0;
52  aControlAdm->m_sensorPoseSIN = sensorPose;
53  aControlAdm->init(0.001);
54 
55  aControlAdm->m_dqSOUT.recompute(0);
56  {
57  dynamicgraph::Vector expected(6);
58  expected << 1.1, 0.0, -0.109, 0.0, 0.03, 0.0;
59  BOOST_CHECK(aControlAdm->m_dqSOUT(0).isApprox(expected));
60  }
61 }
Eigen::VectorXd Vector
Admittance controller for an operational point wrt to a force sensor. It can be a point of the model ...
Eigen::Transform< double, 3, Eigen::Affine > MatrixHomogeneous
void init(const double &dt)
Initialize the entity.
BOOST_AUTO_TEST_CASE(control_admittance)


sot-core
Author(s): Olivier Stasse, ostasse@laas.fr
autogenerated on Wed Jun 21 2023 02:51:26