Commit 07819c60 authored by Andrew Wilson's avatar Andrew Wilson 🐘
Browse files

Merge branch 'NeedleInteractions' into 'master'

ENH: Needle Interaction Examples

See merge request !662
parents 1ab71b43 809a88cf
Pipeline #248634 passed with stage
in 0 seconds
......@@ -16,7 +16,7 @@ set(copy_data_command
)
include(imstkAddExternalProject)
set(GIT_SHA "14824e3d53328ed6be481981959780f32881030b")
set(GIT_SHA "2166a3715358e9794e53baf5dfa7deeaf36321cd")
set(DATA_URL "https://gitlab.kitware.com/iMSTK/imstk-data/-/archive/${GIT_SHA}/imstk-data-${GIT_SHA}.zip")
imstk_add_external_project( iMSTKData
......
......@@ -97,7 +97,6 @@ main()
Logger::startLogger();
imstkNew<Scene> scene("FemurCut");
scene->getConfig()->taskParallelizationEnabled = false;
imstkNew<FemurObject> femurObj;
scene->addSceneObject(femurObj);
......
###########################################################################
#
# Copyright (c) Kitware, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0.txt
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#
###########################################################################
project(Example-PBDStaticSuture)
if (iMSTK_USE_OpenHaptics)
#-----------------------------------------------------------------------------
# Create executable
#-----------------------------------------------------------------------------
imstk_add_executable(${PROJECT_NAME}
pbdStaticSutureExample.cpp
NeedleInteraction.h
NeedleInteraction.cpp
NeedleObject.h
NeedleObject.cpp
NeedleRigidBodyCH.h
NeedleRigidBodyCH.cpp
RbdPointToArcConstraint.h)
#-----------------------------------------------------------------------------
# Add the target to Examples folder
#-----------------------------------------------------------------------------
SET_TARGET_PROPERTIES (${PROJECT_NAME} PROPERTIES FOLDER Examples/PBD)
#-----------------------------------------------------------------------------
# Link libraries to executable
#-----------------------------------------------------------------------------
target_link_libraries(${PROJECT_NAME} SimulationManager)
endif()
/*=========================================================================
Library: iMSTK
Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
& Imaging in Medicine, Rensselaer Polytechnic Institute.
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0.txt
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
=========================================================================*/
#include "NeedleInteraction.h"
#include "NeedleObject.h"
#include "NeedleRigidBodyCH.h"
#include "imstkCollisionDetectionAlgorithm.h"
#include "imstkLineMesh.h"
#include "imstkImplicitGeometry.h"
#include "imstkTaskGraph.h"
using namespace imstk;
NeedleInteraction::NeedleInteraction(std::shared_ptr<CollidingObject> tissueObj,
std::shared_ptr<NeedleObject> needleObj) : RigidObjectCollision(needleObj, tissueObj, "ImplicitGeometryToPointSetCD")
{
if (std::dynamic_pointer_cast<LineMesh>(needleObj->getCollidingGeometry()) == nullptr)
{
LOG(WARNING) << "NeedleInteraction only works with LineMesh collision geometry on rigid DrillObject";
}
if (std::dynamic_pointer_cast<ImplicitGeometry>(tissueObj->getCollidingGeometry()) == nullptr)
{
LOG(WARNING) << "NeedleInteraction only works with SDF colliding geometry on colliding tissueObj";
}
// First replace the handlers with our needle subclasses
// This handler consumes collision data to resolve the tool from the tissue
// except when the needle is inserted
auto needleRbdCH = std::make_shared<NeedleRigidBodyCH>();
needleRbdCH->setInputRigidObjectA(needleObj);
needleRbdCH->setInputCollidingObjectB(tissueObj);
needleRbdCH->setInputCollisionData(getCollisionDetection()->getCollisionData());
needleRbdCH->setBeta(0.01);
needleRbdCH->getTaskNode()->m_isCritical = true;
setCollisionHandlingA(needleRbdCH);
}
\ No newline at end of file
/*=========================================================================
Library: iMSTK
Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
& Imaging in Medicine, Rensselaer Polytechnic Institute.
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0.txt
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
=========================================================================*/
#pragma once
#include "imstkRigidObjectCollision.h"
using namespace imstk;
class NeedleObject;
///
/// \class NeedleInteraction
///
/// \brief Defines interaction between NeedleObject and PbdObject
///
class NeedleInteraction : public RigidObjectCollision
{
public:
NeedleInteraction(std::shared_ptr<CollidingObject> tissueObj,
std::shared_ptr<NeedleObject> needleObj);
~NeedleInteraction() override = default;
};
\ No newline at end of file
/*=========================================================================
Library: iMSTK
Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
& Imaging in Medicine, Rensselaer Polytechnic Institute.
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0.txt
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
=========================================================================*/
#include "NeedleObject.h"
#include "imstkIsometricMap.h"
#include "imstkLineMesh.h"
#include "imstkMeshIO.h"
#include "imstkRbdConstraint.h"
#include "imstkRenderMaterial.h"
#include "imstkRigidBodyModel2.h"
#include "imstkSurfaceMesh.h"
#include "imstkVecDataArray.h"
#include "imstkVisualModel.h"
using namespace imstk;
NeedleObject::NeedleObject() : RigidObject2("Needle")
{
auto sutureMesh = MeshIO::read<SurfaceMesh>(iMSTK_DATA_ROOT "/Surgical Instruments/Needles/c6_suture.stl");
auto sutureLineMesh = MeshIO::read<LineMesh>(iMSTK_DATA_ROOT "/Surgical Instruments/Needles/c6_suture_hull.vtk");
const Mat4d rot = mat4dRotation(Rotd(-PI_2, Vec3d(0.0, 1.0, 0.0))) *
mat4dRotation(Rotd(-0.6, Vec3d(1.0, 0.0, 0.0)));
sutureMesh->transform(rot, Geometry::TransformType::ApplyToData);
sutureLineMesh->transform(rot, Geometry::TransformType::ApplyToData);
setVisualGeometry(sutureMesh);
setCollidingGeometry(sutureLineMesh);
setPhysicsGeometry(sutureLineMesh);
setPhysicsToVisualMap(std::make_shared<IsometricMap>(sutureLineMesh, sutureMesh));
getVisualModel(0)->getRenderMaterial()->setColor(Color(0.9, 0.9, 0.9));
getVisualModel(0)->getRenderMaterial()->setShadingModel(RenderMaterial::ShadingModel::PBR);
getVisualModel(0)->getRenderMaterial()->setRoughness(0.5);
getVisualModel(0)->getRenderMaterial()->setMetalness(1.0);
std::shared_ptr<RigidBodyModel2> rbdModel = std::make_shared<RigidBodyModel2>();
rbdModel->getConfig()->m_gravity = Vec3d::Zero();
rbdModel->getConfig()->m_maxNumIterations = 5;
setDynamicalModel(rbdModel);
getRigidBody()->m_mass = 1.0;
getRigidBody()->m_intertiaTensor = Mat3d::Identity() * 10000.0;
getRigidBody()->m_initPos = Vec3d(0.0, 0.0, 0.0);
// Manually setup an arc aligned with the geometry, some sort of needle+arc generator
// could be a nice addition to imstk
Mat3d arcBasis = Mat3d::Identity();
arcBasis.col(0) = Vec3d(0.0, 0.0, -1.0);
arcBasis.col(1) = Vec3d(1.0, 0.0, 0.0);
arcBasis.col(2) = Vec3d(0.0, 1.0, 0.0);
arcBasis = rot.block<3, 3>(0, 0) * arcBasis;
const Vec3d arcCenter = (rot * Vec4d(0.0, -0.005455, 0.008839, 1.0)).head<3>();
const double arcRadius = 0.010705;
setArc(arcCenter, arcBasis, arcRadius, 0.558, 2.583);
}
const Mat3d
NeedleObject::getArcBasis()
{
return getRigidBody()->getOrientation().toRotationMatrix() * m_arcBasis;
}
const Vec3d
NeedleObject::getArcCenter()
{
return getRigidBody()->getPosition() + getRigidBody()->getOrientation().toRotationMatrix() * m_arcCenter;
}
\ No newline at end of file
/*=========================================================================
Library: iMSTK
Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
& Imaging in Medicine, Rensselaer Polytechnic Institute.
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0.txt
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
=========================================================================*/
#pragma once
#include "imstkRigidObject2.h"
using namespace imstk;
class NeedleObject : public RigidObject2
{
public:
enum class CollisionState
{
REMOVED,
TOUCHING,
INSERTED
};
public:
NeedleObject();
virtual ~NeedleObject() = default;
virtual const std::string getTypeName() const override { return "NeedleObject"; }
public:
// *INDENT-OFF*
SIGNAL(NeedleObject, inserted);
SIGNAL(NeedleObject, removed);
// *INDENT-ON*
public:
void setCollisionState(const CollisionState state)
{
// If current state is inserted and previous was not inserted
if (m_collisionState == CollisionState::INSERTED && state != CollisionState::INSERTED)
{
this->postEvent(Event(removed()));
}
// If current state not inserted and previous inserted
else if (m_collisionState != CollisionState::INSERTED && state == CollisionState::INSERTED)
{
this->postEvent(Event(inserted()));
}
m_collisionState = state;
}
CollisionState getCollisionState() const { return m_collisionState; }
///
/// \brief Set the force threshold for the needle
///
void setForceThreshold(const double forceThreshold) { m_forceThreshold = forceThreshold; }
double getForceThreshold() const { return m_forceThreshold; }
void setArc(const Vec3d& arcCenter, const Mat3d& arcBasis,
double arcRadius, double beginRad, double endRad)
{
m_arcCenter = arcCenter;
m_arcBasis = arcBasis;
m_beginRad = beginRad;
m_endRad = endRad;
m_arcRadius = arcRadius;
}
///
/// \brief Get the basis post transformation of the rigid body
///
const Mat3d getArcBasis();
///
/// \brief Get the arc center post transformation of the rigid body
///
const Vec3d getArcCenter();
const double getBeginRad() const { return m_beginRad; }
const double getEndRad() const { return m_endRad; }
const double getArcRadius() const { return m_arcRadius; }
protected:
CollisionState m_collisionState = CollisionState::REMOVED;
double m_forceThreshold = 5.0;
Mat3d m_arcBasis = Mat3d::Identity();
Vec3d m_arcCenter = Vec3d::Zero();
double m_arcRadius = 1.0;
double m_beginRad = 0.0;
double m_endRad = PI * 2.0;
};
\ No newline at end of file
/*=========================================================================
Library: iMSTK
Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
& Imaging in Medicine, Rensselaer Polytechnic Institute.
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0.txt
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
=========================================================================*/
#include "NeedleRigidBodyCH.h"
#include "NeedleObject.h"
#include "imstkRbdContactConstraint.h"
#include "imstkRigidBodyModel2.h"
#include "RbdPointToArcConstraint.h"
void
NeedleRigidBodyCH::handle(
const std::vector<CollisionElement>& elementsA,
const std::vector<CollisionElement>& elementsB)
{
// Do it the normal way
RigidBodyCH::handle(elementsA, elementsB);
// If no collision, needle must be removed
auto needleObj = std::dynamic_pointer_cast<NeedleObject>(getInputObjectA());
NeedleObject::CollisionState state = needleObj->getCollisionState();
if (elementsA.size() != 0)
{
if (state == NeedleObject::CollisionState::INSERTED)
{
const Mat3d& arcBasis = needleObj->getArcBasis();
const Vec3d& arcCenter = needleObj->getArcCenter();
const double arcRadius = needleObj->getArcRadius();
const double arcBeginRad = needleObj->getBeginRad();
const double arcEndRad = needleObj->getEndRad();
// Constrain along the axes, whilst allowing "pushing" of the contact point
auto pointToArcConstraint = std::make_shared<RbdPointToArcConstraint>(
needleObj->getRigidBody(),
arcCenter,
arcBeginRad, arcEndRad,
arcRadius,
arcBasis,
m_initContactPt,
m_beta);
pointToArcConstraint->compute(needleObj->getRigidBodyModel2()->getTimeStep());
needleObj->getRigidBodyModel2()->addConstraint(pointToArcConstraint);
}
}
else
{
if (state == NeedleObject::CollisionState::INSERTED || state == NeedleObject::CollisionState::TOUCHING)
{
if (state == NeedleObject::CollisionState::INSERTED)
{
LOG(INFO) << "Unpuncture!";
}
needleObj->setCollisionState(NeedleObject::CollisionState::REMOVED);
}
}
}
void
NeedleRigidBodyCH::addConstraint(
std::shared_ptr<RigidObject2> rbdObj,
const Vec3d& contactPt, const Vec3d& contactNormal,
const double contactDepth)
{
auto obj = std::dynamic_pointer_cast<NeedleObject>(rbdObj);
// If removed and we are here, we must now be touching
if (obj->getCollisionState() == NeedleObject::CollisionState::REMOVED)
{
obj->setCollisionState(NeedleObject::CollisionState::TOUCHING);
}
// If touching we may test for insertion
const Vec3d n = contactNormal.normalized();
if (obj->getCollisionState() == NeedleObject::CollisionState::TOUCHING)
{
// Get all inwards force
const double fN = std::max(-contactNormal.dot(obj->getRigidBody()->getForce()), 0.0);
// If the normal force exceeds threshold the needle may insert
if (fN > obj->getForceThreshold())
{
LOG(INFO) << "Puncture!";
obj->setCollisionState(NeedleObject::CollisionState::INSERTED);
// Record the initial contact point
m_initOrientation = Quatd(rbdObj->getCollidingGeometry()->getRotation());
m_initContactPt = contactPt;
}
}
// Only add contact normal constraint if not inserted
NeedleObject::CollisionState state = obj->getCollisionState();
if (state == NeedleObject::CollisionState::TOUCHING)
{
auto contactConstraint = std::make_shared<RbdContactConstraint>(
rbdObj->getRigidBody(), nullptr,
n, contactPt, contactDepth,
m_beta,
RbdConstraint::Side::A);
contactConstraint->compute(rbdObj->getRigidBodyModel2()->getTimeStep());
rbdObj->getRigidBodyModel2()->addConstraint(contactConstraint);
}
}
\ No newline at end of file
/*=========================================================================
Library: iMSTK
Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
& Imaging in Medicine, Rensselaer Polytechnic Institute.
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0.txt
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
=========================================================================*/
#pragma once
#include "imstkRigidBodyCH.h"
using namespace imstk;
class NeedleRigidBodyCH : public RigidBodyCH
{
public:
NeedleRigidBodyCH() = default;
~NeedleRigidBodyCH() override = default;
virtual const std::string getTypeName() const override { return "NeedleRigidBodyCH"; }
protected:
///
/// \brief Handle the collision/contact data
///
virtual void handle(
const std::vector<CollisionElement>& elementsA,
const std::vector<CollisionElement>& elementsB) override;
///
/// \brief Add constraint for the rigid body given contact
///
void addConstraint(
std::shared_ptr<RigidObject2> rbdObj,
const Vec3d& contactPt, const Vec3d& contactNormal,
const double contactDepth) override;
protected:
Vec3d m_initContactPt = Vec3d::Zero();
Vec3d m_initAxes = Vec3d::Zero();
Quatd m_initOrientation = Quatd::Identity();
};
\ No newline at end of file
/*=========================================================================
Library: iMSTK
Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
& Imaging in Medicine, Rensselaer Polytechnic Institute.
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0.txt
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
=========================================================================*/
#pragma once
#include "imstkRbdConstraint.h"
using namespace imstk;
///
/// \class RbdPointToArcConstraint
///
/// \brief Constrains an rigid body arc geometry to a point by computing the
/// linear force and angular torque to get the arc to the point
///
class RbdPointToArcConstraint : public RbdConstraint
{
public:
///
/// \param the Rigid body
/// \param the center of the circle arc is defined with
/// \param the radians/range of the arc, with relation to the arcBasis
/// \param the radians/range of the arc, with relation to the arcBasis
/// \param the radius of the circle the arc is defined with
/// \param the basis of the arc. Where any point on the plane has a radian with relation to x,y columns.
/// and the z column gives the normal of the plane the circle+arc lie on
/// \param the fixed point
/// \param baumgarte stabilization, varies step length
///
RbdPointToArcConstraint(
std::shared_ptr<RigidBody> obj,
const Vec3d arcCenter, const double beginRadian, const double endRadian,
const double arcCircleRadius, const Mat3d arcBasis,
const Vec3d fixedPoint,
const double beta = 0.05) : RbdConstraint(obj, nullptr, Side::A),
m_arcCenter(arcCenter), m_beginRadian(beginRadian), m_endRadian(endRadian),
m_arcCircleRadius(arcCircleRadius), m_arcBasis(arcBasis),
m_fixedPoint(fixedPoint),
m_beta(beta)
{
// Check orthonormal basis