From 3de253a6d8fd1a6790f7e8a4ca650f383e674569 Mon Sep 17 00:00:00 2001
From: Winson Han <winson.whan@gmail.com>
Date: Fri, 26 Jul 2024 14:09:32 -0700
Subject: [PATCH] Fix to stretch arm initialization not including gripper
 variants in collision listener logic

---
 .../RobotArmTest/stretch_arm_rig_gripper.prefab      | 12 ++++++------
 unity/Assets/Scripts/StretchAgentController.cs       |  7 ++++++-
 unity/Assets/Scripts/Stretch_Robot_Arm_Controller.cs |  3 +++
 3 files changed, 15 insertions(+), 7 deletions(-)

diff --git a/unity/Assets/Scripts/RobotArmTest/stretch_arm_rig_gripper.prefab b/unity/Assets/Scripts/RobotArmTest/stretch_arm_rig_gripper.prefab
index 1f938af748..662117c0ae 100644
--- a/unity/Assets/Scripts/RobotArmTest/stretch_arm_rig_gripper.prefab
+++ b/unity/Assets/Scripts/RobotArmTest/stretch_arm_rig_gripper.prefab
@@ -2075,7 +2075,7 @@ GameObject:
   m_Icon: {fileID: 0}
   m_NavMeshLayer: 0
   m_StaticEditorFlags: 0
-  m_IsActive: 0
+  m_IsActive: 1
 --- !u!4 &3520020699051343488
 Transform:
   m_ObjectHideFlags: 0
@@ -29256,7 +29256,7 @@ GameObject:
   m_Icon: {fileID: 0}
   m_NavMeshLayer: 0
   m_StaticEditorFlags: 0
-  m_IsActive: 0
+  m_IsActive: 1
 --- !u!4 &4305213092866356922
 Transform:
   m_ObjectHideFlags: 0
@@ -36573,7 +36573,7 @@ GameObject:
   m_Icon: {fileID: 0}
   m_NavMeshLayer: 0
   m_StaticEditorFlags: 0
-  m_IsActive: 0
+  m_IsActive: 1
 --- !u!4 &5141672049479370786
 Transform:
   m_ObjectHideFlags: 0
@@ -47082,7 +47082,7 @@ GameObject:
   m_Icon: {fileID: 0}
   m_NavMeshLayer: 0
   m_StaticEditorFlags: 0
-  m_IsActive: 0
+  m_IsActive: 1
 --- !u!4 &261595647541202395
 Transform:
   m_ObjectHideFlags: 0
@@ -48173,7 +48173,7 @@ GameObject:
   m_Icon: {fileID: 0}
   m_NavMeshLayer: 0
   m_StaticEditorFlags: 0
-  m_IsActive: 0
+  m_IsActive: 1
 --- !u!4 &3448685651741388282
 Transform:
   m_ObjectHideFlags: 0
@@ -50975,7 +50975,7 @@ GameObject:
   m_Icon: {fileID: 0}
   m_NavMeshLayer: 0
   m_StaticEditorFlags: 0
-  m_IsActive: 0
+  m_IsActive: 1
 --- !u!4 &1789519952878528289
 Transform:
   m_ObjectHideFlags: 0
diff --git a/unity/Assets/Scripts/StretchAgentController.cs b/unity/Assets/Scripts/StretchAgentController.cs
index b28e9de058..e342445345 100644
--- a/unity/Assets/Scripts/StretchAgentController.cs
+++ b/unity/Assets/Scripts/StretchAgentController.cs
@@ -568,6 +568,12 @@ protected int gripperOpenFloatToState(float openness) {
         }
 
         public ActionFinished SetGripperOpenness(float? openness, int? openState = null) {
+            setGripperOpenness(openness: openness, openState: openState);
+            return ActionFinished.Success;
+        }
+
+        //moving this to a helper function so we can call it without sending back an ActionFinished call
+        public void setGripperOpenness(float? openness, int? openState = null) {
             if (openness.HasValue == openState.HasValue) {
                 throw new InvalidOperationException(
                     $"Only one of openness or openState should have a value"
@@ -583,7 +589,6 @@ public ActionFinished SetGripperOpenness(float? openness, int? openState = null)
 
             GripperOpennessStates[openState.Value].SetActive(true);
             gripperOpennessState = openState.Value;
-            return ActionFinished.Success;
         }
 
         //        public void RotateCameraBase(float yawDegrees, float rollDegrees) {
diff --git a/unity/Assets/Scripts/Stretch_Robot_Arm_Controller.cs b/unity/Assets/Scripts/Stretch_Robot_Arm_Controller.cs
index a0b91362b4..453d593746 100644
--- a/unity/Assets/Scripts/Stretch_Robot_Arm_Controller.cs
+++ b/unity/Assets/Scripts/Stretch_Robot_Arm_Controller.cs
@@ -143,6 +143,9 @@ void Start() {
         }
 
         solver = this.gameObject.GetComponentInChildren<Stretch_Arm_Solver>();
+
+        //call SetGripperOpenness logic in StretchAgentController here
+        ((StretchAgentController)PhysicsController).setGripperOpenness(openness: null, openState: 1);
     }
 
     public void resetPosRotManipulator() {