Skip to content

Commit

Permalink
Merge pull request #58 from robotology/feat/redballtest-gazebo
Browse files Browse the repository at this point in the history 
Upgrade the Red Ball Demo Test to the use of Gazebo simulator
  • Loading branch information
@pattacini
pattacini authored Jul 17, 2021
2 parents f5a19ba + 13614be commit e026e54
Show file tree
Hide file tree
Showing 6 changed files with 188 additions and 164 deletions.
7 changes: 3 additions & 4 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@ set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/plugins)
set(CMAKE_SHARED_MODULE_PREFIX "")

# options
option(ICUB_TESTS_USES_ICUB_MAIN "Turn on to compile the tests that depend on the icub-main repository" OFF)
option(ICUB_TESTS_USES_ICUB_MAIN "Turn on to compile the tests that depend on the icub-main repository" ON)
option(ICUB_TESTS_USES_CODYCO "Turn on to compile the test that depend on the codyco-superbuil repository" OFF)

# Build examples?
Expand Down Expand Up @@ -58,9 +58,6 @@ add_subdirectory(src/cartesian-control)
# Build gaze controller tests
add_subdirectory(src/gaze-control)

# Build demoRedBall test
add_subdirectory(src/demoRedBall)

if(ICUB_TESTS_USES_CODYCO)
add_subdirectory(src/torqueControl-gravityConsistency)
endif()
Expand All @@ -72,7 +69,9 @@ add_subdirectory(src/motorEncodersSignCheck)

# Build model consistency check
if(ICUB_TESTS_USES_ICUB_MAIN)
find_package(ICUB REQUIRED)
add_subdirectory(src/models-consistency)
add_subdirectory(src/demoRedBall)
endif()

# Build motor tests
Expand Down
3 changes: 2 additions & 1 deletion src/demoRedBall/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,8 @@ target_link_libraries(${PROJECT_NAME} RobotTestingFramework::RTF
YARP::YARP_os
YARP::YARP_init
YARP::YARP_math
YARP::YARP_robottestingframework)
YARP::YARP_robottestingframework
ICUB::ctrlLib)

# set the installation options
install(TARGETS ${PROJECT_NAME}
Expand Down
190 changes: 87 additions & 103 deletions src/demoRedBall/DemoRedBallTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,18 +18,20 @@
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*/

#include <memory>
#include <algorithm>
#include <robottestingframework/dll/Plugin.h>
#include <robottestingframework/TestAssert.h>
#include <yarp/os/Time.h>
#include <yarp/os/Network.h>
#include <yarp/os/BufferedPort.h>
#include <yarp/os/Bottle.h>
#include <yarp/os/ResourceFinder.h>
#include <yarp/dev/GazeControl.h>
#include <yarp/sig/Matrix.h>
#include <yarp/math/Math.h>

#include <iCub/ctrl/filters.h>

#include "DemoRedBallTest.h"

using namespace std;
Expand All @@ -38,107 +40,29 @@ using namespace yarp::os;
using namespace yarp::dev;
using namespace yarp::sig;
using namespace yarp::math;
using namespace iCub::ctrl;

// prepare the plugin
ROBOTTESTINGFRAMEWORK_PREPARE_PLUGIN(DemoRedBallTest)


/***********************************************************************************/
class DemoRedBallPosition : public PeriodicThread
{
string name;
IGazeControl *igaze;
string eye;
Vector pos;
bool visible;
BufferedPort<Bottle> port;

bool threadInit()
{
string dest="/demoRedBall/trackTarget:i";
port.open(("/"+name+"/redballpos:o"));
ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(Network::connect(port.getName(),dest,"udp"),
Asserter::format("Unable to connect to %s!",dest.c_str()));
return true;
}

void run()
{
if (igaze!=NULL)
{
Vector x,o;
if (eye=="left")
igaze->getLeftEyePose(x,o);
else
igaze->getRightEyePose(x,o);

Matrix T=axis2dcm(o);
T.setSubcol(x,0,3);
Vector pos_=SE3inv(T)*pos;

Bottle &cmd=port.prepare();
cmd.clear();
cmd.addDouble(pos_[0]);
cmd.addDouble(pos_[1]);
cmd.addDouble(pos_[2]);
cmd.addDouble(0.0);
cmd.addDouble(0.0);
cmd.addDouble(0.0);
cmd.addDouble(visible?1.0:0.0);
port.write();
}
}

void threadRelease()
{
port.close();
}

public:
DemoRedBallPosition(const string &name_,
PolyDriver &driver,
const string &eye_) :
PeriodicThread(0.1), name(name_),
eye(eye_), pos(4,0.0),
visible(false)
{
if (!driver.view(igaze))
igaze=NULL;
pos[3]=1.0;
}

bool setPos(const Vector &pos)
{
if (pos.length()>=3)
{
this->pos.setSubvector(0,pos.subVector(0,2));
return true;
}
else
return false;
}

void setVisible() { visible=true; }
void setInvisible() { visible=false; }
};


/***********************************************************************************/
DemoRedBallTest::DemoRedBallTest() : yarp::robottestingframework::TestCase("DemoRedBallTest"), redBallPos(NULL)
DemoRedBallTest::DemoRedBallTest() : yarp::robottestingframework::TestCase("DemoRedBallTest")
{
}


/***********************************************************************************/
DemoRedBallTest::~DemoRedBallTest()
{
delete redBallPos;
}


/***********************************************************************************/
bool DemoRedBallTest::setup(Property &property)
{
Time::useNetworkClock("/clock");

string context=property.check("context",Value("demoRedBall")).asString();
string from=property.check("from",Value("config-test.ini")).asString();

Expand Down Expand Up @@ -238,8 +162,15 @@ bool DemoRedBallTest::setup(Property &property)
"Unable to open clients for torso!");
}

