From 587e225ae4f12edc7f339a4405cf0ea1be9943d2 Mon Sep 17 00:00:00 2001 From: Jacob Moore Date: Wed, 23 Mar 2022 12:23:30 -0400 Subject: [PATCH 1/7] FEAT: Adding coupled grasping. WIP --- Examples/PBD/PBDGrasping/CMakeLists.txt | 34 + .../PBD/PBDGrasping/PBDGraspingExample.cpp | 614 ++++++++++++++++++ Source/Constraint/CMakeLists.txt | 1 + .../PbdRigidBaryPointToPointConstraint.cpp | 98 +++ .../PbdRigidBaryPointToPointConstraint.h | 69 ++ 5 files changed, 816 insertions(+) create mode 100644 Examples/PBD/PBDGrasping/CMakeLists.txt create mode 100644 Examples/PBD/PBDGrasping/PBDGraspingExample.cpp create mode 100644 Source/Constraint/MultiModelConstraints/PbdRigidBaryPointToPointConstraint.cpp create mode 100644 Source/Constraint/MultiModelConstraints/PbdRigidBaryPointToPointConstraint.h diff --git a/Examples/PBD/PBDGrasping/CMakeLists.txt b/Examples/PBD/PBDGrasping/CMakeLists.txt new file mode 100644 index 000000000..672f2a513 --- /dev/null +++ b/Examples/PBD/PBDGrasping/CMakeLists.txt @@ -0,0 +1,34 @@ +########################################################################### +# +# 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-PBDGrasping) + +#----------------------------------------------------------------------------- +# Create executable +#----------------------------------------------------------------------------- +imstk_add_executable(${PROJECT_NAME} PBDGraspingExample.cpp) + +#----------------------------------------------------------------------------- +# 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 ViewerVTK) diff --git a/Examples/PBD/PBDGrasping/PBDGraspingExample.cpp b/Examples/PBD/PBDGrasping/PBDGraspingExample.cpp new file mode 100644 index 000000000..272e29be3 --- /dev/null +++ b/Examples/PBD/PBDGrasping/PBDGraspingExample.cpp @@ -0,0 +1,614 @@ +/*========================================================================= + + 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 "imstkCamera.h" +#include "imstkDirectionalLight.h" +#include "imstkImageData.h" +#include "imstkKeyboardDeviceClient.h" +#include "imstkKeyboardSceneControl.h" +#include "imstkLineMesh.h" +#include "imstkMeshIO.h" +#include "imstkMeshToMeshBruteForceCD.h" +#include "imstkMouseControl.h" +#include "imstkMouseSceneControl.h" +#include "imstkNew.h" +#include "imstkOneToOneMap.h" + +#include "imstkPbdBaryPointToPointConstraint.h" + +#include "imstkPbdModel.h" +#include "imstkPbdObject.h" +#include "imstkPbdObjectCollision.h" +// #include "imstkPbdObjectPicking.h" +// #include "imstkPbdPickingCH.h" +#include "imstkPbdObjectGrasping.h" +#include "imstkRbdConstraint.h" +#include "imstkRenderMaterial.h" +#include "imstkRigidBodyModel2.h" +#include "imstkRigidObject2.h" +#include "imstkScene.h" +#include "imstkSceneManager.h" +#include "imstkSimulationManager.h" +#include "imstkSurfaceMesh.h" +#include "imstkTetrahedralMesh.h" +#include "imstkVisualModel.h" +#include "imstkVTKViewer.h" +#include "imstkCapsule.h" +#include "imstkSphere.h" +#include "imstkCollisionHandling.h" +#include "imstkPbdObjectCollision.h" +#include "imstkPointSetToCapsuleCD.h" +#include "imstkSurfaceMeshToCapsuleCD.h" + +#include "imstkTaskNode.h" +#include "imstkTaskGraph.h" +#include "imstkCollisionDetectionAlgorithm.h" +#include "imstkMouseDeviceClient.h" + +#include "PbdRigidBaryPointToPointConstraint.h" + +// If two-way coupling is used haptic forces can be felt when the tool +// hits the tissue +//#define TWOWAY_COUPLING + +// Whether to use FEM or volume+distance constraints +#define USE_FEM + +#ifdef TWOWAY_COUPLING +#include "imstkPbdRigidObjectCollision.h" +#else +#include "imstkPbdObjectCollision.h" +#endif + +#ifdef iMSTK_USE_OPENHAPTICS +#include "imstkHapticDeviceManager.h" +#include "imstkHapticDeviceClient.h" +#include "imstkRigidObjectController.h" +#endif + +using namespace imstk; + +// Define constraint Note: can be given to either solver +// Single point, build multiple of them for each entity in contact +class PbdRigidBaryPointToPointConstraint : public PbdBaryPointToPointConstraint, public RbdConstraint +{ +private: + double m_beta = 0.0001; + +public: + // Need to pass in all data for PbdCollisionConstraint and RbdConstraint? + PbdRigidBaryPointToPointConstraint(std::shared_ptr obj1) : + + PbdBaryPointToPointConstraint(), // (point on capsue, point on mesh) + + RbdConstraint( // Pass in obj1, nullpt and say side A + obj1, + nullptr, + RbdConstraint::Side::A) + { + } + ~PbdRigidBaryPointToPointConstraint() override = default; + +public: + /// + /// \brief compute value and gradient of constraint function + /// + /// \param[inout] c constraint value + /// \param[inout] dcdx constraint gradient for A + /// \param[inout] dcdx constraint gradient for B + /// Call for RBD, push point on mesh to the fixed point + bool computeValueAndGradient(double& c, + std::vector& dcdxA, + std::vector& dcdxB) const override + { + // Compute the difference between the interpolant points (points in the two cells) + Vec3d diff = 0.5*computeInterpolantDifference(); + + c = diff.norm(); + + if (c < IMSTK_DOUBLE_EPS) + { + diff = Vec3d::Zero(); + return false; + } + diff /= c; + + for (size_t i = 0; i < dcdxA.size(); i++) + { + dcdxA[i] = diff * m_weightsA[i]; + } + for (size_t i = 0; i < dcdxB.size(); i++) + { + dcdxB[i] = -diff * m_weightsB[i]; + } + + return true; + } + +public: + // call for RBD + // Give change in impulse in direction of desired deformation + void compute(double dt) override + { + J = Eigen::Matrix::Zero(); + + // Compute the difference between the interpolant points (points in the two cells) + Vec3d diff = 0.5*computeInterpolantDifference(); + + J(0, 0) = -diff[0]; J(0, 1) = 0.0; + J(1, 0) = -diff[1]; J(1, 1) = 0.0; + J(2, 0) = -diff[2]; J(2, 1) = 0.0; + + // B stabilization term + vu = diff.norm() * m_beta / dt; + } +}; + + +class PbdRigidObjectGrasping : public PbdObjectGrasping +{ +protected: + std::shared_ptr m_rbdObj = nullptr; + std::shared_ptr m_pbdObj = nullptr; + +public: + PbdRigidObjectGrasping( + std::shared_ptr obj1, + std::shared_ptr obj2) : + PbdObjectGrasping(obj1), m_rbdObj(obj2) + { + m_pbdObj = obj1; + + m_taskGraph->addNode(m_pickingNode); + m_taskGraph->addNode(obj2->getRigidBodyModel2()->getSolveNode()); + } + ~PbdRigidObjectGrasping() override = default; + + const std::string getTypeName() const override { return "PbdRigidObjectGrasping"; } + + void updatePicking() + { + PbdObjectGrasping::updatePicking(); + + // LOG(INFO) << "Before PBDRBD constraints"; + for (int i = 0; i < m_constraints.size(); i++) + { + auto constraint = std::dynamic_pointer_cast(m_constraints[i]); + + constraint->compute(m_rbdObj->getRigidBodyModel2()->getConfig()->m_dt); + m_rbdObj->getRigidBodyModel2()->addConstraint(constraint); + } + } + + void addConstraint( + std::vector ptsA, // Point data + std::vector weightsA, // Barycentric weight + std::vector ptsB, + std::vector weightsB, + double stiffnessA, // Constraint stiffness + double stiffnessB) + override + { + // Create constraint + auto constraint = std::make_shared(m_rbdObj->getRigidBody()); + constraint->initConstraint( + ptsA, + weightsA, + ptsB, + weightsB, + stiffnessA, + stiffnessB); + // Add to constraints + m_constraints.push_back(constraint); + } + + void initGraphEdges(std::shared_ptr source, std::shared_ptr sink) override + { + LOG(INFO) << "Inside initGraphEdges"; + + PbdObjectGrasping::initGraphEdges(source, sink); + + std::shared_ptr pbdObj = m_pbdObj; + + std::shared_ptr pbdModel = pbdObj->getPbdModel(); + std::shared_ptr rbdModel = m_rbdObj->getRigidBodyModel2(); + + + m_taskGraph->addEdge(m_pickingNode, rbdModel->getSolveNode()); + m_taskGraph->addEdge(rbdModel->getSolveNode(), m_taskGraph->getSink()); + + LOG(INFO) << "Leaving initGraphEdges"; + } +}; + +/// +/// \brief Creates a tetraheral grid +/// \param physical dimension of tissue +/// \param dimensions of tetrahedral grid used for tissue +/// \param center of grid +/// +static std::shared_ptr +makeTetGrid(const Vec3d& size, const Vec3i& dim, const Vec3d& center) +{ + imstkNew prismMesh; + + imstkNew> verticesPtr(dim[0] * dim[1] * dim[2]); + VecDataArray& vertices = *verticesPtr.get(); + const Vec3d dx = size.cwiseQuotient((dim - Vec3i(1, 1, 1)).cast()); + for (int z = 0; z < dim[2]; z++) + { + for (int y = 0; y < dim[1]; y++) + { + for (int x = 0; x < dim[0]; x++) + { + vertices[x + dim[0] * (y + dim[1] * z)] = Vec3i(x, y, z).cast().cwiseProduct(dx) - size * 0.5 + center; + } + } + } + + // Add connectivity data + imstkNew> indicesPtr; + VecDataArray& indices = *indicesPtr.get(); + for (int z = 0; z < dim[2] - 1; z++) + { + for (int y = 0; y < dim[1] - 1; y++) + { + for (int x = 0; x < dim[0] - 1; x++) + { + int cubeIndices[8] = + { + x + dim[0] * (y + dim[1] * z), + (x + 1) + dim[0] * (y + dim[1] * z), + (x + 1) + dim[0] * (y + dim[1] * (z + 1)), + x + dim[0] * (y + dim[1] * (z + 1)), + x + dim[0] * ((y + 1) + dim[1] * z), + (x + 1) + dim[0] * ((y + 1) + dim[1] * z), + (x + 1) + dim[0] * ((y + 1) + dim[1] * (z + 1)), + x + dim[0] * ((y + 1) + dim[1] * (z + 1)) + }; + + // Alternate the pattern so the edges line up on the sides of each voxel + if ((z % 2 ^ x % 2) ^ y % 2) + { + indices.push_back(Vec4i(cubeIndices[0], cubeIndices[7], cubeIndices[5], cubeIndices[4])); + indices.push_back(Vec4i(cubeIndices[3], cubeIndices[7], cubeIndices[2], cubeIndices[0])); + indices.push_back(Vec4i(cubeIndices[2], cubeIndices[7], cubeIndices[5], cubeIndices[0])); + indices.push_back(Vec4i(cubeIndices[1], cubeIndices[2], cubeIndices[0], cubeIndices[5])); + indices.push_back(Vec4i(cubeIndices[2], cubeIndices[6], cubeIndices[7], cubeIndices[5])); + } + else + { + indices.push_back(Vec4i(cubeIndices[3], cubeIndices[7], cubeIndices[6], cubeIndices[4])); + indices.push_back(Vec4i(cubeIndices[1], cubeIndices[3], cubeIndices[6], cubeIndices[4])); + indices.push_back(Vec4i(cubeIndices[3], cubeIndices[6], cubeIndices[2], cubeIndices[1])); + indices.push_back(Vec4i(cubeIndices[1], cubeIndices[6], cubeIndices[5], cubeIndices[4])); + indices.push_back(Vec4i(cubeIndices[0], cubeIndices[3], cubeIndices[1], cubeIndices[4])); + } + } + } + } + + imstkNew> uvCoordsPtr(dim[0] * dim[1] * dim[2]); + VecDataArray& uvCoords = *uvCoordsPtr.get(); + for (int z = 0; z < dim[2]; z++) + { + for (int y = 0; y < dim[1]; y++) + { + for (int x = 0; x < dim[0]; x++) + { + uvCoords[x + dim[0] * (y + dim[1] * z)] = Vec2f(static_cast(x) / dim[0], static_cast(z) / dim[2]) * 3.0; + } + } + } + + // Ensure correct windings + for (int i = 0; i < indices.size(); i++) + { + if (tetVolume(vertices[indices[i][0]], vertices[indices[i][1]], vertices[indices[i][2]], vertices[indices[i][3]]) < 0.0) + { + std::swap(indices[i][0], indices[i][2]); + } + } + + prismMesh->initialize(verticesPtr, indicesPtr); + prismMesh->setVertexTCoords("uvs", uvCoordsPtr); + + return prismMesh; +} + + +static std::shared_ptr +makePbdObjSurface(const std::string& name, + const Vec3d& size, + const Vec3i& dim, + const Vec3d& center, + const int numIter) +{ + imstkNew prismObj(name); + + // Setup the Geometry + std::shared_ptr prismMesh = makeTetGrid(size, dim, center); + std::shared_ptr surfMesh = prismMesh->extractSurfaceMesh(); + + // Setup the Parameters + imstkNew pbdParams; + + // Use volume+distance constraints, worse results. More performant (can use larger mesh) + pbdParams->enableConstraint(PbdModelConfig::ConstraintGenType::Dihedral, 1.0); + pbdParams->enableConstraint(PbdModelConfig::ConstraintGenType::Distance, 1.0); + + pbdParams->m_doPartitioning = true; + pbdParams->m_uniformMassValue = 0.05; + pbdParams->m_gravity = Vec3d(0.0, 0.0, 0.0); + pbdParams->m_dt = 0.005; + pbdParams->m_iterations = numIter; + pbdParams->m_viscousDampingCoeff = 0.003; + + // Fix the borders + for (int vert_id = 0; vert_id < surfMesh->getNumVertices(); vert_id++) + { + auto position = surfMesh->getVertexPosition(vert_id); + + if (position(1) == -2.0) { + pbdParams->m_fixedNodeIds.push_back(vert_id); + } + + } + + // Setup the Model + imstkNew pbdModel; + pbdModel->setModelGeometry(surfMesh); + pbdModel->configure(pbdParams); + + // Setup the material + imstkNew material; + material->setDisplayMode(RenderMaterial::DisplayMode::Wireframe); + + // Add a visual model to render the surface of the tet mesh + imstkNew visualModel; + visualModel->setGeometry(surfMesh); + visualModel->setRenderMaterial(material); + prismObj->addVisualModel(visualModel); + + + // Setup the Object + prismObj->setPhysicsGeometry(surfMesh); + prismObj->setCollidingGeometry(surfMesh); + prismObj->setDynamicalModel(pbdModel); + + return prismObj; +} + +static std::shared_ptr +makeCapsuleToolObj() +{ + // Set up rigid body model + std::shared_ptr rbdModel = std::make_shared(); + rbdModel->getConfig()->m_gravity = Vec3d::Zero(); + rbdModel->getConfig()->m_maxNumIterations = 8; + rbdModel->getConfig()->m_velocityDamping = 1.0; + rbdModel->getConfig()->m_angularVelocityDamping = 1.0; + rbdModel->getConfig()->m_maxNumConstraints = 40; + + + auto toolGeometry = std::make_shared(); + toolGeometry->setRadius(0.5); + toolGeometry->setLength(1); + toolGeometry->setPosition(Vec3d(0.0, 0.0, 0.0)); + toolGeometry->setOrientation(Quatd(0.707, 0.0, 0.0, 0.707)); + + std::shared_ptr toolObj = std::make_shared("Tool"); + + // Create the object + toolObj->setVisualGeometry(toolGeometry); + toolObj->setPhysicsGeometry(toolGeometry); + toolObj->setCollidingGeometry(toolGeometry); + toolObj->setDynamicalModel(rbdModel); + toolObj->getRigidBody()->m_mass = 1.0; + toolObj->getRigidBody()->m_intertiaTensor = Mat3d::Identity() * 1.0; + toolObj->getRigidBody()->m_initPos = Vec3d(0.0, 1.0, 2.0); + + toolObj->getVisualModel(0)->getRenderMaterial()->setOpacity(0.5); + + return toolObj; +} + + +/// +/// \brief This example demonstrates collision interaction with a 3d pbd +/// simulated tissue (tetrahedral) +/// +int +main() +{ + // Setup logger (write to file and stdout) + Logger::startLogger(); + + // Setup the scene + imstkNew scene("PBDGrasping"); + scene->getActiveCamera()->setPosition(0.12, 4.51, 16.51); + scene->getActiveCamera()->setFocalPoint(0.0, 0.0, 0.0); + scene->getActiveCamera()->setViewUp(0.0, 0.96, -0.28); + + scene->getConfig()->writeTaskGraph = true; + + // Setup a tissue + std::shared_ptr PbdObj = makePbdObjSurface("Tissue", + Vec3d(4.0, 4.0, 4.0), Vec3i(5, 5, 5), Vec3d(0.0, 0.0, 0.0), 8); + scene->addSceneObject(PbdObj); + + std::shared_ptr toolObj = makeCapsuleToolObj(); + scene->addSceneObject(toolObj); + + auto rbdGhost = std::make_shared("ghost"); + auto ghostCapsule = std::make_shared(); + ghostCapsule->setRadius(0.5); + ghostCapsule->setLength(1); + ghostCapsule->setPosition(Vec3d(0.0, 0.0, 0.0)); + ghostCapsule->setOrientation(Quatd(0.707, 0.0, 0.0, 0.707)); + rbdGhost->setVisualGeometry(ghostCapsule); + + + std::shared_ptr ghostMat = + std::make_shared(*toolObj->getVisualModel(0)->getRenderMaterial()); + ghostMat->setOpacity(0.4); + ghostMat->setColor(Color::Red); + rbdGhost->getVisualModel(0)->setRenderMaterial(ghostMat); + scene->addSceneObject(rbdGhost); + + // Add collision + // auto pbdToolCollision = std::make_shared(PbdObj, toolObj, "SurfaceMeshToCapsuleCD"); + // scene->addInteraction(pbdToolCollision); + + // Create new picking with constraints + auto toolPicking = std::make_shared(PbdObj, toolObj); + scene->addInteraction(toolPicking); + + + // auto toolPicking = std::make_shared(PbdObj); + // scene->addInteraction(toolPicking); + + // Light + imstkNew light; + light->setFocalPoint(Vec3d(5.0, -8.0, -5.0)); + light->setIntensity(1); + scene->addLight("Light", light); + + // scene->getConfig()->writeTaskGraph = true; + + // Run the simulation + { + // Setup a viewer to render + imstkNew viewer; + viewer->setActiveScene(scene); + viewer->setVtkLoggerMode(VTKViewer::VTKLoggerMode::MUTE); + + // Setup a scene manager to advance the scene + imstkNew sceneManager; + sceneManager->setActiveScene(scene); + sceneManager->pause(); // Start simulation paused + + imstkNew driver; + driver->addModule(viewer); + driver->addModule(sceneManager); + driver->setDesiredDt(0.002); + +#ifdef iMSTK_USE_OPENHAPTICS + imstkNew hapticManager; + hapticManager->setSleepDelay(1.0); // Delay for 1ms (haptics thread is limited to max 1000hz) + std::shared_ptr hapticDeviceClient = hapticManager->makeDeviceClient(); + driver->addModule(hapticManager); + + imstkNew controller(toolObj, hapticDeviceClient); + controller->setTranslationScaling(0.05); + controller->setLinearKs(10000.0); + controller->setAngularKs(1000.0); + //controller->setAngularKs(0.0); + controller->setUseCritDamping(true); + controller->setForceScaling(0.001); + controller->setSmoothingKernelSize(15); + controller->setUseForceSmoothening(true); + scene->addController(controller); + + connect(sceneManager, &SceneManager::postUpdate, + [&](Event*) + { + ghostMat->setOpacity(std::min(1.0, controller->getDeviceForce().norm() / 15.0)); + + // Also apply controller transform to ghost geometry + ghostCapsule->setTranslation(controller->getPosition()); + ghostCapsule->setRotation(controller->getOrientation()); + ghostCapsule->updatePostTransformData(); + ghostCapsule->postModified(); + }); + connect(hapticDeviceClient, &HapticDeviceClient::buttonStateChanged, + [&](ButtonEvent* e) + { + if (e->m_buttonState == BUTTON_PRESSED) + { + if (e->m_button == 1) + { + toolPicking->beginVertexGrasp(std::dynamic_pointer_cast(toolObj->getCollidingGeometry())); + } + } + else if (e->m_buttonState == BUTTON_RELEASED) + { + toolPicking->endGrasp(); + } + }); +#else + connect(sceneManager, &SceneManager::postUpdate, + [&](Event*) + { + const Vec2d mousePos = viewer->getMouseDevice()->getPos(); + const Vec3d worldPos = Vec3d(mousePos[0] - 0.5, mousePos[1] - 0.5, 0.0) * 10.0; + + const Vec3d fS = (worldPos - toolObj->getRigidBody()->getPosition()) * 100000.0; // Spring force + const Vec3d fD = -toolObj->getRigidBody()->getVelocity() * 100.0; // Spring damping + + (*toolObj->getRigidBody()->m_force) += (fS + fD); + + // Also apply controller transform to ghost geometry + ghostCapsule->setTranslation(worldPos); + ghostCapsule->setRotation(Mat3d::Identity()); + ghostCapsule->updatePostTransformData(); + ghostCapsule->postModified(); + }); + + // Add click event and side effects + connect(viewer->getMouseDevice(), &MouseDeviceClient::mouseButtonPress, + [&](Event*) + { + toolPicking->beginVertexGrasp(std::dynamic_pointer_cast(toolObj->getCollidingGeometry())); + }); + + // Add click event and side effects + connect(viewer->getMouseDevice(), &MouseDeviceClient::mouseButtonRelease, + [&](Event*) + { + // When click, grab all verticese associated with the triangle + // faces in contact and force motion with tool. + toolPicking->endGrasp(); + }); +#endif + + // Add mouse and keyboard controls to the viewer + imstkNew mouseControl(viewer->getMouseDevice()); + mouseControl->setSceneManager(sceneManager); + viewer->addControl(mouseControl); + + imstkNew keyControl(viewer->getKeyboardDevice()); + keyControl->setSceneManager(sceneManager); + keyControl->setModuleDriver(driver); + viewer->addControl(keyControl); + + connect(sceneManager, &SceneManager::postUpdate, [&](Event*) + { + // Simulate the cube in real time + PbdObj->getPbdModel()->getConfig()->m_dt = sceneManager->getDt(); + toolObj->getRigidBodyModel2()->getConfig()->m_dt = sceneManager->getDt(); + }); + + driver->start(); + } + + return 0; +} \ No newline at end of file diff --git a/Source/Constraint/CMakeLists.txt b/Source/Constraint/CMakeLists.txt index 0b56ac633..9d7bd3297 100644 --- a/Source/Constraint/CMakeLists.txt +++ b/Source/Constraint/CMakeLists.txt @@ -6,6 +6,7 @@ imstk_add_library( Constraints SUBDIR_LIST PbdConstraints RigidBodyConstraints + MultiModelConstraints DEPENDS Common DataStructures diff --git a/Source/Constraint/MultiModelConstraints/PbdRigidBaryPointToPointConstraint.cpp b/Source/Constraint/MultiModelConstraints/PbdRigidBaryPointToPointConstraint.cpp new file mode 100644 index 000000000..303daec03 --- /dev/null +++ b/Source/Constraint/MultiModelConstraints/PbdRigidBaryPointToPointConstraint.cpp @@ -0,0 +1,98 @@ +/*========================================================================= + + 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 "imstkPbdBaryPointToPointConstraint.h" +#include "imstkRbdConstraint.h" +#include "PbdRigidBaryPointToPointConstraint.h" + + +namespace imstk +{ + + +PbdRigidBaryPointToPointConstraint::PbdRigidBaryPointToPointConstraint(std::shared_ptr obj1) : + PbdBaryPointToPointConstraint(), // (point on capsue, point on mesh) + RbdConstraint( // Pass in obj1, nullpt and say side A + obj1, + nullptr, + RbdConstraint::Side::A) + { + } + + +// PbdRigidBaryPointToPointConstraint::~PbdRigidBaryPointToPointConstraint(){}; + +/// +/// \brief compute value and gradient of constraint function +/// +/// \param[inout] c constraint value +/// \param[inout] dcdx constraint gradient for A +/// \param[inout] dcdx constraint gradient for B +/// Call for RBD, push point on mesh to the fixed point +bool +PbdRigidBaryPointToPointConstraint::computeValueAndGradient( + double& c, + std::vector& dcdxA, + std::vector& dcdxB) const override +{ + // Compute the difference between the interpolant points (points in the two cells) + Vec3d diff = 0.5*computeInterpolantDifference(); + + c = diff.norm(); + + if (c < IMSTK_DOUBLE_EPS) + { + diff = Vec3d::Zero(); + return false; + } + diff /= c; + + for (size_t i = 0; i < dcdxA.size(); i++) + { + dcdxA[i] = diff * m_weightsA[i]; + } + for (size_t i = 0; i < dcdxB.size(); i++) + { + dcdxB[i] = -diff * m_weightsB[i]; + } + + return true; +} + + +void +PbdRigidBaryPointToPointConstraint::compute(double dt) override +{ + J = Eigen::Matrix::Zero(); + + // Compute the difference between the interpolant points (points in the two cells) + Vec3d diff = 0.5*computeInterpolantDifference(); + + J(0, 0) = -diff[0]; J(0, 1) = 0.0; + J(1, 0) = -diff[1]; J(1, 1) = 0.0; + J(2, 0) = -diff[2]; J(2, 1) = 0.0; + + // B stabilization term + vu = diff.norm() * m_beta / dt; +} + + +} // namespace imstk \ No newline at end of file diff --git a/Source/Constraint/MultiModelConstraints/PbdRigidBaryPointToPointConstraint.h b/Source/Constraint/MultiModelConstraints/PbdRigidBaryPointToPointConstraint.h new file mode 100644 index 000000000..60ee59921 --- /dev/null +++ b/Source/Constraint/MultiModelConstraints/PbdRigidBaryPointToPointConstraint.h @@ -0,0 +1,69 @@ +/*========================================================================= + + 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 "imstkPbdBaryPointToPointConstraint.h" +#include "imstkRbdConstraint.h" + +namespace imstk +{ +// Define constraint Note: can be given to either solver +// Single point, build multiple of them for each entity in contact +class PbdRigidBaryPointToPointConstraint : public PbdBaryPointToPointConstraint, public RbdConstraint +{ +private: + double m_beta = 0.0001; + +public: + // Need to pass in all data for PbdCollisionConstraint and RbdConstraint? + PbdRigidBaryPointToPointConstraint(std::shared_ptr obj1) : + + PbdBaryPointToPointConstraint(), // (point on capsue, point on mesh) + + RbdConstraint( // Pass in obj1, nullpt and say side A + obj1, + nullptr, + RbdConstraint::Side::A){}; + ~PbdRigidBaryPointToPointConstraint() override = default; + +public: + /// + /// \brief compute value and gradient of constraint function + /// + /// \param[inout] c constraint value + /// \param[inout] dcdx constraint gradient for A + /// \param[inout] dcdx constraint gradient for B + /// Call for RBD, push point on mesh to the fixed point + bool computeValueAndGradient( + double& c, + std::vector& dcdxA, + std::vector& dcdxB) const override; + +public: + // Constraint call for RBD + // Give change in impulse in direction of desired deformation + void compute(double dt) override; + +}; + + +} // namespace imstk \ No newline at end of file -- GitLab From 9a7de60f694d7c7e72725a3b287c61a130d821ab Mon Sep 17 00:00:00 2001 From: Harald Scheirich Date: Wed, 23 Mar 2022 14:07:45 -0400 Subject: [PATCH 2/7] Updates from call --- .../PBD/PBDGrasping/PBDGraspingExample.cpp | 95 +--- .../PbdRigidBaryPointToPointConstraint.cpp | 6 +- .../PbdRigidBaryPointToPointConstraint.h | 13 +- Source/Scene/imstkPbdObjectGrasping.cpp | 426 ++++++++++++++++++ Source/Scene/imstkPbdObjectGrasping.h | 171 +++++++ 5 files changed, 613 insertions(+), 98 deletions(-) create mode 100644 Source/Scene/imstkPbdObjectGrasping.cpp create mode 100644 Source/Scene/imstkPbdObjectGrasping.h diff --git a/Examples/PBD/PBDGrasping/PBDGraspingExample.cpp b/Examples/PBD/PBDGrasping/PBDGraspingExample.cpp index 272e29be3..c89980aba 100644 --- a/Examples/PBD/PBDGrasping/PBDGraspingExample.cpp +++ b/Examples/PBD/PBDGrasping/PBDGraspingExample.cpp @@ -86,83 +86,6 @@ using namespace imstk; -// Define constraint Note: can be given to either solver -// Single point, build multiple of them for each entity in contact -class PbdRigidBaryPointToPointConstraint : public PbdBaryPointToPointConstraint, public RbdConstraint -{ -private: - double m_beta = 0.0001; - -public: - // Need to pass in all data for PbdCollisionConstraint and RbdConstraint? - PbdRigidBaryPointToPointConstraint(std::shared_ptr obj1) : - - PbdBaryPointToPointConstraint(), // (point on capsue, point on mesh) - - RbdConstraint( // Pass in obj1, nullpt and say side A - obj1, - nullptr, - RbdConstraint::Side::A) - { - } - ~PbdRigidBaryPointToPointConstraint() override = default; - -public: - /// - /// \brief compute value and gradient of constraint function - /// - /// \param[inout] c constraint value - /// \param[inout] dcdx constraint gradient for A - /// \param[inout] dcdx constraint gradient for B - /// Call for RBD, push point on mesh to the fixed point - bool computeValueAndGradient(double& c, - std::vector& dcdxA, - std::vector& dcdxB) const override - { - // Compute the difference between the interpolant points (points in the two cells) - Vec3d diff = 0.5*computeInterpolantDifference(); - - c = diff.norm(); - - if (c < IMSTK_DOUBLE_EPS) - { - diff = Vec3d::Zero(); - return false; - } - diff /= c; - - for (size_t i = 0; i < dcdxA.size(); i++) - { - dcdxA[i] = diff * m_weightsA[i]; - } - for (size_t i = 0; i < dcdxB.size(); i++) - { - dcdxB[i] = -diff * m_weightsB[i]; - } - - return true; - } - -public: - // call for RBD - // Give change in impulse in direction of desired deformation - void compute(double dt) override - { - J = Eigen::Matrix::Zero(); - - // Compute the difference between the interpolant points (points in the two cells) - Vec3d diff = 0.5*computeInterpolantDifference(); - - J(0, 0) = -diff[0]; J(0, 1) = 0.0; - J(1, 0) = -diff[1]; J(1, 1) = 0.0; - J(2, 0) = -diff[2]; J(2, 1) = 0.0; - - // B stabilization term - vu = diff.norm() * m_beta / dt; - } -}; - - class PbdRigidObjectGrasping : public PbdObjectGrasping { protected: @@ -199,12 +122,11 @@ public: } void addConstraint( - std::vector ptsA, // Point data - std::vector weightsA, // Barycentric weight - std::vector ptsB, - std::vector weightsB, - double stiffnessA, // Constraint stiffness - double stiffnessB) + const std::vector& ptsA, + const std::vector& weightsA, + const std::vector& ptsB, + const std::vector& weightsB, + double stiffnessA, double stiffnessB) override { // Create constraint @@ -524,7 +446,7 @@ main() //controller->setAngularKs(0.0); controller->setUseCritDamping(true); controller->setForceScaling(0.001); - controller->setSmoothingKernelSize(15); + controller->setSmoothingKernelSize(10); controller->setUseForceSmoothening(true); scene->addController(controller); @@ -551,7 +473,10 @@ main() } else if (e->m_buttonState == BUTTON_RELEASED) { - toolPicking->endGrasp(); + if (e->m_button == 1) + { + toolPicking->endGrasp(); + } } }); #else diff --git a/Source/Constraint/MultiModelConstraints/PbdRigidBaryPointToPointConstraint.cpp b/Source/Constraint/MultiModelConstraints/PbdRigidBaryPointToPointConstraint.cpp index 303daec03..fd3db9b1e 100644 --- a/Source/Constraint/MultiModelConstraints/PbdRigidBaryPointToPointConstraint.cpp +++ b/Source/Constraint/MultiModelConstraints/PbdRigidBaryPointToPointConstraint.cpp @@ -45,13 +45,13 @@ PbdRigidBaryPointToPointConstraint::PbdRigidBaryPointToPointConstraint(std::shar /// /// \param[inout] c constraint value /// \param[inout] dcdx constraint gradient for A -/// \param[inout] dcdx constraint gradient for B /// Call for RBD, push point on mesh to the fixed point +/// \param[inout] dcdx constraint gradient for B bool PbdRigidBaryPointToPointConstraint::computeValueAndGradient( double& c, std::vector& dcdxA, - std::vector& dcdxB) const override + std::vector& dcdxB) const { // Compute the difference between the interpolant points (points in the two cells) Vec3d diff = 0.5*computeInterpolantDifference(); @@ -79,7 +79,7 @@ PbdRigidBaryPointToPointConstraint::computeValueAndGradient( void -PbdRigidBaryPointToPointConstraint::compute(double dt) override +PbdRigidBaryPointToPointConstraint::compute(double dt) { J = Eigen::Matrix::Zero(); diff --git a/Source/Constraint/MultiModelConstraints/PbdRigidBaryPointToPointConstraint.h b/Source/Constraint/MultiModelConstraints/PbdRigidBaryPointToPointConstraint.h index 60ee59921..a5290cb4a 100644 --- a/Source/Constraint/MultiModelConstraints/PbdRigidBaryPointToPointConstraint.h +++ b/Source/Constraint/MultiModelConstraints/PbdRigidBaryPointToPointConstraint.h @@ -35,14 +35,7 @@ private: public: // Need to pass in all data for PbdCollisionConstraint and RbdConstraint? - PbdRigidBaryPointToPointConstraint(std::shared_ptr obj1) : - - PbdBaryPointToPointConstraint(), // (point on capsue, point on mesh) - - RbdConstraint( // Pass in obj1, nullpt and say side A - obj1, - nullptr, - RbdConstraint::Side::A){}; + PbdRigidBaryPointToPointConstraint(std::shared_ptr obj1); ~PbdRigidBaryPointToPointConstraint() override = default; public: @@ -50,8 +43,8 @@ public: /// \brief compute value and gradient of constraint function /// /// \param[inout] c constraint value - /// \param[inout] dcdx constraint gradient for A - /// \param[inout] dcdx constraint gradient for B + /// \param[inout] dcdxA constraint gradient for A + /// \param[inout] dcdxB constraint gradient for B /// Call for RBD, push point on mesh to the fixed point bool computeValueAndGradient( double& c, diff --git a/Source/Scene/imstkPbdObjectGrasping.cpp b/Source/Scene/imstkPbdObjectGrasping.cpp new file mode 100644 index 000000000..1e7e5dc49 --- /dev/null +++ b/Source/Scene/imstkPbdObjectGrasping.cpp @@ -0,0 +1,426 @@ +/*========================================================================= + +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 "imstkPbdObjectGrasping.h" +#include "imstkAnalyticalGeometry.h" +#include "imstkCDObjectFactory.h" +#include "imstkCellPicker.h" +#include "imstkLineMesh.h" +#include "imstkPbdBaryPointToPointConstraint.h" +#include "imstkPbdModel.h" +#include "imstkPbdObject.h" +#include "imstkPointPicker.h" +#include "imstkSurfaceMesh.h" +#include "imstkTaskGraph.h" +#include "imstkTetrahedralMesh.h" +#include "imstkVertexPicker.h" + +namespace imstk +{ +/// +/// \brief Packs the info needed to add a constraint to a side +/// by reference (this way dynamic casting & dereferencing is not +/// being done in tight loops) +/// +struct MeshSide +{ + MeshSide(VecDataArray& vertices, VecDataArray& velocities, DataArray& invMasses, + AbstractDataArray* indicesPtr) : m_vertices(vertices), m_velocities(velocities), + m_invMasses(invMasses), m_indicesPtr(indicesPtr) + { + } + + VecDataArray& m_vertices; + VecDataArray& m_velocities; + DataArray& m_invMasses; + AbstractDataArray* m_indicesPtr = nullptr; +}; + +template +static std::vector> +getElement(const PickData& pickData, const MeshSide& side) +{ + std::vector> results(N); + if (pickData.idCount == 1) // If given cell index + { + const Eigen::Matrix& cell = (*dynamic_cast*>(side.m_indicesPtr))[pickData.ids[0]]; + for (int i = 0; i < N; i++) + { + const int vertexId = cell[i]; + results[i] = { vertexId, { &side.m_vertices[vertexId], side.m_invMasses[vertexId], &side.m_velocities[vertexId] } }; + } + } + else // If given vertex indices + { + for (int i = 0; i < N; i++) + { + const int vertexId = pickData.ids[i]; + results[i] = { vertexId, { &side.m_vertices[vertexId], side.m_invMasses[vertexId], &side.m_velocities[vertexId] } }; + } + } + return results; +} + +PbdObjectGrasping::PbdObjectGrasping(std::shared_ptr obj) : + SceneObject("PbdObjectGrasping_" + obj->getName()), + m_objectToGrasp(obj), m_pickMethod(std::make_shared()) +{ + // We have 3 implementations for 3 methods + // - picking all points inside the primitive (uses CD) + // - picking nearest point to obj2 geometry center + // - picking point on element via interpolation + + m_pickingNode = std::make_shared(std::bind(&PbdObjectGrasping::updatePicking, this), + "PbdPickingUpdate", true); + m_taskGraph->addNode(m_pickingNode); + + m_taskGraph->addNode(m_objectToGrasp->getPbdModel()->getSolveNode()); + m_taskGraph->addNode(m_objectToGrasp->getPbdModel()->getUpdateVelocityNode()); + + m_taskGraph->addNode(m_objectToGrasp->getTaskGraph()->getSource()); + m_taskGraph->addNode(m_objectToGrasp->getTaskGraph()->getSink()); +} + +void +PbdObjectGrasping::beginVertexGrasp(std::shared_ptr geometry) +{ + auto vertexPicker = std::make_shared(); + vertexPicker->setPickingGeometry(geometry); + m_pickMethod = vertexPicker; + m_graspMode = GraspMode::Vertex; + m_graspGeom = geometry; + + m_isGrasping = true; + LOG(INFO) << "Begin grasp"; +} + +void +PbdObjectGrasping::beginCellGrasp(std::shared_ptr geometry, std::string cdType) +{ + auto cellPicker = std::make_shared(); + cellPicker->setPickingGeometry(geometry); + cellPicker->setCollisionDetection(CDObjectFactory::makeCollisionDetection(cdType)); + m_pickMethod = cellPicker; + m_graspMode = GraspMode::Cell; + m_graspGeom = geometry; + + m_isGrasping = true; + LOG(INFO) << "Begin grasp"; +} + +void +PbdObjectGrasping::beginRayPointGrasp(std::shared_ptr geometry, + const Vec3d& rayStart, const Vec3d& rayDir, const double maxDist) +{ + auto pointPicker = std::make_shared(); + pointPicker->setPickingRay(rayStart, rayDir, maxDist); + m_pickMethod = pointPicker; + m_graspMode = GraspMode::RayPoint; + m_graspGeom = geometry; + + m_isGrasping = true; + LOG(INFO) << "Begin grasp"; +} + +void +PbdObjectGrasping::beginRayCellGrasp(std::shared_ptr geometry, + const Vec3d& rayStart, const Vec3d& rayDir, const double maxDist) +{ + auto pointPicker = std::make_shared(); + pointPicker->setPickingRay(rayStart, rayDir, maxDist); + m_pickMethod = pointPicker; + m_graspMode = GraspMode::RayCell; + m_graspGeom = geometry; + + m_isGrasping = true; + LOG(INFO) << "Begin grasp"; +} + +void +PbdObjectGrasping::endGrasp() +{ + m_isGrasping = false; + LOG(INFO) << "End grasp"; +} + +void +PbdObjectGrasping::removePickConstraints() +{ + m_constraints.clear(); + m_constraintPts.clear(); +} + +void +PbdObjectGrasping::addPickConstraints() +{ + removePickConstraints(); + + auto pointSetToPick = std::dynamic_pointer_cast(m_objectToGrasp->getPhysicsGeometry()); + CHECK(pointSetToPick != nullptr) << "Trying to vertex pick with geometry that has no vertices"; + + std::shared_ptr> verticesPtr = pointSetToPick->getVertexPositions(); + VecDataArray& vertices = *verticesPtr; + + auto velocitiesPtr = + std::dynamic_pointer_cast>(pointSetToPick->getVertexAttribute("Velocities")); + CHECK(velocitiesPtr != nullptr) << "Trying to vertex pick with geometry that has no Velocities"; + VecDataArray& velocities = *velocitiesPtr; + + auto invMassesPtr = + std::dynamic_pointer_cast>(pointSetToPick->getVertexAttribute("InvMass")); + CHECK(invMassesPtr != nullptr) << "Trying to vertex pick with geometry that has no InvMass"; + const DataArray& invMasses = *invMassesPtr; + + std::shared_ptr indicesPtr = nullptr; + if (auto lineMesh = std::dynamic_pointer_cast(pointSetToPick)) + { + indicesPtr = lineMesh->getLinesIndices(); + } + if (auto surfMesh = std::dynamic_pointer_cast(pointSetToPick)) + { + indicesPtr = surfMesh->getTriangleIndices(); + } + else if (auto tetMesh = std::dynamic_pointer_cast(pointSetToPick)) + { + indicesPtr = tetMesh->getTetrahedraIndices(); + } + + // Place all the data into a struct to pass around & for quick access without casting + // or dereferencing + MeshSide meshStruct( + *verticesPtr, + *velocitiesPtr, + *invMassesPtr, + indicesPtr.get()); + + const Vec3d& pickGeomPos = m_graspGeom->getPosition(); + const Mat3d pickGeomRot = m_graspGeom->getRotation().transpose(); + + // Perform the picking + const std::vector& pickData = m_pickMethod->pick(pointSetToPick); + + // Digest the pick data based on grasp mode + if (m_graspMode == GraspMode::Vertex) + { + for (size_t i = 0; i < pickData.size(); i++) + { + const PickData& data = pickData[i]; + const int vertexId = data.ids[0]; + + const Vec3d relativePos = pickGeomRot * (vertices[vertexId] - pickGeomPos); + m_constraintPts.push_back({ + vertices[vertexId], + relativePos, + Vec3d::Zero() }); + std::tuple& cPt = m_constraintPts.back(); + + addConstraint( + { { &vertices[vertexId], invMasses[vertexId], &velocities[vertexId] } }, { 1.0 }, + { { &std::get<0>(cPt), 0.0, &std::get<2>(cPt) } }, { 1.0 }, + m_stiffness, 0.0); + } + } + else if (m_graspMode == GraspMode::Cell || m_graspMode == GraspMode::RayCell) + { + for (size_t i = 0; i < pickData.size(); i++) + { + const PickData& data = pickData[i]; + const CellTypeId pickedCellType = data.cellType; + + std::vector> cellVerts; + if (pickedCellType == IMSTK_TETRAHEDRON) + { + cellVerts = getElement<4>(data, meshStruct); + } + else if (pickedCellType == IMSTK_TRIANGLE) + { + cellVerts = getElement<3>(data, meshStruct); + } + else if (pickedCellType == IMSTK_EDGE) + { + cellVerts = getElement<2>(data, meshStruct); + } + + // Does not resolve duplicate vertices yet + // But pbd implicit solve with reprojection avoids issues + for (size_t j = 0; j < cellVerts.size(); j++) + { + const int vertexId = cellVerts[j].first; + + const Vec3d relativePos = pickGeomRot * (vertices[vertexId] - pickGeomPos); + m_constraintPts.push_back({ + vertices[vertexId], + relativePos, + Vec3d::Zero() }); + std::tuple& cPt = m_constraintPts.back(); + + addConstraint( + { { &vertices[vertexId], invMasses[vertexId], &velocities[vertexId] } }, { 1.0 }, + { { &std::get<0>(cPt), 0.0, &std::get<2>(cPt) } }, { 1.0 }, + m_stiffness, 0.0); + } + } + } + else if (m_graspMode == GraspMode::RayPoint) + { + for (size_t i = 0; i < pickData.size(); i++) + { + const PickData& data = pickData[i]; + const CellTypeId pickedCellType = data.cellType; + + std::vector> cellIdVerts; + if (pickedCellType == IMSTK_TETRAHEDRON) + { + cellIdVerts = getElement<4>(data, meshStruct); + } + else if (pickedCellType == IMSTK_TRIANGLE) + { + cellIdVerts = getElement<3>(data, meshStruct); + } + else if (pickedCellType == IMSTK_EDGE) + { + cellIdVerts = getElement<2>(data, meshStruct); + } + std::vector cellVerts(cellIdVerts.size()); + for (size_t j = 0; j < cellIdVerts.size(); j++) + { + cellVerts[j] = cellIdVerts[j].second; + } + + // The point to constrain the element too + const Vec3d pickingPt = data.pickPoint; + + std::vector weights(cellVerts.size()); + if (pickedCellType == IMSTK_TETRAHEDRON) + { + const Vec4d baryCoord = baryCentric(pickingPt, + *cellVerts[0].vertex, + *cellVerts[1].vertex, + *cellVerts[2].vertex, + *cellVerts[3].vertex); + weights[0] = baryCoord[0]; + weights[1] = baryCoord[1]; + weights[2] = baryCoord[2]; + weights[3] = baryCoord[3]; + } + else if (pickedCellType == IMSTK_TRIANGLE) + { + const Vec3d baryCoord = baryCentric(pickingPt, *cellVerts[0].vertex, *cellVerts[1].vertex, *cellVerts[2].vertex); + weights[0] = baryCoord[0]; + weights[1] = baryCoord[1]; + weights[2] = baryCoord[2]; + } + else if (pickedCellType == IMSTK_EDGE) + { + const Vec2d baryCoord = baryCentric(pickingPt, *cellVerts[0].vertex, *cellVerts[1].vertex); + weights[0] = baryCoord[0]; + weights[1] = baryCoord[1]; + } + + const Vec3d relativePos = pickGeomRot * (pickingPt - pickGeomPos); + m_constraintPts.push_back({ + pickingPt, + relativePos, + Vec3d::Zero() }); + std::tuple& cPt = m_constraintPts.back(); + + // Cell to single point constraint + addConstraint( + cellVerts, weights, + { { &std::get<0>(cPt), 0.0, &std::get<2>(cPt) } }, { 1.0 }, + m_stiffness, 0.0); + } + } +} + +void PbdObjectGrasping::addConstraint( + const std::vector& ptsA, + const std::vector& weightsA, + const std::vector& ptsB, + const std::vector& weightsB, + double stiffnessA, double stiffnessB) +{ + auto constraint = std::make_shared(); + constraint->initConstraint( + ptsA, weightsA, + ptsB, weightsB, + stiffnessA, stiffnessB); + m_constraints.push_back(constraint); +} + +void +PbdObjectGrasping::updatePicking() +{ + m_objectToGrasp->updateGeometries(); + + // If started picking + if (!m_isPrevGrasping && m_isGrasping) + { + addPickConstraints(); + } + // If stopped picking + if (!m_isGrasping && m_isPrevGrasping) + { + removePickConstraints(); + } + // Push back the picking state + m_isPrevGrasping = m_isGrasping; + + if (m_isGrasping) + { + updateConstraints(); + } +} + +void +PbdObjectGrasping::updateConstraints() +{ + // Update constraint point positions + { + const Vec3d& pos = m_graspGeom->getPosition(); + const Mat3d rot = m_graspGeom->getRotation(); + for (auto& cPt : m_constraintPts) + { + const Vec3d relativePos = std::get<1>(cPt); + std::get<0>(cPt) = pos + rot * relativePos; + } + } + + // Directly solve here + for (const auto& constraint : m_constraints) + { + constraint->solvePosition(); + } +} + +void +PbdObjectGrasping::initGraphEdges(std::shared_ptr source, std::shared_ptr sink) +{ + std::shared_ptr pbdModel = m_objectToGrasp->getPbdModel(); + + m_taskGraph->addEdge(source, m_objectToGrasp->getTaskGraph()->getSource()); + m_taskGraph->addEdge(m_objectToGrasp->getTaskGraph()->getSink(), sink); + + // The ideal location is after the internal positional solve + m_taskGraph->addEdge(pbdModel->getSolveNode(), m_pickingNode); + m_taskGraph->addEdge(m_pickingNode, pbdModel->getUpdateVelocityNode()); +} +} // namespace imstk \ No newline at end of file diff --git a/Source/Scene/imstkPbdObjectGrasping.h b/Source/Scene/imstkPbdObjectGrasping.h new file mode 100644 index 000000000..825e6a5b2 --- /dev/null +++ b/Source/Scene/imstkPbdObjectGrasping.h @@ -0,0 +1,171 @@ +/*========================================================================= + +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 "imstkCollisionInteraction.h" +#include "imstkMath.h" +#include "imstkPbdCollisionConstraint.h" + +#include + +namespace imstk +{ +class AnalyticalGeometry; +class PbdCollisionConstraint; +class PbdObject; +class PickingAlgorithm; + +/// +/// \class PbdObjectGrasping +/// +/// \brief This class defines grasping of a PbdObject via different +/// picking methods. Where grasping is define as grabbing & attaching +/// of a PbdObject's mesh to points. +/// +/// Given an input PickData the appropriate grasping will be produced. +/// +class PbdObjectGrasping : public SceneObject +{ +protected: + enum class GraspMode + { + Vertex, // Grab a vertex (most performant) + Cell, // Grab an entire cell/s + RayPoint, // Grab a point on the nearest cell along the ray + RayCell // Grab a cell along the ray + //NearestCells // Grab the nearest element + }; + +public: + PbdObjectGrasping(std::shared_ptr obj1); + ~PbdObjectGrasping() override = default; + + const std::string getTypeName() const override { return "PbdObjectGrasping"; } + + /// + /// \brief Set/Get the stiffness, 0-1 value that alters the step size in + /// the solver + ///@{ + void setStiffness(const double stiffness) { m_stiffness = stiffness; } + double getStiffness() const { return m_stiffness; } + ///@} + + /// + /// \brief Begin a vertex grasp (picking will begin on the next update) + /// \param Geometry attached/grasped too + /// + void beginVertexGrasp(std::shared_ptr geometry); + + /// + /// \brief Begin a cell grasp (picking will begin on the next update) + /// \param Geometry attached/grasped too + /// \param The intersection type/class name + /// + void beginCellGrasp(std::shared_ptr geometry, std::string cdType); + + /// + /// \brief Begin a ray point grasp (picking will begin on the next update) + /// \param Geometry attached/grasped too + /// \param Global space ray start + /// \param Global space ray direction + /// + void beginRayPointGrasp(std::shared_ptr geometry, + const Vec3d& rayStart, const Vec3d& rayDir, const double maxDist = -1.0); + + /// + /// \brief Begin a ray point grasp (picking will begin on the next update) + /// \param Geometry attached/grasped too + /// \param Global space ray start + /// \param Global space ray direction + /// + void beginRayCellGrasp(std::shared_ptr geometry, + const Vec3d& rayStart, const Vec3d& rayDir, const double maxDist = -1.0); + + /// + /// \brief End a grasp (picking will end on next update) + /// + void endGrasp(); + + /// + /// \brief Compute/generate the constraints for picking + /// + void addPickConstraints(); + + /// + /// \brief Remove the constraints for picking + /// + void removePickConstraints(); + + /// + /// \brief Add constraint between a point on each element given via + /// barycentric coordinates + /// pt position = weightA_0 * ptsA_0 + weightA_1 * ptsA_1 + ... + /// + virtual void addConstraint( + const std::vector& ptsA, + const std::vector& weightsA, + const std::vector& ptsB, + const std::vector& weightsB, + double stiffnessA, double stiffnessB); + + /// + /// \brief Get/Set the method use for picking, default is CellPicker + ///@{ + void setPickingAlgorithm(std::shared_ptr pickMethod) { m_pickMethod = pickMethod; } + std::shared_ptr getPickingAlgorithm() const { return m_pickMethod; } + ///@} + + std::shared_ptr getPickingNode() const { return m_pickingNode; } + + void initGraphEdges(std::shared_ptr source, std::shared_ptr sink) override; + +protected: + /// + /// \brief Update picking state, this should move grasp points + /// + virtual void updatePicking(); + + /// + /// \brief Update the constraints used for picking + /// + void updateConstraints(); + +protected: + std::shared_ptr m_pickingNode = nullptr; + + std::shared_ptr m_objectToGrasp = nullptr; + std::shared_ptr m_graspGeom = nullptr; + + std::shared_ptr m_pickMethod = nullptr; + GraspMode m_graspMode = GraspMode::Cell; + + bool m_isGrasping = false; + bool m_isPrevGrasping = false; + + /// Stiffness of grasp, when 1 the position is completely moved too the grasp point + /// when stiffness < 1 it will slowly converge on the grasp point + double m_stiffness = 0.4; + + std::list> m_constraintPts; ///< Position, Relative position, Velocity + std::vector> m_constraints; ///< List of PBD constraints +}; +} // namespace imstk \ No newline at end of file -- GitLab From f3bb44d9d4d8f6d950547917f6c925ec1ac71f2d Mon Sep 17 00:00:00 2001 From: Jacob Moore Date: Thu, 24 Mar 2022 13:40:55 -0400 Subject: [PATCH 3/7] ENH: Added coupled constraint between rigid body object and PBD object --- .../PBD/PBDGrasping/PBDGraspingExample.cpp | 174 ++++-------------- ...stkPbdRigidBaryPointToPointConstraint.cpp} | 41 ++--- ...imstkPbdRigidBaryPointToPointConstraint.h} | 11 +- Source/Scene/imstkPbdObjectGrasping.cpp | 3 +- Source/Scene/imstkPbdRigidObjectGrasping.cpp | 90 +++++++++ Source/Scene/imstkPbdRigidObjectGrasping.h | 61 ++++++ 6 files changed, 207 insertions(+), 173 deletions(-) rename Source/Constraint/MultiModelConstraints/{PbdRigidBaryPointToPointConstraint.cpp => imstkPbdRigidBaryPointToPointConstraint.cpp} (67%) rename Source/Constraint/MultiModelConstraints/{PbdRigidBaryPointToPointConstraint.h => imstkPbdRigidBaryPointToPointConstraint.h} (90%) create mode 100644 Source/Scene/imstkPbdRigidObjectGrasping.cpp create mode 100644 Source/Scene/imstkPbdRigidObjectGrasping.h diff --git a/Examples/PBD/PBDGrasping/PBDGraspingExample.cpp b/Examples/PBD/PBDGrasping/PBDGraspingExample.cpp index c89980aba..596b54bd0 100644 --- a/Examples/PBD/PBDGrasping/PBDGraspingExample.cpp +++ b/Examples/PBD/PBDGrasping/PBDGraspingExample.cpp @@ -20,6 +20,9 @@ =========================================================================*/ #include "imstkCamera.h" +#include "imstkCapsule.h" +#include "imstkCollisionDetectionAlgorithm.h" +#include "imstkCollisionHandling.h" #include "imstkDirectionalLight.h" #include "imstkImageData.h" #include "imstkKeyboardDeviceClient.h" @@ -28,17 +31,16 @@ #include "imstkMeshIO.h" #include "imstkMeshToMeshBruteForceCD.h" #include "imstkMouseControl.h" +#include "imstkMouseDeviceClient.h" #include "imstkMouseSceneControl.h" #include "imstkNew.h" #include "imstkOneToOneMap.h" - #include "imstkPbdBaryPointToPointConstraint.h" - +#include "imstkPbdRigidBaryPointToPointConstraint.h" +#include "imstkPbdRigidObjectGrasping.h" #include "imstkPbdModel.h" #include "imstkPbdObject.h" #include "imstkPbdObjectCollision.h" -// #include "imstkPbdObjectPicking.h" -// #include "imstkPbdPickingCH.h" #include "imstkPbdObjectGrasping.h" #include "imstkRbdConstraint.h" #include "imstkRenderMaterial.h" @@ -48,35 +50,13 @@ #include "imstkSceneManager.h" #include "imstkSimulationManager.h" #include "imstkSurfaceMesh.h" -#include "imstkTetrahedralMesh.h" -#include "imstkVisualModel.h" -#include "imstkVTKViewer.h" -#include "imstkCapsule.h" -#include "imstkSphere.h" -#include "imstkCollisionHandling.h" -#include "imstkPbdObjectCollision.h" -#include "imstkPointSetToCapsuleCD.h" #include "imstkSurfaceMeshToCapsuleCD.h" - +#include "imstkSphere.h" #include "imstkTaskNode.h" #include "imstkTaskGraph.h" -#include "imstkCollisionDetectionAlgorithm.h" -#include "imstkMouseDeviceClient.h" - -#include "PbdRigidBaryPointToPointConstraint.h" - -// If two-way coupling is used haptic forces can be felt when the tool -// hits the tissue -//#define TWOWAY_COUPLING - -// Whether to use FEM or volume+distance constraints -#define USE_FEM - -#ifdef TWOWAY_COUPLING -#include "imstkPbdRigidObjectCollision.h" -#else -#include "imstkPbdObjectCollision.h" -#endif +#include "imstkTetrahedralMesh.h" +#include "imstkVisualModel.h" +#include "imstkVTKViewer.h" #ifdef iMSTK_USE_OPENHAPTICS #include "imstkHapticDeviceManager.h" @@ -86,81 +66,6 @@ using namespace imstk; -class PbdRigidObjectGrasping : public PbdObjectGrasping -{ -protected: - std::shared_ptr m_rbdObj = nullptr; - std::shared_ptr m_pbdObj = nullptr; - -public: - PbdRigidObjectGrasping( - std::shared_ptr obj1, - std::shared_ptr obj2) : - PbdObjectGrasping(obj1), m_rbdObj(obj2) - { - m_pbdObj = obj1; - - m_taskGraph->addNode(m_pickingNode); - m_taskGraph->addNode(obj2->getRigidBodyModel2()->getSolveNode()); - } - ~PbdRigidObjectGrasping() override = default; - - const std::string getTypeName() const override { return "PbdRigidObjectGrasping"; } - - void updatePicking() - { - PbdObjectGrasping::updatePicking(); - - // LOG(INFO) << "Before PBDRBD constraints"; - for (int i = 0; i < m_constraints.size(); i++) - { - auto constraint = std::dynamic_pointer_cast(m_constraints[i]); - - constraint->compute(m_rbdObj->getRigidBodyModel2()->getConfig()->m_dt); - m_rbdObj->getRigidBodyModel2()->addConstraint(constraint); - } - } - - void addConstraint( - const std::vector& ptsA, - const std::vector& weightsA, - const std::vector& ptsB, - const std::vector& weightsB, - double stiffnessA, double stiffnessB) - override - { - // Create constraint - auto constraint = std::make_shared(m_rbdObj->getRigidBody()); - constraint->initConstraint( - ptsA, - weightsA, - ptsB, - weightsB, - stiffnessA, - stiffnessB); - // Add to constraints - m_constraints.push_back(constraint); - } - - void initGraphEdges(std::shared_ptr source, std::shared_ptr sink) override - { - LOG(INFO) << "Inside initGraphEdges"; - - PbdObjectGrasping::initGraphEdges(source, sink); - - std::shared_ptr pbdObj = m_pbdObj; - - std::shared_ptr pbdModel = pbdObj->getPbdModel(); - std::shared_ptr rbdModel = m_rbdObj->getRigidBodyModel2(); - - - m_taskGraph->addEdge(m_pickingNode, rbdModel->getSolveNode()); - m_taskGraph->addEdge(rbdModel->getSolveNode(), m_taskGraph->getSink()); - - LOG(INFO) << "Leaving initGraphEdges"; - } -}; - /// /// \brief Creates a tetraheral grid /// \param physical dimension of tissue @@ -173,8 +78,8 @@ makeTetGrid(const Vec3d& size, const Vec3i& dim, const Vec3d& center) imstkNew prismMesh; imstkNew> verticesPtr(dim[0] * dim[1] * dim[2]); - VecDataArray& vertices = *verticesPtr.get(); - const Vec3d dx = size.cwiseQuotient((dim - Vec3i(1, 1, 1)).cast()); + VecDataArray& vertices = *verticesPtr.get(); + const Vec3d dx = size.cwiseQuotient((dim - Vec3i(1, 1, 1)).cast()); for (int z = 0; z < dim[2]; z++) { for (int y = 0; y < dim[1]; y++) @@ -188,7 +93,7 @@ makeTetGrid(const Vec3d& size, const Vec3i& dim, const Vec3d& center) // Add connectivity data imstkNew> indicesPtr; - VecDataArray& indices = *indicesPtr.get(); + VecDataArray& indices = *indicesPtr.get(); for (int z = 0; z < dim[2] - 1; z++) { for (int y = 0; y < dim[1] - 1; y++) @@ -229,7 +134,7 @@ makeTetGrid(const Vec3d& size, const Vec3i& dim, const Vec3d& center) } imstkNew> uvCoordsPtr(dim[0] * dim[1] * dim[2]); - VecDataArray& uvCoords = *uvCoordsPtr.get(); + VecDataArray& uvCoords = *uvCoordsPtr.get(); for (int z = 0; z < dim[2]; z++) { for (int y = 0; y < dim[1]; y++) @@ -256,19 +161,18 @@ makeTetGrid(const Vec3d& size, const Vec3i& dim, const Vec3d& center) return prismMesh; } - static std::shared_ptr makePbdObjSurface(const std::string& name, - const Vec3d& size, - const Vec3i& dim, - const Vec3d& center, - const int numIter) + const Vec3d& size, + const Vec3i& dim, + const Vec3d& center, + const int numIter) { imstkNew prismObj(name); // Setup the Geometry std::shared_ptr prismMesh = makeTetGrid(size, dim, center); - std::shared_ptr surfMesh = prismMesh->extractSurfaceMesh(); + std::shared_ptr surfMesh = prismMesh->extractSurfaceMesh(); // Setup the Parameters imstkNew pbdParams; @@ -277,10 +181,10 @@ makePbdObjSurface(const std::string& name, pbdParams->enableConstraint(PbdModelConfig::ConstraintGenType::Dihedral, 1.0); pbdParams->enableConstraint(PbdModelConfig::ConstraintGenType::Distance, 1.0); - pbdParams->m_doPartitioning = true; + pbdParams->m_doPartitioning = true; pbdParams->m_uniformMassValue = 0.05; - pbdParams->m_gravity = Vec3d(0.0, 0.0, 0.0); - pbdParams->m_dt = 0.005; + pbdParams->m_gravity = Vec3d(0.0, 0.0, 0.0); + pbdParams->m_dt = 0.005; pbdParams->m_iterations = numIter; pbdParams->m_viscousDampingCoeff = 0.003; @@ -289,10 +193,10 @@ makePbdObjSurface(const std::string& name, { auto position = surfMesh->getVertexPosition(vert_id); - if (position(1) == -2.0) { + if (position(1) == -2.0) + { pbdParams->m_fixedNodeIds.push_back(vert_id); } - } // Setup the Model @@ -310,7 +214,6 @@ makePbdObjSurface(const std::string& name, visualModel->setRenderMaterial(material); prismObj->addVisualModel(visualModel); - // Setup the Object prismObj->setPhysicsGeometry(surfMesh); prismObj->setCollidingGeometry(surfMesh); @@ -325,11 +228,10 @@ makeCapsuleToolObj() // Set up rigid body model std::shared_ptr rbdModel = std::make_shared(); rbdModel->getConfig()->m_gravity = Vec3d::Zero(); - rbdModel->getConfig()->m_maxNumIterations = 8; - rbdModel->getConfig()->m_velocityDamping = 1.0; + rbdModel->getConfig()->m_maxNumIterations = 8; + rbdModel->getConfig()->m_velocityDamping = 1.0; rbdModel->getConfig()->m_angularVelocityDamping = 1.0; - rbdModel->getConfig()->m_maxNumConstraints = 40; - + rbdModel->getConfig()->m_maxNumConstraints = 40; auto toolGeometry = std::make_shared(); toolGeometry->setRadius(0.5); @@ -353,7 +255,6 @@ makeCapsuleToolObj() return toolObj; } - /// /// \brief This example demonstrates collision interaction with a 3d pbd /// simulated tissue (tetrahedral) @@ -380,7 +281,7 @@ main() std::shared_ptr toolObj = makeCapsuleToolObj(); scene->addSceneObject(toolObj); - auto rbdGhost = std::make_shared("ghost"); + auto rbdGhost = std::make_shared("ghost"); auto ghostCapsule = std::make_shared(); ghostCapsule->setRadius(0.5); ghostCapsule->setLength(1); @@ -388,7 +289,6 @@ main() ghostCapsule->setOrientation(Quatd(0.707, 0.0, 0.0, 0.707)); rbdGhost->setVisualGeometry(ghostCapsule); - std::shared_ptr ghostMat = std::make_shared(*toolObj->getVisualModel(0)->getRenderMaterial()); ghostMat->setOpacity(0.4); @@ -404,10 +304,6 @@ main() auto toolPicking = std::make_shared(PbdObj, toolObj); scene->addInteraction(toolPicking); - - // auto toolPicking = std::make_shared(PbdObj); - // scene->addInteraction(toolPicking); - // Light imstkNew light; light->setFocalPoint(Vec3d(5.0, -8.0, -5.0)); @@ -487,7 +383,7 @@ main() const Vec3d worldPos = Vec3d(mousePos[0] - 0.5, mousePos[1] - 0.5, 0.0) * 10.0; const Vec3d fS = (worldPos - toolObj->getRigidBody()->getPosition()) * 100000.0; // Spring force - const Vec3d fD = -toolObj->getRigidBody()->getVelocity() * 100.0; // Spring damping + const Vec3d fD = -toolObj->getRigidBody()->getVelocity() * 100.0; // Spring damping (*toolObj->getRigidBody()->m_force) += (fS + fD); @@ -509,8 +405,6 @@ main() connect(viewer->getMouseDevice(), &MouseDeviceClient::mouseButtonRelease, [&](Event*) { - // When click, grab all verticese associated with the triangle - // faces in contact and force motion with tool. toolPicking->endGrasp(); }); #endif @@ -525,11 +419,11 @@ main() keyControl->setModuleDriver(driver); viewer->addControl(keyControl); - connect(sceneManager, &SceneManager::postUpdate, [&](Event*) - { - // Simulate the cube in real time - PbdObj->getPbdModel()->getConfig()->m_dt = sceneManager->getDt(); - toolObj->getRigidBodyModel2()->getConfig()->m_dt = sceneManager->getDt(); + connect(sceneManager, &SceneManager::postUpdate, [&](Event*) + { + // Simulate the cube in real time + PbdObj->getPbdModel()->getConfig()->m_dt = sceneManager->getDt(); + toolObj->getRigidBodyModel2()->getConfig()->m_dt = sceneManager->getDt(); }); driver->start(); diff --git a/Source/Constraint/MultiModelConstraints/PbdRigidBaryPointToPointConstraint.cpp b/Source/Constraint/MultiModelConstraints/imstkPbdRigidBaryPointToPointConstraint.cpp similarity index 67% rename from Source/Constraint/MultiModelConstraints/PbdRigidBaryPointToPointConstraint.cpp rename to Source/Constraint/MultiModelConstraints/imstkPbdRigidBaryPointToPointConstraint.cpp index fd3db9b1e..e8e631e8b 100644 --- a/Source/Constraint/MultiModelConstraints/PbdRigidBaryPointToPointConstraint.cpp +++ b/Source/Constraint/MultiModelConstraints/imstkPbdRigidBaryPointToPointConstraint.cpp @@ -19,42 +19,34 @@ =========================================================================*/ -#include "imstkPbdBaryPointToPointConstraint.h" -#include "imstkRbdConstraint.h" -#include "PbdRigidBaryPointToPointConstraint.h" - +#include "imstkPbdRigidBaryPointToPointConstraint.h" namespace imstk { - - PbdRigidBaryPointToPointConstraint::PbdRigidBaryPointToPointConstraint(std::shared_ptr obj1) : - PbdBaryPointToPointConstraint(), // (point on capsue, point on mesh) - RbdConstraint( // Pass in obj1, nullpt and say side A + PbdBaryPointToPointConstraint(), + RbdConstraint( obj1, nullptr, RbdConstraint::Side::A) - { - } - - -// PbdRigidBaryPointToPointConstraint::~PbdRigidBaryPointToPointConstraint(){}; +{ +} /// -/// \brief compute value and gradient of constraint function +/// \brief compute value and gradient of constraint function and weight it +/// by half to force the motion to the half way point between two bodies /// /// \param[inout] c constraint value -/// \param[inout] dcdx constraint gradient for A -/// Call for RBD, push point on mesh to the fixed point -/// \param[inout] dcdx constraint gradient for B +/// \param[inout] dcdxA constraint gradient for A +/// \param[inout] dcdxB constraint gradient for B bool PbdRigidBaryPointToPointConstraint::computeValueAndGradient( - double& c, + double& c, std::vector& dcdxA, std::vector& dcdxB) const { - // Compute the difference between the interpolant points (points in the two cells) - Vec3d diff = 0.5*computeInterpolantDifference(); + // Compute the middle position between the point on the rigid body and the PBD object + Vec3d diff = 0.5 * computeInterpolantDifference(); c = diff.norm(); @@ -77,14 +69,13 @@ PbdRigidBaryPointToPointConstraint::computeValueAndGradient( return true; } - -void +void PbdRigidBaryPointToPointConstraint::compute(double dt) { J = Eigen::Matrix::Zero(); - // Compute the difference between the interpolant points (points in the two cells) - Vec3d diff = 0.5*computeInterpolantDifference(); + // Compute the middle position between the point on the rigid body and the PBD object + Vec3d diff = 0.5 * computeInterpolantDifference(); J(0, 0) = -diff[0]; J(0, 1) = 0.0; J(1, 0) = -diff[1]; J(1, 1) = 0.0; @@ -93,6 +84,4 @@ PbdRigidBaryPointToPointConstraint::compute(double dt) // B stabilization term vu = diff.norm() * m_beta / dt; } - - } // namespace imstk \ No newline at end of file diff --git a/Source/Constraint/MultiModelConstraints/PbdRigidBaryPointToPointConstraint.h b/Source/Constraint/MultiModelConstraints/imstkPbdRigidBaryPointToPointConstraint.h similarity index 90% rename from Source/Constraint/MultiModelConstraints/PbdRigidBaryPointToPointConstraint.h rename to Source/Constraint/MultiModelConstraints/imstkPbdRigidBaryPointToPointConstraint.h index a5290cb4a..001dc859e 100644 --- a/Source/Constraint/MultiModelConstraints/PbdRigidBaryPointToPointConstraint.h +++ b/Source/Constraint/MultiModelConstraints/imstkPbdRigidBaryPointToPointConstraint.h @@ -23,6 +23,7 @@ #include "imstkPbdBaryPointToPointConstraint.h" #include "imstkRbdConstraint.h" +#include "imstkPbdConstraint.h" namespace imstk { @@ -34,7 +35,7 @@ private: double m_beta = 0.0001; public: - // Need to pass in all data for PbdCollisionConstraint and RbdConstraint? + PbdRigidBaryPointToPointConstraint(std::shared_ptr obj1); ~PbdRigidBaryPointToPointConstraint() override = default; @@ -45,9 +46,10 @@ public: /// \param[inout] c constraint value /// \param[inout] dcdxA constraint gradient for A /// \param[inout] dcdxB constraint gradient for B - /// Call for RBD, push point on mesh to the fixed point + /// Call for RBD, push point on mesh to the fixed point halfway + /// between the rigid body and the PBD object bool computeValueAndGradient( - double& c, + double& c, std::vector& dcdxA, std::vector& dcdxB) const override; @@ -55,8 +57,5 @@ public: // Constraint call for RBD // Give change in impulse in direction of desired deformation void compute(double dt) override; - }; - - } // namespace imstk \ No newline at end of file diff --git a/Source/Scene/imstkPbdObjectGrasping.cpp b/Source/Scene/imstkPbdObjectGrasping.cpp index 1e7e5dc49..7a9ce6ad8 100644 --- a/Source/Scene/imstkPbdObjectGrasping.cpp +++ b/Source/Scene/imstkPbdObjectGrasping.cpp @@ -351,7 +351,8 @@ PbdObjectGrasping::addPickConstraints() } } -void PbdObjectGrasping::addConstraint( +void +PbdObjectGrasping::addConstraint( const std::vector& ptsA, const std::vector& weightsA, const std::vector& ptsB, diff --git a/Source/Scene/imstkPbdRigidObjectGrasping.cpp b/Source/Scene/imstkPbdRigidObjectGrasping.cpp new file mode 100644 index 000000000..4973699b9 --- /dev/null +++ b/Source/Scene/imstkPbdRigidObjectGrasping.cpp @@ -0,0 +1,90 @@ +/*========================================================================= + + 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 "imstkPbdRigidObjectGrasping.h" + +namespace imstk +{ +PbdRigidObjectGrasping::PbdRigidObjectGrasping( + std::shared_ptr obj1, + std::shared_ptr obj2) : + PbdObjectGrasping(obj1), m_rbdObj(obj2) +{ + m_pbdObj = obj1; + + m_taskGraph->addNode(m_pickingNode); + m_taskGraph->addNode(obj2->getRigidBodyModel2()->getSolveNode()); +} + +const std::string +PbdRigidObjectGrasping::getTypeName() const +{ + return "PbdRigidObjectGrasping"; +} + +void +PbdRigidObjectGrasping::updatePicking() +{ + PbdObjectGrasping::updatePicking(); + for (int i = 0; i < m_constraints.size(); i++) + { + auto constraint = std::dynamic_pointer_cast(m_constraints[i]); + + constraint->compute(m_rbdObj->getRigidBodyModel2()->getConfig()->m_dt); + m_rbdObj->getRigidBodyModel2()->addConstraint(constraint); + } +} + +void +PbdRigidObjectGrasping::addConstraint( + const std::vector& ptsA, + const std::vector& weightsA, + const std::vector& ptsB, + const std::vector& weightsB, + double stiffnessA, double stiffnessB) +{ + // Create constraint + auto constraint = std::make_shared(m_rbdObj->getRigidBody()); + constraint->initConstraint( + ptsA, + weightsA, + ptsB, + weightsB, + stiffnessA, + stiffnessB); + // Add to constraints + m_constraints.push_back(constraint); +} + +void +PbdRigidObjectGrasping::initGraphEdges(std::shared_ptr source, std::shared_ptr sink) +{ + PbdObjectGrasping::initGraphEdges(source, sink); + + std::shared_ptr pbdObj = m_pbdObj; + + std::shared_ptr pbdModel = pbdObj->getPbdModel(); + std::shared_ptr rbdModel = m_rbdObj->getRigidBodyModel2(); + + m_taskGraph->addEdge(m_pickingNode, rbdModel->getSolveNode()); + m_taskGraph->addEdge(rbdModel->getSolveNode(), m_taskGraph->getSink()); +} +} // namespace imstk diff --git a/Source/Scene/imstkPbdRigidObjectGrasping.h b/Source/Scene/imstkPbdRigidObjectGrasping.h new file mode 100644 index 000000000..a9b229bee --- /dev/null +++ b/Source/Scene/imstkPbdRigidObjectGrasping.h @@ -0,0 +1,61 @@ +/*========================================================================= + + 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 "imstkPbdObject.h" +#include "imstkPbdObjectGrasping.h" +#include "imstkPbdRigidBaryPointToPointConstraint.h" +#include "imstkRigidBodyModel2.h" +#include "imstkRigidObject2.h" +#include "imstkTaskNode.h" +#include "imstkTaskGraph.h" + +namespace imstk +{ +class PbdRigidObjectGrasping : public PbdObjectGrasping +{ +protected: + std::shared_ptr m_rbdObj = nullptr; + std::shared_ptr m_pbdObj = nullptr; + +public: + PbdRigidObjectGrasping( + std::shared_ptr obj1, + std::shared_ptr obj2); + + ~PbdRigidObjectGrasping() override = default; + + const std::string getTypeName() const override; + + void updatePicking(); + + void addConstraint( + const std::vector& ptsA, + const std::vector& weightsA, + const std::vector& ptsB, + const std::vector& weightsB, + double stiffnessA, double stiffnessB) + override; + + void initGraphEdges(std::shared_ptr source, std::shared_ptr sink) override; +}; +} // namespace imstk \ No newline at end of file -- GitLab From d48f30be1a395f4925581e5c31637bba7e278aa2 Mon Sep 17 00:00:00 2001 From: Jacob Moore Date: Tue, 5 Apr 2022 14:41:25 -0400 Subject: [PATCH 4/7] REFAC: general cleanup --- Source/Scene/imstkPbdRigidObjectGrasping.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/Source/Scene/imstkPbdRigidObjectGrasping.cpp b/Source/Scene/imstkPbdRigidObjectGrasping.cpp index 4973699b9..a8c97a250 100644 --- a/Source/Scene/imstkPbdRigidObjectGrasping.cpp +++ b/Source/Scene/imstkPbdRigidObjectGrasping.cpp @@ -28,8 +28,6 @@ PbdRigidObjectGrasping::PbdRigidObjectGrasping( std::shared_ptr obj2) : PbdObjectGrasping(obj1), m_rbdObj(obj2) { - m_pbdObj = obj1; - m_taskGraph->addNode(m_pickingNode); m_taskGraph->addNode(obj2->getRigidBodyModel2()->getSolveNode()); } @@ -79,9 +77,7 @@ PbdRigidObjectGrasping::initGraphEdges(std::shared_ptr source, std::sh { PbdObjectGrasping::initGraphEdges(source, sink); - std::shared_ptr pbdObj = m_pbdObj; - - std::shared_ptr pbdModel = pbdObj->getPbdModel(); + std::shared_ptr pbdModel = m_objectToGrasp->getPbdModel(); std::shared_ptr rbdModel = m_rbdObj->getRigidBodyModel2(); m_taskGraph->addEdge(m_pickingNode, rbdModel->getSolveNode()); -- GitLab From 9573d66a9af01fcdae6d3ef9df92789da0f48c62 Mon Sep 17 00:00:00 2001 From: Harald Scheirich Date: Fri, 1 Apr 2022 14:04:36 -0400 Subject: [PATCH 5/7] ENH: Add getter to check for actual constraints --- Source/Scene/imstkPbdObjectGrasping.cpp | 6 ++++++ Source/Scene/imstkPbdObjectGrasping.h | 2 ++ 2 files changed, 8 insertions(+) diff --git a/Source/Scene/imstkPbdObjectGrasping.cpp b/Source/Scene/imstkPbdObjectGrasping.cpp index 7a9ce6ad8..ed06dca5f 100644 --- a/Source/Scene/imstkPbdObjectGrasping.cpp +++ b/Source/Scene/imstkPbdObjectGrasping.cpp @@ -424,4 +424,10 @@ PbdObjectGrasping::initGraphEdges(std::shared_ptr source, std::shared_ m_taskGraph->addEdge(pbdModel->getSolveNode(), m_pickingNode); m_taskGraph->addEdge(m_pickingNode, pbdModel->getUpdateVelocityNode()); } + +bool PbdObjectGrasping::hasConstraints() const +{ + return !m_constraints.empty(); +} + } // namespace imstk \ No newline at end of file diff --git a/Source/Scene/imstkPbdObjectGrasping.h b/Source/Scene/imstkPbdObjectGrasping.h index 825e6a5b2..ddefe5268 100644 --- a/Source/Scene/imstkPbdObjectGrasping.h +++ b/Source/Scene/imstkPbdObjectGrasping.h @@ -138,6 +138,8 @@ public: void initGraphEdges(std::shared_ptr source, std::shared_ptr sink) override; + bool hasConstraints() const; + protected: /// /// \brief Update picking state, this should move grasp points -- GitLab From 172e8c0954539a673a2713183a960256d9970c1d Mon Sep 17 00:00:00 2001 From: Harald Scheirich Date: Fri, 1 Apr 2022 14:05:46 -0400 Subject: [PATCH 6/7] ENH: Add C# wrappers for grasping --- Source/Wrappers/SwigInterface/imstkCWrapper.i | 12 ++++++++++++ .../SwigInterface/shared_ptr_instantiation.i | 6 ++++++ 2 files changed, 18 insertions(+) diff --git a/Source/Wrappers/SwigInterface/imstkCWrapper.i b/Source/Wrappers/SwigInterface/imstkCWrapper.i index be74bbaf1..6b9750068 100644 --- a/Source/Wrappers/SwigInterface/imstkCWrapper.i +++ b/Source/Wrappers/SwigInterface/imstkCWrapper.i @@ -162,9 +162,15 @@ #include "imstkCollisionInteraction.h" #include "imstkRigidObjectCollision.h" #include "imstkPbdObjectCutting.h" +<<<<<<< HEAD #include "imstkPbdObjectPicking.h" #include "imstkPbdRigidObjectCollision.h" +======= +#include "imstkPbdObjectGrasping.h" +>>>>>>> 685d443b... ENH: Add C# wrappers for grasping #include "imstkPbdObjectCollision.h" +#include "imstkPbdRigidObjectCollision.h" +#include "imstkPbdRigidObjectGrasping.h" #include "imstkSphObjectCollision.h" /* @@ -394,9 +400,15 @@ namespace std %include "../../Scene/imstkCollisionInteraction.h" %include "../../Scene/imstkRigidObjectCollision.h" %include "../../Scene/imstkPbdObjectCutting.h" +<<<<<<< HEAD %include "../../Scene/imstkPbdObjectPicking.h" %include "../../Scene/imstkPbdRigidObjectCollision.h" +======= +%include "../../Scene/imstkPbdObjectGrasping.h" +>>>>>>> 685d443b... ENH: Add C# wrappers for grasping %include "../../Scene/imstkPbdObjectCollision.h" +%include "../../Scene/imstkPbdRigidObjectCollision.h" +%include "../../Scene/imstkPbdRigidObjectGrasping.h" %include "../../Scene/imstkSphObjectCollision.h" /* diff --git a/Source/Wrappers/SwigInterface/shared_ptr_instantiation.i b/Source/Wrappers/SwigInterface/shared_ptr_instantiation.i index 2186ab737..523d98506 100644 --- a/Source/Wrappers/SwigInterface/shared_ptr_instantiation.i +++ b/Source/Wrappers/SwigInterface/shared_ptr_instantiation.i @@ -171,9 +171,15 @@ %shared_ptr(imstk::CollisionInteraction) %shared_ptr(imstk::RigidObjectCollision) %shared_ptr(imstk::PbdObjectCutting) +<<<<<<< HEAD %shared_ptr(imstk::PbdObjectPicking) %shared_ptr(imstk::PbdRigidObjectCollision) +======= +%shared_ptr(imstk::PbdObjectGrasping) +>>>>>>> 685d443b... ENH: Add C# wrappers for grasping %shared_ptr(imstk::PbdObjectCollision) +%shared_ptr(imstk::PbdRigidObjectCollision) +%shared_ptr(imstk::PbdRigidObjectGrasping) %shared_ptr(imstk::SphObjectCollision) /* -- GitLab From 43aa1c5339f724b39b46bb944550c990afcf0365 Mon Sep 17 00:00:00 2001 From: Jacob Moore Date: Wed, 6 Apr 2022 09:14:30 -0400 Subject: [PATCH 7/7] STYLE: Cleaning up headers --- .../imstkPbdRigidBaryPointToPointConstraint.h | 1 - Source/Scene/imstkPbdRigidObjectGrasping.cpp | 2 ++ Source/Scene/imstkPbdRigidObjectGrasping.h | 4 ++-- 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/Source/Constraint/MultiModelConstraints/imstkPbdRigidBaryPointToPointConstraint.h b/Source/Constraint/MultiModelConstraints/imstkPbdRigidBaryPointToPointConstraint.h index 001dc859e..c8e2900d6 100644 --- a/Source/Constraint/MultiModelConstraints/imstkPbdRigidBaryPointToPointConstraint.h +++ b/Source/Constraint/MultiModelConstraints/imstkPbdRigidBaryPointToPointConstraint.h @@ -23,7 +23,6 @@ #include "imstkPbdBaryPointToPointConstraint.h" #include "imstkRbdConstraint.h" -#include "imstkPbdConstraint.h" namespace imstk { diff --git a/Source/Scene/imstkPbdRigidObjectGrasping.cpp b/Source/Scene/imstkPbdRigidObjectGrasping.cpp index a8c97a250..9683e29bc 100644 --- a/Source/Scene/imstkPbdRigidObjectGrasping.cpp +++ b/Source/Scene/imstkPbdRigidObjectGrasping.cpp @@ -20,6 +20,8 @@ =========================================================================*/ #include "imstkPbdRigidObjectGrasping.h" +#include "imstkRigidBodyModel2.h" +#include "imstkTaskGraph.h" namespace imstk { diff --git a/Source/Scene/imstkPbdRigidObjectGrasping.h b/Source/Scene/imstkPbdRigidObjectGrasping.h index a9b229bee..8dee76043 100644 --- a/Source/Scene/imstkPbdRigidObjectGrasping.h +++ b/Source/Scene/imstkPbdRigidObjectGrasping.h @@ -24,10 +24,10 @@ #include "imstkPbdObject.h" #include "imstkPbdObjectGrasping.h" #include "imstkPbdRigidBaryPointToPointConstraint.h" -#include "imstkRigidBodyModel2.h" +// #include "imstkRigidBodyModel2.h" #include "imstkRigidObject2.h" #include "imstkTaskNode.h" -#include "imstkTaskGraph.h" +// #include "imstkTaskGraph.h" namespace imstk { -- GitLab