/**
* \brief Define the inputs of the MLP for the stabilization task. \f[
* \begin{pmatrix}
* \theta & \phi & \psi & v_{\theta} & v_{\phi} & v_{\psi} & y \\
* \end{pmatrix}
* \f]
* Like the objective funtion, it's not very realistic.
* Technically it is very difficult to use the speed of the drone because a standard IMU provides linear acceleration.
*/
public override void UCSignal(Rigidbody rigid, Vector3 targetPosition)
{
signal.input[0] = UAngle.SteerAngle(rigid.transform.eulerAngles.x);
signal.input[1] = UAngle.SteerAngle(rigid.transform.eulerAngles.y);
signal.input[2] = UAngle.SteerAngle(rigid.transform.eulerAngles.z);
signal.input[3] = rigid.angularVelocity.x;
signal.input[4] = rigid.angularVelocity.y;
signal.input[5] = rigid.angularVelocity.z;
signal.input[6] = rigid.position.y;
/*signal.input[7] = rigid.velocity.y;
* signal.input[8] = rigid.velocity.z;*/
}
/**
* \brief Reset the orientation of the drone define by an index. Take care of a specific condition to define a specific orientation.
*/
public bool ResetOrientation(bool condition, Rigidbody rigid)
{
if (condition)
{
AngleRandomRotation = angleRandomRotation[indexRRArr++];
}
if (indexRRArr == angleRandomRotation.Length)
{
return(false);
}
rigid.transform.eulerAngles = new Vector3(UAngle.ReverseSteerAngle((float)distribution.Sample()), UAngle.ReverseSteerAngle((float)distribution.Sample()), UAngle.ReverseSteerAngle((float)distribution.Sample()));
return(true);
}
/**
* \brief Define the objective function of the stabilization task. This version is perfectly working but not really realistic.
* \f[ evaluation = \frac{1}{1+(|initialY - y| + |\theta| + |\phi| + |\psi| + |v_{\theta}| + |v_{\phi}| + |v_{\psi}| )} \f]
* Technically it is very difficult to use the speed of the drone because a standard IMU provides linear acceleration.
*/
public override float EvaluateIndividual(int i, Rigidbody rigid, Vector3 targetPosition)
{
return(1 / (1 + Mathf.Abs(initialY - rigid.position.y) + Mathf.Abs(UAngle.SteerAngle(rigid.transform.eulerAngles.x)) + Mathf.Abs(UAngle.SteerAngle(rigid.transform.eulerAngles.y)) + Mathf.Abs(UAngle.SteerAngle(rigid.transform.eulerAngles.z)) + Mathf.Abs(rigid.angularVelocity.x) + Mathf.Abs(rigid.angularVelocity.z) + Mathf.Abs(rigid.angularVelocity.y)));
}
void FixedUpdate()
{
if (constant)
{
ControlSignal signal = new ControlSignal();
signal.Throttle = constantThrottle;
signal.Rudder = constantRudder;
signal.Elevator = constantElevator;
signal.Aileron = constantAileron;
MainBoard.SendControlSignal(signal);
}
else
{
if (manual)
{
ControlSignal signal = new ControlSignal();
if (stabilization || move)
{
Rigidbody rigid = this.gameObject.GetComponentInChildren <Rigidbody>();
if (stabilization)
{
Debug.Log("Stable !");
MainBoard.inputMLP = Vector <float> .Build.DenseOfArray(new float[] { UAngle.SteerAngle(rigid.transform.eulerAngles.x), UAngle.SteerAngle(rigid.transform.eulerAngles.y), UAngle.SteerAngle(rigid.transform.eulerAngles.z), rigid.angularVelocity.x, rigid.angularVelocity.y, rigid.angularVelocity.z, rigid.velocity.x, rigid.velocity.y, rigid.velocity.z });
}
else
{
Debug.Log("Move !");
MainBoard.inputMLP = Vector <float> .Build.DenseOfArray(new float[] { MainBoard.deltaPosition.x, MainBoard.deltaPosition.y, MainBoard.deltaPosition.z, UAngle.SteerAngle(rigid.transform.eulerAngles.x), UAngle.SteerAngle(rigid.transform.eulerAngles.y), UAngle.SteerAngle(rigid.transform.eulerAngles.z), rigid.angularVelocity.x, rigid.angularVelocity.y, rigid.angularVelocity.z, rigid.velocity.x, rigid.velocity.y, rigid.velocity.z });
}
signal.Throttle = MainBoard.mlp.layers[MainBoard.mlp.shapesSize - 1][3, 0];
signal.Rudder = MainBoard.mlp.layers[MainBoard.mlp.shapesSize - 1][0, 0];
signal.Elevator = MainBoard.mlp.layers[MainBoard.mlp.shapesSize - 1][1, 0];
signal.Aileron = MainBoard.mlp.layers[MainBoard.mlp.shapesSize - 1][2, 0];
}
else
{
signal.Throttle = Input.GetAxis("LeftY");
signal.Rudder = Input.GetAxis("LeftX");
signal.Elevator = Input.GetAxis("RightY");
signal.Aileron = Input.GetAxis("RightX");
}
MainBoard.SendControlSignal(signal);
}
}
}
