Merge branch 'nanna_arm_teleport' into nanna

unity/Assets/Scripts/AgentManager.cs

@@ -1541,6 +1541,8 @@ public class JointMetadata {
     public Vector4 rotation;
     public Vector4 rootRelativeRotation;
     public Vector4 localRotation;
+    public float? armBaseHeight;
+    public float? elbowOrientation;
 }
 
 [Serializable]

unity/Assets/Scripts/ArmAgentController.cs

@@ -50,6 +50,89 @@ public void ToggleMagnetVisibility(bool? visible = null) {
             actionFinished(true);
         }
 
+        public void TeleportArm(
+            Vector3? position = null,
+            Vector4? rotation = null,
+            float? armHeight = null,
+            float? elbowOrientation = null,
+            bool worldRelative = false,
+            bool forceAction = false
+        ) {
+            GameObject heightManip = this.GetComponent<BaseAgentComponent>().IKArm;
+            GameObject posRotManip = this.GetComponent<BaseAgentComponent>().IKArm.GetComponent<IK_Robot_Arm_Controller>().GetArmTarget();
+            GameObject elbowManip = this.GetComponent<BaseAgentComponent>().IKArm.GetComponent<IK_Robot_Arm_Controller>().GetElbowTarget();
+
+            // cache old values in case there's a failure
+            Vector3 oldLocalPosition = posRotManip.transform.localPosition;
+            float oldLocalRotationAngle;
+            Vector3 oldLocalRotationAxis;
+            posRotManip.transform.localRotation.ToAngleAxis(angle: out oldLocalRotationAngle, axis: out oldLocalRotationAxis);
+            float oldArmHeight = heightManip.transform.localPosition.y;
+            float oldElbowOrientation = elbowManip.transform.localEulerAngles.z;
+
+            // establish defaults in the absence of inputs
+            if (position == null) {
+                position = new Vector3(0f, 0f, 0.4f);
+            }
+
+            if (rotation == null) {
+                rotation = new Vector4(1f, 0f, 0f, 0f);
+            }
+
+            if (armHeight == null) {
+                armHeight = -0.003f;
+            }
+
+            if (elbowOrientation == null) {
+                elbowOrientation = 0f;
+            }
+
+            // teleport arm! (height first, since world-relative positioning needs to take it into account)
+            heightManip.transform.localPosition = new Vector3(
+                heightManip.transform.localPosition.x,
+                (float)armHeight,
+                heightManip.transform.localPosition.z
+            );
+
+            // teleport arm-elements
+            if (!worldRelative) {
+                    posRotManip.transform.localPosition = (Vector3)position;
+                    posRotManip.transform.localRotation = Quaternion.AngleAxis(
+                        ((Vector4)rotation).w % 360,
+                        new Vector3(((Vector4)rotation).x, ((Vector4)rotation).y, ((Vector4)rotation).z)
+                    );
+            } else {
+                    posRotManip.transform.position = (Vector3)position;
+                    posRotManip.transform.rotation = Quaternion.AngleAxis(
+                        ((Vector4)rotation).w % 360,
+                        new Vector3(((Vector4)rotation).x, ((Vector4)rotation).y, ((Vector4)rotation).z)
+                    );
+            }
+
+            elbowManip.transform.localEulerAngles = new Vector3(
+                elbowManip.transform.localEulerAngles.x,
+                elbowManip.transform.localEulerAngles.y,
+                (float)elbowOrientation
+            );
+
+            if (Arm.IsArmColliding() && !forceAction) {
+                errorMessage = "collision detected at desired transform, cannot teleport";
+                heightManip.transform.localPosition = new Vector3(
+                    heightManip.transform.localPosition.x,
+                    oldArmHeight,
+                    heightManip.transform.localPosition.z);
+                posRotManip.transform.localPosition = oldLocalPosition;
+                posRotManip.transform.localRotation = Quaternion.AngleAxis(oldLocalRotationAngle, oldLocalRotationAxis);
+                elbowManip.transform.localEulerAngles = new Vector3(
+                    elbowManip.transform.localEulerAngles.x,
+                    elbowManip.transform.localEulerAngles.y,
+                    oldElbowOrientation);
+                actionFinished(false);
+            } else {
+                actionFinished(true);
+            }
+        }
+
         /*
         This function is identical to `MoveArm` except that rather than
         giving a target position you instead give an "offset" w.r.t.

unity/Assets/Scripts/DebugInputField.cs

@@ -3444,6 +3444,14 @@ IEnumerator executeBatch(JArray jActions) {
                         break;
                     }
 
+                case "telearm": {
+                        Dictionary<string, object> action = new Dictionary<string, object>();
+                        action["action"] = "TeleportArm";
+
+                        CurrentActiveController().ProcessControlCommand(action);
+                        break;
+                }
+
                 case "expfit": {
                         Dictionary<string, object> action = new Dictionary<string, object>();
                         action["action"] = "WhichContainersDoesAvailableObjectFitIn";

unity/Assets/Scripts/IK_Robot_Arm_Controller.cs

@@ -6,7 +6,7 @@
 
 public partial class IK_Robot_Arm_Controller : MonoBehaviour {
     [SerializeField]
-    private Transform armBase, armTarget, handCameraTransform, FirstJoint, FinalJoint;
+    private Transform armBase, armTarget, elbowTarget, handCameraTransform, FirstJoint, FinalJoint;
 
     [SerializeField]
     private SphereCollider magnetSphere = null;
@@ -49,6 +49,9 @@ public GameObject GetArmBase() {
     public GameObject GetArmTarget() {
         return armTarget.gameObject;
     }
+        public GameObject GetElbowTarget() {
+        return elbowTarget.gameObject;
+    }
 
     public GameObject GetMagnetSphere() {
         return magnetSphere.gameObject;
@@ -799,7 +802,7 @@ public ArmMetadata GenerateMetadata() {
             // ROOT-JOINT RELATIVE ROTATION
             // Root-forward and agent-forward are always the same
 
-            //Grab rotation of current joint's angler relative to root joint
+            // Grab rotation of current joint's angler relative to root joint
             currentRotation = Quaternion.Inverse(armBase.rotation) * joint.GetChild(0).rotation;
 
             // Check that root-relative rotation is angle-axis-notation-compatible
@@ -827,6 +830,9 @@ public ArmMetadata GenerateMetadata() {
             } else {
                 // Special case for robot_arm_1_jnt because it has no parent-joint
                 jointMeta.localRotation = jointMeta.rootRelativeRotation;
+
+                jointMeta.armBaseHeight = this.transform.localPosition.y;
+                jointMeta.elbowOrientation = elbowTarget.localEulerAngles.z;
             }
 
             joints.Add(jointMeta);

unity/Assets/Scripts/RobotArmTest/robot_arm_rig_gripper.prefab

@@ -2874,6 +2874,9 @@ MonoBehaviour:
   armElbow: {fileID: 8593510233495944790}
   armWrist: {fileID: 4868417420362964741}
   armHand: {fileID: 4794036240972261876}
+  bone1Length: 0
+  bone2Length: 0
+  bone3Length: 0
   FKRootTarget: {fileID: 5855119031362891387}
   FKShoulderTarget: {fileID: 5855119031444671683}
   FKElbowTarget: {fileID: 5855119030947992120}
@@ -3526,6 +3529,7 @@ MonoBehaviour:
   m_EditorClassIdentifier: 
   armBase: {fileID: 5855119031745003460}
   armTarget: {fileID: 3463435401257780163}
+  elbowTarget: {fileID: 94940350324805794}
   handCameraTransform: {fileID: 4868417420362964741}
   FirstJoint: {fileID: 3619724777740159344}
   FinalJoint: {fileID: 4868417420362964741}
@@ -5075,15 +5079,15 @@ SphereCollider:
     type: 3}
   m_PrefabInstance: {fileID: 5667289294689422201}
   m_PrefabAsset: {fileID: 0}
---- !u!4 &4242414869712351179 stripped
-Transform:
-  m_CorrespondingSourceObject: {fileID: 8378476852940661938, guid: ebceff23aaca6114a9d1b6a0b5b9671a,
-    type: 3}
-  m_PrefabInstance: {fileID: 5667289294689422201}
-  m_PrefabAsset: {fileID: 0}
 --- !u!1 &2830633750730162890 stripped
 GameObject:
   m_CorrespondingSourceObject: {fileID: 7633091127902898611, guid: ebceff23aaca6114a9d1b6a0b5b9671a,
     type: 3}
   m_PrefabInstance: {fileID: 5667289294689422201}
   m_PrefabAsset: {fileID: 0}
+--- !u!4 &4242414869712351179 stripped
+Transform:
+  m_CorrespondingSourceObject: {fileID: 8378476852940661938, guid: ebceff23aaca6114a9d1b6a0b5b9671a,
+    type: 3}
+  m_PrefabInstance: {fileID: 5667289294689422201}
+  m_PrefabAsset: {fileID: 0}