// debug for static arm collisions from collision listener public void GetMidLevelArmCollisions() { IK_Robot_Arm_Controller arm = getArm(); CollisionListener collisionListener = arm.GetComponentInChildren <CollisionListener>(); if (collisionListener != null) { List <Dictionary <string, string> > collisions = new List <Dictionary <string, string> >(); foreach (var sc in collisionListener.StaticCollisions()) { Dictionary <string, string> element = new Dictionary <string, string>(); if (sc.simObjPhysics != null) { element["objectType"] = "simObjPhysics"; element["name"] = sc.simObjPhysics.objectID; } else { element["objectType"] = "gameObject"; element["name"] = sc.gameObject.name; } collisions.Add(element); } actionFinished(true, collisions); } }
private static void continuousMoveFinish <T>( PhysicsRemoteFPSAgentController controller, CollisionListener collisionListener, Transform moveTransform, System.Action <Transform, T> setProp, T target, T resetProp ) { bool actionSuccess = true; string debugMessage = ""; IK_Robot_Arm_Controller arm = controller.GetComponentInChildren <IK_Robot_Arm_Controller>(); var staticCollisions = collisionListener.StaticCollisions(); if (staticCollisions.Count > 0) { var sc = staticCollisions[0]; // decide if we want to return to original property or last known property before collision setProp(moveTransform, resetProp); // if we hit a sim object if (sc.isSimObj) { debugMessage = "Collided with static sim object: '" + sc.simObjPhysics.name + "', could not reach target: '" + target + "'."; } // if we hit a structural object that isn't a sim object but still has static collision if (!sc.isSimObj) { debugMessage = "Collided with static structure in scene: '" + sc.gameObject.name + "', could not reach target: '" + target + "'."; } actionSuccess = false; } controller.errorMessage = debugMessage; controller.actionFinished(actionSuccess, debugMessage); }
public static IEnumerator updateTransformPropertyFixedUpdate <T>( PhysicsRemoteFPSAgentController controller, CollisionListener collisionListener, Transform moveTransform, T target, Func <Transform, T> getProp, Action <Transform, T> setProp, Func <Transform, T, T> nextProp, // We could remove this one, but it is a speedup to not compute direction for position update calls at every addToProp call and just outside while Func <T, T, T> getDirection, Func <T, T, float> distanceMetric, float fixedDeltaTime, bool returnToStartPropIfFailed, double epsilon = 1e-3 ) { T originalProperty = getProp(moveTransform); var previousProperty = originalProperty; var arm = controller.GetComponentInChildren <IK_Robot_Arm_Controller>(); var ikSolver = arm.gameObject.GetComponentInChildren <FK_IK_Solver>(); // commenting out the WaitForEndOfFrame here since we shoudn't need // this as we already wait for a frame to pass when we execute each action // yield return yieldInstruction; var currentProperty = getProp(moveTransform); float currentDistance = distanceMetric(target, currentProperty); T directionToTarget = getDirection(target, currentProperty); while (currentDistance > epsilon && collisionListener.StaticCollisions().Count == 0) { previousProperty = getProp(moveTransform); T next = nextProp(moveTransform, directionToTarget); float nextDistance = distanceMetric(target, next); // allows for snapping behaviour to target when the target is close // if nextDistance is too large then it will overshoot, in this case we snap to the target // this can happen if the speed it set high if ( nextDistance <= epsilon || nextDistance > distanceMetric(target, getProp(moveTransform)) ) { setProp(moveTransform, target); } else { setProp(moveTransform, next); } // this will be a NOOP for Rotate/Move/Height actions ikSolver.ManipulateArm(); if (!Physics.autoSimulation) { Physics.Simulate(fixedDeltaTime); } yield return(new WaitForFixedUpdate()); currentDistance = distanceMetric(target, getProp(moveTransform)); } T resetProp = previousProperty; if (returnToStartPropIfFailed) { resetProp = originalProperty; } continuousMoveFinish( controller, collisionListener, moveTransform, setProp, target, resetProp ); // we call this one more time in the event that the arm collided and was reset ikSolver.ManipulateArm(); if (!Physics.autoSimulation) { Physics.Simulate(fixedDeltaTime); } }