Skip to content
GitLab
Commit f7b9f1f6 authored by Olivier Stasse's avatar Olivier Stasse Committed by olivier stasse
Browse files

[unitTesting] Fix test_device.cpp

parent a6ba57f8
Hide whitespace changes
Inline Side-by-side
src/tools/device.cpp
View file @ f7b9f1f6
......@@ -597,7 +597,7 @@ void Device::integrate( const double & dt )
else{
// Position integration
state_.tail(controlIN.size()) += vel_control_ * dt;
}
}
// Position bounds check
if (sanityCheck_) {
......
unitTesting/CMakeLists.txt
View file @ f7b9f1f6
......@@ -61,6 +61,10 @@ SET(TEST_test_control_pd_LIBS
control-pd
)
SET(TEST_test_feature_generic_INC
pinocchio
)
SET(TEST_test_filter_differentiator_LIBS
filter-differentiator
)
......@@ -69,6 +73,7 @@ SET(TEST_test_madgwick_ahrs_LIBS
madgwickahrs
)
#test paths and names (without .cpp extension)
SET (tests
dummy
......@@ -77,10 +82,10 @@ SET (tests
features/test_feature_point6d
features/test_feature_generic
filters/test_filter_differentiator
filters/test_madgwick_ahrs
signal/test_signal
signal/test_depend
signal/test_ptr
......@@ -98,7 +103,7 @@ SET (tests
task/test_task
tools/test_boost
tools/test_device
tools/test_device
tools/test_mailbox
tools/test_matrix
tools/test_robot_utils
......@@ -143,6 +148,10 @@ FOREACH(test ${tests})
TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} ${PROJECT_NAME})
ADD_DEPENDENCIES (${EXECUTABLE_NAME} ${PROJECT_NAME})
IF( TEST_${EXECUTABLE_NAME}_INC )
PKG_CONFIG_USE_DEPENDENCY(${EXECUTABLE_NAME} pinocchio)
ENDIF(TEST_${EXECUTABLE_NAME}_INC)
IF( TEST_${EXECUTABLE_NAME}_LIBS )
TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} ${TEST_${EXECUTABLE_NAME}_LIBS})
ADD_DEPENDENCIES (${EXECUTABLE_NAME} ${TEST_${EXECUTABLE_NAME}_LIBS})
......@@ -157,6 +166,7 @@ FOREACH(test ${tests})
ENDIF(UNIX)
PKG_CONFIG_USE_DEPENDENCY(${EXECUTABLE_NAME} dynamic-graph)
IF(BUILD_PYTHON_INTERFACE)
PKG_CONFIG_USE_DEPENDENCY(${EXECUTABLE_NAME} dynamic-graph-python)
ENDIF(BUILD_PYTHON_INTERFACE)
......
unitTesting/features/test_feature_point6d.cpp
View file @ f7b9f1f6
......@@ -30,7 +30,7 @@ class TestPoint6d
int time_;
int dim_,robotDim_,featureDim_;
dynamicgraph::Vector manual_;
TestPoint6d(unsigned dim,std::string &name):
feature_("feature"+name),
featureDes_("featureDes"+name),
......@@ -62,15 +62,15 @@ class TestPoint6d
feature_.positionSIN = s;
feature_.positionSIN.access(time_);
feature_.positionSIN.setReady();
featureDes_.positionSIN = sd;
featureDes_.positionSIN.access(time_);
featureDes_.positionSIN.setReady();
featureDes_.velocitySIN = vd;
featureDes_.velocitySIN.access(time_);
featureDes_.velocitySIN.setReady();
task_.controlGainSIN = gain;
task_.controlGainSIN.access(time_);
task_.controlGainSIN.setReady();
......@@ -91,12 +91,12 @@ class TestPoint6d
int recompute()
{
feature_.errorSOUT.recompute(time_);
feature_.errordotSOUT.recompute(time_);
task_.taskSOUT.recompute(time_);
task_.errorSOUT.recompute(time_);
task_.errorTimeDerivativeSOUT.recompute(time_);
task_.errorTimeDerivativeSOUT.recompute(time_);
time_++;
MatrixHomogeneous s = feature_.positionSIN;
MatrixHomogeneous sd = featureDes_.positionSIN;
......@@ -105,7 +105,8 @@ class TestPoint6d
dynamicgraph::Vector manual;
const std::vector<dynamicgraph::sot::MultiBound> & taskTaskSOUT
=task_.taskSOUT(time_);
/// Verify the computation of the desired frame.
/// -gain *(s-sd) - ([w]x (sd -s)-vd)
dynamicgraph::Matrix aM;
......@@ -117,9 +118,10 @@ class TestPoint6d
aM(2,0) = -vd(4); aM(2,1) = vd(3); aM(2,2) = 0.0;
for(unsigned int i=0;i<3;i++)
aV(i) = sd(i,3)-s(i,3);
aV = aM*aV;
/// Recompute error_th.
for(unsigned int i=0;i<3;i++)
{
manual_[i] = - gain*(s(i,3)-sd(i,3)) - ( aV(i)-vd(i));
......@@ -138,11 +140,11 @@ class TestPoint6d
std::cout << "feature.errordotSOUT: " << feature_.errordotSOUT(time_).transpose()
<< std::endl;
std::cout << "task.taskSOUT: " << task_.taskSOUT(time_)
<< std::endl;
<< std::endl;
// std::cout << "task.errorSOUT: " << task_.errorSOUT(time_)
//<< std::endl;
//std::cout << "task.errordtSOUT: " << task_.errorTimeDerivativeSOUT(time_)
//<< std::endl;
//<< std::endl;
std::cout << "manual: " << manual_.transpose() << std::endl;
}
......@@ -173,28 +175,28 @@ int main( void )
double gain;
// Result of test
int r;
TestPoint6d testFeaturePoint6d(dim,srobot);
std::cout << " ----- Test Velocity -----" << std::endl;
s .setIdentity();
sd.setIdentity();
vd.setConstant(1.);
gain=0.0;
if ((r=testFeaturePoint6d.runTest(s,sd,vd,gain))<0)
{
std::cerr << "Failure on 1st test." << std::endl;
return r;
}
std::cout << " ----- Test Position -----" << std::endl;
s .setIdentity();
sd.setIdentity();
sd.translation()[2] = 2.0;
vd.setZero();
gain=1.0;
if ((r=testFeaturePoint6d.runTest(s,sd,vd,gain))<0)
{
std::cerr << "Failure on 2nd test." << std::endl;
......@@ -207,7 +209,7 @@ int main( void )
sd.translation()[2] = 2.0;
vd.setConstant(1.);
gain=3.0;
if ((r=testFeaturePoint6d.runTest(s,sd,vd,gain))<0)
{
std::cerr << "Failure on 3th test." << std::endl;
......@@ -220,7 +222,7 @@ int main( void )
sd.translation()[2] = 2.0;
vd.setConstant(1.);
gain=3.0;
if ((r=testFeaturePoint6d.runTest(s,sd,vd,gain))<0)
{
std::cerr << "Failure on 4th test." << std::endl;
......
unitTesting/tools/test_device.cpp
View file @ f7b9f1f6
......@@ -32,37 +32,68 @@ public:
~TestDevice() {}
};
BOOST_AUTO_TEST_CASE(test_device) {
TestDevice aDevice(std::string("simple_humanoid"));
/// Fix constant vector for the control entry in position
dg::Vector aStateVector(38);
dg::Vector aVelocityVector(38);
dg::Vector aLowerVelBound(38), anUpperVelBound(38);
dg::Vector aLowerBound(38), anUpperBound(38);
dg::Vector anAccelerationVector(38);
dg::Vector aControlVector(38);
for (unsigned int i = 0; i < 38; i++) {
// Specify lower velocity bound
aLowerVelBound[i] = -3.14;
aStateVector[i] = -0.5;
// Specify lower position bound
aLowerBound[i]=-3.14;
// Specify state vector
aStateVector[i] = 0.1;
// Specify upper velocity bound
anUpperVelBound[i] = 3.14;
aControlVector(i)=-0.1;
// Specify upper position bound
anUpperBound[i]=3.14;
// Specify control vector
aControlVector(i)= 0.1;
}
/// Specify state size
aDevice.setStateSize(38);
/// Specify state bounds
aDevice.setPositionBounds(aLowerBound,anUpperBound);
/// Specify velocity size
aDevice.setVelocitySize(38);
aDevice.setVelocityBounds(aLowerVelBound, anUpperVelBound);
/// Specify velocity
aDevice.setVelocity(aStateVector);
/// Specify velocity bounds
aDevice.setVelocityBounds(aLowerVelBound, anUpperVelBound);
/// Specify current state value
aDevice.setState(aStateVector); // entry signal in position
/// Specify constant control value
aDevice.controlSIN.setConstant(aControlVector);
for (unsigned int i = 0; i < 2000; i++) {
aDevice.stateSOUT.recompute(i);
double dt=0.001;
aDevice.increment(dt);
if (i == 0)
{
aDevice.stateSOUT.get(std::cout);
std::ostringstream anoss;
aDevice.stateSOUT.get(anoss);
for (unsigned int i = 0; i < 38; i++)
aControlVector[i]= 0.5;
}
if (i == 1)
std::cout << "First integration:\n" << aDevice.stateSOUT(i).transpose() << std::endl;
{
aDevice.stateSOUT.get(std::cout);
std::ostringstream anoss;
aDevice.stateSOUT.get(anoss);
}
}
aDevice.display(std::cout);
aDevice.cmdDisplay();
// const dg::Vector &aControl = aDevice.motorcontrolSOUT(2001);
// double diff = 0, ldiff;
}
Supports Markdown
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