Initial Commit

Assets/Imstk/Scripts/HighLevel/GraspingController.cs

@@ -0,0 +1,188 @@
+/*=========================================================================
+
+   Library: iMSTK-Unity
+
+   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.
+
+=========================================================================*/
+
+using UnityEngine;
+
+namespace ImstkUnity
+{
+    /// <summary>
+    /// GraspingController takes care of some of the higher level aspects of 
+    /// grasping and tool operation, it can manipulate two jaws and open and close 
+    /// them, it can take input from a variety of sources as well
+    /// </summary>
+    [RequireComponent(typeof(GraspingManager))]
+    public class GraspingController : MonoBehaviour
+    {
+        // Assumes that the jaws are currently closed, will open
+        // to the max opening angle
+        // Rotation is currently just fixed to the Z Axis
+        public TrackingDevice device;
+
+        // TODO disable collision on grasp
+
+        /// Drive via Buttons
+        public int openButton = 0;
+        public int closeButton = 1;
+
+        // Drive via Keys
+        public string openKey = "]";
+        public string closeKey = "[";
+
+        public bool useSnap = false;
+        public float transitionTime = 3.0f;
+        private float openValue = 0.0f;
+
+        /// Drive by analog Value
+        public bool driveWithAnalog = false;
+        public int analogChannel = 0;
+        public float analogClosed = 0;
+        public float analogOpen = 1024;
+
+        public GameObject upperJaw;
+        public GameObject lowerJaw;
+
+
+        [Tooltip("The largest angle between the two jaws")]
+        public float maxOpenAngle;
+
+        GraspingManager _manager;
+        public GraspingManager manager { get { return _manager; } }
+
+
+        private float _lowerCloseAngle = 0.0f;
+        private float _upperCloseAngle = 0.0f;
+
+        void Start()
+        {
+            if (lowerJaw != null)
+            {
+                _lowerCloseAngle = lowerJaw.transform.localRotation.eulerAngles.z;
+            }
+            if (upperJaw != null)
+            {
+                _upperCloseAngle = upperJaw.transform.localRotation.eulerAngles.z;
+            }
+
+            if (!device.isActiveAndEnabled)
+            {
+                device = null;
+            }
+
+            // need to check startup order to see if we can initialize the imstk values at this point
+            // we'd want to do the type check and casting here rather than every frame
+
+            _manager = GetComponent<GraspingManager>();
+        }
+
+        // Update is called once per frame
+        void Update()
+        {
+            float newValue = openValue;
+            if (device == null) return;
+
+            if (driveWithAnalog)
+            {
+                float input = device.GetAnalog(analogChannel);
+#pragma warning disable 1718 // Warning for comparing the same variable
+                if (input == input)
+                {
+                    if (analogClosed < analogOpen)
+                    {
+                        newValue = (input - analogClosed) / (analogOpen - analogClosed);
+                    }
+                    else
+                    {
+                        newValue = (analogClosed - input) / (analogClosed - analogOpen);
+                    }
+                }
+#if UNITY_EDITOR
+                else
+                {
+                    Debug.LogWarning("Can't read analog from channel: " + analogChannel);
+                }
+#endif
+#pragma warning restore 1718
+
+                //Debug.Log(Time.realtimeSinceStartup-start + " " + input.ToString());
+            }
+            else
+            {
+                if (useSnap)
+                {
+                    if (device.IsButtonDown(openButton) || Input.GetKey(openKey)) newValue = 0.0f;
+                    if (device.IsButtonDown(closeButton) || Input.GetKey(closeKey)) newValue = 1.0f;
+                }
+                else
+                {
+                    // Map buttons to axis, this should probably be in the input _manager
+                    if (device.IsButtonDown(openButton) || Input.GetKey(openKey)) newValue = openValue + Time.deltaTime / transitionTime;
+                    if (device.IsButtonDown(closeButton) || Input.GetKey(closeKey)) newValue = openValue - Time.deltaTime / transitionTime;
+                }
+            }
+
+
+
+            var oldClosed = isClosed();
+
+            setOpenValue(Mathf.Clamp01(newValue));
+
+            if (isClosed() && !oldClosed)
+            {
+                Debug.Log("Start Grasp");
+                _manager.StartGrasp();
+            }
+
+            if (!isClosed() && oldClosed)
+            {
+                _manager.EndGrasp();
+            }
+        }
+
+        bool isClosed()
+        {
+            return openValue < 0.1;
+        }
+
+        void setOpenValue(float value)
+        {
+            float half = (maxOpenAngle * value) / 2.0f;
+
+            if (upperJaw != null)
+            {
+                var rot = upperJaw.transform.localRotation;
+                var angles = upperJaw.transform.localRotation.eulerAngles;
+                angles.z = _upperCloseAngle + half;
+                rot.eulerAngles = angles;
+                upperJaw.transform.localRotation = rot;
+            }
+
+            if (lowerJaw != null)
+            {
+                var rot = lowerJaw.transform.localRotation;
+                var angles = lowerJaw.transform.localRotation.eulerAngles;
+                angles.z = _lowerCloseAngle - half;
+                rot.eulerAngles = angles;
+                lowerJaw.transform.localRotation = rot;
+            }
+            openValue = value;
+        }
+    }
+}
+

