Commit 0b5d584b authored by Andrew Wilson's avatar Andrew Wilson 🐘
Browse files

Merge branch 'VirtualCouplingExampleUpdate' into 'master'

REFAC: Update the virtual coupling example to use a medically relevant object, getRotation renamed to getOrientation in TrackingDevice, inversions fixed, inversion tests added

See merge request !724
parents a84f8e29 7c16ecd8
Pipeline #256725 failed with stage
in 0 seconds
......@@ -16,7 +16,7 @@ set(copy_data_command
)
include(imstkAddExternalProject)
set(GIT_SHA "577b7fda9634c0c4ebb2da46b5d1dcb0fda83a11")
set(GIT_SHA "d4319557228d085ccb076e6f90661785997b7694")
set(DATA_URL "https://gitlab.kitware.com/iMSTK/imstk-data/-/archive/${GIT_SHA}/imstk-data-${GIT_SHA}.zip")
imstk_add_external_project( iMSTKData
......
......@@ -140,7 +140,7 @@ main()
[&](Event*)
{
const Vec3d pos = camController->getPosition();
const Quatd orientation = camController->getRotation();
const Quatd orientation = camController->getOrientation();
light->setPosition(pos);
light->setFocalPoint(pos - orientation.toRotationMatrix().col(2));
......
......@@ -192,7 +192,7 @@ main()
// Also apply controller transform to ghost geometry
ghostMesh->setTranslation(controller->getPosition());
ghostMesh->setRotation(controller->getRotation());
ghostMesh->setRotation(controller->getOrientation());
ghostMesh->updatePostTransformData();
ghostMesh->postModified();
});
......
......@@ -285,7 +285,7 @@ main()
// Transform the ghost tool to show the real tool location
ghostClampsObj->getVisualGeometry()->setTransform(
mat4dTranslate(controller->getPosition()) * mat4dRotation(controller->getRotation()) *
mat4dTranslate(controller->getPosition()) * mat4dRotation(controller->getOrientation()) *
mat4dTranslate(clampOffset) *
mat4dRotation(Rotd(PI, Vec3d(0.0, 1.0, 0.0))));
ghostClampsObj->getVisualGeometry()->updatePostTransformData();
......
......@@ -387,7 +387,7 @@ main()
// Also apply controller transform to ghost geometry
toolGhostMesh->setTranslation(controller->getPosition());
toolGhostMesh->setRotation(controller->getRotation());
toolGhostMesh->setRotation(controller->getOrientation());
toolGhostMesh->updatePostTransformData();
toolGhostMesh->postModified();
});
......
......@@ -413,7 +413,7 @@ main()
{
// Update the ghost debug geometry
std::shared_ptr<Geometry> toolGhostMesh = ghostToolObj->getVisualGeometry();
toolGhostMesh->setRotation(controller->getRotation());
toolGhostMesh->setRotation(controller->getOrientation());
toolGhostMesh->setTranslation(controller->getPosition());
toolGhostMesh->updatePostTransformData();
toolGhostMesh->postModified();
......
......@@ -212,7 +212,7 @@ main()
// Update the ghost debug geometry
std::shared_ptr<Geometry> toolGhostMesh = ghostToolObj->getVisualGeometry();
toolGhostMesh->setRotation(controller->getRotation());
toolGhostMesh->setRotation(controller->getOrientation());
toolGhostMesh->setTranslation(controller->getPosition());
toolGhostMesh->updatePostTransformData();
toolGhostMesh->postModified();
......
......@@ -24,11 +24,16 @@
#include "imstkDirectionalLight.h"
#include "imstkHapticDeviceClient.h"
#include "imstkHapticDeviceManager.h"
#include "imstkIsometricMap.h"
#include "imstkKeyboardSceneControl.h"
#include "imstkLineMesh.h"
#include "imstkMeshIO.h"
#include "imstkMouseSceneControl.h"
#include "imstkNew.h"
#include "imstkOrientedBox.h"
#include "imstkPlane.h"
#include "imstkRbdConstraint.h"
#include "imstkRenderMaterial.h"
#include "imstkRigidBodyModel2.h"
#include "imstkRigidObject2.h"
#include "imstkRigidObjectCollision.h"
......@@ -36,9 +41,12 @@
#include "imstkScene.h"
#include "imstkSceneManager.h"
#include "imstkSimulationManager.h"
#include "imstkSphere.h"
#include "imstkSurfaceMesh.h"
#include "imstkVisualModel.h"
#include "imstkVTKViewer.h"
#include "imstkVTKRenderer.h"
using namespace imstk;
///
......@@ -53,49 +61,75 @@ main()
// Setup haptics manager
imstkNew<HapticDeviceManager> hapticsManager;
const std::string deviceName = "";
std::shared_ptr<HapticDeviceClient> client = hapticsManager->makeDeviceClient(deviceName);
std::shared_ptr<HapticDeviceClient> client = hapticsManager->makeDeviceClient();
// Scene
imstkNew<Scene> scene("VirtualCoupling");
// Create a plane in the scene to touch
imstkNew<Plane> plane(Vec3d(0.0, -50.0, 0.0), Vec3d(0.0, 1.0, 0.0));
plane->setWidth(400.0);
imstkNew<CollidingObject> planeObj("Plane");
planeObj->setVisualGeometry(plane);
planeObj->setCollidingGeometry(plane);
scene->addSceneObject(planeObj);
// Create a virtual coupling object
imstkNew<Sphere> sphere(Vec3d(0.0, 0.0, 0.0), 20.0);
imstkNew<RigidObject2> sphereObj("VirtualCouplingObject");
imstkNew<RigidBodyModel2> rbdModel;
rbdModel->getConfig()->m_dt = 0.001;
rbdModel->getConfig()->m_gravity = Vec3d::Zero();
sphereObj->setDynamicalModel(rbdModel);
sphereObj->getRigidBody()->m_mass = 1.0;
sphereObj->setCollidingGeometry(sphere);
sphereObj->setVisualGeometry(sphere);
sphereObj->setPhysicsGeometry(sphere);
scene->addSceneObject(sphereObj);
std::shared_ptr<CollidingObject> obstacleObjs[] =
{
std::make_shared<CollidingObject>("Plane"),
std::make_shared<CollidingObject>("Cube")
};
// Create a plane and cube for collision with scissors
imstkNew<Plane> plane(Vec3d(0.0, -1.0, 0.0), Vec3d(0.0, 1.0, 0.0));
plane->setWidth(7.0);
obstacleObjs[0]->setVisualGeometry(plane);
obstacleObjs[0]->setCollidingGeometry(plane);
imstkNew<OrientedBox> cube(Vec3d(0.0, -1.0, 0.0), Vec3d(0.5, 0.5, 0.5), Quatd(Rotd(1.0, Vec3d(0.0, 1.0, 0.0))));
obstacleObjs[1]->setVisualGeometry(cube);
obstacleObjs[1]->setCollidingGeometry(cube);
for (int i = 0; i < 2; i++)
{
scene->addSceneObject(obstacleObjs[i]);
}
// The visual geometry is the scissor mesh read in from file
imstkNew<RigidObject2> rbdObj("rbdObj1");
{
imstkNew<RigidBodyModel2> rbdModel;
rbdModel->getConfig()->m_dt = 0.001;
rbdModel->getConfig()->m_gravity = Vec3d::Zero();
rbdObj->setDynamicalModel(rbdModel);
rbdObj->getRigidBody()->m_mass = 0.5;
rbdObj->getRigidBody()->m_intertiaTensor = Mat3d::Identity() * 1000000.0;
auto surfMesh = MeshIO::read<SurfaceMesh>(iMSTK_DATA_ROOT "/Surgical Instruments/Scissors/Metzenbaum Scissors/Metz_Scissors.stl");
rbdObj->setCollidingGeometry(surfMesh);
rbdObj->setVisualGeometry(surfMesh);
rbdObj->setPhysicsGeometry(surfMesh);
std::shared_ptr<RenderMaterial> mat = rbdObj->getVisualModel(0)->getRenderMaterial();
mat->setShadingModel(RenderMaterial::ShadingModel::PBR);
mat->setRoughness(0.5);
mat->setMetalness(1.0);
}
scene->addSceneObject(rbdObj);
// Create a virtual coupling controller
imstkNew<RigidObjectController> controller(sphereObj, client);
imstkNew<RigidObjectController> controller(rbdObj, client);
controller->setLinearKs(10000.0);
controller->setLinearKd(100.0);
controller->setAngularKs(0.0);
controller->setAngularKd(0.0);
controller->setForceScaling(0.00001);
controller->setAngularKs(1000000000.0);
controller->setAngularKd(300000000.0);
controller->setTranslationScaling(0.02);
controller->setForceScaling(0.001);
//controller->setInversionFlags(RigidObjectController::InvertFlag::rotY);
scene->addController(controller);
// Add interaction between the rigid object sphere and static plane
scene->getCollisionGraph()->addInteraction(
std::make_shared<RigidObjectCollision>(sphereObj, planeObj, "UnidirectionalPlaneToSphereCD"));
std::make_shared<RigidObjectCollision>(rbdObj, obstacleObjs[0], "PointSetToPlaneCD"));
scene->getCollisionGraph()->addInteraction(
std::make_shared<RigidObjectCollision>(rbdObj, obstacleObjs[1], "PointSetToOrientedBoxCD"));
// Camera
scene->getActiveCamera()->setPosition(Vec3d(0.0, 269.0, 295.0));
scene->getActiveCamera()->setFocalPoint(Vec3d(0.0, -20.0, 5.7));
scene->getActiveCamera()->setPosition(Vec3d(0.0, 5.0, 10.0));
scene->getActiveCamera()->setFocalPoint(Vec3d(0.0, -1.0, 0.0));
scene->getActiveCamera()->setViewUp(Vec3d(0.0, 1.0, 0.0));
// Light
......@@ -120,11 +154,10 @@ main()
driver->addModule(sceneManager);
driver->setDesiredDt(0.001);
connect<Event>(sceneManager, &SceneManager::postUpdate,
[&](Event*)
connect<Event>(sceneManager, &SceneManager::postUpdate, [&](Event*)
{
// Run the rbd model in real time
rbdModel->getConfig()->m_dt = driver->getDt();
rbdObj->getRigidBodyModel2()->getConfig()->m_dt = driver->getDt();
});
// Add mouse and keyboard controls to the viewer
......
......@@ -125,15 +125,6 @@ using RigidTransform3d = Eigen::Isometry3d;
using AffineTransform3f = Eigen::Affine3f;
using AffineTransform3d = Eigen::Affine3d;
// Handy Cartesian vectors in 3d
#define UP_VECTOR Vec3d(0.0, 1.0, 0.0)
#define DOWN_VECTOR Vec3d(0, -1, 0)
#define RIGHT_VECTOR Vec3d(1, 0, 0)
#define LEFT_VECTOR Vec3d(-1, 0, 0)
#define FORWARD_VECTOR Vec3d(0, 0, -1)
#define BACKWARD_VECTOR Vec3d(0, 0, 1)
#define WORLD_ORIGIN Vec3d::Zero()
/// Some commonly used math constants
#define PI 3.14159265358979323846
#define PI_2 1.57079632679489661923
......
......@@ -9,7 +9,7 @@
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
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,
......@@ -79,7 +79,7 @@ TEST_F(TrackingDeviceControlTest, Basics)
control.updateTrackingData(0.0);
EXPECT_TRUE(pos.isApprox(control.getPosition()));
EXPECT_TRUE(rot.isApprox(control.getRotation()));
EXPECT_TRUE(rot.isApprox(control.getOrientation()));
}
TEST_F(TrackingDeviceControlTest, EndEffector)
......@@ -98,17 +98,20 @@ TEST_F(TrackingDeviceControlTest, EndEffector)
auto expected = offset * rot;
EXPECT_TRUE(pos.isApprox(control.getPosition())) << "Expected: " << pos.transpose() << " Actual: " << control.getPosition();
EXPECT_TRUE(expected.isApprox(control.getRotation())) << "Expected: " << expected.coeffs().transpose() << " Actual: " << control.getRotation().coeffs().transpose();
EXPECT_TRUE(pos.isApprox(control.getPosition()))
<< "Expected: " << pos.transpose()
<< " Actual: " << control.getPosition();
EXPECT_TRUE(expected.isApprox(control.getOrientation()))
<< "Expected: " << expected.coeffs().transpose()
<< " Actual: " << control.getOrientation().coeffs().transpose();
}
TEST_F(TrackingDeviceControlTest, InvertTranslation)
{
unsigned char flags = static_cast<unsigned char>(TrackingDeviceControl::InvertFlag::transX) |
static_cast<unsigned char>(TrackingDeviceControl::InvertFlag::transY) |
static_cast<unsigned char>(TrackingDeviceControl::InvertFlag::transZ);
control.setInversionFlags(flags);
control.setInversionFlags(
TrackingDeviceControl::InvertFlag::transX |
TrackingDeviceControl::InvertFlag::transY |
TrackingDeviceControl::InvertFlag::transZ);
auto pos = Vec3d(1.0, 2.0, 3.0);
auto rot = Quatd(4.0, 5.0, 6.0, 7.0).normalized();
......@@ -118,8 +121,55 @@ TEST_F(TrackingDeviceControlTest, InvertTranslation)
control.updateTrackingData(0.0);
EXPECT_TRUE((pos * -1.0).isApprox(control.getPosition())) << "Expected: " << (pos * -1).transpose() << " Actual: " << control.getPosition().transpose();
EXPECT_TRUE(rot.isApprox(control.getRotation())) << "Expected: " << rot.coeffs().transpose() << " Actual: " << control.getRotation().coeffs().transpose();
EXPECT_TRUE((pos * -1.0).isApprox(control.getPosition()))
<< "Expected: " << (pos * -1).transpose() << " Actual: " << control.getPosition().transpose();
EXPECT_TRUE(rot.isApprox(control.getOrientation()))
<< "Expected: " << rot.coeffs().transpose() << " Actual: " << control.getOrientation().coeffs().transpose();
}
TEST_F(TrackingDeviceControlTest, InvertOrientation)
{
// For reference: iMSTK uses OpenGL which uses right handed system
// +y
// | -z
// | /
// +-----> +x
Vec3d dir[] = { Vec3d(1.0, 0.0, 0.0), Vec3d(0.0, 1.0, 0.0), Vec3d(0.0, 0.0, 1.0) };
TrackingDeviceControl::InvertFlag invertFlags[] =
{
TrackingDeviceControl::InvertFlag::rotX,
TrackingDeviceControl::InvertFlag::rotY,
TrackingDeviceControl::InvertFlag::rotZ
};
// Inverts for rotations happen on a plane so invert X will flip Y & Z rotations.
for (int i = 0; i < 3; i++)
{
// 45 deg rotation around one of the axes that is not this one (i+1)%3
Quatd orientation = Quatd(Rotd(PI_2 * 0.5, dir[(i + 1) % 3]));
client->setPosition(Vec3d(0.0, 0.0, 0.0));
client->setOrientation(orientation);
// Flip over dir
control.setInversionFlags(invertFlags[i]);
control.updateTrackingData(0.0);
// Position should remain untouched
EXPECT_TRUE(Vec3d(0.0, 0.0, 0.0).isApprox(control.getPosition()))
<< "Direction: " << dir[i].transpose() << std::endl
<< "Expected: " << Vec3d(0.0, 0.0, 0.0).transpose() << std::endl
<< "Actual: " << control.getPosition().transpose();
// Expected
Quatd expectedOrientation = Quatd(Rotd(-PI_2 * 0.5, dir[(i + 1) % 3]));
Quatd actualOrientation = control.getOrientation();
EXPECT_TRUE(expectedOrientation.isApprox(actualOrientation))
<< "Direction: " << dir[i].transpose() << std::endl
<< "Expected: " << expectedOrientation << std::endl
<< "Actual: " << actualOrientation;
}
}
TEST_F(TrackingDeviceControlTest, TranslationOffset)
......@@ -138,7 +188,9 @@ TEST_F(TrackingDeviceControlTest, TranslationOffset)
Vec3d expectedPos = pos + offset;
Quatd expectedRot = rot;
EXPECT_TRUE(expectedPos.isApprox(control.getPosition()))
<< "Expected: " << expectedPos.transpose() << " Actual: " << control.getPosition();
EXPECT_TRUE(expectedRot.isApprox(control.getRotation()))
<< "Expected: " << expectedRot.coeffs().transpose() << " Actual: " << control.getRotation().coeffs().transpose();
}
<< "Expected: " << expectedPos.transpose()
<< " Actual: " << control.getPosition();
EXPECT_TRUE(expectedRot.isApprox(control.getOrientation()))
<< "Expected: " << expectedRot.coeffs().transpose()
<< " Actual: " << control.getOrientation().coeffs().transpose();
}
\ No newline at end of file
......@@ -45,7 +45,7 @@ CameraController::update(const double dt)
}
Vec3d p = getPosition();
Quatd r = getRotation();
Quatd r = getOrientation();
// Apply Offsets over the device pose
p = p + m_translationOffset; // Offset the device position
......@@ -53,8 +53,8 @@ CameraController::update(const double dt)
// Set camera info
m_camera->setPosition(p);
m_camera->setFocalPoint((r * FORWARD_VECTOR) + p);
m_camera->setViewUp(r * UP_VECTOR);
m_camera->setFocalPoint((r * Vec3d(0.0, 0.0, -1.0)) + p);
m_camera->setViewUp(r * Vec3d(0.0, 1.0, 0.0));
}
void
......
......@@ -63,7 +63,7 @@ LaparoscopicToolController::update(const double dt)
}
const Vec3d controllerPosition = getPosition();
const Quatd controllerOrientation = getRotation();
const Quatd controllerOrientation = getOrientation();
// Controller transform
m_controllerWorldTransform = mat4dTranslate(controllerPosition) * mat4dRotation(controllerOrientation);
......
......@@ -72,7 +72,7 @@ RigidObjectController::update(const double dt)
Vec3d& currTorque = *m_rigidObject->getRigidBody()->m_torque;
const Vec3d& devicePos = getPosition();
const Quatd& deviceOrientation = getRotation();
const Quatd& deviceOrientation = getOrientation();
//const Vec3d& deviceVelocity = getVelocity();
//const Vec3d& deviceAngularVelocity = getAngularVelocity();
const Vec3d& deviceOffset = Vec3d(0.0, 0.0, 0.0);
......@@ -114,7 +114,7 @@ RigidObjectController::update(const double dt)
*m_rigidObject->getRigidBody()->m_torque = Vec3d(0.0, 0.0, 0.0);
// Directly set position/rotation
(*m_rigidObject->getRigidBody()->m_pos) = getPosition();
(*m_rigidObject->getRigidBody()->m_orientation) = getRotation();
(*m_rigidObject->getRigidBody()->m_orientation) = getOrientation();
}
this->postEvent(Event(RigidObjectController::modified()));
......
......@@ -61,7 +61,7 @@ SceneObjectController::update(const double dt)
// Update geometry
// \todo revisit this; what if we need to move a group of objects
m_sceneObject->getMasterGeometry()->setTranslation(getPosition());
m_sceneObject->getMasterGeometry()->setRotation(getRotation());
m_sceneObject->getMasterGeometry()->setRotation(getOrientation());
m_sceneObject->getMasterGeometry()->postModified();
}
}
\ No newline at end of file
......@@ -27,14 +27,14 @@
namespace imstk
{
TrackingDeviceControl::TrackingDeviceControl() :
m_translationOffset(WORLD_ORIGIN),
m_translationOffset(Vec3d::Zero()),
m_rotationOffset(Quatd::Identity())
{
}
TrackingDeviceControl::TrackingDeviceControl(std::shared_ptr<DeviceClient> device) :
DeviceControl(device),
m_translationOffset(WORLD_ORIGIN),
m_translationOffset(Vec3d::Zero()),
m_rotationOffset(Quatd::Identity())
{
}
......@@ -77,17 +77,20 @@ TrackingDeviceControl::updateTrackingData(const double dt)
}
if (m_invertFlags & InvertFlag::rotX)
{
m_currentOrientation.x() = -m_currentOrientation.x();
m_currentOrientation.y() = -m_currentOrientation.y();
m_currentOrientation.z() = -m_currentOrientation.z();
m_currentAngularVelocity[0] = -m_currentAngularVelocity[0];
}
if (m_invertFlags & InvertFlag::rotY)
{
m_currentOrientation.y() = -m_currentOrientation.y();
m_currentOrientation.x() = -m_currentOrientation.x();
m_currentOrientation.z() = -m_currentOrientation.z();
m_currentAngularVelocity[1] = -m_currentAngularVelocity[1];
}
if (m_invertFlags & InvertFlag::rotZ)
{
m_currentOrientation.z() = -m_currentOrientation.z();
m_currentOrientation.x() = -m_currentOrientation.x();
m_currentOrientation.y() = -m_currentOrientation.y();
m_currentAngularVelocity[2] = -m_currentAngularVelocity[2];
}
......@@ -152,13 +155,13 @@ TrackingDeviceControl::setPosition(const Vec3d& pos)
}
const imstk::Quatd&
TrackingDeviceControl::getRotation() const
TrackingDeviceControl::getOrientation() const
{
return m_currentOrientation;
}
void
TrackingDeviceControl::setRotation(const Quatd& orientation)
TrackingDeviceControl::setOrientation(const Quatd& orientation)
{
this->m_currentOrientation = orientation;
}
......
......@@ -80,8 +80,8 @@ public:
///
/// \brief Set/Get the orientation of the tracker
///@{
const Quatd& getRotation() const;;
void setRotation(const Quatd& orientation);
const Quatd& getOrientation() const;;
void setOrientation(const Quatd& orientation);
///@}
///
......
......@@ -258,8 +258,8 @@ protected:
protected:
// Lookat camera parameters
Vec3d m_position = Vec3d(0, 2, 5); ///> camera position
Vec3d m_focalPoint = WORLD_ORIGIN; ///> camera focal point
Vec3d m_viewUp = UP_VECTOR; ///> camera up vector
Vec3d m_position = Vec3d(0.0, 2.0, 5.0); ///> camera position
Vec3d m_focalPoint = Vec3d::Zero(); ///> camera focal point
Vec3d m_viewUp = Vec3d::UnitY(); ///> camera up vector
};
}
......@@ -236,6 +236,7 @@ public:
imstkQuatf(float x, float y, float z, float w) : m_data(w, x, y, z) { }
inline const EigenData& get() const { return m_data; }
inline imstk::Mat3f toRotationMatrix() { return m_data.toRotationMatrix(); }
inline float x() const { return m_data.x(); }
inline float y() const { return m_data.y(); }
inline float z() const { return m_data.z(); }
......@@ -254,6 +255,7 @@ public:
imstkQuatd(double x, double y, double z, double w) : m_data(w, x, y, z) { }
inline const EigenData& get() const { return m_data; }
inline imstk::Mat3d toRotationMatrix() { return m_data.toRotationMatrix(); }
inline double x() const { return m_data.x(); }
inline double y() const { return m_data.y(); }
inline double z() const { return m_data.z(); }
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment