Skip to content

Commit

Permalink
Merge pull request #786 from epernod/geomagic_fix
Browse files Browse the repository at this point in the history 
[Geomagic] Fix dll export and some enhancements
  • Loading branch information
@guparan
guparan authored Oct 4, 2018
2 parents 8ec9a51 + 8e08754 commit b353019
Show file tree
Hide file tree
Showing 4 changed files with 162 additions and 66 deletions.
3 changes: 3 additions & 0 deletions applications/plugins/Geomagic/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@ endif()
set(HEADER_FILES
src/GeomagicDriver.h
src/initPlugin.h
src/config.h
)

set(SOURCE_FILES
Expand All @@ -26,7 +27,9 @@ include_directories( ${OPENHAPTICS_INCLUDE_DIR})

add_library(${PROJECT_NAME} SHARED ${HEADER_FILES} ${SOURCE_FILES} ${SCENES_FILES} ${README_FILES})
set_target_properties(${PROJECT_NAME} PROPERTIES COMPILE_FLAGS "-DPLUGIN_DATA_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\"")
set_target_properties(${PROJECT_NAME} PROPERTIES COMPILE_FLAGS "-DSOFA_BUILD_GEOMAGIC")
target_link_libraries(${PROJECT_NAME} SofaHelper SofaUserInteraction ${OPENHAPTICS_LIBRARIES})
include_directories("${CMAKE_CURRENT_SOURCE_DIR}/..")

if(NOT ${SOFA_NO_OPENGL})
target_link_libraries(${PROJECT_NAME} SofaOpenglVisual)
Expand Down
178 changes: 117 additions & 61 deletions applications/plugins/Geomagic/src/GeomagicDriver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,14 +22,12 @@

#include "GeomagicDriver.h"
#include <sofa/core/ObjectFactory.h>
//#include <sofa/core/objectmodel/HapticDeviceEvent.h>
#include <sofa/simulation/AnimateBeginEvent.h>
#include <sofa/simulation/AnimateEndEvent.h>
#include <sofa/simulation/Node.h>
#include <sofa/simulation/MechanicalVisitor.h>
#include <sofa/simulation/UpdateMappingVisitor.h>
#include <sofa/core/objectmodel/KeypressedEvent.h>
#include <sofa/core/objectmodel/KeyreleasedEvent.h>
#include <sofa/core/objectmodel/ScriptEvent.h>
#include <sofa/core/objectmodel/MouseEvent.h>
#include <sofa/helper/system/thread/CTime.h>
#include <sofa/core/visual/VisualParams.h>
Expand Down Expand Up @@ -189,9 +187,12 @@ GeomagicDriver::GeomagicDriver()
, d_posDevice(initData(&d_posDevice, "positionDevice", "position of the base of the part of the device"))
, d_button_1(initData(&d_button_1,"button1","Button state 1"))
, d_button_2(initData(&d_button_2,"button2","Button state 2"))
, d_emitButtonEvent(initData(&d_emitButtonEvent, false, "emitButtonEvent", "If true, will send event through the graph when button are pushed/released"))
, d_inputForceFeedback(initData(&d_inputForceFeedback, Vec3d(0,0,0), "inputForceFeedback","Input force feedback in case of no LCPForceFeedback is found (manual setting)"))
, d_maxInputForceFeedback(initData(&d_maxInputForceFeedback, double(1.0), "maxInputForceFeedback","Maximum value of the normed input force feedback for device security"))
, d_manualStart(initData(&d_manualStart, false, "manualStart", "If true, will not automatically initDevice at component init phase."))
, m_simulationStarted(false)
, m_errorDevice(0)
{
this->f_listening.setValue(true);
m_forceFeedback = NULL;
Expand All @@ -200,10 +201,13 @@ GeomagicDriver::GeomagicDriver()
GeomagicDriver::~GeomagicDriver()
{
hdMakeCurrentDevice(m_hHD);

if (!m_hStateHandles.empty()) {
hdStopScheduler();
}

hdStopScheduler();

for (std::vector< HDCallbackCode >::iterator i = m_hStateHandles.begin();
for (std::vector< HDSchedulerHandle >::iterator i = m_hStateHandles.begin();
i != m_hStateHandles.end(); ++i)
{
hdUnschedule(*i);
Expand All @@ -215,9 +219,27 @@ GeomagicDriver::~GeomagicDriver()

//executed once at the start of Sofa, initialization of all variables excepts haptics-related ones
void GeomagicDriver::init()
{

}

void GeomagicDriver::bwdInit()
{
if(m_errorDevice != 0)
return;

simulation::Node *context = dynamic_cast<simulation::Node *>(this->getContext()); // access to current node
m_forceFeedback = context->get<ForceFeedback>(this->getTags(), sofa::core::objectmodel::BaseContext::SearchRoot);

if (d_manualStart.getValue() == false)
initDevice();
}


void GeomagicDriver::initDevice()
{
m_initVisuDone = false;
m_errorDevice = false;
m_errorDevice = 0;
HDErrorInfo error;

HDSchedulerHandle hStateHandle = HD_INVALID_HANDLE;
Expand All @@ -227,7 +249,7 @@ void GeomagicDriver::init()
{
msg_error() << "Failed to initialize the device called " << d_deviceName.getValue().c_str();
d_omniVisu.setValue(false);
m_errorDevice = true;
m_errorDevice = error.errorCode;
//init the positionDevice data to avoid any crash in the scene
m_posDeviceVisu.clear();
m_posDeviceVisu.resize(1);
Expand All @@ -246,58 +268,73 @@ void GeomagicDriver::init()
if (HD_DEVICE_ERROR(error = hdGetError()))
{
printError(&error, "Error with the device Default PHANToM");
m_errorDevice = true;
m_errorDevice = error.errorCode;
return;
}

if(d_maxInputForceFeedback.isSet())
if (d_maxInputForceFeedback.isSet())
{
msg_info() << "maxInputForceFeedback value ("<< d_maxInputForceFeedback.getValue() <<") is set, carefully set the max force regarding your haptic device";
msg_info() << "maxInputForceFeedback value (" << d_maxInputForceFeedback.getValue() << ") is set, carefully set the max force regarding your haptic device";

if(d_maxInputForceFeedback.getValue() <= 0.0)
if (d_maxInputForceFeedback.getValue() <= 0.0)
{
msg_error() << "maxInputForceFeedback value ("<< d_maxInputForceFeedback.getValue() <<") is negative or 0, it should be strictly positive";
msg_error() << "maxInputForceFeedback value (" << d_maxInputForceFeedback.getValue() << ") is negative or 0, it should be strictly positive";
d_maxInputForceFeedback.setValue(0.0);
}
}

reinit();

hdStartScheduler();

if (HD_DEVICE_ERROR(error = hdGetError()))
{
msg_info() << "Failed to start the scheduler";
m_errorDevice = error.errorCode;
if (m_hStateHandles.size()) m_hStateHandles[0] = HD_INVALID_HANDLE;
return;
}
updatePosition();


if (!d_omniVisu.getValue())
return;

//Initialization of the visual components
//resize vectors
m_posDeviceVisu.resize(NVISUALNODE+1);
m_posDeviceVisu.resize(NVISUALNODE + 1);

m_visuActive = false;

for(int i=0; i<NVISUALNODE; i++)
for (int i = 0; i<NVISUALNODE; i++)
{
visualNode[i].visu = NULL;
visualNode[i].mapping = NULL;
}

//create a specific node containing rigid position for visual models
sofa::simulation::Node::SPtr rootContext = static_cast<simulation::Node*>(this->getContext()->getRootContext());
m_omniVisualNode = rootContext->createChild("omniVisu "+d_deviceName.getValue());
m_omniVisualNode = rootContext->createChild("omniVisu " + d_deviceName.getValue());
m_omniVisualNode->updateContext();

rigidDOF = sofa::core::objectmodel::New<component::container::MechanicalObject<sofa::defaulttype::Rigid3dTypes> >();
m_omniVisualNode->addObject(rigidDOF);
rigidDOF->name.setValue("rigidDOF");

VecCoord& posDOF =*(rigidDOF->x.beginEdit());
posDOF.resize(NVISUALNODE+1);
VecCoord& posDOF = *(rigidDOF->x.beginEdit());
posDOF.resize(NVISUALNODE + 1);
rigidDOF->x.endEdit();

rigidDOF->init();
m_omniVisualNode->updateContext();


//creation of subnodes for each part of the device visualization
for(int i=0; i<NVISUALNODE; i++)
for (int i = 0; i<NVISUALNODE; i++)
{
visualNode[i].node = m_omniVisualNode->createChild(visualNodeNames[i]);

if(visualNode[i].visu == NULL && visualNode[i].mapping == NULL)
if (visualNode[i].visu == NULL && visualNode[i].mapping == NULL)
{

// create the visual model and add it to the graph //
Expand All @@ -311,60 +348,38 @@ void GeomagicDriver::init()
visualNode[i].visu->updateVisual();

// create the visual mapping and at it to the graph //
visualNode[i].mapping = sofa::core::objectmodel::New< sofa::component::mapping::RigidMapping< Rigid3dTypes, ExtVec3fTypes > > ();
visualNode[i].mapping = sofa::core::objectmodel::New< sofa::component::mapping::RigidMapping< Rigid3dTypes, ExtVec3fTypes > >();
visualNode[i].node->addObject(visualNode[i].mapping);
visualNode[i].mapping->setModels(rigidDOF.get(), visualNode[i].visu.get());
visualNode[i].mapping->name.setValue("RigidMapping");
visualNode[i].mapping->f_mapConstraints.setValue(false);
visualNode[i].mapping->f_mapForces.setValue(false);
visualNode[i].mapping->f_mapMasses.setValue(false);
visualNode[i].mapping->index.setValue(i+1);
visualNode[i].mapping->index.setValue(i + 1);
visualNode[i].mapping->init();
}
if(i<NVISUALNODE)
if (i<NVISUALNODE)
m_omniVisualNode->removeChild(visualNode[i].node);
}

m_omniVisualNode->updateContext();

for(int i=0; i<NVISUALNODE; i++)
for (int i = 0; i<NVISUALNODE; i++)
{
visualNode[i].node->updateContext();
}

m_initVisuDone = true;

for(int j=0; j<NVISUALNODE; j++)
for (int j = 0; j<NVISUALNODE; j++)
{
sofa::defaulttype::ResizableExtVector< sofa::defaulttype::Vec<3,float> > &scaleMapping = *(visualNode[j].mapping->points.beginEdit());
for(size_t i=0; i<scaleMapping.size(); i++)
sofa::defaulttype::ResizableExtVector< sofa::defaulttype::Vec<3, float> > &scaleMapping = *(visualNode[j].mapping->points.beginEdit());
for (size_t i = 0; i<scaleMapping.size(); i++)
scaleMapping[i] *= (float)(d_scale.getValue());
visualNode[j].mapping->points.endEdit();
}
}

void GeomagicDriver::bwdInit()
{
if(m_errorDevice)
return;

simulation::Node *context = dynamic_cast<simulation::Node *>(this->getContext()); // access to current node
m_forceFeedback = context->get<ForceFeedback>(this->getTags(), sofa::core::objectmodel::BaseContext::SearchRoot);


hdStartScheduler();
HDErrorInfo error;

if (HD_DEVICE_ERROR(error = hdGetError()))
{
msg_info() <<"Failed to start the scheduler";
m_errorDevice = true;
if (m_hStateHandles.size()) m_hStateHandles[0] = HD_INVALID_HANDLE;
return;
}
updatePosition();
}

Mat<4,4, GLdouble> GeomagicDriver::compute_dh_Matrix(double teta,double alpha, double a, double d)
{
Mat<4,4, GLdouble> M;
Expand Down Expand Up @@ -402,7 +417,7 @@ Mat<4,4, GLdouble> GeomagicDriver::compute_dh_Matrix(double teta,double alpha, d

void GeomagicDriver::reinit()
{
if(!m_errorDevice)
if(m_errorDevice != 0)
{
Quat * q_b = d_orientationBase.beginEdit();
q_b->normalize();
Expand All @@ -422,18 +437,15 @@ void GeomagicDriver::updatePosition()
Mat3x3d mrot;

Vector6 & angle = *d_angle.beginEdit();
GeomagicDriver::Coord & posDevice = *d_posDevice.beginEdit();
bool & button_1 = *d_button_1.beginEdit();
bool & button_2 = *d_button_2.beginEdit();
GeomagicDriver::Coord & posDevice = *d_posDevice.beginEdit();

const Vector3 & positionBase = d_positionBase.getValue();
const Quat & orientationBase = d_orientationBase.getValue();
const Quat & orientationTool = d_orientationTool.getValue();
const double & scale = d_scale.getValue();

//copy button state
button_1 = m_simuData.buttonState & HD_DEVICE_BUTTON_1;
button_2 = m_simuData.buttonState & HD_DEVICE_BUTTON_2;
// update button state
updateButtonStates(d_emitButtonEvent.getValue());

//copy angle
angle[0] = m_simuData.angle1[0];
Expand All @@ -460,9 +472,6 @@ void GeomagicDriver::updatePosition()
posDevice.getCenter() = positionBase + orientationBase.rotate(position*scale);
posDevice.getOrientation() = orientationBase * orientation * orientationTool;

d_button_1.endEdit();
d_button_2.endEdit();

if(m_initVisuDone && d_omniVisu.getValue())
{
sofa::defaulttype::SolidTypes<double>::Transform tampon;
Expand Down Expand Up @@ -518,6 +527,53 @@ void GeomagicDriver::updatePosition()
d_angle.endEdit();
}


void GeomagicDriver::updateButtonStates(bool emitEvent)
{
int nbrButton = 2;
sofa::helper::fixed_array<bool, 2> buttons;
buttons[0] = d_button_1.getValue();
buttons[1] = d_button_2.getValue();

//copy button state
sofa::helper::fixed_array<bool, 2> oldStates;
for (int i = 0; i < nbrButton; i++)
oldStates[i] = buttons[i];

// get new values
buttons[0] = m_simuData.buttonState & HD_DEVICE_BUTTON_1;
buttons[1] = m_simuData.buttonState & HD_DEVICE_BUTTON_2;

d_button_1.setValue(buttons[0]);
d_button_2.setValue(buttons[1]);

// emit event if requested
if (!emitEvent)
return;

sofa::simulation::Node::SPtr rootContext = static_cast<simulation::Node*>(this->getContext()->getRootContext());
if (!rootContext)
{
msg_error() << "Rootcontext can't be found using this->getContext()->getRootContext()";
return;
}

for (int i = 0; i < nbrButton; i++)
{
std::string eventString;
if (buttons[i] && !oldStates[i]) // button pressed
eventString = "button" + std::to_string(i) + "pressed";
else if (!buttons[i] && oldStates[i]) // button released
eventString = "button" + std::to_string(i) + "released";

if (!eventString.empty())
{
sofa::core::objectmodel::ScriptEvent eventS(static_cast<simulation::Node*>(this->getContext()), eventString.c_str());
rootContext->propagateEvent(core::ExecParams::defaultInstance(), &eventS);
}
}
}

void GeomagicDriver::getMatrix(Mat<4,4, GLdouble> & M1, int index, double teta)
{
const double ct = cos(teta);
Expand Down Expand Up @@ -548,7 +604,7 @@ void GeomagicDriver::getMatrix(Mat<4,4, GLdouble> & M1, int index, double teta)

void GeomagicDriver::draw(const sofa::core::visual::VisualParams* vparams)
{
if(m_errorDevice)
if(m_errorDevice != 0)
return;

vparams->drawTool()->saveLastState();
Expand Down Expand Up @@ -625,7 +681,7 @@ void GeomagicDriver::draw(const sofa::core::visual::VisualParams* vparams)

void GeomagicDriver::computeBBox(const core::ExecParams* params, bool )
{
if(m_errorDevice)
if(m_errorDevice != 0)
return;

SReal minBBox[3] = {1e10,1e10,1e10};
Expand All @@ -644,7 +700,7 @@ void GeomagicDriver::computeBBox(const core::ExecParams* params, bool )

void GeomagicDriver::handleEvent(core::objectmodel::Event *event)
{
if(m_errorDevice)
if(m_errorDevice != 0)
return;

if (dynamic_cast<sofa::simulation::AnimateBeginEvent *>(event))
Expand Down
Loading

0 comments on commit b353019

Please sign in to comment.