Commit efaf360d authored by Olivier Stasse's avatar Olivier Stasse
Browse files

[tests] Add test on feature point6d + fix other unit tests

parent 3998459a
......@@ -45,6 +45,14 @@ SET(TEST_test_task_LIBS
gain-adaptive feature-visual-point task
)
SET(TEST_test_feature_point6d_LIBS
gain-adaptive feature-point6d task
)
SET(TEST_test_feature_generic_LIBS
gain-adaptive feature-generic task
)
SET(TEST_test_mailbox_LIBS
mailbox-vector
)
......@@ -67,6 +75,9 @@ SET (tests
control/test_control_pd
features/test_feature_point6d
features/test_feature_generic
filters/test_filter_differentiator
filters/test_madgwick_ahrs
......
/*
* Copyright 2019,
* François Bleibel,
* Olivier Stasse,
*
* CNRS/AIST
*
*/
/* -------------------------------------------------------------------------- */
/* --- INCLUDES ------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
#include <iostream>
#include <sot/core/sot.hh>
#include <sot/core/feature-point6d.hh>
#include <sot/core/feature-abstract.hh>
#include <sot/core/debug.hh>
#include <sot/core/task.hh>
#include <sot/core/gain-adaptive.hh>
#include <dynamic-graph/linear-algebra.h>
using namespace std;
using namespace dynamicgraph::sot;
class TestPoint6d
{
public:
FeaturePoint6d feature_,featureDes_;
Task task_;
int time_;
int dim_,robotDim_,featureDim_;
dynamicgraph::Vector manual_;
TestPoint6d(unsigned dim,std::string &name):
feature_("feature"+name),
featureDes_("featureDes"+name),
task_("task"+name),
time_(0)
{
feature_.computationFrame("desired");
feature_.setReference(&featureDes_);
feature_.selectionSIN=Flags(true);
task_.addFeature(feature_);
task_.setWithDerivative(true);
dim_ = dim;
robotDim_ = featureDim_ = dim;
dynamicgraph::Matrix Jq(dim,dim);
Jq.setIdentity();
feature_.articularJacobianSIN.setReference(&Jq);
manual_.resize(dim);
}
void setInputs(MatrixHomogeneous & s,
MatrixHomogeneous &sd,
dynamicgraph::Vector &vd,
double gain)
{
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();
}
void printInputs()
{
std::cout << "----- inputs -----" << std::endl;
std::cout << "feature_.position: " << feature_.positionSIN(time_)
<< std::endl;
std::cout << "featureDes_.position: " << featureDes_.positionSIN(time_)
<< std::endl;
std::cout << "featureDes_.velocity: " << featureDes_.velocitySIN(time_)
<< std::endl;
std::cout << "task.controlGain: " << task_.controlGainSIN(time_)
<< std::endl;
}
int recompute()
{
feature_.errorSOUT.recompute(time_);
feature_.errordotSOUT.recompute(time_);
task_.taskSOUT.recompute(time_);
task_.errorSOUT.recompute(time_);
task_.errorTimeDerivativeSOUT.recompute(time_);
time_++;
MatrixHomogeneous s = feature_.positionSIN;
MatrixHomogeneous sd = featureDes_.positionSIN;
dynamicgraph::Vector vd = featureDes_.velocitySIN;
double gain = task_.controlGainSIN;
dynamicgraph::Vector manual;
const std::vector<dynamicgraph::sot::MultiBound> & taskTaskSOUT
=task_.taskSOUT(time_);
for(unsigned int i=0;i<3;i++)
{
manual_[i] = vd[i] + gain*(sd(i,3)-s(i,3));
if (manual_[i]!=taskTaskSOUT[i].getSingleBound())
return -1;
}
return 0;
}
void printOutput()
{
std::cout << "----- output -----" << std::endl;
std::cout << "time: " << time_ << std::endl;
std::cout << "feature.errorSOUT: " << feature_.errorSOUT(time_)
<< std::endl;
std::cout << "feature.errordotSOUT: " << feature_.errordotSOUT(time_)
<< std::endl;
std::cout << "task.taskSOUT: " << task_.taskSOUT(time_)
<< std::endl;
// std::cout << "task.errorSOUT: " << task_.errorSOUT(time_)
//<< std::endl;
//std::cout << "task.errordtSOUT: " << task_.errorTimeDerivativeSOUT(time_)
//<< std::endl;
std::cout << "manual: " << manual_ << std::endl;
}
int runTest(MatrixHomogeneous &s,
MatrixHomogeneous &sd,
dynamicgraph::Vector &vd,
double gain)
{
setInputs(s,sd,vd,gain);
printInputs();
int r= recompute();
printOutput();
return r;
}
};
int main( void )
{
// Name of the robot
std::string srobot("Robot");
// Dimension of the robot.
unsigned int dim=6;
// Feature and Desired Feature
MatrixHomogeneous s, sd;
// Desired velocity
dynamicgraph::Vector vd(6);
// Task gain.
double gain;
// Result of test
int r;
TestPoint6d testFeaturePoint6d(dim,srobot);
std::cout << " ----- Test Velocity -----" << std::endl;
s(0,0) = 1.0; s(1,1) = 1.0; s(2,2) = 1.0;
sd(0,0) = 1.0; sd(1,1) = 1.0; sd(2,2) = 1.0;
for(unsigned int i=0;i<6;i++)
vd(i)=1.0;
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(0,0) = 1.0; s(1,1) = 1.0; s(2,2) = 1.0;
sd(0,0) = 1.0; sd(1,1) = 1.0; sd(2,2) = 1.0;
sd(2,3) = 2.0;
for(unsigned int i=0;i<6;i++)
vd(i)=0.0;
gain=1.0;
if ((r=testFeaturePoint6d.runTest(s,sd,vd,gain))<0)
{
std::cerr << "Failure on 2nd test." << std::endl;
return r;
}
std::cout << " ----- Test both -----" << std::endl;
s(0,0) = 1.0; s(1,1) = 1.0; s(2,2) = 1.0;
sd(0,0) = 1.0; sd(1,1) = 1.0; sd(2,2) = 1.0;
sd(2,3) = 2.0;
for(unsigned int i=0;i<3;i++)
vd(i)=1.0;
gain=3.0;
if ((r=testFeaturePoint6d.runTest(s,sd,vd,gain))<0)
{
std::cerr << "Failure on 3th test." << std::endl;
return r;
}
std::cout << " ----- Test both again -----" << std::endl;
s(0,0) = 1.0; s(1,1) = 1.0; s(2,2) = 1.0;
sd(0,0) = 1.0; sd(1,1) = 1.0; sd(2,2) = 1.0;
sd(2,3) = 2.0;
for(unsigned int i=0;i<6;i++)
vd(i)=1.0;
gain=3.0;
if ((r=testFeaturePoint6d.runTest(s,sd,vd,gain))<0)
{
std::cerr << "Failure on 4th test." << std::endl;
return r;
}
std::cout << "Test successfull !" << std::endl;
return 0;
}
......@@ -37,8 +37,10 @@ int main( void )
srand(12);
dynamicgraph::Matrix Jq(6,6); Jq.setIdentity();
dynamicgraph::Vector p1xy(2); p1xy(0)=1.; p1xy(1)=-2;
dynamicgraph::Vector p1xy(6);
p1xy(0)=1.; p1xy(1)=-2; p1xy(2)=1.;
p1xy(3)=1.; p1xy(4)=-2; p1xy(5)=1.;
sotDEBUGF("Create feature");
FeatureVisualPoint * p1 = new FeatureVisualPoint("p1");
FeatureVisualPoint * p1des = new FeatureVisualPoint("p1d");
......@@ -48,14 +50,13 @@ int main( void )
p1->setReference(p1des);
p1->xySIN = p1xy;
p1des->xySIN = dynamicgraph::Vector(2);
p1des->xySIN = dynamicgraph::Vector(6);
sotDEBUGF("Create Task");
// sotDEBUG(0) << dynamicgraph::MATLAB;
Task * task = new Task("t");
task->addFeature(*p1);
task->addFeature(*p1);
GainAdaptive * lambda = new GainAdaptive("g");
lambda->errorSIN.plug( &task->errorSOUT );
......
......@@ -41,7 +41,8 @@ int main( void )
std::cout << "cp_get_joint_limits_for_id works !" << std::endl; }
else
{
std::cout << "ERROR: cp_get_joint_limits_for_id does not work !" << std::endl;
std::cout << "ERROR: cp_get_joint_limits_for_id does not work !"
<< std::endl;
}
......@@ -84,11 +85,13 @@ int main( void )
robot_util->joints_sot_to_urdf(q_sot, q_test_urdf);
if (q_urdf == q_test_urdf )
{
std::cout << "joints_urdf_to_sot and joints_sot_to_urdf work !" << std::endl;
std::cout << "joints_urdf_to_sot and joints_sot_to_urdf work !"
<< std::endl;
}
else
{
std::cout << "ERROR: joints_urdf_to_sot or joints_sot_to_urdf do not work !" << std::endl;
std::cout << "ERROR: joints_urdf_to_sot or joints_sot_to_urdf "
"do not work !" << std::endl;
}
/*Test velocity_sot_to_urdf and velocity_urdf_to_sot */
......@@ -97,7 +100,8 @@ int main( void )
dg::Vector v_sot(9);
robot_util->velocity_urdf_to_sot(q2_urdf, v_urdf, v_sot);
robot_util->velocity_sot_to_urdf(q2_urdf, v_sot, v_urdf);
std::cout << "velocity_sot_to_urdf and velocity_urdf_to_sot work !" << std::endl;
std::cout << "velocity_sot_to_urdf and velocity_urdf_to_sot work !"
<< std::endl;
/*Test base_urdf_to_sot and base_sot_to_urdf */
dg::Vector base_q_urdf(7);
......@@ -140,7 +144,8 @@ int main( void )
}
else
{
std::cout << "ERROR: force_util set and get id_from_name do not work !" << std::endl;
std::cout << "ERROR: force_util set and get id_from_name do not work !"
<< std::endl;
}
if( robot_util->m_force_util.get_name_from_id(0) == rf &&
robot_util->m_force_util.get_name_from_id(1) == lf &&
......@@ -151,7 +156,8 @@ int main( void )
}
else
{
std::cout << "ERROR: force_util get name_from_id does not work !" << std::endl;
std::cout << "ERROR: force_util get name_from_id does not work !"
<< std::endl;
}
if (robot_util->m_force_util.get_limits_from_id(1).upper == uf_lim &&
robot_util->m_force_util.get_limits_from_id(1).lower == lf_lim)
......@@ -160,7 +166,8 @@ int main( void )
}
else
{
std::cout << "ERROR: force_util set and get id to limits works do not work !" << std::endl;
std::cout << "ERROR: force_util set and get id to "
"limits works do not work !" << std::endl;
}
robot_util->m_force_util.display(std::cout);
robot_util->m_foot_util.display(std::cout);
......
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