redBallPos=new DemoRedBallPosition(getName(),drvGaze,params.eye);
redBallPos->start();
rpcPort.open("/"+getName()+"/rpc");
string dest="/demoRedBall/rpc";
ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(Network::connect(rpcPort.getName(),dest),
Asserter::format("Unable to connect to %s!",dest.c_str()));

guiPort.open("/"+getName()+"/gui:i");
string src="/demoRedBall/gui:o";
ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(Network::connect(src,guiPort.getName()),
Asserter::format("Unable to connect to %s!",src.c_str()));

return true;
}
Expand All @@ -248,9 +179,9 @@ bool DemoRedBallTest::setup(Property &property)
/***********************************************************************************/
void DemoRedBallTest::tearDown()
{
redBallPos->stop();

ROBOTTESTINGFRAMEWORK_TEST_REPORT("Closing Clients");
rpcPort.close();
guiPort.close();
if (params.use_left)
{
ROBOTTESTINGFRAMEWORK_ASSERT_FAIL_IF_FALSE(drvJointArmL.close()&&drvCartArmL.close(),
Expand All @@ -271,24 +202,65 @@ void DemoRedBallTest::tearDown()


/***********************************************************************************/
void DemoRedBallTest::testBallPosition(const Vector &pos)
bool DemoRedBallTest::getBallPosition(const Bottle* b, Vector& pos)
{
DemoRedBallPosition *ball=dynamic_cast<DemoRedBallPosition*>(redBallPos);
ball->setPos(pos);
ball->setVisible();
if (b->size()>=15)
{
if (b->get(0).isString() && (b->get(0).asString()=="object"))
{
pos.resize(3);
pos[0]=b->get(5).asDouble()/1000.;
pos[1]=b->get(6).asDouble()/1000.;
pos[2]=b->get(7).asDouble()/1000.;
return true;
}
}
return false;
}


/***********************************************************************************/
void DemoRedBallTest::testBallPosition(const Vector &dpos)
{
Vector x,o,encs;
int nEncs; IEncoders* ienc;
bool done=false;
double t0;

Bottle cmd,rep;
cmd.addString("update_pose");
cmd.addDouble(dpos[0]);
cmd.addDouble(dpos[1]);
cmd.addDouble(dpos[2]);
rpcPort.write(cmd,rep);

Time::delay(3.0);

cmd.clear();
cmd.addString("start");
cmd.addDouble(0.);
cmd.addDouble(-50.);
cmd.addDouble(10.);
rpcPort.write(cmd,rep);

auto filt = make_unique<MedianFilter>(5, Vector{0., 0., 0.});

IGazeControl* igaze;
drvGaze.view(igaze);

t0=Time::now();
while (Time::now()-t0<10.0)
{
if (auto* gui=guiPort.read(false))
{
Vector pos;
if (getBallPosition(gui,pos))
{
filt->filt(pos);
}
}
igaze->getFixationPoint(x);
if (norm(pos-x)<2.0*params.reach_tol)
if (norm(filt->output()-x)<2.0*params.reach_tol)
{
done=true;
break;
Expand All @@ -297,12 +269,21 @@ void DemoRedBallTest::testBallPosition(const Vector &pos)
}
ROBOTTESTINGFRAMEWORK_TEST_CHECK(done,"Ball gazed at with the eyes!");

filt->init(Vector{0., 0., 0.});
done=false;
t0=Time::now();
while (Time::now()-t0<10.0)
{
if (auto* gui=guiPort.read(false))
{
Vector pos;
if (getBallPosition(gui,pos))
{
filt->filt(pos);
}
}
arm_under_test.iarm->getPose(x,o);
if (norm(pos-x)<params.reach_tol)
if (norm(filt->output()-x)<params.reach_tol)
{
done=true;
break;
Expand All @@ -312,7 +293,9 @@ void DemoRedBallTest::testBallPosition(const Vector &pos)
ROBOTTESTINGFRAMEWORK_TEST_CHECK(done,"Ball reached with the hand!");

ROBOTTESTINGFRAMEWORK_TEST_REPORT("Going home");
ball->setInvisible();
cmd.clear();
cmd.addString("stop");
rpcPort.write(cmd,rep);

arm_under_test.ienc->getAxes(&nEncs);
encs.resize(nEncs,0.0);
Expand Down Expand Up @@ -371,26 +354,27 @@ void DemoRedBallTest::testBallPosition(const Vector &pos)

/***********************************************************************************/
void DemoRedBallTest::run()
{
Vector pos(3,0.0);
pos[0]=-0.3;

{
if (params.use_torso || params.use_left)
{
pos[1]=-0.15;
Vector dpos(3,0.0);
dpos[1]=-0.06;
dpos[2]=-0.3;
drvJointArmL.view(arm_under_test.ienc);
drvCartArmL.view(arm_under_test.iarm);
ROBOTTESTINGFRAMEWORK_TEST_REPORT("Reaching with the left hand");
testBallPosition(pos);
testBallPosition(dpos);
}

if (params.use_torso || params.use_right)
if (params.use_torso || params.use_right)
{
pos[1]=+0.15;
Vector dpos(3,0.0);
dpos[1]=+0.06;
dpos[2]=-0.3;
drvJointArmR.view(arm_under_test.ienc);
drvCartArmR.view(arm_under_test.iarm);
ROBOTTESTINGFRAMEWORK_TEST_REPORT("Reaching with the right hand");
testBallPosition(pos);
testBallPosition(dpos);
}
}

8 changes: 6 additions & 2 deletions src/demoRedBall/DemoRedBallTest.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,8 +23,10 @@

#include <string>
#include <yarp/robottestingframework/TestCase.h>
#include <yarp/os/Bottle.h>
#include <yarp/os/Property.h>
#include <yarp/os/PeriodicThread.h>
#include <yarp/os/RpcClient.h>
#include <yarp/os/BufferedPort.h>
#include <yarp/dev/PolyDriver.h>
#include <yarp/dev/IEncoders.h>
#include <yarp/dev/CartesianControl.h>
Expand Down Expand Up @@ -70,8 +72,10 @@ class DemoRedBallTest : public yarp::robottestingframework::TestCase
yarp::dev::IEncoders *ienc;
} arm_under_test;

yarp::os::PeriodicThread *redBallPos;
yarp::os::RpcClient rpcPort;
yarp::os::BufferedPort<yarp::os::Bottle> guiPort;
void testBallPosition(const yarp::sig::Vector &pos);
bool getBallPosition(const yarp::os::Bottle* b, yarp::sig::Vector& pos);

public:
DemoRedBallTest();
Expand Down
13 changes: 2 additions & 11 deletions src/models-consistency/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,17 +23,8 @@ endif()

project(iKiniDynConsistencyTest)

find_package(ICUB REQUIRED)

set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${ICUB_MODULE_PATH})

include(iCubHelpers)

set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${ICUB_LINK_FLAGS}")

# add include directories
include_directories(${ICUB_INCLUDE_DIRS})

# import math symbols from standard cmath
add_definitions(-D_USE_MATH_DEFINES)

Expand All @@ -47,8 +38,8 @@ target_link_libraries(${PROJECT_NAME} RobotTestingFramework::RTF
YARP::YARP_init
YARP::YARP_math
YARP::YARP_robottestingframework
iKin
iDyn)
ICUB::iKin
ICUB::iDyn)

# set the installation options
install(TARGETS ${PROJECT_NAME}
Expand Down
Loading

0 comments on commit e026e54

Please sign in to comment.