Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Upgrade the Red Ball Demo Test to the use of Gazebo simulator #58

Merged
merged 1 commit into from
Jul 17, 2021
Merged

Upgrade the Red Ball Demo Test to the use of Gazebo simulator #58

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 3 additions & 4 deletions CMakeLists.txt
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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