Commit ce734e78 authored by Olivier Stasse's avatar Olivier Stasse Committed by olivier stasse
Browse files

Add unit test for device and feature_point6d.

parent 6abaa1e3
---
ColumnLimit: 80
...
......@@ -98,6 +98,7 @@ SET (tests
task/test_task
tools/test_boost
tools/test_device
tools/test_mailbox
tools/test_matrix
tools/test_robot_utils
......
......@@ -105,10 +105,24 @@ 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;
dynamicgraph::Vector aV;
aM.resize(3,3);
aV.resize(3);
aM(0,0) = 0.0;aM(0,1) = -vd(5); aM(0,2) = vd(4);
aM(1,0) = vd(5);aM(1,1) = 0.0; aM(1,2) =-vd(3);
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;
for(unsigned int i=0;i<3;i++)
{
manual_[i] = - gain*(s(i,3)-sd(i,3)) - (-vd(i));
manual_[i] = - gain*(s(i,3)-sd(i,3)) - ( aV(i)-vd(i));
if (manual_[i]!=taskTaskSOUT[i].getSingleBound())
return -1;
}
......
/*
* Copyright 2019,
* Olivier Stasse,
*
* CNRS/AIST
*
*/
#include <iostream>
#include <sot/core/debug.hh>
#ifndef WIN32
#include <unistd.h>
#endif
using namespace std;
#include <boost/test/unit_test.hpp>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/entity.h>
#include <sot/core/device.hh>
#include <sstream>
using namespace dynamicgraph;
using namespace dynamicgraph::sot;
class TestDevice: public dg::sot::Device
{
public:
TestDevice(const std::string & RobotName):
Device(RobotName)
{
timestep_=0.001;
}
~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 anAccelerationVector(38);
std::ostringstream ossControlVector;
std::istringstream issControlVector;
ossControlVector<<"[38](";
for (unsigned int i = 0; i < 38; i++) {
aLowerVelBound[i]= -3.14;
aStateVector[i] = -0.5;
anUpperVelBound[i]= 3.14;
ossControlVector << "-0.1";
if (i<37)
ossControlVector << ",";
}
ossControlVector << ")";
issControlVector.str(ossControlVector.str());
aDevice.setVelocitySize(38);
aDevice.setVelocityBounds(aLowerVelBound,anUpperVelBound);
aDevice.setVelocity(aStateVector);
aDevice.setState(aStateVector); // entry signal in position
aDevice.controlSIN.set(issControlVector);
for (unsigned int i = 0; i < 2000; i++)
{
aDevice.stateSOUT.recompute(i);
if (i==1)
std::cout << " First integration " << aDevice.stateSOUT << std::endl;
}
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