iMSTK
Interactive Medical Simulation Toolkit
All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Pages
RbdSDFPivotNeedleExample.cpp
1 /*
2 ** This file is part of the Interactive Medical Simulation Toolkit (iMSTK)
3 ** iMSTK is distributed under the Apache License, Version 2.0.
4 ** See accompanying NOTICE for details.
5 */
6 
7 #include "imstkCamera.h"
8 #include "imstkDirectionalLight.h"
9 #include "imstkIsometricMap.h"
10 #include "imstkKeyboardDeviceClient.h"
11 #include "imstkKeyboardSceneControl.h"
12 #include "imstkLineMesh.h"
13 #include "imstkMeshIO.h"
14 #include "imstkMouseDeviceClient.h"
15 #include "imstkMouseSceneControl.h"
16 #include "imstkObjectControllerGhost.h"
17 #include "imstkPlane.h"
18 #include "imstkPuncturable.h"
19 #include "imstkRbdConstraint.h"
20 #include "imstkRenderMaterial.h"
21 #include "imstkRigidBodyModel2.h"
22 #include "imstkRigidObject2.h"
23 #include "imstkRigidObjectController.h"
24 #include "imstkScene.h"
25 #include "imstkSceneManager.h"
26 #include "imstkSimulationManager.h"
27 #include "imstkSimulationUtils.h"
28 #include "imstkStraightNeedle.h"
29 #include "imstkSurfaceMesh.h"
30 #include "imstkVisualModel.h"
31 #include "imstkVTKViewer.h"
32 #include "NeedleInteraction.h"
33 
34 #ifdef iMSTK_USE_HAPTICS
35 #include "imstkDeviceManager.h"
36 #include "imstkDeviceManagerFactory.h"
37 #else
38 #include "imstkDummyClient.h"
39 #endif
40 
41 using namespace imstk;
42 
46 static std::shared_ptr<CollidingObject>
47 createTissueObj()
48 {
49  auto tissueObj = std::make_shared<CollidingObject>("Tissue");
50 
51  auto plane = std::make_shared<Plane>();
52  plane->setWidth(0.1);
53 
54  tissueObj->setVisualGeometry(plane);
55  tissueObj->setCollidingGeometry(plane);
56 
57  auto material = std::make_shared<RenderMaterial>();
58  material->setShadingModel(RenderMaterial::ShadingModel::PBR);
59  material->setColor(Color::Bone);
60  material->setRoughness(0.5);
61  material->setMetalness(0.1);
62  tissueObj->getVisualModel(0)->setRenderMaterial(material);
63 
64  tissueObj->addComponent<Puncturable>();
65 
66  return tissueObj;
67 }
68 
69 static std::shared_ptr<RigidObject2>
70 createNeedleObj()
71 {
72  auto toolGeom = std::make_shared<LineMesh>();
73  VecDataArray<double, 3> vertices = { Vec3d(0.0, 0.0, 0.0), Vec3d(0.0, 0.0, -0.1) };
74  VecDataArray<int, 2> cells = { Vec2i(0, 1) };
75  toolGeom->initialize(
76  std::make_shared<VecDataArray<double, 3>>(vertices),
77  std::make_shared<VecDataArray<int, 2>>(cells));
78 
79  auto syringeMesh = MeshIO::read<SurfaceMesh>(iMSTK_DATA_ROOT "/Surgical Instruments/Syringes/Disposable_Syringe.stl");
80  syringeMesh->scale(0.0075, Geometry::TransformType::ApplyToData);
81  toolGeom->rotate(Vec3d(0.0, 1.0, 0.0), PI, Geometry::TransformType::ApplyToData);
82  syringeMesh->translate(Vec3d(0.0, 0.0, 0.1), Geometry::TransformType::ApplyToData);
83 
84  auto toolObj = std::make_shared<RigidObject2>("NeedleRbdTool");
85  toolObj->setVisualGeometry(syringeMesh);
86  toolObj->setCollidingGeometry(toolGeom);
87  toolObj->setPhysicsGeometry(toolGeom);
88  toolObj->setPhysicsToVisualMap(std::make_shared<IsometricMap>(toolGeom, syringeMesh));
89  toolObj->getVisualModel(0)->getRenderMaterial()->setColor(Color(0.9, 0.9, 0.9));
90  toolObj->getVisualModel(0)->getRenderMaterial()->setShadingModel(RenderMaterial::ShadingModel::PBR);
91  toolObj->getVisualModel(0)->getRenderMaterial()->setRoughness(0.5);
92  toolObj->getVisualModel(0)->getRenderMaterial()->setMetalness(1.0);
93  toolObj->getVisualModel(0)->getRenderMaterial()->setOpacity(0.5);
94 
95  auto lineModel = std::make_shared<VisualModel>();
96  lineModel->setGeometry(toolGeom);
97  toolObj->addVisualModel(lineModel);
98  //toolObj->getVisualModel(0)->getRenderMaterial()->setLineWidth(5.0);
99 
100  std::shared_ptr<RigidBodyModel2> rbdModel = std::make_shared<RigidBodyModel2>();
101  rbdModel->getConfig()->m_gravity = Vec3d::Zero();
102  rbdModel->getConfig()->m_maxNumIterations = 20;
103  rbdModel->getConfig()->m_angularVelocityDamping = 0.8; // Helps with lack of 6dof
104  toolObj->setDynamicalModel(rbdModel);
105 
106  toolObj->getRigidBody()->m_mass = 1.0;
107  toolObj->getRigidBody()->m_intertiaTensor = Mat3d::Identity() * 1000.0;
108  toolObj->getRigidBody()->m_initPos = Vec3d(0.0, 0.1, 0.0);
109 
110  auto needle = toolObj->addComponent<StraightNeedle>();
111  needle->setNeedleGeometry(toolGeom);
112 
113  // Add a component for controlling via another device
114  auto controller = toolObj->addComponent<RigidObjectController>();
115  controller->setControlledObject(toolObj);
116  controller->setLinearKs(8000.0);
117  controller->setLinearKd(200.0);
118  controller->setAngularKs(1000000.0);
119  controller->setAngularKd(100000.0);
120  controller->setForceScaling(0.02);
121  controller->setSmoothingKernelSize(5);
122  controller->setUseForceSmoothening(true);
123 
124  // Add extra component to tool for the ghost
125  auto controllerGhost = toolObj->addComponent<ObjectControllerGhost>();
126  controllerGhost->setUseForceFade(true);
127  controllerGhost->setController(controller);
128 
129  return toolObj;
130 }
131 
135 int
136 main()
137 {
138  // Write log to stdout and file
140 
141  auto scene = std::make_shared<Scene>("RbdSDFNeedle");
142 
143  // Create the bone
144  std::shared_ptr<CollidingObject> tissueObj = createTissueObj();
145  scene->addSceneObject(tissueObj);
146 
147  // Create the needle
148  std::shared_ptr<RigidObject2> needleObj = createNeedleObj();
149  scene->addSceneObject(needleObj);
150 
151  // Setup interaction between tissue and needle
152  scene->addInteraction(std::make_shared<NeedleInteraction>(tissueObj, needleObj));
153 
154  // Camera
155  scene->getActiveCamera()->setPosition(0.0, 0.2, 0.2);
156  scene->getActiveCamera()->setFocalPoint(0.0, 0.0, 0.0);
157  scene->getActiveCamera()->setViewUp(0.0, 1.0, 0.0);
158 
159  // Light
160  auto light = std::make_shared<DirectionalLight>();
161  light->setDirection(Vec3d(0.0, -1.0, -1.0));
162  light->setIntensity(1.0);
163  scene->addLight("light", light);
164 
165  // Run the simulation
166  {
167  // Setup a viewer to render in its own thread
168  auto viewer = std::make_shared<VTKViewer>();
169  viewer->setActiveScene(scene);
170  viewer->setDebugAxesLength(0.005, 0.005, 0.005);
171 
172  // Setup a scene manager to advance the scene in its own thread
173  auto sceneManager = std::make_shared<SceneManager>();
174  sceneManager->setActiveScene(scene);
175  sceneManager->pause();
176 
177  auto driver = std::make_shared<SimulationManager>();
178  driver->addModule(viewer);
179  driver->addModule(sceneManager);
180  driver->setDesiredDt(0.001);
181 
182 #ifdef iMSTK_USE_HAPTICS
183  // Setup default haptics manager
184  std::shared_ptr<DeviceManager> hapticManager = DeviceManagerFactory::makeDeviceManager();
185  std::shared_ptr<DeviceClient> deviceClient = hapticManager->makeDeviceClient();
186  driver->addModule(hapticManager);
187 #else
188  auto deviceClient = std::make_shared<DummyClient>();
189  connect<Event>(sceneManager, &SceneManager::postUpdate, [&](Event*)
190  {
191  const Vec2d mousePos = viewer->getMouseDevice()->getPos();
192  const Vec3d desiredPos = Vec3d(mousePos[0] - 0.5, mousePos[1] - 0.5, 0.0) * 0.25;
193  const Quatd desiredOrientation = Quatd(Rotd(-1.0, Vec3d(1.0, 0.0, 0.0)));
194 
195  deviceClient->setPosition(desiredPos);
196  deviceClient->setOrientation(desiredOrientation);
197  });
198 #endif
199  auto controller = needleObj->getComponent<RigidObjectController>();
200  controller->setDevice(deviceClient);
201 
202  connect<Event>(sceneManager, &SceneManager::preUpdate, [&](Event*)
203  {
204  // Keep the tool moving in real time
205  needleObj->getRigidBodyModel2()->getConfig()->m_dt = sceneManager->getDt();
206  });
207 
208  // Add default mouse and keyboard controls to the viewer
209  std::shared_ptr<Entity> mouseAndKeyControls =
210  SimulationUtils::createDefaultSceneControl(driver);
211  scene->addSceneObject(mouseAndKeyControls);
212 
213  driver->start();
214  }
215 
216  return 0;
217 }
Base class for events which contain a type, priority, and data priority defaults to 0 and uses a grea...
Place this on an object to make it puncturable by a needle. This allows puncturables to know they&#39;ve ...
void setUseForceFade(bool useForceFade)
Get/Set whether to use force fade or not. Force fade sets opacity of ghost geometry according to forc...
Compound Geometry.
Definition of straight, single segment needle.
static std::shared_ptr< DeviceManager > makeDeviceManager()
Attempts to create a new DeviceManager by whichever is default If multiple haptic managers are built ...
This class uses the provided device to control the provided rigid object via virtual coupling...
Color in RGB space.
Definition: imstkColor.h:24
A behaviour that renders a second copy of the controlled object at a lower opacity in the physical po...
Physically based rendering.
static LoggerG3 & startLogger()
Starts logger with default sinks, use getInstance to create a logger with no sinks.