3 using System.Collections.Generic;
16 namespace droid.Runtime.Environments {
33 return this.CollectState();
42 lock (this._Reaction_Lock) {
43 if (this._Terminable) {
46 Debug.LogWarning($
"Environment {this.Identifier} as terminated because {reason}");
50 this._Terminated =
true;
51 this._LastTermination_Reason = reason;
59 public override void Tick() {
60 if (this._Resetting) {
61 if (this._reset_i >= this._Simulation_Manager.SimulatorConfiguration.ResetIterations) {
62 this._Resetting =
false;
64 this.UpdateConfigurableValues();
65 this.UpdateObserversData();
67 this.EnvironmentReset();
73 Debug.Log($
"Reset {this._reset_i}/{this._Simulation_Manager.SimulatorConfiguration.ResetIterations}");
77 if (this.update_observations_with_every_tick) {
78 this.UpdateObserversData();
92 foreach (var obs
in this.Observers.Values) {
93 obs?.UpdateObservation();
100 foreach (var con
in this.Configurables.Values) {
102 con?.UpdateCurrentConfiguration();
106 #region PrivateMembers 110 Vector3[] _reset_positions;
114 Quaternion[] _reset_rotations;
118 GameObject[] _tracked_game_objects;
122 Vector3[] _reset_velocities;
126 Vector3[] _reset_angulars;
138 Pose[] _received_poses;
142 Body[] _received_bodies;
150 Animation[] _animations;
154 float[] _reset_animation_times;
180 [Header(
"References", order = 20)]
186 [Header(
"General", order = 30)]
193 protected bool _track_only_children =
true;
202 [Header(
"(Optional)", order = 80)]
219 protected WaitForFixedUpdate _Wait_For_Fixed_Update =
new WaitForFixedUpdate();
224 protected List<float> _Observables =
new List<float>();
229 protected List<ActuatorMotion> _Sample_Motions =
new List<ActuatorMotion>();
235 protected bool update_observations_with_every_tick =
true;
239 #region Private Methods 243 void SaveInitialPoses() {
244 var ignored_layer = LayerMask.NameToLayer(
"IgnoredByNeodroid");
245 if (this._track_only_children) {
246 this._tracked_game_objects =
247 NeodroidUtilities.RecursiveChildGameObjectsExceptLayer(this.transform, ignored_layer);
249 this._tracked_game_objects = NeodroidUtilities.FindAllGameObjectsExceptLayer(ignored_layer);
252 var length = this._tracked_game_objects.Length;
254 this._reset_positions =
new Vector3[length];
255 this._reset_rotations =
new Quaternion[length];
256 this._Poses =
new Transform[length];
257 for (var i = 0; i < length; i++) {
258 var go = this._tracked_game_objects[i];
259 var trans = go.transform;
260 this._reset_positions[i] = trans.position;
261 this._reset_rotations[i] = trans.rotation;
262 this._Poses[i] = trans;
263 var maybe_joint = go.GetComponent<Joint>();
264 if (maybe_joint != null) {
265 var maybe_joint_fix = maybe_joint.GetComponent<
JointFix>();
266 if (maybe_joint_fix == null) {
268 maybe_joint_fix = maybe_joint.gameObject.AddComponent<
JointFix>();
271 if (this.Debugging) {
272 Debug.Log($
"Added a JointFix component to {maybe_joint_fix.name}");
281 void SaveInitialBodies() {
293 this._Tracked_Rigid_Bodies = this._tracked_game_objects.Where(go => go != null)
294 .Select(go => go.GetComponent<Rigidbody>()).Where(body => body)
297 this._reset_velocities =
new Vector3[this._Tracked_Rigid_Bodies.Length];
298 this._reset_angulars =
new Vector3[this._Tracked_Rigid_Bodies.Length];
299 for (var i = 0; i < this._Tracked_Rigid_Bodies.Length; i++) {
300 this._reset_velocities[i] = this._Tracked_Rigid_Bodies[i].velocity;
301 this._reset_angulars[i] = this._Tracked_Rigid_Bodies[i].angularVelocity;
307 void SaveInitialAnimations() {
308 this._animations = this._tracked_game_objects.Where(go => go != null)
309 .Select(go => go.GetComponent<Animation>()).Where(anim => anim).ToArray();
310 this._reset_animation_times =
new float[this._animations.Length];
311 for (var i = 0; i < this._animations.Length; i++) {
312 if (this._animations[i]) {
313 if (this._animations[i].clip) {
314 this._reset_animation_times[i] =
315 this._animations[i].CrossFadeQueued(this._animations[i].clip.name)
327 if (this._objective_function == null) {
328 this._objective_function = this.GetComponent<ObjectiveFunction>();
331 if (!this.PlayableArea) {
332 this.PlayableArea = this.GetComponent<BoundingBox>();
336 if (this.Debugging) {
337 Debug.Log(
"Setting up");
341 if (this._tracked_game_objects == null || this._tracked_game_objects.Length == 0) {
342 this.SaveInitialPoses();
343 this.SaveInitialAnimations();
344 this.StartCoroutine(this.SaveInitialBodiesIe());
348 if (this.Debugging) {
349 Debug.Log(
"Setup done");
353 foreach (var configurable
in this.Configurables.Values) {
354 configurable?.PostEnvironmentSetup();
361 IEnumerator SaveInitialBodiesIe() {
362 yield
return this._Wait_For_Fixed_Update;
363 this.SaveInitialBodies();
370 lock (this._Reaction_Lock) {
371 this.PreStepEvent?.Invoke();
374 this.CurrentFrameNumber++;
377 if (this.Debugging) {
378 Debug.LogWarning(
"Step did not count towards CurrentFrameNumber");
383 this.InnerStep(reaction);
385 this.StepEvent?.Invoke();
387 this.UpdateObserversData();
397 lock (this._Reaction_Lock) {
398 this.LastReaction = reaction;
404 if (!this._ReplyWithDescriptionThisStep) {
415 this.SendToDisplayers(reaction);
418 this.Terminate($
"{(reaction.Parameters.IsExternal ? "External
" : "Internal
")} reaction caused a reset");
419 this._Resetting =
true;
422 if (this.Debugging) {
423 Debug.Log($
"Stepping in environment({this.Identifier})");
435 this.PostStepEvent?.Invoke();
436 if (this._Configure) {
438 if (this.Debugging) {
439 Debug.Log(
"Configuring");
442 this._Configure =
false;
446 if (!this._Simulation_Manager.IsSyncingEnvironments) {
447 this._ReplyWithDescriptionThisStep =
false;
454 this._Lastest_Reset_Time = Time.time;
455 this.CurrentFrameNumber = 0;
458 ResetEnvironmentPoses(ref this._tracked_game_objects,
459 ref this._reset_positions,
460 ref this._reset_rotations,
461 this._Simulation_Manager.SimulatorConfiguration.ResetIterations);
463 ResetEnvironmentBodies(ref this._Tracked_Rigid_Bodies,
464 ref this._reset_velocities,
465 ref this._reset_angulars,
468 ResetEnvironmentBodies(ref this._tracked_rigid_bodies,
469 ref this._reset_velocities,
470 ref this._reset_angulars);
473 this.ResetRegisteredObjects();
477 if (this.Debugging) {
478 Debug.Log($
"Reset called on environment {this.Identifier}");
482 this._Terminated =
false;
489 if (this.Debugging) {
490 Debug.Log(
"Resetting registered objects");
494 foreach (var configurable
in this.Configurables.Values) {
495 configurable?.EnvironmentReset();
498 foreach (var resetable
in this.Listeners.Values) {
499 resetable?.EnvironmentReset();
502 this.InnerResetRegisteredObjects();
504 foreach (var observer
in this.Observers.Values) {
505 observer?.EnvironmentReset();
512 if (this._received_poses != null) {
513 var positions =
new Vector3[this._received_poses.Length];
514 var rotations =
new Quaternion[this._received_poses.Length];
515 for (var i = 0; i < this._received_poses.Length; i++) {
516 positions[i] = this._received_poses[i].position;
517 rotations[i] = this._received_poses[i].rotation;
520 ResetEnvironmentPoses(ref this._tracked_game_objects,
523 this._Simulation_Manager.SimulatorConfiguration.ResetIterations);
526 if (this._received_bodies != null) {
527 var velocities =
new Vector3[this._received_bodies.Length];
528 var angulars =
new Vector3[this._received_bodies.Length];
529 for (var i = 0; i < this._received_bodies.Length; i++) {
530 velocities[i] = this._received_bodies[i].
Velocity;
534 ResetEnvironmentBodies(ref this._Tracked_Rigid_Bodies, ref velocities, ref angulars);
537 if (this._received_configurations != null) {
539 if (this.Debugging) {
540 Debug.Log($
"Configuration length: {this._received_configurations.Length}");
543 foreach (var configuration
in this._received_configurations) {
545 if (this.Debugging) {
546 Debug.Log(
"Configuring configurable with the specified name: " + configuration.ConfigurableName);
549 if (this.Configurables.ContainsKey(configuration.ConfigurableName)) {
550 this.Configurables[configuration.ConfigurableName].ApplyConfiguration(configuration);
553 if (this.Debugging) {
554 Debug.Log($
"Could find not configurable with the specified name: {configuration.ConfigurableName}");
561 if (this.Debugging) {
562 Debug.Log(
"Has no received_configurations");
567 this.UpdateConfigurableValues();
568 this.UpdateObserversData();
574 void SendToDisplayers(
Reaction reaction) {
578 if (this.Debugging) {
579 Debug.Log(
"Applying " + displayable +
" To " + this.name +
"'s displayers");
582 var displayable_name = displayable.DisplayableName;
583 if (this.Displayers.ContainsKey(displayable_name) && this.Displayers[displayable_name] != null) {
584 var v = displayable.DisplayableValue;
585 this.Displayers[displayable_name].Display(v);
588 if (this.Debugging) {
589 Debug.Log(
"Could find not displayer with the specified name: " + displayable_name);
599 #region EnvironmentStateSetters 607 static void ResetEnvironmentPoses(ref GameObject[] child_game_objects,
608 ref Vector3[] positions,
609 ref Quaternion[] rotations,
610 int iterations = 1) {
611 for (var it = 1; it <= iterations; it++) {
612 for (var i = 0; i < child_game_objects.Length; i++) {
613 if (child_game_objects[i] != null && i < positions.Length && i < rotations.Length) {
614 var rigid_body = child_game_objects[i].GetComponent<Rigidbody>();
619 child_game_objects[i].transform.position = positions[i];
620 child_game_objects[i].transform.rotation = rotations[i];
625 var joint_fix = child_game_objects[i].GetComponent<
JointFix>();
630 var anim = child_game_objects[i].GetComponent<Animation>();
648 static void ResetEnvironmentBodies(ref Rigidbody[] bodies,
649 ref Vector3[] velocities,
650 ref Vector3[] angulars,
651 bool debugging =
false) {
652 if (bodies != null && bodies.Length > 0) {
653 for (var i = 0; i < bodies.Length; i++) {
654 if (i < bodies.Length && bodies[i] != null && i < velocities.Length && i < angulars.Length) {
657 Debug.Log($
"Setting {bodies[i].name}, velocity to {velocities[i]} and angular velocity to {angulars[i]}");
663 bodies[i].velocity = velocities[i];
664 bodies[i].angularVelocity = angulars[i];
673 #region Transformations 680 switch (this._coordinate_system) {
681 case CoordinateSystem.Relative_to_reference_point_ when this._coordinate_reference_point:
682 return this._coordinate_reference_point.transform.InverseTransformPoint(point);
684 return this.transform.InverseTransformPoint(point);
688 if (this.Debugging) {
689 Debug.Log(
"Defaulting TransformPoint");
697 switch (this._coordinate_system) {
698 case CoordinateSystem.Relative_to_reference_point_ when this._coordinate_reference_point:
699 point = this._coordinate_reference_point.transform.InverseTransformPoint(point);
703 point = this.transform.InverseTransformPoint(point);
707 if (this.Debugging) {
708 Debug.Log(
"Defaulting TransformPoint");
720 switch (this._coordinate_system) {
721 case CoordinateSystem.Relative_to_reference_point_ when this._coordinate_reference_point:
722 return this._coordinate_reference_point.transform.TransformPoint(point);
725 return this.transform.TransformPoint(point);
728 if (this.Debugging) {
729 Debug.Log(
"Defaulting InverseTransformPoint");
741 switch (this._coordinate_system) {
742 case CoordinateSystem.Relative_to_reference_point_ when this._coordinate_reference_point:
743 point = this._coordinate_reference_point.transform.TransformPoint(point);
747 point = this.transform.TransformPoint(point);
751 if (this.Debugging) {
752 Debug.Log(
"Defaulting InverseTransformPoint");
764 switch (this._coordinate_system) {
765 case CoordinateSystem.Relative_to_reference_point_ when this._coordinate_reference_point:
766 return this._coordinate_reference_point.transform.InverseTransformDirection(direction);
768 return this.transform.InverseTransformDirection(direction);
771 if (this.Debugging) {
772 Debug.Log(
"Defaulting TransformDirection");
784 switch (this._coordinate_system) {
785 case CoordinateSystem.Relative_to_reference_point_ when this._coordinate_reference_point:
786 direction = this._coordinate_reference_point.transform.InverseTransformDirection(direction);
789 direction = this.transform.InverseTransformDirection(direction);
794 if (this.Debugging) {
795 Debug.Log(
"Defaulting TransformDirection");
807 switch (this._coordinate_system) {
808 case CoordinateSystem.Relative_to_reference_point_ when this._coordinate_reference_point:
809 return this._coordinate_reference_point.transform.TransformDirection(direction);
811 return this.transform.TransformDirection(direction);
815 if (this.Debugging) {
816 Debug.Log(
"Defaulting InverseTransformDirection");
824 switch (this._coordinate_system) {
825 case CoordinateSystem.Relative_to_reference_point_ when this._coordinate_reference_point:
826 direction = this._coordinate_reference_point.transform.TransformDirection(direction);
829 direction = this.transform.TransformDirection(direction);
833 if (this.Debugging) {
834 Debug.Log(
"Defaulting InverseTransformDirection");
846 if (this._coordinate_system ==
CoordinateSystem.Relative_to_reference_point_) {
847 if (this._coordinate_reference_point) {
848 return Quaternion.Inverse(this._coordinate_reference_point.rotation) * quaternion;
852 }
else if (this._coordinate_system ==
CoordinateSystem.Local_coordinates_) {
853 if (this._coordinate_reference_point) {
854 return Quaternion.Inverse(this.Transform.rotation) * quaternion;
866 if (this._coordinate_system ==
CoordinateSystem.Relative_to_reference_point_) {
867 if (this._coordinate_reference_point) {
868 quaternion = Quaternion.Inverse(this._coordinate_reference_point.rotation) * quaternion;
872 }
else if (this._coordinate_system ==
CoordinateSystem.Local_coordinates_) {
873 if (this._coordinate_reference_point) {
874 quaternion = Quaternion.Inverse(this.Transform.rotation) * quaternion;
880 if (this._coordinate_system ==
CoordinateSystem.Relative_to_reference_point_) {
881 if (this._coordinate_reference_point) {
882 return this._coordinate_reference_point.rotation * quaternion;
886 }
else if (this._coordinate_system ==
CoordinateSystem.Local_coordinates_) {
887 if (this._coordinate_reference_point) {
888 return this.Transform.rotation * quaternion;
896 if (this._coordinate_system ==
CoordinateSystem.Relative_to_reference_point_) {
897 if (this._coordinate_reference_point) {
898 quaternion = this._coordinate_reference_point.rotation * quaternion;
899 }
else if (this._coordinate_system ==
CoordinateSystem.Local_coordinates_) {
900 if (this._coordinate_reference_point) {
901 quaternion = this.Transform.rotation * quaternion;
915 public Dictionary<string, IDisplayer> Displayers {
get; } =
new Dictionary<string, IDisplayer>();
919 public Dictionary<string, IConfigurable> Configurables {
get; } =
new Dictionary<string, IConfigurable>();
923 public SortedDictionary<string, IObserver> Observers {
get; } =
new SortedDictionary<string, IObserver>();
927 public Dictionary<string, IEnvironmentListener> Listeners {
get; } =
928 new Dictionary<string, IEnvironmentListener>();
933 public override string PrototypingTypeName {
get {
return "PrototypingEnvironment"; } }
938 get {
return this._objective_function; }
944 public Transform Transform {
get {
return this.transform; } }
949 get {
return this._playable_area; }
950 set { this._playable_area = value; }
955 public Transform CoordinateReferencePoint {
956 get {
return this._coordinate_reference_point; }
957 set { this._coordinate_reference_point = value; }
963 get {
return this._coordinate_system; }
964 set { this._coordinate_system = value; }
982 if (!this.Displayers.ContainsKey(identifier)) {
984 if (this.Debugging) {
985 Debug.Log($
"Environment {this.name} has registered displayer {identifier}");
988 this.Displayers.Add(identifier, obj);
990 Debug.LogWarning($
"WARNING! Please check for duplicates, Environment {this.name} already has displayer {identifier} registered");
1007 if (!this.Observers.ContainsKey(identifier)) {
1009 if (this.Debugging) {
1010 Debug.Log($
"Environment {this.name} has registered observer {identifier}");
1014 this.Observers.Add(identifier, observer);
1016 Debug.LogWarning($
"WARNING! Please check for duplicates, Environment {this.name} already has observer {identifier} registered");
1032 if (!this.Configurables.ContainsKey(identifier)) {
1034 if (this.Debugging) {
1035 Debug.Log($
"Environment {this.name} has registered configurable {identifier}");
1039 this.Configurables.Add(identifier, configurable);
1041 Debug.LogWarning($
"WARNING! Please check for duplicates, Environment {this.name} already has configurable {identifier} registered");
1050 this.Register(environment_listener, environment_listener.
Identifier);
1059 if (!this.Listeners.ContainsKey(identifier)) {
1061 if (this.Debugging) {
1062 Debug.Log($
"Environment {this.name} has registered resetable {identifier}");
1065 this.Listeners.Add(identifier, environment_listener);
1067 Debug.LogWarning($
"WARNING! Please check for duplicates, Environment {this.name} already has resetable {identifier} registered");
1082 if (this.Observers.ContainsKey(identifier)) {
1084 if (this.Debugging) {
1085 Debug.Log($
"Environment {this.name} unregistered observer {identifier}");
1088 this.Observers.Remove(identifier);
1096 this.UnRegister(configurable, configurable.
Identifier);
1105 if (this.Configurables.ContainsKey(identifier)) {
1107 if (this.Debugging) {
1108 Debug.Log($
"Environment {this.name} unregistered configurable {identifier}");
1111 this.Configurables.Remove(identifier);
1126 if (this.Displayers.ContainsKey(identifier)) {
1128 if (this.Debugging) {
1129 Debug.Log($
"Environment {this.name} unregistered configurable {identifier}");
1132 this.Displayers.Remove(identifier);
1140 this.UnRegister(environment_listener, environment_listener.
Identifier);
1149 if (this.Listeners.ContainsKey(identifier)) {
1151 if (this.Debugging) {
1152 Debug.Log($
"Environment {this.name} unregistered resetable {identifier}");
1155 this.Listeners.Remove(identifier);
1164 protected abstract void InnerStep(
Reaction reaction);
1169 protected abstract void InnerResetRegisteredObjects();
void Terminate(string reason="None")
Termination of an episode, can be supplied with a reason for various purposes debugging or clarificat...
void Register(IEnvironmentListener environment_listener, string identifier)
void UnRegister(IObserver t, string identifier)
void UnRegister(IEnvironmentListener environment_listener)
void UnRegister(IDisplayer t, string identifier)
void ResetRegisteredObjects()
override void React(Reaction reaction)
void Register(IDisplayer obj, string identifier)
void Register(IObserver observer, string identifier)
void UnRegister(IDisplayer displayer)
BoundingBox _playable_area
void UnRegister(IConfigurable configurable)
void UnRegister(IConfigurable t, string identifier)
void UnRegister(IEnvironmentListener t, string identifier)
Rigidbody [] _Tracked_Rigid_Bodies
void TransformRotation(ref Quaternion quaternion)
void InverseTransformDirection(ref Vector3 direction)
void UpdateObserversData()
void UpdateConfigurableValues()
void TransformDirection(ref Vector3 direction)
void UnRegister(IObserver observer)
ObjectiveFunction _objective_function
ReactionParameters Parameters
Configuration [] Configurations
void InverseTransformPoint(ref Vector3 point)
void InverseTransformRotation(ref Quaternion quaternion)
void Register(IConfigurable configurable, string identifier)
Vector3 TransformDirection(Vector3 direction)
Transform _coordinate_reference_point
void Register(IDisplayer displayer)
void Register(IObserver observer)
Vector3 InverseTransformDirection(Vector3 direction)
Displayable [] Displayables
Unobservables Unobservables
Vector3 TransformPoint(Vector3 point)
void TransformPoint(ref Vector3 point)
Quaternion InverseTransformRotation(Quaternion quaternion)
Vector3 InverseTransformPoint(Vector3 point)
Quaternion TransformRotation(Quaternion quaternion)
void Register(IConfigurable configurable)
void Register(IEnvironmentListener environment_listener)
override void EnvironmentReset()
override EnvironmentState ReactAndCollectState(Reaction reaction)