[SOLVED] Using Sensable plugin in custion c++ application

[Rus]

I am creating a scene using C++. I need to use OmniDriver component in this scene. There is linking error occurred when I was trying to build the my project. Could you please show me the proper way of creating instance of the OmniDriver component in project. I included the error status and content of my cpp file.

include <iostream>
#include <sstream>
#include <fstream>
#include <sofa/helper/ArgumentParser.h>
#include <sofa/helper/UnitTest.h>
#include <sofa/helper/vector_algebra.h>
#include <sofa/helper/vector.h>
#include <sofa/helper/BackTrace.h>
#include <sofa/helper/system/PluginManager.h>

#include <sofa/simulation/Node.h>
#include <sofa/simulation/Simulation.h>
#include <SofaSimulationTree/init.h>
#include <SofaSimulationTree/TreeSimulation.h>

#include <sofa/gui/GUIManager.h>
#include <sofa/gui/Main.h>
#include <sofa/helper/system/FileRepository.h>

#include <SofaComponentBase/initComponentBase.h>
#include <SofaComponentCommon/initComponentCommon.h>
#include <SofaComponentGeneral/initComponentGeneral.h>
#include <SofaComponentAdvanced/initComponentAdvanced.h>
#include <SofaComponentMisc/initComponentMisc.h>
#include <SofaMiscMapping/SubsetMultiMapping.h>
#include <SofaBaseTopology/MeshTopology.h>
#include <SofaBaseTopology/EdgeSetTopologyContainer.h>
#include <SofaBaseTopology/RegularGridTopology.h>
#include <SofaBaseCollision/SphereModel.h>
#include <SofaGeneralTopology/CubeTopology.h>
#include <SofaBaseVisual/VisualStyle.h>
#include <SofaImplicitOdeSolver/EulerImplicitSolver.h>
#include <SofaBaseLinearSolver/CGLinearSolver.h>
#include <SofaBaseCollision/DefaultPipeline.h>
#include <SofaBaseCollision/DefaultContactManager.h>
#include <SofaBaseCollision/MinProximityIntersection.h>
#include <SofaConstraint/FreeMotionAnimationLoop.h>
#include <SofaConstraint/LCPConstraintSolver.h>
#include <SofaGeneralLoader/MeshGmshLoader.h>
#include <SofaLoader/MeshObjLoader.h>
#include <SofaBaseTopology/TetrahedronSetGeometryAlgorithms.h>
#include <SofaTopologyMapping/Tetra2TriangleTopologicalMapping.h>
#include <SofaBaseTopology/TetrahedronSetTopologyModifier.h>
#include <SofaMeshCollision/LineModel.h>
#include <SofaHaptics/LCPForceFeedback.h>
#include <plugins\Sensable\OmniDriver.h>
#include <sofa/helper/system/PluginManager.h>
#include <sofa/helper/Utils.h>
#include <SofaGraphComponent\RequiredPlugin.h>
#include <conio.h>
#include  <windows.h>

//Using double by default, if you have SOFA_FLOAT in use in you sofa-default.cfg, then it will be FLOAT.
#include <sofa/component/typedef/Sofa_typedef.h>

#include <utility>

using namespace sofa;
using namespace sofa::helper;
using helper::vector;
using namespace sofa::simulation;
using namespace sofa::core::objectmodel;
using namespace sofa::component::container;
using namespace sofa::component::topology;
using namespace sofa::component::collision;
using namespace sofa::component::visualmodel;
using namespace sofa::component::mapping;
using namespace sofa::component::forcefield;
using namespace sofa::component::animationloop;
using namespace sofa::component::constraintset;
using namespace sofa::component::loader;
using namespace sofa::component::controller;
using namespace sofa::component::misc;
using sofa::helper::system::DataRepository;
using sofa::helper::system::PluginRepository;
using sofa::helper::system::PluginManager;

typedef SReal Scalar;
typedef Vec<3, SReal> Vec3;
typedef Vec<1, SReal> Vec1;
typedef component::odesolver::EulerImplicitSolver EulerImplicitSolver;
typedef component::linearsolver::CGLinearSolver<component::linearsolver::GraphScatteredMatrix, component::linearsolver::GraphScatteredVector> CGLinearSolver;

