PbdRigidBodyGraspingExample.cpp

/*
** This file is part of the Interactive Medical Simulation Toolkit (iMSTK)
** iMSTK is distributed under the Apache License, Version 2.0.
** See accompanying NOTICE for details.
*/

#include "imstkAxesModel.h"
#include "imstkCamera.h"
#include "imstkCapsule.h"
#include "imstkControllerForceText.h"
#include "imstkDeviceManager.h"
#include "imstkDeviceManagerFactory.h"
#include "imstkDirectionalLight.h"
#include "imstkIsometricMap.h"
#include "imstkKeyboardDeviceClient.h"
#include "imstkMeshIO.h"
#include "imstkObjectControllerGhost.h"
#include "imstkPbdModel.h"
#include "imstkPbdModelConfig.h"
#include "imstkPbdObject.h"
#include "imstkPbdObjectCollision.h"
#include "imstkPbdObjectController.h"
#include "imstkPbdRigidObjectGrasping.h"
#include "imstkPlane.h"
#include "imstkRenderMaterial.h"
#include "imstkScene.h"
#include "imstkSceneManager.h"
#include "imstkSimulationManager.h"
#include "imstkSimulationUtils.h"
#include "imstkSphere.h"
#include "imstkTextVisualModel.h"
#include "imstkVTKViewer.h"

//#define USE_TWO_HAPTIC_DEVICES

#ifndef USE_TWO_HAPTIC_DEVICES
#include "imstkDummyClient.h"
#include "imstkMouseDeviceClient.h"
#endif

using namespace imstk;

static std::shared_ptr<PbdObject>
makeCapsuleToolObj(std::shared_ptr<PbdModel> model, bool isLeft)
{
    auto toolGeometry = std::make_shared<Capsule>();
    toolGeometry->setRadius(0.005);
    toolGeometry->setLength(0.1);
    toolGeometry->setPosition(Vec3d(0.0, 0.0, 0.0));
    toolGeometry->setOrientation(Quatd::FromTwoVectors(Vec3d(0.0, 1.0, 0.0), Vec3d(0.0, 0.0, 1.0)));

    auto toolObj = std::make_shared<PbdObject>("Tool");

    // Create the object
    toolObj->setVisualGeometry(toolGeometry);
    toolObj->setPhysicsGeometry(toolGeometry);
    toolObj->setCollidingGeometry(toolGeometry);
    toolObj->setDynamicalModel(model);
    toolObj->getPbdBody()->setRigid(
        Vec3d(0.0, 0.1, 0.0),
        30.0,
        Quatd::Identity(),
        Mat3d::Identity() * 1.0);

    toolObj->getVisualModel(0)->getRenderMaterial()->setOpacity(0.9);

    // Add a component for controlling via another device
    auto controller = toolObj->addComponent<PbdObjectController>();
    controller->setControlledObject(toolObj);
    controller->setLinearKs(500000.0);
    controller->setAngularKs(10000.0);
    controller->setUseCritDamping(true);
    controller->setForceScaling(0.002);
    controller->setSmoothingKernelSize(15);
    controller->setUseForceSmoothening(true);

    auto axesModel = toolObj->addComponent<AxesModel>();
    axesModel->setScale(Vec3d(0.05, 0.05, 0.05));

    auto axesUpdate = toolObj->addComponent<LambdaBehaviour>("AxesModelUpdate");
    axesUpdate->setUpdate([ = ](const double& dt)
        {
            axesModel->setPosition((*toolObj->getPbdBody()->vertices)[0]);
            axesModel->setOrientation((*toolObj->getPbdBody()->orientations)[0]);
        });

    // Add extra component to tool for the ghost
    auto controllerGhost = toolObj->addComponent<ObjectControllerGhost>();
    controllerGhost->setController(controller);

    // Add a component to display controller force
    auto controllerForceTxt = toolObj->addComponent<ControllerForceText>();
    if (isLeft)
    {
        controllerForceTxt->getText()->setPosition(TextVisualModel::DisplayPosition::UpperLeft);
    }
    controllerForceTxt->setController(controller);

    return toolObj;
}

