/** * \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 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); } } }