const string cylinderGmshFileURI = "mesh/cylinder.msh";

bool startAnim = true;
bool verbose = false;
SReal complianceValue = 0.1;
Vec3 gravity(0, -1, 0);
SReal dt = 0.01;

/// helper for more compact component creation
template<class Component>
typename Component::SPtr addNew(Node::SPtr parentNode, std::string name = "")
{
	typename Component::SPtr component = New<Component>();
	parentNode->addObject(component);
	component->setName(name);
	return component;
}

/// Create an assembly of a siff hexahedral grid with other objects
simulation::Node::SPtr createGridScene(Vec3 startPoint, Vec3 endPoint, unsigned numX, unsigned numY, unsigned numZ, double totalMass/*, double stiffnessValue, double dampingRatio=0.0*/)
{
	using helper::vector;

	// The graph root node
	Node::SPtr  root = simulation::getSimulation()->createNewGraph("root");
	root->setGravity(Coord3(0, -10, 0));
	root->setAnimate(false);
	root->setDt(0.01);
	addVisualStyle(root)->setShowVisual(true).setShowCollision(true).setShowMapping(false).setShowBehavior(false).setShowInteractionForceFields(true);
	//Components for all child Node of Root 
	RequiredPlugin::SPtr requiredPlugin = addNew<RequiredPlugin>(root);
	DefaultPipeline::SPtr defaultPipeline = addNew<DefaultPipeline>(root, "DefaultCollisionPipeline");
	defaultPipeline->bVerbose.setValue(false);
	defaultPipeline->bDraw.setValue(false);
	defaultPipeline->depth.setValue(6);
	BruteForceDetection::SPtr druteForceDetection = addNew<BruteForceDetection>(root, "Detection");
	DefaultContactManager::SPtr collisionResponse = addNew<DefaultContactManager>(root, "Response");
	MinProximityIntersection::SPtr proximityInterSection = addNew<MinProximityIntersection>(root,"Proximity");
	const SReal alarmDistance = 0.8, contactDistance=0.3;
	proximityInterSection->alarmDistance.setValue(alarmDistance);
	proximityInterSection->contactDistance.setValue(contactDistance);
	proximityInterSection->useSurfaceNormals.setValue(true);
	FreeMotionAnimationLoop::SPtr animationLoop = New<FreeMotionAnimationLoop>(root.get());
	root->addObject(animationLoop);
	animationLoop->setName(root->getName() + "_" +"AnimationLoop");
	LCPConstraintSolver::SPtr lcpConstraint = addNew<LCPConstraintSolver>(root,"LCPConstraint");
	const SReal tolerance = 0.001;
	const int maxIt = 1000;
	lcpConstraint->tol.setValue(tolerance);
	lcpConstraint->maxIt.setValue(maxIt);
	//==================================================
	//Omni driver to make the deformable cylinder controllable
	Node::SPtr omniNode = root->createChild("Omni");
	OmniDriver omniDriver;
	//omniNode->addObject(omniDriver);
	//omniDriver->omniVisu.setValue(true);
	//omniDriver->orientationBase.setValue(Quat(0,0,0.707107,0.0707107));
	//omniDriver->positionBase.setValue(Vec3d(-1,0,2));
	MechanicalObjectRigid3::SPtr dofState = addNew<MechanicalObjectRigid3>(omniNode,"DOFState");
	/*MechanicalObjectRigid3::WriteVecCoord dofStateWriter = dofState->writePositions();
	for (int i = 0; i <= 5; i++)
		dofStateWriter[i] = 0;*/

	MechanicalStateControllerRigid3::SPtr dofStateController = addNew<MechanicalStateControllerRigid3>(omniNode,"DOFStateController");
	dofStateController->f_listening.setValue(true);
	dofStateController->setMainDirection(Vec3d(-1,0,0));

	Node::SPtr refModel = omniNode->createChild("RefModel");
	MeshGmshLoader::SPtr controlToolMesh = addNew<MeshGmshLoader>(refModel, "Loader");
	controlToolMesh->setFilename(DataRepository.getFile(cylinderGmshFileURI));
	controlToolMesh->load();
	MechanicalObject3::SPtr controlToolState = addNew<MechanicalObject3>(refModel,"ControlToolState");
	controlToolState->setSrc("@Loader");
	RigidMappingRigid3_to_3::SPtr rigidMapping = addNew<RigidMappingRigid3_to_3>(refModel);
	rigidMapping->setModels(dofState.get(), controlToolState.get());
	//===================================================

	//Rigid floor to show deformation of the 
	const string floorMeshFileURI = "mesh/floor.obj";
	Node::SPtr floorNode = root->createChild("Floor");
	MeshObjLoader::SPtr floorMeshObjLoader = addNew<MeshObjLoader>(floorNode, "Loader");
	floorMeshObjLoader->setFilename(DataRepository.getFile(floorMeshFileURI));
	floorMeshObjLoader->load();
	MeshTopology::SPtr meshTopology = addNew<MeshTopology>(floorNode, "TopologyFloor");
	meshTopology->setSrc("@Loader");
	MechanicalObject3::SPtr floorState = addNew<MechanicalObject3>(floorNode, "FloorState");
	TriangleModel::SPtr triangleCollision = addNew<TriangleModel>(floorNode, "TriangleCollisionModel");
	triangleCollision->setMoving(false);
	triangleCollision->setSimulated(false);
	triangleCollision->setContactFriction(500);
	triangleCollision->setContactStiffness(0.03);
	//==================================================

	//Cylindric FEM based deformable tool
	Node::SPtr cylindricToolNode = root->createChild("CylinderFEM");
	cylindricToolNode->setGravity(Coord3(0, 0, -10));
	EulerImplicitSolver::SPtr eulerImplicitSolver = addNew<EulerImplicitSolver>(cylindricToolNode,"CGSolver");
	CGLinearSolver::SPtr cgLinearSolver = addNew<CGLinearSolver>(cylindricToolNode,"CGLinearSolver");
	cgLinearSolver->f_maxIter.setValue(25);
	cgLinearSolver->f_tolerance.setValue(1e-9);
	cgLinearSolver->f_smallDenominatorThreshold.setValue(1e-9);
	MeshGmshLoader::SPtr meshToolLoader = addNew<MeshGmshLoader>(cylindricToolNode,"Loader");
	meshToolLoader->setFilename(DataRepository.getFile(cylinderGmshFileURI));
	meshToolLoader->load();
	MechanicalObject3::SPtr cylinderState = addNew<MechanicalObject3>(cylindricToolNode, "CylinderState");
	cylinderState->setSrc("@Loader");
	TetrahedronSetTopologyContainer::SPtr tetrahedronContainer = addNew<TetrahedronSetTopologyContainer>(cylindricToolNode, "Container");
	tetrahedronContainer->setSrc("@Loader");
	TetrahedronSetGeometryAlgorithms<Vec3dTypes> ::SPtr tetrahedroAlgorithm = addNew<TetrahedronSetGeometryAlgorithms<Vec3dTypes>>(cylindricToolNode,"GeoAlgorithm");
	DiagonalMass3::SPtr diagonalMass = addNew<DiagonalMass3>(cylindricToolNode, "DiagonalMass");
	diagonalMass->m_massDensity.setValue(10);
	TetrahedralCorotationalFEMForceField<Vec3dTypes>::SPtr femForceField = addNew<TetrahedralCorotationalFEMForceField<Vec3dTypes>>(cylindricToolNode,"FEMForceField");
	femForceField->_youngModulus.setValue(1000);
	femForceField->_poissonRatio.setValue(0.2);
	femForceField->f_method.setValue("large");
	UncoupledConstraintCorrection<Vec3dTypes>::SPtr constraintCorrection = addNew<UncoupledConstraintCorrection<Vec3dTypes>>(cylindricToolNode, "");
	Node::SPtr cylinderCollisoinModel = cylindricToolNode->createChild("CollsionModel");
	TriangleSetTopologyContainer::SPtr triangleContainer = addNew<TriangleSetTopologyContainer>(cylinderCollisoinModel, "Container");
	TriangleSetTopologyModifier::SPtr triangleModifier = addNew<TriangleSetTopologyModifier>(cylinderCollisoinModel,"Modifier");
	TriangleSetTopologyAlgorithms<Vec3dTypes>::SPtr triangleAlgorithm = addNew<TriangleSetTopologyAlgorithms<Vec3dTypes>>(cylinderCollisoinModel,"TopoAlgorithm");
	TriangleSetGeometryAlgorithms<Vec3dTypes>::SPtr triangleGeoAlgorithm = addNew<TriangleSetGeometryAlgorithms<Vec3dTypes>>(cylinderCollisoinModel, "GeoAlgorithm");
	Tetra2TriangleTopologicalMapping::SPtr tetraTopologicalMapping = addNew<Tetra2TriangleTopologicalMapping>(cylinderCollisoinModel,"");
	tetraTopologicalMapping->setPathInputObject("@../Container");
	tetraTopologicalMapping->setPathOutputObject("@Container");
	TTriangleModel<Vec3dTypes>::SPtr triangleCollisionModel = addNew<TTriangleModel<Vec3dTypes>>(cylinderCollisoinModel, "CylinderCollisionModel");
	LineModel::SPtr cylinderLineCollisionModel = addNew<LineModel>(cylinderCollisoinModel, "CylinderCollisionModel");
	PointModel::SPtr CylinderPointCollisionModel = addNew<PointModel>(cylinderCollisoinModel, "CylinderCollisionModel");
	Node::SPtr cylinderVisualModelNode = cylinderCollisoinModel -> createChild("VisualModel");
	OglModel::SPtr cylinderVisualModel = addNew<OglModel>(cylinderVisualModelNode, "Visual");
	IdentityMapping<Vec3dTypes, ExtVec3f>::SPtr identityMapping = addNew<IdentityMapping<Vec3dTypes, ExtVec3f>>(cylinderVisualModelNode, "");
	identityMapping->setPathInputObject("@../../CylinderState");
	identityMapping->setPathOutputObject("@Visual");
	//==================================================

	return root;
}