int
main()
{
    // Setup logger (write to file and stdout)
    Logger::startLogger();

    // Setup the scene
    auto scene = std::make_shared<Scene>("PbdRigidBodyGrasping");
    scene->getActiveCamera()->setPosition(0.0, 0.5, 0.5);
    scene->getActiveCamera()->setFocalPoint(0.0, 0.0, 0.0);
    scene->getActiveCamera()->setViewUp(0.0, 1.0, 0.0);

    auto                            pbdModel  = std::make_shared<PbdModel>();
    std::shared_ptr<PbdModelConfig> pbdParams = pbdModel->getConfig();
    pbdParams->m_gravity = Vec3d(0.0, -9.8, 0.0);
    //pbdParams->m_gravity = Vec3d::Zero();
    pbdParams->m_dt = 0.002;
    pbdParams->m_iterations = 8;
    pbdParams->m_linearDampingCoeff  = 0.01;
    pbdParams->m_angularDampingCoeff = 0.01;

    // Make a plane
    auto planeObj = std::make_shared<CollidingObject>("PlaneObj");
    auto plane    = std::make_shared<Plane>(Vec3d(0.0, 0.0, 0.0), Vec3d(0.0, 1.0, 0.0));
    plane->setWidth(1.0);
    planeObj->setCollidingGeometry(plane);
    planeObj->setVisualGeometry(plane);
    scene->addSceneObject(planeObj);

    // Make a pbd rigid body needle
    auto needleObj = std::make_shared<PbdObject>();
    {
        auto needleMesh     = MeshIO::read<SurfaceMesh>(iMSTK_DATA_ROOT "/Surgical Instruments/Needles/c6_suture.stl");
        auto needleLineMesh = MeshIO::read<LineMesh>(iMSTK_DATA_ROOT "/Surgical Instruments/Needles/c6_suture_hull.vtk");
        // Transform so center of mass is in center of the needle
        needleMesh->translate(Vec3d(0.0, -0.0047, -0.0087), Geometry::TransformType::ApplyToData);
        needleLineMesh->translate(Vec3d(0.0, -0.0047, -0.0087), Geometry::TransformType::ApplyToData);
        needleMesh->scale(2.0, Geometry::TransformType::ApplyToData);
        needleLineMesh->scale(2.0, Geometry::TransformType::ApplyToData);
        needleObj->setVisualGeometry(needleMesh);
        needleObj->setCollidingGeometry(needleLineMesh);
        needleObj->setPhysicsGeometry(needleLineMesh);
        needleObj->setPhysicsToVisualMap(std::make_shared<IsometricMap>(needleLineMesh, needleMesh));
        needleObj->setDynamicalModel(pbdModel);
        needleObj->getPbdBody()->setRigid(
            Vec3d(-0.1, 0.15, 0.0),
            1.0,
            Quatd::Identity(),
            Mat3d::Identity() * 0.01);
        needleObj->getVisualModel(0)->getRenderMaterial()->setColor(Color::Orange);
    }
    scene->addSceneObject(needleObj);

    // Make a pbd rigid body sphere
    auto sphereObj = std::make_shared<PbdObject>();
    {
        auto sphereGeom = std::make_shared<Sphere>(Vec3d::Zero(), 0.01);
        sphereObj->setVisualGeometry(sphereGeom);
        sphereObj->setCollidingGeometry(sphereGeom);
        sphereObj->setPhysicsGeometry(sphereGeom);
        sphereObj->setDynamicalModel(pbdModel);
        sphereObj->getPbdBody()->setRigid(
            Vec3d(0.1, 0.15, 0.0),
            1.0,
            Quatd::Identity(),
            Mat3d::Identity() * 0.01);
        sphereObj->getVisualModel(0)->getRenderMaterial()->setColor(Color::Blood);
    }
    scene->addSceneObject(sphereObj);

    // Setup a tool to grasp with
    std::shared_ptr<PbdObject> leftToolObj = makeCapsuleToolObj(pbdModel, true);
    scene->addSceneObject(leftToolObj);
    std::shared_ptr<PbdObject> rightToolObj = makeCapsuleToolObj(pbdModel, false);
    scene->addSceneObject(rightToolObj);

    // Add collision between plane and objects
    auto planeCollision0 = std::make_shared<PbdObjectCollision>(needleObj, planeObj);
    planeCollision0->setRigidBodyCompliance(0.00001);
    scene->addInteraction(planeCollision0);
    auto planeCollision1 = std::make_shared<PbdObjectCollision>(sphereObj, planeObj);
    planeCollision1->setRigidBodyCompliance(0.00001);
    scene->addInteraction(planeCollision1);

    // Collision between needle and sphere
    auto sphereNeedleCollision = std::make_shared<PbdObjectCollision>(sphereObj, needleObj, "PointSetToSphereCD");
    sphereNeedleCollision->setRigidBodyCompliance(0.000001);
    scene->addInteraction(sphereNeedleCollision);

    // Add grasping
    auto leftGrasping0 = std::make_shared<PbdObjectGrasping>(needleObj, leftToolObj);
    leftGrasping0->setCompliance(0.00001);
    scene->addInteraction(leftGrasping0);
    auto leftGrasping1 = std::make_shared<PbdObjectGrasping>(sphereObj, leftToolObj);
    leftGrasping1->setCompliance(0.00001);
    scene->addInteraction(leftGrasping1);

    auto rightGrasping0 = std::make_shared<PbdObjectGrasping>(needleObj, rightToolObj);
    rightGrasping0->setCompliance(0.00001);
    scene->addInteraction(rightGrasping0);
    auto rightGrasping1 = std::make_shared<PbdObjectGrasping>(sphereObj, rightToolObj);
    rightGrasping1->setCompliance(0.00001);
    scene->addInteraction(rightGrasping1);

    // Light
    auto light = std::make_shared<DirectionalLight>();
    light->setFocalPoint(Vec3d(5.0, -8.0, -5.0));
    light->setIntensity(1.0);
    scene->addLight("Light", light);

    // Run the simulation
    {
        // Setup a viewer to render
        auto viewer = std::make_shared<VTKViewer>();
        viewer->setActiveScene(scene);
        viewer->setDebugAxesLength(0.05, 0.05, 0.05);

        // Setup a scene manager to advance the scene
        auto sceneManager = std::make_shared<SceneManager>();
        sceneManager->setActiveScene(scene);
        sceneManager->pause();         // Start simulation paused

        // Setup default haptics manager
        std::shared_ptr<DeviceManager> hapticManager = DeviceManagerFactory::makeDeviceManager();

        auto driver = std::make_shared<SimulationManager>();
        driver->addModule(viewer);
        driver->addModule(sceneManager);
        driver->addModule(hapticManager);
        driver->setDesiredDt(0.002);

        auto leftController = leftToolObj->getComponent<PbdObjectController>();

        if (hapticManager->getTypeName() == "HaplyDeviceManager")
        {
            leftController->setTranslationOffset(Vec3d(0.1, 0.0, -0.1));
        }
        std::shared_ptr<DeviceClient> leftDeviceClient = hapticManager->makeDeviceClient();
        leftController->setDevice(leftDeviceClient);

        connect<ButtonEvent>(leftDeviceClient, &DeviceClient::buttonStateChanged,
            [&](ButtonEvent* e)
            {
                if (e->m_button == 1)
                {
                    if (e->m_buttonState == BUTTON_PRESSED)
                    {
                        // Use a slightly larger capsule since collision prevents intersection
                        auto capsule = std::dynamic_pointer_cast<Capsule>(leftToolObj->getCollidingGeometry());
                        auto dilatedCapsule = std::make_shared<Capsule>(*capsule);
                        dilatedCapsule->setRadius(capsule->getRadius() * 1.1);
                        leftGrasping0->beginCellGrasp(dilatedCapsule);
                        leftGrasping1->beginCellGrasp(dilatedCapsule);
                    }
                    else if (e->m_buttonState == BUTTON_RELEASED)
                    {
                        leftGrasping0->endGrasp();
                        leftGrasping1->endGrasp();
                    }
                }
            });
#ifdef USE_TWO_HAPTIC_DEVICES
        std::shared_ptr<DeviceClient> rightDeviceClient = hapticManager->makeDeviceClient("Device2");
        auto                          rightController   = rightToolObj->getComponent<PbdObjectController>();
        rightController->setDevice(rightDeviceClient);

        connect<ButtonEvent>(rightDeviceClient, &DeviceClient::buttonStateChanged,
            [&](ButtonEvent* e)
            {
                if (e->m_button == 1)
                {
                    if (e->m_buttonState == BUTTON_PRESSED)
                    {
                        // Use a slightly larger capsule since collision prevents intersection
                        auto capsule = std::dynamic_pointer_cast<Capsule>(rightToolObj->getCollidingGeometry());
                        auto dilatedCapsule = std::make_shared<Capsule>(*capsule);
                        dilatedCapsule->setRadius(capsule->getRadius() * 1.1);
                        rightGrasping0->beginCellGrasp(dilatedCapsule);
                        rightGrasping1->beginCellGrasp(dilatedCapsule);
                    }
                    else if (e->m_buttonState == BUTTON_RELEASED)
                    {
                        rightGrasping0->endGrasp();
                        rightGrasping1->endGrasp();
                    }
                }
            });
#else
        auto rightDeviceClient = std::make_shared<DummyClient>();
        auto rightController   = rightToolObj->getComponent<PbdObjectController>();
        rightController->setDevice(rightDeviceClient);

        connect<Event>(sceneManager, &SceneManager::postUpdate,
            [&](Event*)
            {
                const Vec2d mousePos = viewer->getMouseDevice()->getPos();
                const Vec3d worldPos = Vec3d(mousePos[0] - 0.5, mousePos[1] - 0.5, 0.0) * 0.1;

                rightDeviceClient->setPosition(worldPos);
            });
        connect<MouseEvent>(viewer->getMouseDevice(), &MouseDeviceClient::mouseButtonPress,
            [&](MouseEvent* e)
            {
                // Use a slightly larger capsule since collision prevents intersection
                auto capsule = std::dynamic_pointer_cast<Capsule>(rightToolObj->getCollidingGeometry());
                auto dilatedCapsule = std::make_shared<Capsule>(*capsule);
                dilatedCapsule->setRadius(capsule->getRadius() * 1.1);
                rightGrasping0->beginCellGrasp(dilatedCapsule);
                rightGrasping1->beginCellGrasp(dilatedCapsule);
            });
        connect<MouseEvent>(viewer->getMouseDevice(), &MouseDeviceClient::mouseButtonRelease,
            [&](MouseEvent* e)
            {
                rightGrasping0->endGrasp();
                rightGrasping1->endGrasp();
            });
#endif

        // Add default mouse and keyboard controls to the viewer
        std::shared_ptr<Entity> mouseAndKeyControls =
            SimulationUtils::createDefaultSceneControl(driver);
        auto instructText = mouseAndKeyControls->getComponent<TextVisualModel>();
        instructText->setText(instructText->getText() +
            "\nMouse Click/Press Haptic Device Button to grasp");
        scene->addSceneObject(mouseAndKeyControls);

        connect<Event>(sceneManager, &SceneManager::preUpdate, [&](Event*)
            {
                // Simulate in real time
                pbdModel->getConfig()->m_dt = sceneManager->getDt();
            });

        driver->start();
    }

    return 0;
}