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

Jump to bottom

[SofaHaptics] Add multithread test on LCPForceFeedback component #1581

Merged
merged 4 commits into from
Nov 13, 2020
Merged

[SofaHaptics] Add multithread test on LCPForceFeedback component #1581

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
c18ac95
[SofaHaptics] In LCPForceFeedback _tests, test multithread access tes…
epernod Oct 30, 2020
5971aa7
[SofaHaptics] Fix LCPForceFeedback_test compilation on gcc
epernod Oct 31, 2020
21b55e1
[SofaHaptics] Fix LCPForceFeedback_test for ci
epernod Nov 6, 2020
a57154a
[SofaHaptics] Fix LCPForceFeedback_test for ci
epernod Nov 7, 2020
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
2 changes: 1 addition & 1 deletion modules/SofaHaptics/SofaHaptics_test/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,6 @@ set(SOURCE_FILES

add_definitions("-DSOFAHAPTICS_TEST_SCENES_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/scenes\"")
add_executable(${PROJECT_NAME} ${SOURCE_FILES})
target_link_libraries(${PROJECT_NAME} SofaGTestMain SofaTest SofaHaptics SofaLoader SofaGeneralLoader )
target_link_libraries(${PROJECT_NAME} SofaGTestMain SofaTest SofaHaptics )

add_test(NAME ${PROJECT_NAME} COMMAND ${PROJECT_NAME})
154 changes: 141 additions & 13 deletions modules/SofaHaptics/SofaHaptics_test/LCPForceFeedback_test.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,8 +21,6 @@
******************************************************************************/

#include <sofa/helper/testing/BaseTest.h>
#include <sofa/helper/testing/NumericTest.h>

#include <SofaSimulationGraph/DAGSimulation.h>

#include <SofaCommon/initSofaCommon.h>
Expand All @@ -31,14 +29,15 @@

#include <SofaBaseMechanics/MechanicalObject.h>
#include <SofaHaptics/LCPForceFeedback.h>
#include <thread>


namespace sofa
{
using sofa::simulation::Simulation;
using sofa::simulation::Node;
using sofa::core::ExecParams;

using namespace sofa::helper::system::thread;

//template <typename _DataTypes>
class LCPForceFeedback_test : public sofa::helper::testing::BaseTest
Expand All @@ -58,14 +57,83 @@ class LCPForceFeedback_test : public sofa::helper::testing::BaseTest

bool test_Collision();

bool test_multiThread();

/// General Haptic thread methods
static void HapticsThread(std::atomic<bool>& terminate, void * p_this);

public:
// point to the LCP for haptic access
LCPRig::SPtr m_LCPFFBack;

/// values to exchange info with haptic thread test
int m_cptLoop = 0;
int m_cptLoopContact = 0;
sofa::defaulttype::Vec3 m_meanForceFFBack = sofa::defaulttype::Vec3(0, 0, 0);

sofa::defaulttype::Vec3 m_currentPosition = sofa::defaulttype::Vec3(0, 0, 0);
std::mutex mtxPosition;
protected:
/// Internal method to load a scene test file
void loadTestScene(const std::string& filename);

/// pointer to the simulation root node loaded by @sa loadTestScene
Node::SPtr m_root;

/// Epsilon value for the numerical check
SReal epsilonTest = 1e-6;

/// variables for thread test
std::thread haptic_thread;
std::atomic<bool> m_terminate;
};



void LCPForceFeedback_test::HapticsThread(std::atomic<bool>& terminate, void * p_this)
{
LCPForceFeedback_test* driverTest = static_cast<LCPForceFeedback_test*>(p_this);

// Loop Timer
long targetSpeedLoop = 1; // Target loop speed: 1ms

ctime_t refTicksPerMs = CTime::getRefTicksPerSec() / 1000;
ctime_t targetTicksPerLoop = targetSpeedLoop * refTicksPerMs;

// Haptics Loop
while (!terminate)
{
ctime_t startTime = CTime::getRefTime();

driverTest->mtxPosition.lock();
sofa::defaulttype::Vec3 posInSofa = driverTest->m_currentPosition; // will apply -1 on y to simulate penetration
driverTest->mtxPosition.unlock();
sofa::defaulttype::Vec3 force;
driverTest->m_LCPFFBack->computeForce(posInSofa[0], posInSofa[1]-1.0, posInSofa[2], 0, 0, 0, 0,
force[0], force[1], force[2]);

if (force.norm() > 0.0) // in contact
{
driverTest->m_cptLoopContact++;
driverTest->m_meanForceFFBack += force;
}


ctime_t endTime = CTime::getRefTime();
ctime_t duration = endTime - startTime;

// If loop is quicker than the target loop speed. Wait here.
while (duration < targetTicksPerLoop)
{
endTime = CTime::getRefTime();
duration = endTime - startTime;
}

driverTest->m_cptLoop++;
}
}


void LCPForceFeedback_test::loadTestScene(const std::string& filename)
{
sofa::component::initSofaBase();
Expand All @@ -85,18 +153,19 @@ void LCPForceFeedback_test::loadTestScene(const std::string& filename)
sofa::simulation::getSimulation()->init(m_root.get());
}


bool LCPForceFeedback_test::test_InitScene()
{
loadTestScene("ToolvsFloorCollision_test.scn");

simulation::Node::SPtr instruNode = m_root->getChild("Instrument");
EXPECT_NE(instruNode, nullptr);
MecaRig::SPtr meca = instruNode->get<MecaRig>(instruNode->SearchDown);
LCPRig::SPtr lcp = instruNode->get<LCPRig>(instruNode->SearchDown);
m_LCPFFBack = instruNode->get<LCPRig>(instruNode->SearchDown);

// Check components access
EXPECT_NE(meca, nullptr);
EXPECT_NE(lcp, nullptr);
EXPECT_NE(m_LCPFFBack, nullptr);

// Check meca size and init position
EXPECT_EQ(meca->getSize(), 1);
Expand Down Expand Up @@ -187,14 +256,14 @@ bool LCPForceFeedback_test::test_Collision()
simulation::Node::SPtr instruNode = m_root->getChild("Instrument");
EXPECT_NE(instruNode, nullptr);
MecaRig::SPtr meca = instruNode->get<MecaRig>(instruNode->SearchDown);
LCPRig::SPtr lcp = instruNode->get<LCPRig>(instruNode->SearchDown);
m_LCPFFBack = instruNode->get<LCPRig>(instruNode->SearchDown);

// Force only 2 iteration max for ci tests
lcp->d_solverMaxIt.setValue(2);
m_LCPFFBack->d_solverMaxIt.setValue(2);

// Check components access
EXPECT_NE(meca, nullptr);
EXPECT_NE(lcp, nullptr);
EXPECT_NE(m_LCPFFBack, nullptr);

// Check meca size and init position
EXPECT_EQ(meca->getSize(), 1);
Expand All @@ -218,13 +287,13 @@ bool LCPForceFeedback_test::test_Collision()
sofa::defaulttype::Vec3 trueForce;

// check out of problem position
lcp->computeForce(position[0], position[1], position[2], 0, 0, 0, 0, force[0], force[1], force[2]);
m_LCPFFBack->computeForce(position[0], position[1], position[2], 0, 0, 0, 0, force[0], force[1], force[2]);
trueForce = sofa::defaulttype::Vec3(0.0, 0.0, 0.0);
EXPECT_EQ(force, trueForce);


// check position in contact
lcp->computeForce(coords[0][0], coords[0][1], coords[0][2], 0, 0, 0, 0, force[0], force[1], force[2]);
m_LCPFFBack->computeForce(coords[0][0], coords[0][1], coords[0][2], 0, 0, 0, 0, force[0], force[1], force[2]);

// test with groundtruth, do it index by index for better log
Coord coordT = Coord(sofa::defaulttype::Vec3d(0.1083095508, -9.45640795, 0.01134330546), sofa::defaulttype::Quatd(0.01623300333, -0.006386979003, -0.408876291, 0.9124230788));
Expand All @@ -247,7 +316,7 @@ bool LCPForceFeedback_test::test_Collision()

// check position inside collision
Coord inside = Coord(sofa::defaulttype::Vec3d(coords[0][0], coords[0][1] - 1.0, coords[0][2]), sofa::defaulttype::Quatd(0.01623300333, -0.006386979003, -0.408876291, 0.9124230788));
lcp->computeForce(inside[0], inside[1], inside[2], 0, 0, 0, 0, force[0], force[1], force[2]);
m_LCPFFBack->computeForce(inside[0], inside[1], inside[2], 0, 0, 0, 0, force[0], force[1], force[2]);

// test with groundtruth, do it index by index for better log
coordT = Coord(sofa::defaulttype::Vec3d(0.1083095508, -10.45640795, 0.01134330546), sofa::defaulttype::Quatd(0.01623300333, -0.006386979003, -0.408876291, 0.9124230788));
Expand All @@ -270,7 +339,7 @@ bool LCPForceFeedback_test::test_Collision()

// check rigidTypes computeForce method
VecDeriv forces;
lcp->computeForce(coords, forces);
m_LCPFFBack->computeForce(coords, forces);

EXPECT_EQ(forces.size(), 1);
EXPECT_FLOAT_EQ(forces[0][0], -0.00164953925);
Expand All @@ -284,6 +353,61 @@ bool LCPForceFeedback_test::test_Collision()
}


bool LCPForceFeedback_test::test_multiThread()
{
loadTestScene("ToolvsFloorCollision_test.scn");

simulation::Node::SPtr instruNode = m_root->getChild("Instrument");
EXPECT_NE(instruNode, nullptr);
MecaRig::SPtr meca = instruNode->get<MecaRig>(instruNode->SearchDown);
m_LCPFFBack = instruNode->get<LCPRig>(instruNode->SearchDown);

// Force only 2 iteration max for ci tests
m_LCPFFBack->d_solverMaxIt.setValue(2);

// Check components access
EXPECT_NE(meca, nullptr);
EXPECT_NE(m_LCPFFBack, nullptr);

// create and launch haptic thread
m_terminate = false;
haptic_thread = std::thread(HapticsThread, std::ref(this->m_terminate), this);

// run simulation for n steps
simulation::Simulation* simu = sofa::simulation::getSimulation();
for (int step = 0; step < 500; step++)
{
simu->animate(m_root.get());

const VecCoord& coords = meca->x.getValue();
mtxPosition.lock();
m_currentPosition[0] = coords[0][0];
m_currentPosition[1] = coords[0][1];
m_currentPosition[2] = coords[0][2];
mtxPosition.unlock();
CTime::sleep(0.001);
}

// stop thread
m_terminate = true;
haptic_thread.join();

// get back info from haptic thread
m_meanForceFFBack = m_meanForceFFBack / m_cptLoopContact;

EXPECT_GT(m_cptLoop, 500);
EXPECT_GT(m_cptLoopContact, 400);

// make a simple test FFBack not equal to 0. Not possible to test exact value as CI have different thread speed
EXPECT_NE(m_meanForceFFBack[0], 0.0);
EXPECT_NE(m_meanForceFFBack[1], 0.0);
EXPECT_NE(m_meanForceFFBack[2], 0.0);

return true;
}



TEST_F(LCPForceFeedback_test, test_InitScene)
{
ASSERT_TRUE(test_InitScene());
Expand All @@ -294,11 +418,15 @@ TEST_F(LCPForceFeedback_test, test_SimpleCollision)
ASSERT_TRUE(test_SimpleCollision());
}


TEST_F(LCPForceFeedback_test, test_Collision)
{
ASSERT_TRUE(test_Collision());
}

TEST_F(LCPForceFeedback_test, test_multiThread)
{
ASSERT_TRUE(test_multiThread());
}


} // namespace sofa