int main(int argc, char** argv)
{
	sofa::simulation::tree::init();
	sofa::helper::BackTrace::autodump();
	sofa::core::ExecParams::defaultInstance()->setAspectID(0);

	sofa::helper::parse("This is a SOFA application. Here are the command line arguments")
		.option(&startAnim, 'a', "start", "start the animation loop")
		.option(&verbose, 'v', "verbose", "print debug info")
		(argc, argv);

	sofa::component::initComponentBase();
	sofa::component::initComponentCommon();
	sofa::component::initComponentGeneral();
	sofa::component::initComponentAdvanced();
	sofa::component::initComponentMisc();
	sofa::gui::initMain();

	const string pluginDir = Utils::getExecutableDirectory();

	PluginRepository.addFirstPath(pluginDir);
	{
		std::ostringstream no_error_message; // no to get an error on the console if Sensable does not exist
		sofa::helper::system::PluginManager::getInstance().loadPlugin("Sensable", &no_error_message);
	}

	PluginManager::getInstance().init();

	if (int err = sofa::gui::GUIManager::Init(argv[0], "")) return err;
	if (int err = sofa::gui::GUIManager::createGUI(NULL)) return err;
	sofa::gui::GUIManager::SetDimension(800, 600);

	sofa::simulation::setSimulation(new sofa::simulation::tree::TreeSimulation());
	

	//=================================================
	sofa::simulation::Node::SPtr groot = createGridScene(Vec3(0, 0, 0), Vec3(5, 1, 1), 6, 2, 2, 1.0);
	//=================================================

	sofa::simulation::getSimulation()->init(groot.get());
	sofa::gui::GUIManager::SetScene(groot);

#ifdef PS3
	groot->setAnimate(true);
#endif

	// Run the main loop
	if (int err = sofa::gui::GUIManager::MainLoop(groot))
		return err;

	sofa::simulation::getSimulation()->unload(groot);
	sofa::gui::GUIManager::closeGUI();

	sofa::simulation::tree::cleanup();
	return 0;
}

