public override void CollectObservations(VectorSensor sensor)
{
float x, y, z, min_z;
// Adding the z-depth of the object that is closest to the camera, if any is present.
// min_z = 99.0f;
// // target_in_camera_coords = agent_camera.WorldToViewportPoint(this.Target.position);
// // if(is_on_camera(target_in_camera_coords)){ // Inside X and Y
// // min_z = target_in_camera_coords.z;
// // }
// z = -1.0f;
// y = -1.0f;
// x = -1.0f;
// target_in_camera_coords = agent_camera.WorldToViewportPoint(this.Obstacle.position);
// if(is_on_camera(target_in_camera_coords)){ // Inside X and Y
// if(target_in_camera_coords.z < min_z){
// min_z = target_in_camera_coords.z;
// x = target_in_camera_coords.x;
// y = target_in_camera_coords.y;
// }
// }
// // target_in_camera_coords = agent_camera.WorldToViewportPoint(ac.Base.position);
// // if(is_on_camera(target_in_camera_coords)){ // Inside X and Y
// // if(target_in_camera_coords.z < min_z){
// // min_z = target_in_camera_coords.z;
// // x = target_in_camera_coords.x;
// // y = target_in_camera_coords.y;
// // }
// // }
// // for(int i = 0; i < 5; i++){
// // target_in_camera_coords = agent_camera.WorldToViewportPoint(ac.GetJointPos(i, true));
// // if(is_on_camera(target_in_camera_coords)){ // Inside X and Y
// // if(target_in_camera_coords.z < min_z){
// // min_z = target_in_camera_coords.z;
// // x = target_in_camera_coords.x;
// // y = target_in_camera_coords.y;
// // }
// // }
// // }
// if(min_z != 99.0f)
// z = min_z;
// else{
// z = 10.0f;
// y = 0.0f;
// x = 0.0f;
// }
sensor.AddObservation(Obstacle.localPosition.x);
sensor.AddObservation(Obstacle.localPosition.y - 1);
sensor.AddObservation(Obstacle.localPosition.z);
// Debug.Log("X: " + x + ", Y: " + y + ", Z:" + z);
// sensor.AddObservation(x);
// sensor.AddObservation(y);
// sensor.AddObservation(z);
// Goal info (either spawn sphere or target)
// sensor.AddObservation(TargetSpawnSphere.localPosition.x);
// sensor.AddObservation(TargetSpawnSphere.localPosition.y);
// sensor.AddObservation(TargetSpawnSphere.localPosition.z);
sensor.AddObservation(Target.localPosition.x);
sensor.AddObservation(Target.localPosition.y - 1);
sensor.AddObservation(Target.localPosition.z);
// Planner info
// Debug.Log(transform.localPosition);
sensor.AddObservation(transform.localPosition.x);
sensor.AddObservation(transform.localPosition.y - 1);
sensor.AddObservation(transform.localPosition.z);
sensor.AddObservation(transform.localRotation);
// Obstacle info (remove if using vision)
// sensor.AddObservation(Obstacle.localPosition);
// NO ARM TEST
if (this.ac != null)
{
// Arm info:
sensor.AddObservation(this.ac.JointAngles[0] / 180.0f);
sensor.AddObservation(this.ac.JointAngles[1] / 180.0f);
sensor.AddObservation(this.ac.JointAngles[3] / 180.0f);
sensor.AddObservation(this.ac.JointAngles[4] / 180.0f);
sensor.AddObservation(this.ac.JointAngles[5] / 180.0f);
sensor.AddObservation(this.ac.JointAngles[6] / 180.0f);
// sensor.AddObservation(ac.GetJointPos(3).x);
// sensor.AddObservation(ac.GetJointPos(3).y-1);
// sensor.AddObservation(ac.GetJointPos(3).z);
// sensor.AddObservation(ac.GetJointPos(5).x);
// sensor.AddObservation(ac.GetJointPos(5).y-1);
// sensor.AddObservation(ac.GetJointPos(5).z);
sensor.AddObservation(ac.GetJointPos(6).x);
sensor.AddObservation(ac.GetJointPos(6).y - 1);
sensor.AddObservation(ac.GetJointPos(6).z);
sensor.AddObservation(ac.GetJointRot(6, true));
}
}