From 13614bed22dfa88a3c7f370a4828f79b97f808cb Mon Sep 17 00:00:00 2001 From: Ugo Pattacini Date: Mon, 12 Jul 2021 21:47:57 +0000 Subject: [PATCH] added redballtest w/ gazebo --- CMakeLists.txt | 7 +- src/demoRedBall/CMakeLists.txt | 3 +- src/demoRedBall/DemoRedBallTest.cpp | 190 ++++++++---------- src/demoRedBall/DemoRedBallTest.h | 8 +- src/models-consistency/CMakeLists.txt | 13 +- .../fixtures/icubsim-demoRedBall-fixture.xml | 131 ++++++++---- 6 files changed, 188 insertions(+), 164 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4f84ce25..73f8d327 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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? @@ -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() @@ -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 diff --git a/src/demoRedBall/CMakeLists.txt b/src/demoRedBall/CMakeLists.txt index 43a2b771..7638be06 100644 --- a/src/demoRedBall/CMakeLists.txt +++ b/src/demoRedBall/CMakeLists.txt @@ -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} diff --git a/src/demoRedBall/DemoRedBallTest.cpp b/src/demoRedBall/DemoRedBallTest.cpp index fe5a2225..cffcf2a0 100644 --- a/src/demoRedBall/DemoRedBallTest.cpp +++ b/src/demoRedBall/DemoRedBallTest.cpp @@ -18,18 +18,20 @@ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA */ +#include #include #include #include #include #include #include -#include #include #include #include #include +#include + #include "DemoRedBallTest.h" using namespace std; @@ -38,93 +40,14 @@ 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 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") { } @@ -132,13 +55,14 @@ DemoRedBallTest::DemoRedBallTest() : yarp::robottestingframework::TestCase("Demo /***********************************************************************************/ 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(); @@ -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; } @@ -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(), @@ -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(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(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; @@ -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)output()-x)setInvisible(); + cmd.clear(); + cmd.addString("stop"); + rpcPort.write(cmd,rep); arm_under_test.ienc->getAxes(&nEncs); encs.resize(nEncs,0.0); @@ -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); } } diff --git a/src/demoRedBall/DemoRedBallTest.h b/src/demoRedBall/DemoRedBallTest.h index 9b4d075b..fb0bd898 100644 --- a/src/demoRedBall/DemoRedBallTest.h +++ b/src/demoRedBall/DemoRedBallTest.h @@ -23,8 +23,10 @@ #include #include +#include #include -#include +#include +#include #include #include #include @@ -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 guiPort; void testBallPosition(const yarp::sig::Vector &pos); + bool getBallPosition(const yarp::os::Bottle* b, yarp::sig::Vector& pos); public: DemoRedBallTest(); diff --git a/src/models-consistency/CMakeLists.txt b/src/models-consistency/CMakeLists.txt index 41126479..8833af00 100644 --- a/src/models-consistency/CMakeLists.txt +++ b/src/models-consistency/CMakeLists.txt @@ -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) @@ -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} diff --git a/suites/fixtures/icubsim-demoRedBall-fixture.xml b/suites/fixtures/icubsim-demoRedBall-fixture.xml index a5ab20df..802e2ac4 100644 --- a/suites/fixtures/icubsim-demoRedBall-fixture.xml +++ b/suites/fixtures/icubsim-demoRedBall-fixture.xml @@ -6,53 +6,66 @@ Ugo Pattacini - iCub_SIM + gzserver + -s libgazebo_yarp_clock.so grasp-ball-gazebo.sdf + localhost + + + gzclient localhost - - 10 - yarprobotinterface - --context simCartesianControl --config no_legs.xml + --context gazeboCartesianControl --config no_legs.xml localhost + /icubSim/torso/state:o /icubSim/left_arm/state:o /icubSim/right_arm/state:o + YARP_CLOCK=/clock + + 5 + iKinCartesianSolver - --context simCartesianControl --part right_arm + --context gazeboCartesianControl --part right_arm localhost + /icubSim/torso/state:o /icubSim/right_arm/state:o + YARP_CLOCK=/clock iKinCartesianSolver - --context simCartesianControl --part left_arm + --context gazeboCartesianControl --part left_arm localhost + /icubSim/torso/state:o /icubSim/left_arm/state:o + YARP_CLOCK=/clock iKinGazeCtrl - --from configSim.ini + --context gazeboCartesianControl --from iKinGazeCtrl.ini localhost + /icubSim/torso/state:o /icubSim/head/state:o /icubSim/inertial + YARP_CLOCK=/clock - iCubGui - --xpos 800 --ypos 80 --width 370 + pf3dTracker + --from pf3dTracker-gazebo.ini localhost - - + + demoRedBall --from config-test.ini localhost @@ -61,36 +74,68 @@ /icubSim/cartesianController/right_arm/state:o /iKinGazeCtrl/rpc - + + + yarpview + --name /PF3DTracker_viewer --x 320 --y 80 --p 50 --compact + localhost + + /demoRedBall/rpc + + YARP_CLOCK=/clock + + + iCubGui + --xpos 1400 --ypos 80 --width 370 + localhost + + /demoRedBall/rpc + + - - /icubSim/inertial - /iCubGui/inertials/measures:i - udp - - - /icubSim/head/state:o - /iCubGui/head:i - udp - - - /icubSim/torso/state:o - /iCubGui/torso:i - udp - - - /icubSim/left_arm/state:o - /iCubGui/left_arm:i - udp - - - /icubSim/right_arm/state:o - /iCubGui/right_arm:i - udp - - - /demoRedBall/gui:o - /iCubGui/objects - tcp - + + /icubSim/inertial + /iCubGui/inertials/measures:i + udp + + + /icubSim/head/state:o + /iCubGui/head:i + udp + + + /icubSim/torso/state:o + /iCubGui/torso:i + udp + + + /icubSim/left_arm/state:o + /iCubGui/left_arm:i + udp + + + /icubSim/right_arm/state:o + /iCubGui/right_arm:i + udp + + + /demoRedBall/gui:o + /iCubGui/objects + fast_tcp + + + /icubSim/cam/left/rgbImage:o + /pf3dTracker/video:i + fast_tcp + + + /pf3dTracker/video:o + /PF3DTracker_viewer + fast_tcp + + + /pf3dTracker/data:o + /demoRedBall/trackTarget:i + fast_tcp +