[Rus]

http://prntscr.com/dapf9w

[Hugo]

Hi Ruslan,

Could you let us know about the SOFA version you are using ?
Is it the master branch ? up-to-date ?

Isn’t it just the creation of your component that would require a new to allow dynamic memory allocation? I haven’t been writing a scene in Cpp for a long time, I’d rather use Python.

By the way, there is now a Geomagic plugin to interface with the haptic devices from Geomagic. You should have a look at it: https://www.sofa-framework.org/about/news/new-geomagic-plugin/

Hope this helps,
Cheers,

Hugo

[Rus]

Hello Hugo,

I am using master branch and it is up-to-date. I saw some changes in Sensable plugin a few days ago, then I pulled the last changes.

I did this as well, but the same error occurred. Actually, I have a source code of mine which is written in C++ and I want to integrate it with my sofa application.

OmniDriver::SPtr omniDriver = New<OmniDriver>(omniNode);
	omniNode->addObject(omniDriver);
	omniDriver->omniVisu.setValue(true);
	omniDriver->orientationBase.setValue(Quat(0,0,0.707107,0.0707107));
	omniDriver->positionBase.setValue(Vec3d(-1,0,2));

I have tried Geomagic plugin as well, by this is what it says: http://prntscr.com/db1mwx

Thank you for your quick response