Assets/Imstk/Scripts/HighLevel/GraspingController.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 484fa8084caf41f4498e3ac6a9a715d1
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {fileID: 2800000, guid: 936dbb32eb653d84a9bcbe9caad48547, type: 3}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/Imstk/Scripts/HighLevel/GraspingManager.cs

@@ -0,0 +1,120 @@
+/*=========================================================================
+
+   Library: iMSTK-Unity
+
+   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.
+
+=========================================================================*/
+
+using System;
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+namespace ImstkUnity
+{
+    /// <summary>
+    /// Set ups multiple grasping components, handles grasping and upgrasping
+    /// this set of components. This alleviates the burden to the user to set 
+    /// up multiple grasping components. Plays together with the Grasping Controller
+    /// that handles the User interaction side of making a tool that can grasp, but
+    /// can also be used on its own
+    /// </summary>
+    public class GraspingManager : MonoBehaviour
+    {
+        [Serializable]
+        public struct GraspingData
+        {
+            public bool enabled;
+            public DynamicalModel target;
+            public Grasping.GraspType type;
+        }
+
+        public Rigid rigid;
+        public GeometryFilter graspingGeometry;
+        public List<GraspingData> graspingData = new List<GraspingData>();
+
+        List<Grasping> _graspings = new List<Grasping>();
+        Coroutine _coroutine;
+        
+        public double deformableGraspingStiffness = 0.4;
+        public double rigidGraspingStiffness = 1 / 0.0001;
+        
+        public
+        void Awake()
+        {
+            // These will be created _before_ the simulation manager searches for 
+            // its component and therefor will participate in the norm imstk-unity
+            // discovery
+            foreach (var item in graspingData )
+            {
+                if (item.enabled)
+                {
+                    var grasping = gameObject.AddComponent<Grasping>();
+                    grasping.rigidModel = rigid;
+                    grasping.graspingGeometry = graspingGeometry;
+                    grasping.graspedObject = item.target;
+                    grasping.graspType = item.type;
+                    grasping.deformableGraspingStiffness = deformableGraspingStiffness;
+                    grasping.rigidGraspingStiffness = rigidGraspingStiffness;
+                    _graspings.Add(grasping);
+                }
+            }
+        }
+
+        /// <summary>
+        /// Sets off a corouting to go through all of the graspings that 
+        /// this component created, as it takes a frame to check wether
+        /// a grasp was successful
+        /// </summary>
+        public void StartGrasp()
+        {
+            _coroutine = StartCoroutine(CoroutineGrasp());
+        }
+
+        private IEnumerator CoroutineGrasp()
+        {
+            foreach (var grasp in _graspings)
+            {
+                if (!grasp.enabled) continue;
+
+                // It takes a frame for us to detect if the grasp
+                // has generated constraints, only then do we know
+                // that something has _actually_ been taken
+                grasp.StartGrasp();
+
+                yield return null;
+
+                if (grasp.HasConstraints())
+                {
+                    yield break;
+                }
+            }
+        }
+        public void EndGrasp()
+        {
+            if (_coroutine != null) {
+                StopCoroutine(_coroutine);
+                _coroutine = null;
+            }
+            foreach (var grasp in _graspings)
+            {
+                grasp.EndGrasp();
+            }
+        }
+
+    }
+
+}

Assets/Imstk/Scripts/HighLevel/GraspingManager.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 9e4ee5917383f4f4c9fa28352c34a7ee
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {fileID: 2800000, guid: 936dbb32eb653d84a9bcbe9caad48547, type: 3}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/Imstk/Scripts/HighLevel/SimpleGraspingController.cs

@@ -0,0 +1,62 @@
+/*=========================================================================
+
+   Library: iMSTK-Unity
+
+   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.
+
+=========================================================================*/
+
+using UnityEngine;
+
+namespace ImstkUnity
+{
+
+    /// <summary>
+    /// Simple controller for grasping, using a tracking device
+    /// </summary>
+    [RequireComponent(typeof(GraspingManager))]
+    public class SimpleGraspingController : MonoBehaviour
+    {
+        public TrackingDevice device;
+        // Start is called before the first frame update
+        GraspingManager _manager;
+
+        bool _isGrasping = false;
+
+        void Start()
+        {
+            _manager = GetComponent<GraspingManager>();
+            Debug.Assert(_manager != null);
+        }
+
+        // Update is called once per frame
+        void Update()
+        {
+            bool doGrasp = device.IsButtonDown(0) || device.IsButtonDown(1);
+            if (doGrasp && !_isGrasping)
+            {
+                Debug.Log("Grasping");
+                _manager.StartGrasp();
+                _isGrasping = true;
+            }
+            else if (!doGrasp && _isGrasping)
+            {
+                Debug.Log("Releasing");
+                _manager.EndGrasp();
+                _isGrasping = false;
+            }
+        }
+    }
+}

