added simulation flag, use robot resolution constructor, clean up sce…

…ne files
mli0603 · Mar 4, 2020 · b582ab2 · b582ab2
1 parent 2df34f5
commit b582ab2
Show file tree

Hide file tree

Showing 16 changed files with 77 additions and 90 deletions.
diff --git a/code/mtsDerivedIntuitiveResearchKitPSM.cpp b/code/mtsDerivedIntuitiveResearchKitPSM.cpp
@@ -69,6 +69,7 @@ void mtsDerivedIntuitiveResearchKitPSM::Configure(const std::string &filename)
         CMN_ASSERT(RobotInterface);
         RobotInterface->AddCommandWrite(&mtsDerivedIntuitiveResearchKitPSM::SetConstraintMotionEnable, this, "SetConstraintMotionEnable");
         RobotInterface->AddCommandRead(&mtsDerivedIntuitiveResearchKitPSM::GetSlackVector, this, "GetSlackForceDirection");
+        RobotInterface->AddCommandRead(&mtsDerivedIntuitiveResearchKitPSM::GetSimulation, this, "GetSimulation");
     }
 }
 
@@ -136,8 +137,8 @@ void mtsDerivedIntuitiveResearchKitPSM::SetupVF()
     mController->AddVFPlane(mPlaneRight);
 
     // mesh constraint
-    mMeshFile = cisstMesh(true);
-    if (mMeshFile.LoadMeshFromSTLFile("/home/dvrk-pc/dvrk_ws/src/USAblation/mesh/Skull.STL")==-1){
+    mMeshFile = cisstMesh(0.5,true); // 0.5 mm error for PSM
+    if (mMeshFile.LoadMeshFromSTLFile("/home/dvrk-pc/dvrk_ws/src/USAblation/mesh/Skull.stl")==-1){
         CMN_LOG_CLASS_RUN_ERROR << "Cannot load STL file" << std::endl;
         cmnThrow("Cannot load STL file");
     }
@@ -315,6 +316,11 @@ void mtsDerivedIntuitiveResearchKitPSM::GetSlackVector(vct3 &force) const
     force.Assign(mSlackVector);
 }
 
+void mtsDerivedIntuitiveResearchKitPSM::GetSimulation(bool &status) const
+{
+    status = mSimulated;
+}
+
 void mtsDerivedIntuitiveResearchKitPSM::ComputeSlackVector(vctDoubleVec &jointInc, vctDoubleVec &jointSlack)
 {
     mSlackVector.Zeros();

diff --git a/code/mtsDerivedTeleOperationPSM.cpp b/code/mtsDerivedTeleOperationPSM.cpp
@@ -72,6 +72,8 @@ void mtsDerivedTeleOperationPSM::Configure(const std::string & CMN_UNUSED(filena
                               PSMSetConstraintMotionEnable);
     interfacePSM->AddFunction("GetSlackForceDirection",
                               PSMGetSlackForceDirection);
+    interfacePSM->AddFunction("GetSimulation",
+                              PSMGetSimulation);
 
     // Same for MTM
     mtsInterfaceRequired * interfaceMTM = GetInterfaceRequired("MTM");
@@ -156,9 +158,13 @@ void mtsDerivedTeleOperationPSM::RunEnabled(void)
             // Re-orient based on rotation between MTM and PSM
             force = mRegistrationRotation.Inverse() * force;
             // set force to MTM
-            prmForceCartesianSet wrenchMTM;
-            wrenchMTM.Force().Ref<3>(0) = force;
-            mMTM.SetWrenchBody(wrenchMTM);
+            bool psmSimulated;
+            PSMGetSimulation(psmSimulated);
+            if (!psmSimulated){
+                prmForceCartesianSet wrenchMTM;
+                wrenchMTM.Force().Ref<3>(0) = force;
+                mMTM.SetWrenchBody(wrenchMTM);
+            }
         }
     }
 }

diff --git a/include/mtsDerivedIntuitiveResearchKitPSM.h b/include/mtsDerivedIntuitiveResearchKitPSM.h
@@ -79,6 +79,7 @@ class mtsDerivedIntuitiveResearchKitPSM : public mtsIntuitiveResearchKitPSM
     virtual void SetSimulation(const bool & status);
     virtual void SetSkullToPSMTransform(const vctFrm4x4 & transform);
     virtual void GetSlackVector(vct3 & force) const;
+    virtual void GetSimulation(bool & status) const;
     bool mConstraintMotionEnabled;
     bool mSimulated;
     prmPositionCartesianGet mProxyCartesianPosition;

diff --git a/include/mtsDerivedTeleOperationPSM.h b/include/mtsDerivedTeleOperationPSM.h
@@ -45,6 +45,7 @@ class mtsDerivedTeleOperationPSM: public mtsTeleOperationPSM
     mtsFunctionWrite MTMSetWrenchBodyOrientationAbsolute;
     mtsFunctionWrite PSMSetConstraintMotionEnable;
     mtsFunctionRead  PSMGetSlackForceDirection;
+    mtsFunctionRead  PSMGetSimulation;
 
     vctFrm4x4 mPSMProxyMeasuredCartesianPosition;
     vct3 elasticityGain;

diff --git a/scene/2020-02-06-Scene.mrml b/scene/2020-02-06-Scene.mrml
diff --git a/scene/Displacement to color.txt b/scene/Displacement to color.txt
diff --git a/scene/Displacement to color_1.txt b/scene/Displacement to color_1.txt
diff --git a/scene/Displacement to color_2.txt b/scene/Displacement to color_2.txt
diff --git a/scene/Displacement to color_3.txt b/scene/Displacement to color_3.txt
diff --git a/scene/Locator_Skull To PSM.vtk b/scene/Locator_Skull To PSM.vtk
diff --git a/scene/Master Scene View.png b/scene/Master Scene View.png
diff --git a/scene/Measured.fcsv b/scene/Measured.fcsv
@@ -1,4 +1,4 @@
 # Markups fiducial file version = 4.8
 # CoordinateSystem = 0
 # columns = id,x,y,z,ow,ox,oy,oz,vis,sel,lock,label,desc,associatedNodeID
-vtkMRMLMarkupsFiducialNode_1,199.771,-21.4751,58.122,0,0,0,1,1,1,0,,,
+vtkMRMLMarkupsFiducialNode_1,195.099,-28.4652,76.0968,0,0,0,1,1,1,0,,,
diff --git a/scene/Plane.fcsv b/scene/Plane.fcsv
diff --git a/scene/Servo.fcsv b/scene/Servo.fcsv
@@ -1,4 +1,4 @@
 # Markups fiducial file version = 4.8
 # CoordinateSystem = 0
 # columns = id,x,y,z,ow,ox,oy,oz,vis,sel,lock,label,desc,associatedNodeID
-vtkMRMLMarkupsFiducialNode_2,200.054,-21.4196,57.7781,0,0,0,1,1,1,0,,,
+vtkMRMLMarkupsFiducialNode_2,233.122,29.9612,68.9746,0,0,0,1,1,1,0,,,
diff --git a/scene/Skull.stl b/scene/Skull.stl
diff --git a/scene/Skull_with_path.stl b/scene/Skull_with_path.stl