[Hugo]

Dear Rus,

We had many issues with Sensable.
With the Geomagic plugin, make sure to use the GeomagicDriver (and not Omni Driver anymore). See the example in applications/plugins/Geomagic/scenes/Geomagic-Demo.scn

<RequiredPlugin pluginName="Geomagic"/>
<GeomagicDriver name="GeomagicDevice" deviceName="Default Device" scale="1" drawDeviceFrame="1" positionBase="0 0 0" orientationBase="0 0 0 1"  />

Cheers,

Hugo

[jnbrunet]

Hi Rus,

The first error you have with the Sensable plugin seems to indicate that your application aren’t able to find the Sensable’s functions. I can see a couple of ways to get that error :

1. You aren’t linking against the libSensable.lib when compiling your executable.
2. The Sensable plugin was compiled with another c++ standard (ex c++99 for the plugin and c++11 for your application). To be sure, use the same c++ standard and compiler sofa’s libraries, sofa’s plugin and for your application. Also, try to load the Sensable plugin with the plugin loader of runSofa binary. Does it get you an error?
3. Finally, you should use the NewOmniDriver instead of the OmniDriver. The OmniDriver is obsolete (well, more obsolete than NewOmniDriver) in my knowledge.

For the second error you have with Geomagic Plugin, make sure you specify the good device name in the deviceName=”Default Device” attribute of the GeomagicDriver node of your scene. To get the device name, launch the Geomagic diagnostic tool.

Good luck!

[Rus]

Hello Jnbrunet and Hugo,

Thank you for your responses. I followed your instructions and they were helpful.

I have another question about GeomagicDriver now. I am still working on my c++ sofa application and can not link GeomagicDriver::p_positionDev with MechanicalObjectRigid3::x (position) to control an object.

In my xml scene it looks like this `<GeomagicDriver name=”GeomagicDevice” deviceName=”Default PHANToM” positionBase=”0 0 0″ orientationBase=”0 0.707107 0 -0.707107″ />
<MechanicalObject template=”Rigid” name=”DOFs” position=”@GeomagicDevice.positionDevice” />`

But in my application, I don`t have any idea how to do the same thing

GeomagicDriver::SPtr geoDriver = New<GeomagicDriver>();
	geoNode->addObject(geoDriver);
	geoDriver->setName("GeomagicDevice");
	geoDriver->d_deviceName.setValue("Default PHANToM");
	geoDriver->d_scale.setValue(1);
	geoDriver->d_positionBase.setValue(Vec3d(0, 0, 0));
	geoDriver->d_orientationBase.setValue(Quat(0, 0, 0.707107, 0.0707107));
MechanicalObjectRigid3::SPtr dofState = addNew<MechanicalObjectRigid3>(geoNode,"DOFs");
// no idea what to do further 

[Guillaume]

Hi Rus,

I am not an expert with SOFA but here are some hints:

If you just want to copy GeomagicDevice.positionDevice into MechanicalObject.position, have a look at DiagonalMass_test::createSceneGraph.

If you want to link GeomagicDevice.positionDevice to MechanicalObject.position, check BezierTetrahedronTopology::createScene.

I guess what you want is the link solution.
Basically you can find a lot of C++ scene creations in all *_test.cpp files.

Hope this helps,
Guillaume.

[Author]

Posts