Assets/Imstk/Scripts/HighLevel/SimpleGraspingController.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 2a73ab6aebdeb714e9700ee1a1c0c783
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {fileID: 2800000, guid: 936dbb32eb653d84a9bcbe9caad48547, type: 3}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/Imstk/Scripts/HighLevel/Suturing.cs

@@ -0,0 +1,127 @@
+/*=========================================================================
+
+   Library: iMSTK-Unity
+
+   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.
+
+=========================================================================*/
+
+using UnityEngine;
+
+namespace ImstkUnity
+{
+    /// <summary>
+    /// High Level interaction that implements suturing. Needs a needle, thread and tissue.
+    /// It will deal with the needle and the tissue collision as well as the needle puncturing
+    /// the tissue and the thread being pulled through the tissue.
+    /// Notes: currently the constraints generated in imstk for the needle/tissue and thread/tissue
+    /// punctures are only one way. This means that no forces will be exterted on the needle as 
+    /// it violates the constraint.
+    /// Additionally there is an assumption in ismtk at the moment that the tip of the needle is
+    /// that _back end_ of the needle mesh.
+    /// </summary>
+    public class Suturing : ImstkBehaviour
+    {
+        public Rigid needle;
+        public Deformable thread;
+        public Deformable tissue;
+        public string activationKey = "s";
+        private bool _activated = false;
+
+        public float needleSurfaceStiffness = 0.5f;
+        public float threadSurfaceStiffness = 0.5f;
+        public float punctureDotThreshold = 0.8f;
+
+        Imstk.NeedleInteraction _needleInteraction;
+
+        protected override void OnImstkInit()
+        {
+            if (needle == null)
+            {
+                Debug.LogError("Needle is null");
+                return;
+            }
+            if (thread == null)
+            {
+                Debug.LogError("Thread is null");
+                return;
+            }
+            if (tissue == null)
+            {
+                Debug.LogError("Tissue is null");
+                return;
+            }
+
+            // Disable if any of the objects are not active
+            if (!needle.isActiveAndEnabled || !thread.isActiveAndEnabled || !tissue.isActiveAndEnabled)
+            {
+                enabled = false;
+                return;
+            }
+
+            // Make sure everything is initialized
+            needle.ImstkInit();
+            thread.ImstkInit();
+            tissue.ImstkInit();
+
+            var needlePbd = Imstk.Utils.CastTo<Imstk.PbdObject>(needle.GetDynamicObject());
+            var threadPbd = Imstk.Utils.CastTo<Imstk.PbdObject>(thread.GetDynamicObject());
+            var tissuePbd = Imstk.Utils.CastTo<Imstk.PbdObject>(tissue.GetDynamicObject());
+
+            if (needlePbd == null || threadPbd == null || tissuePbd == null)
+            {
+                Debug.LogError("Needle, Thread or Tissue do not have a dynamic Object");
+                enabled = false;
+                return;
+            }
+
+            _needleInteraction = new Imstk.NeedleInteraction(tissuePbd, needlePbd, threadPbd);
+
+            var ch = Imstk.Utils.CastTo<Imstk.NeedlePbdCH>(_needleInteraction.getCollisionHandlingAB());
+            if (ch == null)
+            {
+                Debug.LogError("Needle interaction does not have a NeedlePbdCH");
+                enabled = false;
+                return;
+            } else
+            {
+                ch.setNeedleToSurfaceStiffness(needleSurfaceStiffness);
+                ch.setSurfaceToNeedleStiffness(needleSurfaceStiffness);
+                ch.setThreadToSurfaceStiffness(threadSurfaceStiffness);
+                ch.setSurfaceToThreadStiffness(threadSurfaceStiffness);
+                ch.setPunctureDotThreshold(punctureDotThreshold);
+            }
+
+            SimulationManager.sceneManager.getActiveScene().addInteraction(_needleInteraction);
+        }
+
+        public void Pull()
+        {
+            _needleInteraction.stitch();
+        }
+
+        public void Update()
+        {
+            if (!_activated && Input.GetKeyDown(activationKey))
+            {
+                Pull();
+            }
+        }
+        public Imstk.NeedlePbdCH.PunctureData GetPunctureData()
+        {
+            return _needleInteraction.getPunctureData();
+        }
+    }
+}

Assets/Imstk/Scripts/HighLevel/Suturing.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: a7a8cdd270f1b2c468a724647826fc16
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {fileID: 2800000, guid: 936dbb32eb653d84a9bcbe9caad48547, type: 3}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: