public void moveArmBase( PhysicsRemoteFPSAgentController controller, float height, float unitsPerSecond, float fixedDeltaTime = 0.02f, bool returnToStartPositionIfFailed = false, bool disableRendering = false ) { // clearing out colliders here since OnTriggerExit is not consistently called in Editor collisionListener.Reset(); // first check if the target position is within bounds of the agent's capsule center/height extents // if not, actionFinished false with error message listing valid range defined by extents CapsuleCollider cc = controller.GetComponent <CapsuleCollider>(); Vector3 cc_center = cc.center; Vector3 cc_maxY = cc.center + new Vector3(0, cc.height / 2f, 0); Vector3 cc_minY = cc.center + new Vector3(0, (-cc.height / 2f) / 2f, 0); // this is halved to prevent arm clipping into floor // linear function that take height and adjusts targetY relative to min/max extents float targetY = ((cc_maxY.y - cc_minY.y) * (height)) + cc_minY.y; Vector3 target = new Vector3(this.transform.localPosition.x, targetY, 0); IEnumerator moveCall = ContinuousMovement.move( controller: controller, collisionListener: collisionListener, moveTransform: this.transform, targetPosition: target, fixedDeltaTime: disableRendering ? fixedDeltaTime : Time.fixedDeltaTime, unitsPerSecond: unitsPerSecond, returnToStartPropIfFailed: returnToStartPositionIfFailed, localPosition: true ); if (disableRendering) { controller.unrollSimulatePhysics( enumerator: moveCall, fixedDeltaTime: fixedDeltaTime ); } else { StartCoroutine(moveCall); } }
public void moveArmTarget( PhysicsRemoteFPSAgentController controller, Vector3 target, float unitsPerSecond, float fixedDeltaTime = 0.02f, bool returnToStart = false, string coordinateSpace = "arm", bool restrictTargetPosition = false, bool disableRendering = false ) { // clearing out colliders here since OnTriggerExit is not consistently called in Editor collisionListener.Reset(); IK_Robot_Arm_Controller arm = this; // Move arm based on hand space or arm origin space // Vector3 targetWorldPos = handCameraSpace ? handCameraTransform.TransformPoint(target) : arm.transform.TransformPoint(target); Vector3 targetWorldPos = Vector3.zero; switch (coordinateSpace) { case "world": // world space, can be used to move directly toward positions // returned by sim objects targetWorldPos = target; break; case "wrist": // space relative to base of the wrist, where the camera is targetWorldPos = handCameraTransform.TransformPoint(target); break; case "armBase": // space relative to the root of the arm, joint 1 targetWorldPos = arm.transform.TransformPoint(target); break; default: throw new ArgumentException("Invalid coordinateSpace: " + coordinateSpace); } // TODO Remove this after restrict movement is finalized Vector3 targetShoulderSpace = (this.transform.InverseTransformPoint(targetWorldPos) - new Vector3(0, 0, originToShoulderLength)); #if UNITY_EDITOR Debug.Log( $"pos target {target} world {targetWorldPos} remaining {targetShoulderSpace.z}\n" + $"magnitude {targetShoulderSpace.magnitude} extendedArmLength {extendedArmLength}" ); #endif if (restrictTargetPosition && !validArmTargetPosition(targetWorldPos)) { targetShoulderSpace = ( this.transform.InverseTransformPoint(targetWorldPos) - new Vector3(0, 0, originToShoulderLength) ); throw new InvalidOperationException( $"Invalid target: Position '{target}' in space '{coordinateSpace}' is behind shoulder." ); } Vector3 originalPos = armTarget.position; Vector3 targetDirectionWorld = (targetWorldPos - originalPos).normalized; IEnumerator moveCall = ContinuousMovement.move( controller, collisionListener, armTarget, targetWorldPos, disableRendering ? fixedDeltaTime : Time.fixedDeltaTime, unitsPerSecond, returnToStart, false ); if (disableRendering) { controller.unrollSimulatePhysics( moveCall, fixedDeltaTime ); } else { StartCoroutine( moveCall ); } }