diff --git a/.github/workflows/docker-action.yml b/.github/workflows/docker-action.yml index 1d64b4a..3420935 100644 --- a/.github/workflows/docker-action.yml +++ b/.github/workflows/docker-action.yml @@ -21,7 +21,7 @@ jobs: fail-fast: false steps: - uses: actions/checkout@v2 - - uses: coq-community/docker-coq-action@v1 + - uses: rocq-community/docker-rocq-action@v1 with: opam_file: 'robot-rocq.opam' custom_image: ${{ matrix.image }} diff --git a/_CoqProject b/_CoqProject index 5c10075..cee97fc 100644 --- a/_CoqProject +++ b/_CoqProject @@ -17,5 +17,10 @@ scara.v derive_matrix.v differential_kinematics.v extra_trigo.v +tilt_mathcomp.v +tilt_analysis.v +tilt_robot.v +tilt.v + -R . robot diff --git a/derive_matrix.v b/derive_matrix.v index b73157b..a7fe913 100644 --- a/derive_matrix.v +++ b/derive_matrix.v @@ -1,27 +1,27 @@ (* coq-robot (c) 2017 AIST and INRIA. License: LGPL-2.1-or-later. *) +From HB Require Import structures. From mathcomp Require Import all_ssreflect ssralg ssrint ssrnum rat. From mathcomp Require Import closed_field polyrcf matrix mxalgebra mxpoly zmodp. From mathcomp Require Import interval_inference. From mathcomp Require Import realalg complex fingroup perm. -From mathcomp Require Import sesquilinear. +From mathcomp Require Import sesquilinear ring. From mathcomp Require Import boolp reals classical_sets. -From mathcomp Require Import topology normedtype landau derive. +From mathcomp Require Import topology normedtype landau derive trigo. From mathcomp Require Import functions. Require Import ssr_ext euclidean rigid skew. -(******************************************************************************) -(* Derivatives of time-varying matrices *) +(**md**************************************************************************) +(* # Derivatives of time-varying matrices *) (* *) -(* derive1mx M(t) == the derivative matrix of M(t) *) -(* ang_vel_mx M == angular velocity matrix of M(t) 　　　　　　　　　　　　 *) +(* ang_vel_mx M == angular velocity matrix of M(t) *) (* *) (******************************************************************************) Set Implicit Arguments. Unset Strict Implicit. Unset Printing Implicit Defensive. -Import Order.TTheory GRing.Theory Num.Def Num.Theory. +Import Order.TTheory GRing.Theory Num.Def Num.Theory. Import numFieldNormedType.Exports. Local Open Scope ring_scope. @@ -30,193 +30,360 @@ Lemma mx_lin1N (R : pzRingType) n (M : 'M[R]_n) : mx_lin1 (- M) = -1 \*: mx_lin1 M :> ( _ -> _). Proof. by rewrite funeqE => v /=; rewrite scaleN1r mulmxN. Qed. -Lemma mxE_funeqE (R : realFieldType) (V W : normedModType R) - n m (f : V -> 'I_n -> 'I_m -> W) i j : - (fun x => (\matrix_(i < n, j < m) (f x i j)) i j) = - (fun x => f x i j). -Proof. by rewrite funeqE => ?; rewrite mxE. Qed. - -Section Derive_lemmasVW. -Variables (R : numFieldType) (V W : normedModType R). -Implicit Types f g : V -> W. - -(* TODO: Fixme in MCA *) -Lemma derive_cst (k : W) (x v : V) : 'D_v (cst k) x = 0. -Proof. by rewrite derive_val. Qed. - -End Derive_lemmasVW. - -Lemma derive1_cst {R : numFieldType} (V : normedModType R) (k : V) t : ((cst k)^`() t)%classic = 0. -Proof. by rewrite derive1E derive_cst. Qed. - -Section derive_mx. - -Variable (R : realFieldType) (V W : normedModType R). +Import Order.Def. -Definition derivable_mx m n (M : R -> 'M[W]_(m, n)) t v := - forall i j, derivable (fun x : R^o => (M x) i j) t v. - -Definition derive1mx m n (M : R -> 'M[W]_(m, n)) := fun t => - \matrix_(i < m, j < n) (derive1 (fun x => M x i j) t : W). - -Lemma derive1mxE m n t (f : 'I_m -> 'I_n -> R -> W) : - derive1mx (fun x => \matrix_(i, j) f i j x) t = - \matrix_(i, j) (derive1 (f i j) t : W). +(* NB: added to be able to produce the following instance to be able to use bigop lemmas *) +Lemma nng_max0r {K : realFieldType} : left_id ((0:K)%:nng) (@maxr {nonneg K}). Proof. -rewrite /derive1mx; apply/matrixP => ? ?; rewrite !mxE; congr (derive1 _ t). -rewrite funeqE => ?; by rewrite mxE. +move=> x. +rewrite /max; case: ifPn => //. +rewrite -leNgt => x0. +apply/eqP; rewrite eq_le; apply/andP; split; last first. + exact: x0. +by have : 0 <= x%:nngnum by []. (* NB: this should be automatic *) Qed. -Variables m n : nat. -Implicit Types M N : R -> 'M[W]_(m, n). +(* TODO: backport to MCA *) +HB.instance Definition _ {K : realFieldType} := + Monoid.isComLaw.Build {nonneg K} 0%:nng max maxA maxC nng_max0r. -Lemma derivable_mxD M N t : derivable_mx M t 1 -> derivable_mx N t 1 -> - derivable_mx (fun x => M x + N x) t 1. +Lemma norm_trmx (R : realFieldType) m n (M : 'M[R]_(m, n)) : `|M^T| = `|M|. Proof. -move=> Hf Hg a b. evar (f1 : R -> W). evar (f2 : R -> W). -rewrite (_ : (fun x => _) = f1 + f2); last first. - rewrite funeqE => x; rewrite -[RHS]/(f1 x + f2 x) mxE /f1 /f2; reflexivity. -rewrite {}/f1 {}/f2; exact: derivableD. +rewrite [LHS]mx_normE/=. +under eq_bigr do rewrite mxE. +rewrite -(pair_big xpredT xpredT (fun i j => `|M j i|%:nng))/=. +by rewrite exchange_big//= pair_big. Qed. -Lemma derivable_mxN M t : derivable_mx M t 1 -> - derivable_mx (fun x => - M x) t 1. -Proof. -move=> HM a b. -rewrite (_ : (fun x => _) = (fun x => - (M x a b))); first exact: derivableN. -by rewrite funeqE => ?; rewrite mxE. -Qed. +Section pointwise_derivable. +Context {R : realFieldType} {V W : normedModType R} {m n : nat}. +Implicit Types M : V -> 'M[R]_(m, n). -Lemma derivable_mxB M N t : derivable_mx M t 1 -> derivable_mx N t 1 -> - derivable_mx (fun x => M x - N x) t 1. -Proof. move=> Hf Hg; apply derivable_mxD => //; exact: derivable_mxN. Qed. +Definition derivable_mx M t v := + forall i j, derivable (fun x => M x i j) t v. -Lemma trmx_derivable M t v : - derivable_mx M t v = derivable_mx (fun x => (M x)^T) t v. +Lemma derivable_mxP M t v : derivable_mx M t v <-> derivable M t v. Proof. -rewrite propeqE; split => [H j i|H i j]. -by rewrite (_ : (fun _ => _) = (fun x => M x i j)) // funeqE => z; rewrite mxE. -by rewrite (_ : (fun _ => _) = (fun x => (M x)^T j i)) // funeqE => z; rewrite mxE. -Qed. - -Lemma derivable_mx_row M t i : - derivable_mx M t 1 -> derivable_mx (row i \o M) t 1. +split; rewrite /derivable_mx /derivable. +- move=> H. + apply/cvg_ex => /=. + pose l := \matrix_(i < m, j < n) sval (cid ((cvg_ex _).1 (H i j))). + exists l. + apply/cvgrPdist_le => /= e e0. + near=> x. + rewrite /Num.Def.normr/= mx_normrE. + apply: (bigmax_le _ (ltW e0)) => /= i _. + rewrite !mxE/=. + move: i. + near: x. + apply: filter_forall => /= i. + exact: ((@cvgrPdist_le _ _ _ _ (dnbhs_filter 0) _ _).1 + (svalP (cid ((cvg_ex _).1 (H i.1 i.2)))) _ e0). +- move=> /cvg_ex[/= l Hl] i j. + apply/cvg_ex; exists (l i j). + apply/cvgrPdist_le => /= e e0. + move/cvgrPdist_le : Hl => /(_ _ e0)[/= r r0] H. + near=> x. + apply: le_trans; last first. + apply: (H x). + rewrite /ball_/=. + rewrite sub0r normrN. + near: x. + exact: dnbhs0_lt. + near: x. + exact: nbhs_dnbhs_neq. + rewrite [leRHS]/Num.Def.normr/= mx_normrE. + apply: le_trans; last exact: le_bigmax. + by rewrite /= !mxE. +Unshelve. all: by end_near. Qed. + +Lemma derivable_trmx M t v : + derivable (fun x => (M x)^T) t v = derivable M t v. Proof. -move=> H a b. -by rewrite (_ : (fun _ => _) = (fun x => (M x) i b)) // funeqE => z; rewrite mxE. -Qed. - -Lemma derivable_mx_col M t i : - derivable_mx M t 1 -> derivable_mx (trmx \o col i \o M) t 1. +rewrite propeqE; split; rewrite /derivable/=. +- move=> /cvg_ex[/= l Hl]. + apply/cvg_ex => /=; exists l^T. + apply/cvgrPdist_le => /= e e0. + move/cvgrPdist_le : Hl => /(_ _ e0)[/= r r0 re]. + near=> x. + rewrite [leLHS](_ : _ = + `|l - x^-1 *: ((M (x *: v + t))^T - (M t)^T)|); last first. + rewrite -[RHS]norm_trmx. + rewrite [in RHS]linearD/=. + rewrite [in RHS]linearN/=. + congr (`| _ - _ |). + rewrite [RHS]linearZ/=. + rewrite [in RHS]linearB. + by rewrite /= !trmxK. + apply: re => /=. + rewrite sub0r normrN. + by near: x; exact: dnbhs0_lt. + by near: x; exact: nbhs_dnbhs_neq. +- move=> /cvg_ex[/= l Hl]. + apply/cvg_ex => /=; exists l^T. + apply/cvgrPdist_le => /= e e0. + move/cvgrPdist_le : Hl => /(_ _ e0)[/= r r0 re]. + near=> x. + rewrite [leLHS](_ : _ = `|l - x^-1 *: ((M (x *: v + t)) - (M t))|); last first. + rewrite -[RHS]norm_trmx. + rewrite [in RHS]linearD/=. + rewrite [in RHS]linearN/=. + congr (`| _ - _ |). + rewrite [RHS]linearZ/=. + by rewrite [in RHS]linearB. + apply: re => /=. + rewrite sub0r normrN. + by near: x; exact: dnbhs0_lt. + by near: x; exact: nbhs_dnbhs_neq. +Unshelve. all: by end_near. Qed. + +Lemma derivable_coord (a : V -> 'rV[R]_n) t v (i : 'I_n) : + derivable a t v -> + derivable (fun x : V => (a x)``_i) t v. Proof. -move=> H a b. -by rewrite (_ : (fun _ => _) = (fun x => (M x) b i)) // funeqE => z; rewrite 2!mxE. +move=> /cvg_ex[/= l Hl]. +apply/cvg_ex; exists (l``_i) => /=. +apply/cvgrPdist_le => /= e e0. +move/cvgrPdist_le : Hl => /(_ _ e0) Hl. +apply: filterS Hl => x. +rewrite {1}/Num.Def.normr/= mx_normrE. +move/bigmax_leP => -[_/=] /(_ (ord0, i)). +by rewrite !mxE/=; exact. Qed. -Lemma derivable_mx_cst (P : 'M[W]_(m, n)) t : derivable_mx (cst P) t 1. -Proof. move=> a b; by rewrite (_ : (fun x : R => _) = cst (P a b)). Qed. +End pointwise_derivable. +Section pointwise_derivable_TODO. (* TODO: generalize n/m+1 -> n/m*) +Context {R : realFieldType} {V W : normedModType R} {m n : nat}. +Implicit Types M : V -> 'M[R]_(m.+1, n.+1). -Lemma derive1mx_cst (P : 'M[W]_(m, n)) : derive1mx (cst P) = cst 0. +Lemma derivable_row M t v i : derivable M t v -> derivable (row i \o M) t v. Proof. -rewrite /derive1mx funeqE => t; apply/matrixP => i j; rewrite !mxE. -by rewrite (_ : (fun x : R => _) = cst (P i j)) // derive1_cst. -Qed. - -Lemma derive1mx_tr M t : derive1mx (trmx \o M) t = (derive1mx M t)^T. +rewrite /derivable => /cvg_ex[/= l Hl]. +apply/cvg_ex => /=. +exists (row i l). +apply/cvgrPdist_le => /= e e0. +move/cvgrPdist_le : Hl => /(_ _ e0)[r /= r0 re]. +near=> x. +apply: le_trans; last first. + apply: (re x). + rewrite /ball_ /= sub0r normrN. + by near: x; exact: dnbhs0_lt. + by near: x; exact: nbhs_dnbhs_neq. +rewrite /Num.Def.normr/= !mx_normrE. +apply/bigmax_leP => /=. +split. + exact: le_trans (le_bigmax _ _ (ord0, ord0)). +move=> j _. +rewrite !mxE. +under eq_bigr do rewrite !mxE. +exact: le_trans (le_bigmax _ _ (i, j.2)). +Unshelve. all: by end_near. Qed. + +Lemma derivable_col M t v i : derivable M t v -> derivable (col i \o M) t v. Proof. -apply/matrixP => i j; rewrite !mxE. -by rewrite (_ : (fun _ => _) = (fun t => M t j i)) // funeqE => ?; rewrite mxE. -Qed. - -Lemma derive1mxD M N t : derivable_mx M t 1 -> derivable_mx N t 1 -> - derive1mx (M + N) t = derive1mx M t + derive1mx N t. +rewrite /derivable => /cvg_ex[/= l Hl]. +apply/cvg_ex => /=. +exists (col i l). +apply/cvgrPdist_le => /= e e0. +move/cvgrPdist_le : Hl => /(_ _ e0)[r /= r0 re]. +near=> x. +apply: le_trans; last first. + apply: (re x). + rewrite /ball_ /= sub0r normrN. + by near: x; exact: dnbhs0_lt. + by near: x; exact: nbhs_dnbhs_neq. +rewrite /Num.Def.normr/= !mx_normrE. +apply/bigmax_leP => /=. +split. + exact: le_trans (le_bigmax _ _ (ord0, ord0)). +move=> j _. +rewrite !mxE. +under eq_bigr do rewrite !mxE. +exact: le_trans (le_bigmax _ _ (j.1, i)). +Unshelve. all: by end_near. Qed. + +Lemma derivable_row3 (a b c : V -> R) t v : + derivable a t v -> derivable b t v -> derivable c t v -> + derivable (fun x => row3 (a x) (b x) (c x)) t v. Proof. -move=> Hf Hg; apply/matrixP => a b; rewrite /derive1mx !mxE. -rewrite (_ : (fun _ => _) = (fun x => M x a b) \+ fun x => N x a b); last first. - by rewrite funeqE => ?; rewrite mxE. -by rewrite derive1E deriveD 2?{1}derive1E. -Qed. - -Lemma derive1mxN M t : derivable_mx M t 1 -> derive1mx (- M) t = - derive1mx M t. +move=> /cvg_ex[/= l Hl] /cvg_ex[/= o Ho] /cvg_ex[/= p Hp]. +apply/cvg_ex; exists (row3 l o p) => /=. +apply/cvgrPdist_le => /= e e0. +move/cvgrPdist_le : Hl => /(_ _ e0)[r/= r0 re]. +move/cvgrPdist_le : Ho => /(_ _ e0)[s/= s0 se]. +move/cvgrPdist_le : Hp => /(_ _ e0)[u/= u0 ue]. +near=> x. +rewrite /Num.Def.normr/= mx_normrE. +apply: bigmax_le. + exact: ltW. +move=> /= [i j] _. +rewrite (ord1 i){i}/=. +rewrite row3N. +rewrite row3D. +rewrite row3Z. +rewrite row3N. +rewrite row3D. +rewrite row3E. +rewrite ![in leLHS]mxE/=. +case: fintype.splitP => [j0|]. + rewrite (ord1 j0) => _. + rewrite !mxE eqxx/= mulr1n. + apply: re. + rewrite /= sub0r normrN. + by near: x; exact: dnbhs0_lt. + by near: x; exact: nbhs_dnbhs_neq. +move=> k j1k. +rewrite !mxE. +case: fintype.splitP => [k0|k0]. + rewrite (ord1 k0) => _. + rewrite !mxE eqxx/= mulr1n. + apply: se. + rewrite /= sub0r normrN. + by near: x; exact: dnbhs0_lt. + by near: x; exact: nbhs_dnbhs_neq. +rewrite (ord1 k0) => _. +rewrite !mxE eqxx/= mulr1n. +apply: ue. + rewrite /= sub0r normrN. + by near: x; exact: dnbhs0_lt. +by near: x; exact: nbhs_dnbhs_neq. +Unshelve. all: by end_near. Qed. + +End pointwise_derivable_TODO. + +Section pointwise_derive. +Local Open Scope classical_set_scope. +Context {R : realFieldType} {V W : normedModType R} . + +Lemma derive_mx {m n : nat} (M : V -> 'M[R]_(m, n)) t v : + derivable M t v -> + 'D_v M t = \matrix_(i < m, j < n) 'D_v (fun t => M t i j) t. Proof. -move=> Hf; apply/matrixP => a b. -rewrite !mxE [in RHS]derive1E -deriveN; last by []. -by rewrite -derive1E; f_equal; rewrite funeqE => x; rewrite mxE. -Qed. - -Lemma derive1mxB M N t : derivable_mx M t 1 -> derivable_mx N t 1 -> - derive1mx (M - N) t = derive1mx M t - derive1mx N t. +move=> /cvg_ex[/= l Hl]; apply/cvg_lim => //=. +apply/cvgrPdist_le => /= e e0. +move/cvgrPdist_le : (Hl) => /(_ (e / 2)). +rewrite divr_gt0// => /(_ isT)[d /= d0 dle]. +near=> x. +rewrite [in leLHS]/Num.Def.normr/= mx_normrE. +apply/(bigmax_le _ (ltW e0)) => -[/= i j] _. +rewrite [in leLHS]mxE/= [X in _ + X]mxE -[X in X - _](subrK (l i j)). +rewrite -(addrA (_ - _)) (le_trans (ler_normD _ _))// (splitr e) lerD//. +- rewrite mxE. + suff : (h^-1 *: (M (h *: v + t) i j - M t i j)) @[h --> 0^'] --> l i j. + move/cvg_lim => /=; rewrite /derive /= => ->//. + by rewrite subrr normr0 divr_ge0// ltW. + apply/cvgrPdist_le => /= r r0. + move/cvgrPdist_le : Hl => /(_ r r0)[/= s s0] sr. + near=> y. + have : `|l - y^-1 *: (M (y *: v + t) - M t)| <= r. + rewrite sr//=; last by near: y; exact: nbhs_dnbhs_neq. + by rewrite sub0r normrN; near: y; exact: dnbhs0_lt. + apply: le_trans. + rewrite [in leRHS]/Num.Def.normr/= mx_normrE. + by under eq_bigr do rewrite !mxE; exact: (le_bigmax _ _ (i, j)). +- rewrite mxE. + have : `|l - x^-1 *: (M (x *: v + t) - M t)| <= e / 2. + apply: dle => //=; last by near: x; exact: nbhs_dnbhs_neq. + by rewrite sub0r normrN; near: x; exact: dnbhs0_lt. + apply: le_trans. + rewrite [in leRHS]/Num.Def.normr/= mx_normrE/=. + under eq_bigr do rewrite !mxE. + apply: le_trans; last exact: le_bigmax. + by rewrite !mxE. +Unshelve. all: by end_near. Qed. + +Lemma derive_trmx {m n : nat} (M : V -> 'M[R]_(m, n)) t v : + derivable M t v -> 'D_v (trmx \o M) t = ('D_v M t)^T. Proof. -by move=> Hf Hg; rewrite derive1mxD ?derive1mxN; last by exact: derivable_mxN. +move=> Mt1. +rewrite !derive_mx//=; last by rewrite derivable_trmx. +apply/matrixP => i j; rewrite !mxE. +by under eq_fun do rewrite mxE. Qed. -End derive_mx. +End pointwise_derive. -Section derive_mx_R. +Section derivable_mulmx. +Context {R : realFieldType} {V : normedModType R} {m n k : nat}. -Variables (R : realFieldType) (m n k : nat). - -Lemma derivable_mxM (f : R -> 'M[R^o]_(m, k)) (g : R -> 'M[R^o]_(k, n)) t : - derivable_mx f t 1 -> derivable_mx g t 1 -> derivable_mx (fun x => f x *m g x) t 1. +Lemma derivable_mulmx + (f : V -> 'M[R]_(m, k)) (g : V -> 'M[R]_(k, n)) t v : + derivable f t v -> derivable g t v -> derivable (fun x => f x *m g x) t v. Proof. -move=> Hf Hg a b. evar (f1 : 'I_k -> R^o -> R^o). -rewrite (_ : (fun x => _) = (\sum_i f1 i)); last first. +move=> /derivable_mxP Hf /derivable_mxP Hg; apply/derivable_mxP => a b. +evar (f1 : 'I_k -> V -> R). +rewrite (_ : (fun x => _) = \sum_i f1 i); last first. rewrite funeqE => t'; rewrite mxE fct_sumE; apply: eq_bigr => k0 _. - rewrite /f1; reflexivity. + by rewrite /f1; reflexivity. rewrite {}/f1; apply: derivable_sum => k0. -evar (f1 : R^o -> R). evar (f2 : R -> R). +evar (f1 : V -> R). evar (f2 : V -> R). rewrite (_ : (fun t' => _) = f1 * f2); last first. - rewrite funeqE => t'; rewrite -[RHS]/(f1 t' * f2 t') /f1 /f2; reflexivity. -rewrite {}/f1 {}/f2; exact: derivableM. + by rewrite funeqE => t'; rewrite -[RHS]/(f1 t' * f2 t') /f1 /f2; reflexivity. +by rewrite {}/f1 {}/f2; exact: derivableM. Qed. -End derive_mx_R. +End derivable_mulmx. -Section derive_mx_SE. +Section derive_SE. +Context {R : rcfType} {V : normedModType R} (M : V -> 'M[R^o]_4). -Variables (R : rcfType) (M : R -> 'M[R^o]_4). +Lemma derivable_rot_of_hom x v : derivable M x v -> + derivable (@rot_of_hom _ \o M) x v. +Proof. +move=> Mt1. +apply/derivable_mxP => i j; rewrite /rot_of_hom/=. +rewrite (_ : (fun _ => _) = + fun y => (M y) (lshift 1 i) (lshift 1 j)); last first. + by rewrite funeqE => y; rewrite !mxE. +by have /derivable_mxP := Mt1; exact. +Qed. -Lemma derivable_rot_of_hom : (forall t, derivable_mx M t 1) -> - forall x, derivable_mx (@rot_of_hom _ \o M) x 1. +Lemma derivable_trans_of_hom x v : derivable M x v -> + derivable (@trans_of_hom _ \o M) x v. Proof. -move=> H x i j. -rewrite (_ : (fun _ => _) = (fun y => (M y) (lshift 1 i) (lshift 1 j))); last first. - rewrite funeqE => y; by rewrite !mxE. -exact: H. +move=> Mxv; apply/derivable_mxP => i j; rewrite /trans_of_hom/=. +rewrite (_ : (fun _ => _) = + fun y => (M y) (rshift 3 i) (lshift 1 j)); last first. + by rewrite funeqE => y; rewrite !mxE. +by have /derivable_mxP := Mxv; exact. Qed. -Lemma derive1mx_SE : (forall t, M t \in 'SE3[R]) -> - forall t, derive1mx M t = block_mx - (derive1mx (@rot_of_hom R^o \o M) t) 0 - (derive1mx (@trans_of_hom R^o \o M) t) 0. +Lemma derive1mx_SE t v : derivable M t v -> (forall t, M t \in 'SE3[R]) -> + 'D_v M t = block_mx + ('D_v (@rot_of_hom R^o \o M) t) 0 + ('D_v (@trans_of_hom R^o \o M) t) 0. Proof. -move=> MSE t. +move=> Mtv MSE. +rewrite !derive_mx//; [|exact: derivable_trans_of_hom + |exact: derivable_rot_of_hom]. rewrite block_mxEh. -rewrite {1}(_ : M = (fun x => hom (rot_of_hom (M x)) (trans_of_hom (M x)))); last first. - rewrite funeqE => x; by rewrite -(SE3E (MSE x)). +rewrite {1}(_ : M = + fun x => hom (rot_of_hom (M x)) (trans_of_hom (M x))); last first. + by rewrite funeqE => x; rewrite -(SE3E (MSE x)). apply/matrixP => i j. rewrite 2!mxE; case: splitP => [j0 jj0|j0 jj0]. rewrite (_ : j = lshift 1 j0); last exact/val_inj. rewrite mxE; case: splitP => [i1 ii1|i1 ii1]. rewrite (_ : i = lshift 1 i1); last exact/val_inj. - rewrite mxE; congr (derive1 _ t); rewrite funeqE => x. + rewrite mxE; congr ('D_v _ t); rewrite funeqE => x. by rewrite /hom (block_mxEul _ _ _ _ i1 j0). rewrite (_ : i = rshift 3 i1); last exact/val_inj. - rewrite mxE; congr (derive1 _ t); rewrite funeqE => x. + rewrite mxE; congr ('D_v _ t); rewrite funeqE => x. by rewrite /hom (block_mxEdl (rot_of_hom (M x))). rewrite (_ : j = rshift 3 j0) ?mxE; last exact/val_inj. rewrite (ord1 j0). case: (@splitP 3 1 i) => [i0 ii0|i0 ii0]. rewrite (_ : i = lshift 1 i0); last exact/val_inj. - rewrite (_ : (fun _ => _) = (fun=> 0)) ?derive1_cst ?mxE //. - rewrite funeqE => x; by rewrite /hom (block_mxEur (rot_of_hom (M x))) mxE. + rewrite (_ : (fun _ => _) = fun=> 0). + by rewrite derive_cst mxE. + by rewrite funeqE => x; rewrite /hom (block_mxEur (rot_of_hom (M x))) mxE. rewrite (_ : i = rshift 3 i0); last exact/val_inj. -rewrite (_ : (fun _ => _) = (fun=> 1)) ?derive1_cst // (ord1 i0) ?mxE //. +rewrite (_ : (fun _ => _) = (fun=> 1)) ?derive_cst // (ord1 i0) ?mxE //. by rewrite funeqE => x; rewrite /hom (block_mxEdr (rot_of_hom (M x))) mxE. Qed. -End derive_mx_SE. +End derive_SE. Section row_belast. @@ -234,7 +401,8 @@ case: fintype.splitP => /= [j Hj|[] [] //= ? ni]; rewrite mxE /=. rewrite mulr1n; congr (_ ``_ _); apply val_inj; by rewrite /= ni addn0. Qed. -Lemma derivable_row_belast (R : realFieldType) n (u : R -> 'rV[R^o]_n.+1) (t : R) (v : R): +Lemma derivable_row_belast (R : realFieldType) {V : normedModType R} + n (u : V -> 'rV[R]_n.+1) (t : V) (v : V): derivable_mx u t v -> derivable_mx (fun x => row_belast (u x)) t v. Proof. move=> H i j; move: (H ord0 (widen_ord (leqnSn n) j)) => {H}. @@ -251,13 +419,15 @@ rewrite -dotmulDr; congr dotmul; apply/matrixP => i j; rewrite !(castmxE,mxE) /= case: fintype.splitP => [k /= jk|[] [] // ? /= jn]; by rewrite !(mxE,addr0,add0r,mul0rn). Qed. -Lemma derive1mx_dotmul_belast (R : realFieldType) n (u v : R^o -> 'rV[R^o]_n.+1) t : +Lemma derive1mx_dotmul_belast {R : realFieldType} {V : normedModType R} n + (u v : V -> 'rV[R]_n.+1) t w : + derivable v t w -> let u' x := row_belast (u x) in let v' x := row_belast (v x) in - u' t *d derive1mx v' t + (u t)``_ord_max *: derive (fun x => (v x)``_ord_max) t 1 = - u t *d derive1mx v t. + u' t *d 'D_w v' t + (u t)``_ord_max *: derive (fun x => (v x)``_ord_max) t w = + u t *d 'D_w v t. Proof. -move=> u' v'. -rewrite (row_belast_last (derive1mx v t)) ?addn1 // => ?. +move=> vt1 u' v'. +rewrite (row_belast_last ('D_w v t)) ?addn1 // => /= ?. rewrite dotmul_belast; congr (_ + _). rewrite 2!dotmulE [in RHS]big_ord_recr /=. rewrite castmxE mxE /=; case: fintype.splitP => [j /= /eqP/negPn|j _]. @@ -265,9 +435,16 @@ rewrite dotmul_belast; congr (_ + _). rewrite !mxE (_ : _ == _); last by apply/eqP/val_inj => /=; move: j => [[] ?]. rewrite mulr0 addr0; apply/eq_bigr => i _; rewrite castmxE !mxE; congr (_ * _). case: fintype.splitP => [k /= ik|[] [] //= ?]; rewrite !mxE. + rewrite derive_mx//; last first. + rewrite /v'. + apply/derivable_mxP/derivable_row_belast. + exact/derivable_mxP. + rewrite /= !mxE/=. + rewrite derive_mx//. + rewrite mxE/=. f_equal. - rewrite funeqE => x; rewrite /v' !mxE; congr ((v _) _ _); by apply/val_inj. - rewrite addn0 => /eqP/negPn; by rewrite (ltn_eqF (ltn_ord i)). + by rewrite funeqE => x; rewrite /v' !mxE; congr ((v _) _ _); by apply/val_inj. + by rewrite addn0 => /eqP/negPn; rewrite (ltn_eqF (ltn_ord i)). apply/esym. rewrite dotmulE big_ord_recr /= (eq_bigr (fun=> 0)); last first. move=> i _. @@ -277,7 +454,8 @@ rewrite dotmulE big_ord_recr /= (eq_bigr (fun=> 0)); last first. rewrite sumr_const mul0rn add0r castmxE /=; congr (_ * _); rewrite !mxE. case: fintype.splitP => [j /= /eqP/negPn | [] [] //= ? Hn]. by rewrite (gtn_eqF (ltn_ord j)). -by rewrite mxE derive1E (_ : _ == _). +rewrite mxE/= mulr1n. +by rewrite derive_mx// mxE. Qed. End row_belast. @@ -285,79 +463,206 @@ End row_belast. (* TODO: could be derived from more generic lemmas about bilinearity in derive.v? *) Section product_rules. -Lemma derive1mx_dotmul (R : realFieldType) n (u v : R^o -> 'rV[R^o]_n) (t : R^o) : - derivable_mx u t 1 -> derivable_mx v t 1 -> - derive1 (fun x => u x *d v x : R^o) t = - derive1mx u t *d v t + u t *d derive1mx v t. -Proof. -move=> U V. -evar (f : R -> R); rewrite (_ : (fun x : R => u x *d v x : R^o) = f); last first. - rewrite funeqE => x /=; exact: dotmulE. -rewrite derive1E {}/f. -set f := fun i : 'I__ => fun x => ((u x) ``_ i * (v x) ``_ i). -rewrite (_ : (fun _ : R => _) = \sum_(k < _) f k); last first. +Global Instance is_diff_sum {R : numFieldType} {V W : normedModType R} + n (h : 'I_n -> V -> W) (x : V) + (dh : 'I_n -> V -> W) : (forall i, is_diff x (h i) (dh i)) -> + is_diff x (\sum_(i < n) h i) (\sum_(i < n) dh i). +Proof. +by elim/big_ind2 : _ => // [|] *; [exact: is_diff_cst|exact: is_diffD]. +Qed. + +Lemma derive_dotmul {R : realFieldType} {V : normedModType R} n + (u v : V -> 'rV[R]_n) (t : V) (w : V) : + derivable u t w -> derivable v t w -> + 'D_w (fun x => u x *d v x) t = 'D_w u t *d v t + u t *d 'D_w v t. +Proof. +move=> /derivable_mxP utw /derivable_mxP vtw. +under eq_fun do rewrite dotmulE. +set f := fun i : 'I__ => fun x => (u x) ``_ i * (v x) ``_ i. +rewrite (_ : (fun _ : V => _) = \sum_(k < _) f k); last first. by rewrite funeqE => x; rewrite /f /= fct_sumE. -rewrite derive_sum; last by move=> ?; exact: derivableM (U _ _) (V _ _). -rewrite {}/f. -elim: n u v => [|n IH] u v in U V *. - rewrite big_ord0 (_ : v t = 0) ?dotmulv0 ?add0r; last by apply/rowP => [[]]. - rewrite (_ : u t = 0) ?dotmul0v //; by apply/rowP => [[]]. -rewrite [LHS]big_ord_recr /=. -set u' := fun x => row_belast (u x). set v' := fun x => row_belast (v x). -transitivity (derive1mx u' t *d v' t + u' t *d derive1mx v' t + - derive (fun x => (u x)``_ord_max * (v x)``_ord_max) t 1). - rewrite -(IH _ _ (derivable_row_belast U) (derivable_row_belast V)). - apply: f_equal2; last by []. - apply eq_bigr => i _; congr (derive _ t 1). - by rewrite funeqE => x; rewrite !mxE. -rewrite (deriveM (U _ _) (V _ _)) /= -!addrA addrC addrA. -rewrite -(addrA (_ + _)) [in RHS]addrC derive1mx_dotmul_belast; congr (_ + _). -by rewrite [in RHS]dotmulC -derive1mx_dotmul_belast addrC dotmulC. +rewrite derive_sum; last by move=> i; exact: derivableM. +rewrite !dotmulE -big_split/=; apply: eq_bigr => i _. +by rewrite {}/f deriveM// mulrC addrC; congr (_ * _ + _ * _); + rewrite derive_mx ?mxE//=; exact/derivable_mxP. +Qed. + +(* NB: from Damien's LaSalle *) +Global Instance is_diff_component {R : realFieldType} n i (p : 'rV[R]_n) : + is_diff p (fun q => q..[i] : R^o) (fun q => q..[i]). +Proof. +have comp_lin : linear (fun q : 'rV[R]_n => q..[i] : R^o). + by move=> ???; rewrite !mxE. +have comp_cont : continuous (fun q : 'rV[R]_n => q..[i] : R^o). + move=> q A [_/posnumP[e] Ae] /=; apply/nbhs_ballP; exists e%:num => //=. + by move=> r [e0] /(_ ord0) /(_ i) /Ae. +pose glM := GRing.isLinear.Build _ _ _ _ _ comp_lin. +pose gL : {linear 'rV_n -> R^o} := HB.pack (fun q : 'rV_n => q ..[ i]) glM. +apply: DiffDef; first exact: (@linear_differentiable _ _ _ gL). +by rewrite (@diff_lin _ _ _ gL). +Qed. + +Global Instance is_diff_component_comp {R : realFieldType} (V : normedModType R) n + (f : V -> 'rV[R]_n) i p df : is_diff p f df -> + is_diff p (fun q => (f q)..[i] : R^o) (fun q => (df q)..[i]). +Proof. +move=> dfp. +have -> : (fun q => (f q)..[i]) = (fun v => v..[i]) \o f by rewrite funeqE. +(* This should work *) +(* apply: is_diff_eq. *) +exact: is_diff_comp. Qed. +(* /NB: from Damien's LaSalle *) -Lemma derive1mxM (R : realFieldType) n m p (M : R -> 'M[R^o]_(n, m)) - (N : R^o -> 'M[R^o]_(m, p)) (t : R^o) : - derivable_mx M t 1 -> derivable_mx N t 1 -> - derive1mx (fun t => M t *m N t) t = - derive1mx M t *m N t + M t *m (derive1mx N t). +Global Instance is_diff_dotmul {R : realFieldType} m n (V := 'rV[R]_m) + (u v du dv : V -> 'rV[R]_n) (t : V) : + is_diff t u du -> is_diff t v dv -> + is_diff t (fun x => u x *d v x) + (fun x => u t *d dv x + v t *d du x). Proof. -move=> HM HN; apply/matrixP => i j; rewrite ![in LHS]mxE. +move=> udu vdv/=. +under eq_fun do rewrite dotmulE. +set f := fun i : 'I__ => (fun x => (u x) ``_ i) * (fun x => (v x) ``_ i). +rewrite [X in is_diff _ X _](_ : _ = \sum_(k < _) f k); last first. + by rewrite funeqE => x; rewrite /f /= fct_sumE. +rewrite [X in is_diff _ _ X](_ : _ = \sum_(i < n) + ((u t)``_i *: (fun x => (dv x)``_i) + (v t)``_i *: (fun x => (du x)``_i))); last first. + by apply/funext => x; rewrite 2!dotmulE -big_split/= fct_sumE. +apply: is_diff_sum => i. +rewrite {}/f /=. +exact: is_diffM. +Qed. + +Lemma differentiable_dotmul {R : realFieldType} m n (V := 'rV[R]_m) + (u v : V -> 'rV[R]_n) (t : V) : + differentiable u t -> + differentiable v t -> + differentiable (fun x => u x *d v x) t. +Proof. +move=> /differentiableP udu /differentiableP vdv/=. +by have [/=] := is_diff_dotmul udu vdv. +Qed. + +Lemma derive_mulmx {R : realFieldType} {V : normedModType R} n m p + (M : V -> 'M[R]_(n.+1, m.+1)) + (N : V -> 'M[R]_(m.+1, p.+1)) (t : V) w : + derivable M t w -> derivable N t w -> + 'D_w (fun t => M t *m N t) t = 'D_w M t *m N t + M t *m 'D_w N t. +Proof. +move=> HM HN; apply/matrixP => i j. +rewrite derive_mx/=; last exact/derivable_mulmx. +rewrite ![in LHS]mxE. rewrite (_ : (fun x => _) = fun x => \sum_k (M x) i k * (N x) k j); last first. by rewrite funeqE => x; rewrite !mxE. rewrite (_ : (fun x => _) = fun x => (row i (M x)) *d (col j (N x))^T); last first. rewrite funeqE => z; rewrite dotmulE; apply eq_bigr => k _. by rewrite 3!mxE. -rewrite (derive1mx_dotmul (derivable_mx_row HM) (derivable_mx_col HN)). -by rewrite [in RHS]mxE; congr (_ + _); rewrite [in RHS]mxE dotmulE; - apply/eq_bigr => /= k _; rewrite !mxE; apply: f_equal2; - try by congr (@derive1 _ R^o _ t); - rewrite funeqE => z; rewrite !mxE. +rewrite (derive_dotmul (derivable_row HM)); last first. + by rewrite derivable_trmx/=; exact: derivable_col. +rewrite [in RHS]mxE; congr +%R. + rewrite dotmulE. + rewrite [in RHS]mxE. + apply: eq_bigr => /= k _. + rewrite !mxE/=. + congr *%R. + rewrite derive_mx//=; last first. + exact: derivable_row. + rewrite mxE. + rewrite derive_mx//=. + rewrite mxE/=. + congr ('D_w _ t). + by apply/funext => y; rewrite !mxE. +rewrite dotmulE. +rewrite [in RHS]mxE. +apply: eq_bigr => /= k _. +rewrite !mxE/=. +congr *%R. +rewrite derive_mx//=; last first. + by rewrite derivable_trmx//=; exact/derivable_col. +rewrite !mxE//=. +rewrite derive_mx//= !mxE. +congr ('D_w _ t). +by apply/funext => y; rewrite !mxE. Qed. -Lemma derive1mx_crossmul (R : realFieldType) (u v : R -> 'rV[R^o]_3) t : - derivable_mx u t 1 -> derivable_mx v t 1 -> - derive1mx (fun x => (u x *v v x) : 'rV[R^o]_3) t = - derive1mx u t *v v t + u t *v derive1mx v t. -Proof. -move=> U V. -evar (f : R -> 'rV[R]_3); rewrite (_ : (fun x : R => _) = f); last first. - rewrite funeqE => x; exact: crossmulE. -rewrite {}/f {1}/derive1mx; apply/rowP => i; rewrite mxE derive1E. -rewrite (mxE_funeqE (fun x : R^o => _)) /= mxE 2!crossmulE !{1}[in RHS]mxE /=. -case: ifPn => [/eqP _|/ifnot0P/orP[]/eqP -> /=]; - rewrite ?derive1E (deriveD (derivableM (U _ _) (V _ _)) - (derivableN (derivableM (U _ _) (V _ _)))); - rewrite (deriveN (derivableM (U _ _) (V _ _))) 2!(deriveM (U _ _) (V _ _)); - rewrite addrCA -!addrA; congr (_ + (_ + _)); by [ rewrite mulrC | - rewrite opprD addrC; congr (_ + _); rewrite mulrC ]. +Lemma derivable_dotmul {R : realFieldType} {n} + (u v : R -> 'rV[R]_n) t : + derivable u t 1 -> derivable v t 1 -> + derivable (fun x => u x *d v x) t 1. +Proof. +move=> ut1 vt1/=. +rewrite /dotmul. +rewrite (_ : (fun x : R => _) = + \sum_k (fun x : R => (u x)``_k * (v x) 0 k)); last first. + apply/funext => x. + rewrite !mxE. + under eq_bigr do rewrite !mxE. + elim/big_ind2 : _ => //= f a g b -> ->. + by rewrite fctE. +apply: derivable_sum => i. +by apply: derivableM => //=; exact: derivable_coord. +Qed. + +Lemma derive_crossmul {R : realFieldType} {V : normedModType R} + (u v : V -> 'rV[R]_3) t w : + derivable u t w -> derivable v t w -> + 'D_w (fun x => u x *v v x) t = 'D_w u t *v v t + u t *v 'D_w v t. +Proof. +move=> utw vtw. +evar (f : V -> 'rV[R]_3); rewrite (_ : (fun x : V => _) = f); last first. + by rewrite funeqE => x; exact: crossmulE. +rewrite {}/f; apply/rowP => i; rewrite mxE. +rewrite derive_mx/=; last first. + by apply: derivable_row3; + apply: derivableB => //=; + by apply: derivableM => //=; exact: derivable_coord. +rewrite !mxE/=. +under eq_fun do rewrite !mxE/=. +rewrite 2!crossmulE !{1}[in RHS]mxE /=. +case: ifPn => [/eqP _|/ifnot0P/orP[]/eqP -> /=]. +- rewrite deriveB//=; [ | + by apply: derivableM => //=; exact: derivable_coord..]. + rewrite deriveM//=; [|exact: derivable_coord..]. + rewrite deriveM//=; [|exact: derivable_coord..]. + rewrite addrCA -!addrA; congr (_ + (_ + _)). + by rewrite derive_mx//= mxE. + by rewrite mulrC derive_mx//= mxE. + rewrite addrC opprD mulrC. + rewrite derive_mx//= mxE. + congr (_ - _)%R. + by rewrite derive_mx//= mxE. +- (*TOOD: copipe *) + rewrite deriveB//=; [ | + by apply: derivableM => //=; exact: derivable_coord..]. + rewrite deriveM//=; [|exact: derivable_coord..]. + rewrite deriveM//=; [|exact: derivable_coord..]. + rewrite addrCA -!addrA; congr (_ + (_ + _)). + by rewrite derive_mx//= mxE. + by rewrite mulrC derive_mx//= mxE. + rewrite addrC opprD mulrC. + rewrite derive_mx//= mxE. + congr (_ - _)%R. + by rewrite derive_mx//= mxE. +- (*TOOD: copipe *) + rewrite deriveB//=; [ | + by apply: derivableM => //=; exact: derivable_coord..]. + rewrite deriveM//=; [|exact: derivable_coord..]. + rewrite deriveM//=; [|exact: derivable_coord..]. + rewrite addrCA -!addrA; congr (_ + (_ + _)). + by rewrite derive_mx//= mxE. + by rewrite mulrC derive_mx//= mxE. + rewrite addrC opprD mulrC. + rewrite derive_mx//= mxE. + congr (_ - _)%R. + by rewrite derive_mx//= mxE. Qed. End product_rules. Section cross_product_matrix. -Lemma differential_cross_product (R : realFieldType) (v : 'rV[R^o]_3) y : +Lemma differential_crossmul {R : realFieldType} (v : 'rV[R]_3) y : 'd (crossmul v) y = mx_lin1 \S( v ) :> (_ -> _). Proof. rewrite (_ : crossmul v = (fun x => x *m \S( v ))); last first. @@ -374,11 +679,11 @@ apply: differentiable_sum => i. exact/differentiableZl/differentiable_coord. Qed. -Lemma differential_cross_product2 (R : realFieldType) (v y : 'rV[R^o]_3) : - 'd (fun x : 'rV[R^o]_3 => x *v v) y = -1 \*: mx_lin1 \S( v ) :> (_ -> _). +Lemma differential_crossmul2 (R : realFieldType) (v y : 'rV[R]_3) : + 'd (fun x : 'rV[R]_3 => x *v v) y = -1 \*: mx_lin1 \S( v ) :> (_ -> _). Proof. transitivity ('d (crossmul (- v)) y); last first. - by rewrite differential_cross_product spinN mx_lin1N. + by rewrite differential_crossmul spinN mx_lin1N. congr diff. by rewrite funeqE => /= u; rewrite (@lieC _ (vec3 R)) linearNl. Qed. @@ -387,53 +692,48 @@ End cross_product_matrix. (* [sciavicco] p.80-81 *) Section derivative_of_a_rotation_matrix. +Context {R : realFieldType}. +Variable M : R -> 'M[R]_3. -Variables (R : realFieldType) (M : R -> 'M[R^o]_3). +Definition ang_vel_mx t : 'M_3 := (M t)^T * 'D_1 M t. -Definition ang_vel_mx t : 'M_3 := (M t)^T * derive1mx M t. +Definition body_ang_vel_mx t : 'M_3 := 'D_1 M t *m (M t)^T. -Definition body_ang_vel_mx t : 'M_3 := derive1mx M t *m (M t)^T. +Hypothesis MO : forall t, M t \is 'O[ R ]_3. -(* angular velocity (a free vector) *) -Definition ang_vel t := unspin (ang_vel_mx t). +(* [sciavicco] eqn 3.7 *) +Lemma derive1mx_ang_vel t : 'D_1 M t = M t * ang_vel_mx t. +Proof. +by rewrite /ang_vel_mx mulrA -mulmxE orthogonal_mul_tr// mul1mx. +Qed. -Hypothesis MO : forall t, M t \is 'O[ R ]_3. -Hypothesis derivable_M : forall t, derivable_mx M t 1. +Hypothesis derivable_M : forall t, derivable M t 1. Lemma ang_vel_mx_is_so t : ang_vel_mx t \is 'so[ R ]_3. Proof. have : (fun t => (M t)^T * M t) = cst 1. rewrite funeqE => x; by rewrite -orthogonal_inv // mulVr // orthogonal_unit. -move/(congr1 (fun f => derive1mx f t)); rewrite derive1mx_cst -[cst 0 _]/(0). -rewrite derive1mxM // -?trmx_derivable // derive1mx_tr. +move/(congr1 (fun f => 'D_1 f t)). +rewrite derive_cst. +rewrite derive_mulmx // ?derivable_trmx // derive_trmx//. move=> /eqP; rewrite addr_eq0 => /eqP H. by rewrite antiE /ang_vel_mx trmx_mul trmxK H opprK. Qed. +(* angular velocity (a free vector) *) +Definition ang_vel t := unspin (ang_vel_mx t). + Lemma ang_vel_mxE t : ang_vel_mx t = \S( ang_vel t). Proof. by rewrite /ang_vel unspinK // ang_vel_mx_is_so. Qed. -(* [sciavicco] eqn 3.7 *) -Lemma derive1mx_ang_vel t : derive1mx M t = M t * ang_vel_mx t. -Proof. -move: (ang_vel_mx_is_so t); rewrite antiE -subr_eq0 opprK => /eqP. -rewrite {1 2}/ang_vel_mx trmx_mul trmxK => /(congr1 (fun x => (M t) * x)). -rewrite mulr0 mulrDr !mulrA -{1}(orthogonal_inv (MO t)). -rewrite divrr ?orthogonal_unit // mul1r. -move=> /eqP; rewrite addr_eq0 => /eqP {1}->. -rewrite -mulrA -mulrN -mulrA; congr (_ * _). -move: (ang_vel_mx_is_so t); rewrite antiE -/(ang_vel_mx t) => /eqP ->. -by rewrite /ang_vel_mx trmx_mul trmxK mulmxE. -Qed. - -Lemma derive1mx_rot (p' : 'rV[R^o]_3 (* constant vector *)) : +Lemma derive1mx_rot (p' : 'rV[R]_3 (* constant vector *)) : let p := fun t => p' *m M t in - forall t, derive1mx p t = ang_vel t *v p t. + forall t, 'D_1 p t = ang_vel t *v p t. Proof. -move=> p t; rewrite /p derive1mxM; last first. +move=> p t; rewrite /p derive_mulmx; last first. exact: derivable_M. rewrite /derivable_mx => i j; exact: ex_derive. -rewrite derive1mx_cst mul0mx add0r derive1mx_ang_vel mulmxA. +rewrite derive_cst mul0mx add0r derive1mx_ang_vel mulmxA. by rewrite -{1}(unspinK (ang_vel_mx_is_so t)) spinE. Qed. diff --git a/dh.v b/dh.v index 0292a4f..9aa12bd 100644 --- a/dh.v +++ b/dh.v @@ -196,7 +196,7 @@ Variables F0 F1 : tframe T. Definition From1To0 := locked (F1 _R^ F0). Definition p1_in_0 : 'rV[T]_3 := (\o{F1} - \o{F0}) *m (can_tframe T) _R^ F0. -Goal `[ p1_in_0 $ F0 ] = rmap F0 `[ \o{F1} - \o{F0} $ can_tframe T ]. +Goal '[ p1_in_0 $ F0 ] = rmap F0 '[ \o{F1} - \o{F0} $ can_tframe T ]. Proof. rewrite /p1_in_0. by rewrite /rmap. diff --git a/differential_kinematics.v b/differential_kinematics.v index 00d7785..ed2f658 100644 --- a/differential_kinematics.v +++ b/differential_kinematics.v @@ -6,7 +6,7 @@ From mathcomp Require Import realalg complex fingroup perm. From mathcomp Require Import sesquilinear. From mathcomp Require Import boolp reals classical_sets. From mathcomp Require Import topology normedtype landau derive. -From mathcomp Require Import functions. +From mathcomp Require Import functions trigo. Require Import ssr_ext derive_matrix euclidean frame rot skew rigid. (******************************************************************************) @@ -38,6 +38,42 @@ Import Order.TTheory GRing.Theory Num.Def Num.Theory. Import numFieldNormedType.Exports. Local Open Scope ring_scope. + +Lemma derive1_cos (R : realType) (t : R) : derive1 (cos : _ -> R^o) t = - sin t. +Proof. +rewrite derive1E. +have u := is_derive_cos t. +by have := @derive_val _ _ _ _ _ _ _ u. +Qed. + +Lemma derive1_sin (R : realType) (t : R) : derive1 (sin : _ -> R^o) t = cos t. +Proof. +rewrite derive1E. +have u := is_derive_sin t. +by have := @derive_val _ _ _ _ _ _ _ u. +Qed. + +Lemma derivable_sin (R : realType) (t : R) : derivable (sin : R^o -> R^o) t 1. +Proof. exact: ex_derive. Qed. + +Lemma derivable_cos (R : realType) (t : R) : derivable (cos : R^o -> R^o) t 1. +Proof. exact: ex_derive. Qed. + +Lemma derivable_cos_comp (R : realType) (t : R) (a : R^o -> R^o) : + derivable a t 1 -> derivable (cos \o a : _ -> R^o) t 1. +Proof. by move=> /derivableP Hs; exact: ex_derive. Qed. + +Lemma derivable_sin_comp (R : realType) (t : R) (a : R^o -> R^o) : + derivable a t 1 -> derivable ((sin : _ -> R^o) \o a) t 1. +Proof. by move=> /derivableP Hs; exact: ex_derive. Qed. + +Lemma derive1_cos_comp (R : realType) t (a : R^o -> R^o) : derivable a t 1 -> + derive1 (cos \o a : _ -> R^o) t = - (derive1 a t) * sin (a t). +Proof. +move=> H; rewrite (derive1_comp H); last exact: derivable_cos. +by rewrite derive1_cos mulrC mulNr mulrN. +Qed. + Local Open Scope frame_scope. Module BoundVect. (* i.e., point of application prescribed *) @@ -59,7 +95,7 @@ Section about_bound_vectors. Variables (T : pzRingType) (F : tframe T). -Definition FramedVect_of_Bound (p : bvec F) : fvec F := `[ BoundVect.endp p $ F ]. +Definition FramedVect_of_Bound (p : bvec F) : fvec F := '[ BoundVect.endp p $ F ]. Definition BoundVect_add (a b : bvec F) : bvec F := BoundVect.mk F (BoundVect.endp a + BoundVect.endp b). @@ -74,7 +110,7 @@ Lemma BoundFramed_addA (a : bvec F) (b c : fvec F) : Proof. by rewrite /BoundFramed_add /= addrA. Qed. Definition BoundVect_sub (F : tframe T) (a b : bvec F) : fvec F := - `[ BoundVect.endp a - BoundVect.endp b $ F ]. + '[ BoundVect.endp a - BoundVect.endp b $ F ]. Local Notation "a \-b b" := (BoundVect_sub a b). @@ -90,14 +126,14 @@ Lemma derive1mx_BoundFramed_add (R : realFieldType) (F : tframe R^o) (Q : R -> bvec F) (Z : R -> fvec F) t : derivable_mx (fun x => BoundVect.endp (Q x)) t 1 -> derivable_mx (fun x => FramedVect.v (Z x)) t 1 -> - derive1mx (fun x => BoundVect.endp (Q x \+b Z x)) t = - derive1mx (fun x => BoundVect.endp (Q x)) t + - derive1mx (fun x => FramedVect.v (Z x)) t. + 'D_1 (fun x => BoundVect.endp (Q x \+b Z x)) t = + 'D_1 (fun x => BoundVect.endp (Q x)) t + + 'D_1 (fun x => FramedVect.v (Z x)) t. Proof. -move=> H H'. +move=> /derivable_mxP H /derivable_mxP H'. rewrite (_ : (fun x : R => _) = (fun x : R => BoundVect.endp (Q x) + (FramedVect.v (Z x)))); last by rewrite funeqE. -rewrite derive1mxD. +rewrite deriveD. - by []. - exact: H. - exact H'. @@ -159,7 +195,7 @@ Proof. move=> HF HG a b. have @G' : forall t0, rframe (F t0). move=> t0. - exact: (@RFrame.mk _ _ (@BoundVect.mk _ _ \o{F t0}) `[(F t0)~i $ F t0] `[(F t0)~j $ F t0] `[(F t0)~k $ F t0] (F t0)). + exact: (@RFrame.mk _ _ (@BoundVect.mk _ _ \o{F t0}) '[(F t0)~i $ F t0] '[(F t0)~j $ F t0] '[(F t0)~k $ F t0] (F t0)). apply: (@derivable_mx_FromTo' R F G' G). by []. by []. @@ -176,8 +212,8 @@ Qed. Lemma derivable_mx_FromTo_tr (R : realFieldType) (F : tframe R^o) (G : R -> rframe F) t : - derivable_mx (fun x => F _R^ (G x)) t 1 = derivable_mx (fun x => F _R^ (G x)) t 1. -Proof. by rewrite trmx_derivable. Qed. + derivable (fun x => F _R^ (G x)) t 1 = derivable (fun x => F _R^ (G x)) t 1. +Proof. by rewrite -derivable_trmx. Qed. End derivable_FromTo. @@ -218,61 +254,68 @@ apply fv_eq => /=; rewrite -mulmxDl; congr (_ *m _). by rewrite addrCA subrr addr0. Qed. -Lemma derivable_mx_Q t : derivable_mx (fun x => BoundVect.endp (Q x)) t 1. +Lemma derivable_Q t : derivable (fun x => BoundVect.endp (Q x)) t 1. Proof. -rewrite /Q /=; apply derivable_mxD. +rewrite /Q/=; apply: derivableD. +- apply/derivable_mxP. move=> a b. move: (@derivable_F1o t a b). - rewrite (_ : (fun x => \o{F1 x} a b) = - (fun x => BoundVect.endp (RFrame.o (F1 x)) a b)) // funeqE => x. - destruct (F1 x) => /=; by rewrite e. -apply derivable_mxM; last exact: derivable_mx_FromTo. -rewrite (_ : (fun x => _) = (fun _ => BoundVect.endp (Q1 0))); last first. - rewrite funeqE => x; by rewrite Q1_fixed_in_F1. -move=> a b; exact: ex_derive. + rewrite [X in derivable X _ _ -> _](_ : _ = + (fun x => BoundVect.endp (RFrame.o (F1 x)) a b)); last first. + apply/funext => x/=. + by destruct (F1 x) => /=; by rewrite e. + by []. +- apply: derivable_mulmx; last first. + exact/derivable_mxP/derivable_mx_FromTo. + rewrite (_ : (fun x => _) = (fun _ => BoundVect.endp (Q1 0))); last first. + by rewrite funeqE => x; rewrite Q1_fixed_in_F1. + by move=> a b; exact: ex_derive. Qed. Let Rot := fun t => (F1 t) _R^ F. (* generalization of B.4 *) Lemma velocity_composition_rule (t : R) : - derive1mx (fun x => BoundVect.endp (P x)) t = - derive1mx (fun x => BoundVect.endp (Q x)) t + - derive1mx P1 t *m Rot t (* rate of change of the coordinates P1 expressed in the frame F *) + + 'D_1 (fun x => BoundVect.endp (P x)) t = + 'D_1 (fun x => BoundVect.endp (Q x)) t + + 'D_1 (fun x => P1 x : 'M__) t *m Rot t (* rate of change of the coordinates P1 expressed in the frame F *) + ang_vel Rot t *v FramedVect.v (P t \-b Q t). Proof. rewrite {1}(_ : P = fun t => Q t \+b rmap F (P1 t \-b Q1 t)); last first. by rewrite funeqE => t'; rewrite eqnB3. -rewrite (derive1mx_BoundFramed_add (@derivable_mx_Q t)); last first. - apply derivable_mxM; last exact: derivable_mx_FromTo. +have /derivable_mxP tmp := (@derivable_Q t). +rewrite (derive1mx_BoundFramed_add tmp); last first. + apply/derivable_mxP. + apply: derivable_mulmx; last first. + exact/derivable_mxP/derivable_mx_FromTo. rewrite (_ : (fun x => _) = (fun x => FramedVect.v (FramedVect_of_Bound (P1 x)) - FramedVect.v (FramedVect_of_Bound (Q1 0)))); last first. rewrite funeqE => x; by rewrite /= Q1_fixed_in_F1. - apply: derivable_mxB => /=. - exact: derivable_mxP1. - move=> a b; exact: ex_derive. + apply: derivableB => //=. + exact/derivable_mxP/derivable_mxP1. rewrite -addrA; congr (_ + _). -rewrite [in LHS]/rmap [in LHS]/= derive1mxM; last 2 first. +rewrite [in LHS]/rmap [in LHS]/= derive_mulmx; last 2 first. rewrite {1}(_ : (fun x => _) = (fun x => BoundVect.endp (P1 x) - BoundVect.endp (Q1 0))); last first. by rewrite funeqE => ?; rewrite Q1_fixed_in_F1. - apply: derivable_mxB. - exact: derivable_mxP1. - move=> a b; exact: ex_derive. - exact: derivable_mx_FromTo. -rewrite derive1mxB; last 2 first. - exact: derivable_mxP1. + apply: derivableB. + exact/derivable_mxP/derivable_mxP1. + by move=> a b; exact: ex_derive. + exact/derivable_mxP/derivable_mx_FromTo. +rewrite deriveB; last 2 first. + exact/derivable_mxP/derivable_mxP1. rewrite (_ : (fun x => _) = cst (BoundVect.endp (Q1 0))); last first. by rewrite funeqE => x; rewrite Q1_fixed_in_F1. - exact: derivable_mx_cst. -congr (_*m _ + _). + exact: derivable_cst. +congr (_ *m _ + _). rewrite [in X in _ + X = _](_ : (fun x => _) = cst (BoundVect.endp (Q1 0))); last first. by rewrite funeqE => x; rewrite Q1_fixed_in_F1. - by rewrite derive1mx_cst subr0. + by rewrite derive_cst//= subr0. rewrite -spinE unspinK; last first. rewrite ang_vel_mx_is_so; first by []. - move=> t'; by rewrite FromTo_is_O. - move=> t'; exact: derivable_mx_FromTo. + by move=> t'; by rewrite FromTo_is_O. + move=> t'. + exact/derivable_mxP/derivable_mx_FromTo. rewrite /ang_vel_mx mulmxA; congr (_ *m _). rewrite /P /Q /= opprD addrACA subrr add0r mulmxBl -!mulmxA. by rewrite orthogonal_mul_tr ?FromTo_is_O // !mulmx1. @@ -282,16 +325,18 @@ Hypothesis P1_fixed_in_F1 : forall t, BoundVect.endp (P1 t) = BoundVect.endp (P1 (* eqn B.4 *) Lemma velocity_composition_rule_spec (t : R) : - derive1mx (fun x => BoundVect.endp (P x)) t = - derive1mx (fun x => BoundVect.endp (Q x)) t + + 'D_1 (fun x => BoundVect.endp (P x)) t = + 'D_1 (fun x => BoundVect.endp (Q x)) t + ang_vel Rot t *v (FramedVect.v (P t \-b Q t)). Proof. rewrite velocity_composition_rule; congr (_ + _). -suff -> : derive1mx P1 t = 0 by rewrite mul0mx addr0. -apply/matrixP => a b; rewrite !mxE. +suff -> : 'D_1 (fun x => P1 x : 'M__) t = 0 by rewrite mul0mx addr0. +apply/matrixP => a b; rewrite !mxE/=. +rewrite derive_mx//=; last exact/derivable_mxP/derivable_mxP1. +rewrite mxE/=. rewrite (_ : (fun x => _) = cst (P1 0 a b)); last first. rewrite funeqE => x /=; by rewrite /boundvectendp (P1_fixed_in_F1 x). -by rewrite derive1_cst. +by rewrite derive_cst. Qed. End kinematics. @@ -309,13 +354,14 @@ Hypothesis derivable_F1o : forall t, derivable_mx (@TFrame.o R^o \o F1) t 1. Definition p0 := motion p1. Lemma eqn312 t : - derive1mx (fun x => BoundVect.endp (motion p1 x)) t = - derive1mx (fun x => BoundVect.endp (motion (fun=> bvec0 (F1 x)) t)) t + - derive1mx p1 t *m (F1 t) _R^ F + + 'D_1 (fun x => BoundVect.endp (motion p1 x)) t = + 'D_1 (fun x => BoundVect.endp (motion (fun=> bvec0 (F1 x)) t)) t + + 'D_1 (fun x => p1 x : 'M__) t *m (F1 t) _R^ F + ang_vel (fun t => (F1 t) _R^ F) t *v (p1 t *m (F1 t) _R^ F). Proof. rewrite (@velocity_composition_rule _ F _ derivable_F1 derivable_F1o p1 derivable_mx_p1 (fun t => bvec0 (F1 t)) (@BoundVect0_fixed _ _ _ F1)). +rewrite /=. congr (_ + _ *v _). by rewrite /= mul0mx addr0 addrAC subrr add0r. Qed. @@ -330,15 +376,15 @@ Section rigid_body_velocity. Section spatial_velocity. Variables (R : realType) (M : R -> 'M[R^o]_4). -Hypothesis derivableM : forall t, derivable_mx M t 1. +Hypothesis derivableM : forall t, derivable M t 1. Hypothesis MSE : forall t, M t \in 'SE3[R]. -Definition spatial_velocity t : 'M_4 := (M t)^-1 * derive1mx M t. +Definition spatial_velocity t : 'M_4 := (M t)^-1 * 'D_1 M t. Definition spatial_lin_vel := let r : R -> 'M[R^o]_3 := @rot_of_hom _ \o M in let p : R -> 'rV[R^o]_3:= @trans_of_hom _ \o M in - fun t => - p t *m (r t)^T *m derive1mx r t + derive1mx p t. + fun t => - p t *m (r t)^T *m 'D_1 r t + 'D_1 p t. Lemma spatial_velocityE t : let r : R -> 'M[R^o]_3 := @rot_of_hom _ \o M in @@ -346,14 +392,14 @@ Lemma spatial_velocityE t : Proof. move=> r. rewrite /spatial_velocity. -transitivity (inv_hom (M t) * derive1mx M t) => //. +transitivity (inv_hom (M t) * 'D_1 M t) => //. by rewrite inv_homE. rewrite /inv_hom. rewrite /hom. -rewrite derive1mx_SE //. +rewrite derive1mx_SE//=. rewrite (_ : rot_of_hom (M t) = r t) // -/r. rewrite -mulmxE. -rewrite (mulmx_block (r t)^T _ _ _ (derive1mx r t)). +rewrite (mulmx_block (r t)^T _ _ _ ('D_1 r t)). rewrite !(mul0mx,add0r,mul1mx,mulmx0,trmx0,addr0,mulmx1). by rewrite mulmxE -/(ang_vel_mx r t). Qed. @@ -364,7 +410,7 @@ rewrite spatial_velocityE. set r := @rot_of_hom _. rewrite qualifE block_mxKul block_mxKur block_mxKdr 2!eqxx 2!andbT. rewrite ang_vel_mx_is_so // => t0. by rewrite rotation_sub // rot_of_hom_is_SO. -exact: derivable_rot_of_hom. +apply: derivable_rot_of_hom => //=. Qed. Lemma spatial_velocity_is_twist x : @@ -376,7 +422,7 @@ rewrite spatial_velocityE. rewrite /wedge lin_tcoorE ang_tcoorE unspinK //. rewrite ang_vel_mx_is_so // => t0. by rewrite rotation_sub // rot_of_hom_is_SO. -exact: derivable_rot_of_hom. +by apply: derivable_rot_of_hom => //=. Qed. End spatial_velocity. @@ -384,22 +430,24 @@ End spatial_velocity. Section body_velocity. Variables (R : realType) (M : R -> 'M[R^o]_4). -Hypothesis derivableM : forall t, derivable_mx M t 1. +Hypothesis Mt1 : forall t, derivable M t 1. Hypothesis MSE : forall t, M t \in 'SE3[R]. -Definition body_velocity t : 'M_4 := derive1mx M t * (M t)^-1. +Definition body_velocity t : 'M_4 := 'D_1 M t * (M t)^-1. Definition body_lin_vel := let r : R -> 'M[R^o]_3 := @rot_of_hom _ \o M in let p : R -> 'rV[R^o]_3:= @trans_of_hom _ \o M in - fun t => derive1mx p t *m (r t)^T. + fun t => 'D_1 p t *m (r t)^T. -Lemma body_ang_vel_is_so t : body_ang_vel_mx (@rot_of_hom _ \o M) t \is 'so[R]_3. +Lemma body_ang_vel_is_so (t : R) : + body_ang_vel_mx (@rot_of_hom _ \o M) t \is 'so[R]_3. Proof. rewrite /body_ang_vel_mx. have : forall t, (@rot_of_hom R^o \o M) t \is 'O[R]_3. - move=> t0; by rewrite rotation_sub // rot_of_hom_is_SO. -move/ang_vel_mx_is_so => /(_ (derivable_rot_of_hom derivableM))/(_ t). + by move=> t0; rewrite rotation_sub // rot_of_hom_is_SO. +move/ang_vel_mx_is_so => /=. +move => /(_ (fun t => derivable_rot_of_hom (@Mt1 t)))/(_ t). rewrite /ang_vel_mx. move/(conj_so (((rot_of_hom (T:=R) \o M) t)^T)). rewrite !mulmxA !trmxK orthogonal_mul_tr ?rotation_sub // ?rot_of_hom_is_SO //. @@ -411,14 +459,14 @@ Lemma body_velocityE t : let r : R -> 'M[R^o]_3 := @rot_of_hom _ \o M in Proof. move=> r. rewrite /body_velocity. -transitivity (derive1mx M t * inv_hom (M t)). +transitivity ('D_1 M t * inv_hom (M t)). by rewrite inv_homE. rewrite /inv_hom. rewrite /hom. rewrite derive1mx_SE //. rewrite (_ : rot_of_hom (M t) = r t) // -/r. rewrite -mulmxE. -rewrite (mulmx_block (derive1mx r t) _ _ _ (r t)^T). +rewrite (mulmx_block ('D_1 r t) _ _ _ (r t)^T). rewrite !(mul0mx,add0r,mul1mx,mulmx0,trmx0,addr0,mulmx1). by rewrite -/(body_ang_vel_mx _) -/(body_lin_vel _). Qed. @@ -435,7 +483,7 @@ End body_velocity. Section spatial_body_adjoint. Variables (R : realType) (M : R -> 'M[R^o]_4). -Hypothesis derivableM : forall t, derivable_mx M t 1. +Hypothesis derivableM : forall t, derivable M t 1. Hypothesis MSE : forall t, M t \in 'SE3[R]. Lemma spatial_body_velocity x : @@ -443,7 +491,8 @@ Lemma spatial_body_velocity x : Proof. rewrite -/(SE3_action _ _) action_Adjoint; last by []. congr vee; rewrite /spatial_velocity -mulmxE -mulmxA; congr (_ * _). -rewrite veeK; last by rewrite body_velocity_is_se. +rewrite veeK; last first. + by rewrite body_velocity_is_se//=. by rewrite /body_velocity -mulmxA mulVmx ?mulmx1 // SE3_in_unitmx. Qed. @@ -462,7 +511,7 @@ Let o2 t : bvec F := RFrame.o (F2 t). Let r12 : forall t : R, bvec (F1 t) := fun t => BoundVect.mk (F1 t) - (FramedVect.v (rmap (F1 t) `[ \o{F2 t} - \o{F1 t} $ F ])). + (FramedVect.v (rmap (F1 t) '[ \o{F2 t} - \o{F1 t} $ F ])). Hypothesis derivable_F1 : forall t, derivable_mx F1 t 1. Hypothesis derivable_F1o : forall t, derivable_mx (@TFrame.o R^o \o F1) t 1. @@ -478,19 +527,19 @@ Qed. Definition w1 := ang_vel (fun t => (F1 t) _R^ F). -Lemma eqn314_helper t : FramedVect.v (rmap F `[r12 t $ F1 t]) = \o{F2 t} - \o{F1 t}. +Lemma eqn314_helper t : FramedVect.v (rmap F '[r12 t $ F1 t]) = \o{F2 t} - \o{F1 t}. Proof. by rewrite /= -mulmxA FromTo_comp FromToI mulmx1. Qed. (* lin. vel. of Link i as a function of the translational and rotational velocities of Link i-1 *) -Lemma eqn314 t : derive1mx o2 t = derive1mx o1 t + - FramedVect.v (rmap F `[derive1mx r12 t $ F1 t]) +Lemma eqn314 t : 'D_1 (fun x => o2 x : 'M__) t = 'D_1 (fun x => o1 x : 'M__) t + + FramedVect.v (rmap F '['D_1 (fun x => r12 x : 'M__) t $ F1 t]) (* velocity of the origin of Frame i w.r.t. the origin of Frame i-1 *) + w1 t *v (\o{F2 t} - \o{F1 t}). Proof. rewrite -eqn314_helper. move: (@eqn312 _ F _ derivable_F1 _ derivable_r12 derivable_F1o t). -have -> : derive1mx (fun x => BoundVect.endp (motion r12 x)) t = derive1mx o2 t. +have -> : 'D_1 (fun x => BoundVect.endp (motion r12 x)) t = 'D_1 (fun x => o2 x : 'M__) t. rewrite (_ : (fun x => BoundVect.endp (motion r12 x)) = o2) //. rewrite funeqE => t' /=; rewrite -mulmxA FromTo_comp FromToI mulmx1. rewrite addrCA RFrame_o subrr addr0. @@ -510,30 +559,29 @@ Lemma eqn316 t : w2 t = w1 t + w12 t *m ((F1 t) _R^ F). Proof. have : (fun t => (F2 t) _R^ F) = (fun t => ((F2 t) _R^ (F1 t)) *m ((F1 t) _R^ F)). by rewrite funeqE => ?; rewrite FromTo_comp. -move/(congr1 (fun x => derive1mx x)). +move/(congr1 (fun x => 'D_(1:R^o) x)). rewrite funeqE. move/(_ t). -rewrite derive1mxM; last 2 first. - move=> t'; exact: derivable_mx_FromTo'. - move=> t'; exact: derivable_mx_FromTo. -rewrite derive1mx_ang_vel; last 2 first. - move=> t'; by rewrite FromTo_is_O. - move=> t'; exact: derivable_mx_FromTo. -rewrite derive1mx_ang_vel; last 2 first. - move=> t'; by rewrite FromTo_is_O. - move=> t'; exact: derivable_mx_FromTo'. -rewrite derive1mx_ang_vel; last 2 first. - move=> t'; by rewrite FromTo_is_O. - move=> t'; exact: derivable_mx_FromTo. +rewrite derive_mulmx; last 2 first. + exact/derivable_mxP/derivable_mx_FromTo'. + exact/derivable_mxP/derivable_mx_FromTo. +rewrite derive1mx_ang_vel; last first. + by move=> t'; rewrite FromTo_is_O. +rewrite derive1mx_ang_vel; last first. + by move=> t'; rewrite FromTo_is_O. +rewrite derive1mx_ang_vel; last first. + by move=> t'; rewrite FromTo_is_O. rewrite ang_vel_mxE; last 2 first. - move=> t'; by rewrite FromTo_is_O. - move=> t'; exact: derivable_mx_FromTo. + by move=> t'; rewrite FromTo_is_O. + move=> t'; apply/derivable_mxP. + by apply/derivable_mx_FromTo. rewrite ang_vel_mxE; last 2 first. - move=> t'; by rewrite FromTo_is_O. - move=> t'; exact: derivable_mx_FromTo'. + by move=> t'; rewrite FromTo_is_O. + move=> t'; apply/derivable_mxP. + by apply/derivable_mx_FromTo'. rewrite ang_vel_mxE; last 2 first. - move=> t'; by rewrite FromTo_is_O. - move=> t'; exact: derivable_mx_FromTo. + by move=> t'; rewrite FromTo_is_O. + by move=> t'; exact/derivable_mxP/derivable_mx_FromTo. rewrite mulmxE -[in X in _ = X + _](mulr1 ((F2 t) _R^ (F1 t))). rewrite -(@orthogonal_tr_mul _ _ (F _R^ (F1 t))) ?FromTo_is_O //. rewrite -{2}(trmx_FromTo (F1 t) F). @@ -557,77 +605,100 @@ Qed. End link_velocity. -From mathcomp Require Import trigo. +Definition Rz' (T : realType) (a : T) := + col_mx3 (row3 (- sin a) (cos a) 0) (row3 (- cos a) (sin a) 0) 'e_2. -Lemma derive1_cos (R : realType) (t : R) : derive1 (cos : _ -> R^o) t = - sin t. -Proof. -rewrite derive1E. -have u := is_derive_cos t. -by have := @derive_val _ _ _ _ _ _ _ u. -Qed. - -Lemma derive1_sin (R : realType) (t : R) : derive1 (sin : _ -> R^o) t = cos t. -Proof. -rewrite derive1E. -have u := is_derive_sin t. -by have := @derive_val _ _ _ _ _ _ _ u. -Qed. - -Lemma derivable_sin (R : realType) (t : R) : derivable (sin : R^o -> R^o) t 1. -Proof. exact: ex_derive. Qed. - -Lemma derivable_cos (R : realType) (t : R) : derivable (cos : R^o -> R^o) t 1. -Proof. exact: ex_derive. Qed. - -Lemma derivable_cos_comp (R : realType) (t : R) (a : R^o -> R^o) : - derivable a t 1 -> derivable (cos \o a : _ -> R^o) t 1. -Proof. by move=> /derivableP Hs; exact: ex_derive. Qed. - -Lemma derivable_sin_comp (R : realType) (t : R) (a : R^o -> R^o) : - derivable a t 1 -> derivable ((sin : _ -> R^o) \o a) t 1. -Proof. by move=> /derivableP Hs; exact: ex_derive. Qed. - -Lemma derive1_cos_comp (R : realType) t (a : R^o -> R^o) : derivable a t 1 -> - derive1 (cos \o a : _ -> R^o) t = - (derive1 a t) * sin (a t). +Lemma derivable_Rz {R : realType} (a : R -> R) t : + derivable a t 1 -> + derivable (fun x : R^o => Rz (a x)) t 1. Proof. -move=> H; rewrite (derive1_comp H); last exact: derivable_cos. -by rewrite derive1_cos mulrC mulNr mulrN. +move=> at1. +apply/derivable_mxP. +move=> [[|[|[|//=]]]] ? [[|[|[|//=]]]] ?. +- rewrite [X in derivable X t 1](_ : _ = cos \o a); last first. + by apply/funext => x/=; rewrite !mxE/=. + exact/derivable_cos_comp. +- rewrite [X in derivable X t 1](_ : _ = sin \o a); last first. + by apply/funext => x/=; rewrite !mxE/=. + exact/derivable_sin_comp. +- rewrite [X in derivable X t 1](_ : _ = 0); last first. + by apply/funext => x/=; rewrite !mxE/=. + exact/derivable_cst. +- rewrite [X in derivable X t 1](_ : _ = - sin \o a); last first. + by apply/funext => x/=; rewrite !mxE/=. + apply/derivableN. + exact/derivable_sin_comp. +- rewrite [X in derivable X t 1](_ : _ = cos \o a); last first. + by apply/funext => x/=; rewrite !mxE/=. + exact/derivable_cos_comp. +- rewrite [X in derivable X t 1](_ : _ = 0); last first. + by apply/funext => x/=; rewrite !mxE/=. + exact/derivable_cst. +- rewrite [X in derivable X t 1](_ : _ = 0); last first. + by apply/funext => x/=; rewrite !mxE/=. + exact/derivable_cst. +- rewrite [X in derivable X t 1](_ : _ = 0); last first. + by apply/funext => x/=; rewrite !mxE/=. + exact/derivable_cst. +- rewrite [X in derivable X t 1](_ : _ = 1); last first. + by apply/funext => x/=; rewrite !mxE/=. + exact/derivable_cst. Qed. Lemma derive1mx_RzE (R : realType) (a : R^o -> R^o) t : derivable a t 1 -> - derive1mx (fun x => Rz (a x) : 'M[R^o]__) t = + 'D_1 (fun x => Rz (a x) : 'M[R^o]__) t = derive1 a t *: col_mx3 (row3 (- sin (a t)) (cos (a t)) 0) (row3 (- cos (a t)) (- sin (a t)) 0) 0. Proof. move=> Ha. apply/matrix3P/and9P; split; rewrite !mxE /=. -- rewrite (_ : (fun _ => _) = cos \o a); last by rewrite funeqE => x; rewrite !mxE. +- rewrite derive_mx; last exact: derivable_Rz. + rewrite mxE/=. + rewrite (_ : (fun _ => _) = cos \o a); last by rewrite funeqE => x; rewrite !mxE. + rewrite -derive1E. rewrite (derive1_comp Ha); last exact/derivable_cos. by rewrite derive1_cos mulrC. -- rewrite (_ : (fun _ => _) = sin \o a); last by rewrite funeqE => x; rewrite !mxE. +- rewrite derive_mx; last exact: derivable_Rz. + rewrite mxE/=. + rewrite (_ : (fun _ => _) = sin \o a); last by rewrite funeqE => x; rewrite !mxE. + rewrite -derive1E. rewrite (derive1_comp Ha); last exact/derivable_sin. by rewrite derive1_sin mulrC. -- rewrite (_ : (fun _ => _) = \0); last by rewrite funeqE => x; rewrite !mxE. - by rewrite derive1_cst mulr0. -- rewrite (_ : (fun _ => _) = - sin \o a); last by rewrite funeqE => x; rewrite !mxE. +- rewrite derive_mx; last exact: derivable_Rz. + rewrite mxE/=. + rewrite (_ : (fun _ => _) = \0); last by rewrite funeqE => x; rewrite !mxE. + by rewrite derive_cst mulr0. +- rewrite derive_mx; last exact: derivable_Rz. + rewrite mxE/=. + rewrite (_ : (fun _ => _) = - sin \o a); last by rewrite funeqE => x; rewrite !mxE. rewrite (_ : - _ \o _ = - (sin \o a)) // derive1E deriveN; last first. apply/derivable1_diffP/differentiable_comp. exact/derivable1_diffP. exact/derivable1_diffP/derivable_sin. - rewrite -derive1E (derive1_comp Ha); last exact/derivable_sin. + rewrite -!derive1E (derive1_comp Ha); last exact/derivable_sin. by rewrite derive1_sin mulrN mulrC. -- rewrite (_ : (fun _ => _) = cos \o a); last by rewrite funeqE => x; rewrite !mxE. - rewrite (derive1_comp Ha); last exact/derivable_cos. +- rewrite derive_mx; last exact: derivable_Rz. + rewrite mxE/=. + rewrite (_ : (fun _ => _) = cos \o a); last by rewrite funeqE => x; rewrite !mxE. + rewrite -derive1E (derive1_comp Ha); last exact/derivable_cos. by rewrite derive1_cos mulrN mulNr mulrC. -- rewrite (_ : (fun _ => _) = \0); last by rewrite funeqE => x; rewrite !mxE. - by rewrite derive1_cst mulr0. -- rewrite (_ : (fun _ => _) = \0); last by rewrite funeqE => x; rewrite !mxE. - by rewrite derive1_cst mulr0. -- rewrite (_ : (fun _ => _) = \0); last by rewrite funeqE => x; rewrite !mxE. - by rewrite derive1_cst mulr0. -- rewrite (_ : (fun _ => _) = cst 1); last by rewrite funeqE => x; rewrite !mxE. - by rewrite derive1_cst mulr0. +- rewrite derive_mx; last exact: derivable_Rz. + rewrite mxE/=. + rewrite (_ : (fun _ => _) = \0); last by rewrite funeqE => x; rewrite !mxE. + by rewrite derive_cst mulr0. +- rewrite derive_mx; last exact: derivable_Rz. + rewrite mxE/=. + rewrite (_ : (fun _ => _) = \0); last by rewrite funeqE => x; rewrite !mxE. + by rewrite derive_cst mulr0. +- rewrite derive_mx; last exact: derivable_Rz. + rewrite mxE/=. + rewrite (_ : (fun _ => _) = \0); last by rewrite funeqE => x; rewrite !mxE. + by rewrite derive_cst mulr0. +- rewrite derive_mx; last exact: derivable_Rz. + rewrite mxE/=. + rewrite (_ : (fun _ => _) = cst 1); last by rewrite funeqE => x; rewrite !mxE. + by rewrite derive_cst mulr0. Qed. (* example 3.1 [sciavicco]*) @@ -696,11 +767,15 @@ Section scara_geometric_jacobian. Variable R : realType. Variable theta1 : R -> R. +Hypothesis derivable_theta1 : forall t, derivable theta1 t 1. Variable a1 : R. Variable theta2 : R -> R. +Hypothesis derivable_theta2 : forall t, derivable theta2 t 1. Variable a2 : R. Variable d3 : R -> R. +Hypothesis derivable_d3 : forall t, derivable d3 t 1. Variable theta4 : R -> R. +Hypothesis derivable_theta4 : forall t, derivable theta4 t 1. Variable d4 : R. (* direct kinematics equation written in function of the joint variables, from scara.v *) @@ -709,7 +784,7 @@ Let trans t := scara_trans (theta1 t) a1 (theta2 t) a2 (d3 t) d4. Definition scara_end_effector t : 'M[R]_4 := hom (rot t) (trans t). Let scara_lin_vel : R -> 'rV[R]_3 := - derive1mx (@trans_of_hom R^o \o scara_end_effector). + 'D_1 (@trans_of_hom R^o \o scara_end_effector). Let scara_ang_vel : R -> 'rV[R]_3 := ang_vel (@rot_of_hom R^o \o scara_end_effector). @@ -723,7 +798,7 @@ Definition scara_joints : 'I_4 -> joint R := Definition scara_joint_variables t : 'rV[R^o]_4 := \row_i (joint_variable (scara_joints i) t). -Let scara_joint_velocities : R -> 'rV[R^o]_4 := derive1mx scara_joint_variables. +Let scara_joint_velocities : R -> 'rV[R^o]_4 := 'D_1 scara_joint_variables. (* specification of scara frames *) Variables scara_frames : 'I_5 -> R -> tframe R. @@ -740,6 +815,22 @@ Hypothesis o2E : forall t, \o{Fim1 2%:R t} = \o{Fim1 1 t} + Hypothesis o3E : forall t, \o{Fim1 3%:R t} = \o{Fim1 2%:R t} + (d3 t) *: 'e_2. Hypothesis o4E : forall t, \o{Fmax t} = \o{Fim1 3%:R t} + d4 *: 'e_2. +Lemma derivable_joint_variable t : + derivable (fun t0 : R^o => \row_i joint_variable (scara_joints i) t0) t 1. +Proof. +apply/derivable_mxP => a b. +rewrite (ord1 a)/=. +move: b => [[|[|[|[|//]]]]]/= ?. +- under eq_fun do rewrite mxE/=. + exact: derivable_theta1. +- under eq_fun do rewrite mxE/=. + exact: derivable_theta2. +- under eq_fun do rewrite mxE/=. + exact: derivable_d3. +- under eq_fun do rewrite mxE/=. + exact: derivable_theta4. +Qed. + Lemma scale_realType (K : realType) (k1 : K) (k2 : K^o) : k1 *: k2 = k1 * k2. Proof. by []. Qed. @@ -756,24 +847,46 @@ rewrite /geo_jac; set a := (X in _ *m @row_mx _ _ 3 3 X _). rewrite (mul_mx_row _ a) {}/a; congr (@row_mx _ _ 3 3 _ _). - rewrite /scara_lin_vel (_ : @trans_of_hom R \o _ = trans); last first. rewrite funeqE => x /=; exact: trans_of_hom_hom. - rewrite /trans /scara_trans derive1mxE [RHS]row3_proj /= ![in RHS]mxE [in RHS]/=. + rewrite /trans /scara_trans. + rewrite derive_mx//=; last first. + apply: derivable_row3; apply: derivableD => /=; [ | | | + | exact: derivable_cst + | exact: H3]. + apply: derivableM; first exact: derivable_cst. + by apply: derivable_cos_comp; exact: derivableD. + apply: derivableM; first exact: derivable_cst. + exact: derivable_cos_comp. + apply: derivableM; first exact: derivable_cst. + by apply: derivable_sin_comp; exact: derivableD. + apply: derivableM; first exact: derivable_cst. + exact: derivable_sin_comp. + rewrite [RHS]row3_proj /= ![in RHS]mxE [in RHS]/=. transitivity ( derive1 (theta1 : R^o -> R^o) t *: (Fim1 0 t)~k *v (\o{Fmax t} - \o{Fim1 0 t}) + derive1 (theta2 : R^o -> R^o) t *: (Fim1 1 t)~k *v (\o{Fmax t} - \o{Fim1 1 t}) + derive1 (d3 : R^o -> R^o) t *: (Fim1 2 t)~k + derive1 (theta4 : R^o -> R^o) t *: (Fim1 3%:R t)~k *v (\o{Fmax t} - \o{Fim1 3%:R t})). - rewrite /scara_joint_velocities /scara_joint_variables derive1mxE /geo_jac_lin /=. + rewrite /scara_joint_velocities /scara_joint_variables. + rewrite derive_mx; last exact: derivable_joint_variable. + rewrite /geo_jac_lin /=. apply/rowP => i; rewrite 3![in RHS]mxE [in LHS]mxE sum4E; (repeat apply: f_equal2). - rewrite 2!mxE /=. rewrite (linearZl_LR _ (\o{Fmax t} - \o{Fim1``_t}))/=. - by rewrite [in RHS]mxE. + rewrite [in RHS]mxE !derive1E//=. + by under eq_fun do rewrite !mxE/=. - rewrite 2!mxE /=. rewrite (linearZl_LR _ (\o{Fmax t} - \o{Fim1 1 t}))/=. - by rewrite [in RHS]mxE. - - by rewrite 2!mxE [in RHS]mxE. + under eq_fun do rewrite !mxE/=. + rewrite derive1E/=. + by rewrite !mxE. + - rewrite 2!mxE [in RHS]mxE/=. + rewrite derive1E/=. + by under eq_fun do rewrite !mxE/=. - rewrite 2!mxE /=. - by rewrite (linearZl_LR _ (\o{Fmax t} - \o{Fim1 3 t}))/= [in RHS]mxE. + rewrite (linearZl_LR _ (\o{Fmax t} - \o{Fim1 3 t}))/= [in RHS]mxE. + rewrite derive1E. + by under eq_fun do rewrite !mxE/=. rewrite o0E subr0. rewrite {1}o4E. rewrite (linearDr _ (_ *: Fim1``_t|,2)) /=. @@ -811,15 +924,21 @@ rewrite (mul_mx_row _ a) {}/a; congr (@row_mx _ _ 3 3 _ _). rewrite {1}o2E {1}o1E. rewrite (_ : (fun _ => _) = (a2 \*: (cos \o (theta2 + theta1) : R^o -> R^o)) + - (a1 *: (cos \o theta1 : R^o -> R^o))) //. + (a1 *: (cos \o theta1 : R^o -> R^o))); last first. + apply/funext => x/=. + by rewrite !mxE/=. rewrite (_ : (fun _ => _) = (a2 \*: (sin \o (theta2 + theta1) : _ -> R^o)) + - (a1 *: (sin \o theta1 : _ -> R^o))) //. + (a1 *: (sin \o theta1 : _ -> R^o))); last first. + apply/funext => x/=. + by rewrite !mxE/=. rewrite row3e2; congr (_ + _ *: _); last first. - by rewrite Hzvec. - - rewrite [in RHS]derive1E [in RHS]deriveD; last 2 first. + - rewrite derive1E. + under eq_fun do rewrite !mxE/=. + rewrite [in RHS]deriveD; last 2 first. exact: ex_derive. exact: H3. - by rewrite deriveE // diff_cst add0r derive1E. + by rewrite derive_cst// add0r. - rewrite 3!(linearDr _ ((theta1^`())%classic t *: Fim1``_t|,2))/=. rewrite (linearDr _ (Fim1 1 t)|,2)/=. rewrite (linearZr_LR _ _ (a1 * cos (theta1 t)))/=. @@ -834,8 +953,8 @@ rewrite (mul_mx_row _ a) {}/a; congr (@row_mx _ _ 3 3 _ _). rewrite scalerBr. rewrite -!addrA addrCA addrC -!addrA (addrCA (- _)) !addrA. rewrite -2!addrA [in RHS]addrC; congr (_ + _). - - rewrite !scalerA -2!scalerDl row3e1; congr (_ *: _). - rewrite [in RHS]derive1E deriveD; last 2 first. + + rewrite !scalerA -2!scalerDl row3e1; congr (_ *: _). + rewrite deriveD; last 2 first. apply/derivableZ/derivable_sin_comp/derivableD; [exact H2 | exact H1]. exact/derivableZ/derivable_sin_comp. rewrite deriveZ /=; last first. @@ -859,9 +978,9 @@ rewrite (mul_mx_row _ a) {}/a; congr (@row_mx _ _ 3 3 _ _). rewrite derive1E. by rewrite addrC. by rewrite derive1E addrC. - - rewrite !{1}scalerA !addrA -3!{1}scaleNr -2!scalerDl row3e0. + + rewrite !{1}scalerA !addrA -3!{1}scaleNr -2!scalerDl row3e0. congr (_ *: _). - rewrite [in RHS]derive1E deriveD; last 2 first. + rewrite deriveD; last 2 first. by apply/derivableZ/derivable_cos_comp/derivableD; [exact H2 | exact H1]. by apply/derivableZ/derivable_cos_comp; exact H1. rewrite deriveZ /=; last first. @@ -895,9 +1014,22 @@ rewrite (mul_mx_row _ a) {}/a; congr (@row_mx _ _ 3 3 _ _). transitivity (derive1 (theta1 : R^o -> R^o) t *: (Fim1 0 t)~k + derive1 (theta2 : R^o -> R^o) t *: (Fim1 1 t)~k + derive1 (theta4 : R^o -> R^o) t *: (Fim1 3%:R t)~k). - rewrite /scara_joint_velocities /scara_joint_variables derive1mxE /geo_jac_ang /=. + rewrite /scara_joint_velocities /scara_joint_variables. + rewrite derive_mx; last first. + (* derivable (fun t0 : R^o => \row_i joint_variable (scara_joints i) t0) t 1 *) + exact: derivable_joint_variable. + rewrite /geo_jac_ang /=. apply/rowP => i; rewrite !mxE sum4E !mxE {1}mulr0 addr0. - by rewrite -!/(Fim1 _) [Fim1 0 _]lock [Fim1 1 _]lock [Fim1 3%:R _]lock /= -!lock. + rewrite -!/(Fim1 _) [Fim1 0 _]lock [Fim1 1 _]lock [Fim1 3%:R _]lock /= -!lock. + rewrite !derive1E. + under eq_fun do rewrite !mxE/=. + rewrite -[RHS]addrA. + rewrite -[LHS]addrA. + congr (_ + _). + under eq_fun do rewrite !mxE/=. + congr (_ + _). + under eq_fun do rewrite !mxE/=. + done. rewrite !Hzvec -2!scalerDl e2row row3Z mulr0 mulr1. rewrite [in RHS]derive1E deriveD; [|apply/derivableD|by []]; last 2 first. exact: H1. @@ -918,10 +1050,9 @@ Variable (phi : 'rV[R^o]_n -> 'rV[R^o]_3). Let Jphi := jacobian phi. Lemma dp (q : R^o -> 'rV[R^o]_n) t : - derive1mx (p \o q) t = derive1mx q t *m Jp (q t). (* 3.56 *) + 'D_1 (p \o q) t = 'D_1 q t *m Jp (q t). (* 3.56 *) Proof. rewrite /Jp /jacobian mul_rV_lin1. -rewrite /derive1mx. Abort. End analytical_jacobian. diff --git a/euclidean.v b/euclidean.v index 3126675..30ab83f 100644 --- a/euclidean.v +++ b/euclidean.v @@ -1,4 +1,4 @@ -(* coq-robot (c) 2017 AIST and INRIA. License: LGPL-2.1-or-later. *) +(* coq-robot (c) 2025 AIST and INRIA. License: LGPL-2.1-or-later. *) From HB Require Import structures. From mathcomp Require Import all_ssreflect ssralg ssrint ssrnum rat poly. From mathcomp Require Import sesquilinear. @@ -7,8 +7,8 @@ From mathcomp Require Import realalg complex fingroup perm. From mathcomp Require Import reals. Require Import ssr_ext. -(******************************************************************************) -(* Elements of Euclidean geometry *) +(**md**************************************************************************) +(* # Elements of Euclidean geometry *) (* *) (* This file provides elements of Euclidean geometry, with specializations to *) (* the 3D case. It develops the theory of the dot-product and of the *) @@ -17,10 +17,13 @@ Require Import ssr_ext. (* preservation of the dot-product by orthogonal matrices or a closed formula *) (* for the characteristic polynomial of a 3x3 matrix. *) (* *) +(* ``` *) (* jacobi_identity == Jacobi identity *) (* lieAlgebraType R == the type of Lie algebra over R *) (* lie[x, y] == Lie brackets *) +(* ``` *) (* *) +(* ``` *) (* u *d w == the dot-product of the vectors u and v, i.e., the only *) (* component of the 1x1-matrix u * v^T *) (* norm u == the norm of vector u, i.e., the square root of u *d u *) @@ -29,8 +32,10 @@ Require Import ssr_ext. (* 'O[T]_n == the type of orthogonal matrices of size n *) (* 'SO[T]_n == the type of rotation matrices of size n *) (* cross M == generalized cross-product *) +(* ``` *) (* *) (* Specializations to the 3D case: *) +(* ``` *) (* row2 a b == the row vector [a,b] *) (* row3 a b c == the row vector [a,b,c] *) (* col_mx2 u v == specialization of col_mx two row vectors of size 2 *) @@ -41,6 +46,7 @@ Require Import ssr_ext. (* algebra *) (* vaxis_euler M == the vector-axis of the rotation matrix M of Euler's *) (* theorem *) +(* ``` *) (* *) (******************************************************************************) @@ -199,8 +205,8 @@ Section dotmul_bilinear_Pz. Variables (R : comPzRingType) (n : nat). Definition dotmul_rev (v u : 'rV[R]_n) := u *d v. -Canonical rev_dotmul := @RevOp _ _ _ dotmul_rev (@dotmul R n) - (fun _ _ => erefl). +(*Canonical rev_dotmul := @RevOp _ _ _ dotmul_rev (@dotmul R n) + (fun _ _ => erefl).*) Lemma dotmul_is_linear u : linear (dotmul u : 'rV[R]_n -> R^o). Proof. move=> /= k v w; by rewrite dotmulDr dotmulvZ. Qed. @@ -252,17 +258,40 @@ move/allP => H; apply/eqP/rowP => i. apply/eqP; by rewrite mxE -sqrf_eq0 expr2 -(implyTb ( _ == _)) H. Qed. +Lemma dotmul_is_hermitian x y : + (@dotmul T n) x y = (-1) ^+ false * idfun ((@dotmul T n) y x). +Proof. +by rewrite /= expr0 mul1r dotmulC. +Qed. + +HB.instance Definition _ := + @isHermitianSesquilinear.Build _ _ _ _ _ dotmul_is_hermitian. + +Check @dotmul _ _ : {symmetric 'rV[T]_n}. + +Let neq0_norm_gt0 (u : 'rV[T]_n) : u != 0 -> 0 < dotmul u u. +Proof. +move=> u0. +by rewrite lt_neqAle eq_sym dotmulvv0 u0 le0dotmul. +Qed. + +HB.instance Definition _ := isDotProduct.Build _ _ (@dotmul T n) neq0_norm_gt0. + End dot_product. Section norm. - -Variables (T : rcfType) (n : nat). +Context {T : rcfType} {n : nat}. Implicit Types u v : 'rV[T]_n. -Definition norm u := Num.sqrt (u *d u). +Local Notation "''[' u , v ]" := (dotmul u v) : ring_scope. +Local Notation "''[' u ]" := '[u, u]%R : ring_scope. + +Definition norm u : T := Num.sqrt '[u]. Lemma normN u : norm (- u) = norm u. -Proof. by rewrite /norm dotmulNv dotmulvN opprK. Qed. +Proof. +by rewrite /norm (@hnormN T false idfun). +Qed. Lemma norm0 : norm 0 = 0. Proof. by rewrite /norm dotmul0v sqrtr0. Qed. @@ -1672,9 +1701,8 @@ rewrite [X in _ + _ + X](_ : _ = - M 0 2%:R * M 2%:R 0); last first. rewrite [in X in X * _]/=. rewrite coefD coefM sum2E subn0 coefC coefC mulr0 add0r. rewrite coefC mul0r add0r coefM sum2E subn0 subnn coefC [in X in X * _`_1]/=. - rewrite !coefD !coefX !coefN !coefC/=. - rewrite !mul0r !addr0/= subr0 mulr1. - by rewrite mulNr. + rewrite coefD coefX coefN !coefC/= !(subr0,mul0r,mulr0,mulr1,addr0). + by rewrite coefB coefC/= subr0 coefX eqxx mulr1 mulNr. rewrite /Z. apply/(@mulrI _ 2%:R); first exact: pnatf_unit. rewrite mulrA div1r divrr ?pnatf_unit // mul1r. diff --git a/extra_trigo.v b/extra_trigo.v index 3991877..16c3610 100644 --- a/extra_trigo.v +++ b/extra_trigo.v @@ -1,9 +1,9 @@ From mathcomp Require Import all_ssreflect ssralg ssrnum. -From mathcomp Require Import interval reals trigo. +From mathcomp Require Import boolp interval reals trigo. Require Import ssr_ext. -(******************************************************************************) -(* Extra material for trigo *) +(**md**************************************************************************) +(* # Extra material for trigo *) (* *) (******************************************************************************) @@ -121,78 +121,6 @@ rewrite sin_eq0_Npipi //; case: eqP => /= [aE _|_ /eqP //]. by move/eqP: caE; rewrite aE cos0 -eqr_oppLR eqrNxx oner_eq0. Qed. -(* NB: PR to analysis in progress *) -Lemma acos1 : acos (1 : R) = 0. -Proof. -have := @cosK R 0; rewrite cos0 => -> //. -by rewrite in_itv //= lexx pi_ge0. -Qed. - -Lemma acos0 : acos (0 : R) = pi / 2%:R. -Proof. -have := @cosK R (pi / 2%:R). -rewrite cos_pihalf => -> //. -rewrite in_itv //= divr_ge0 ?ler0n ?pi_ge0 //=. -rewrite ler_pdivrMr ?ltr0n //. -by rewrite mulr_natr mulr2n -lerBlDr subrr pi_ge0. -Qed. - -Lemma acosN1 : acos (- 1) = (pi : R). -Proof. -have oneB : -1 <= (-1 : R) <= 1 by rewrite lexx ge0_cp ?(ler0n _ 1). -apply: cos_inj; rewrite ?in_itv//= ?pi_ge0 ?lexx //. - by rewrite acos_ge0 // acos_lepi. -by rewrite acosK ?in_itv//= cospi. -Qed. - -Lemma acosN a : -1 <= a <= 1 -> acos (- a) = pi - acos a. -Proof. -move=> aB. -have aBN : -1 <= - a <= 1 by rewrite lerNl opprK lerNl andbC. -apply: cos_inj; first by rewrite in_itv/= acos_ge0 // acos_lepi. - rewrite in_itv/= subr_ge0 acos_lepi // -subr_le0 addrAC subrr sub0r. - by rewrite oppr_cp0 acos_ge0. -by rewrite addrC cosDpi cosN !acosK. -Qed. - -Lemma cosKN a : - pi <= a <= 0 -> acos (cos a) = - a. -Proof. -move=> Hs. -rewrite -(cosN a) cosK // ?in_itv/=. -by rewrite lerNr oppr0 lerNl andbC. -Qed. - -Lemma atan0 : atan 0 = 0 :> R. -Proof. -apply: tan_inj; first 2 last. -- by rewrite atanK tan0. -- by rewrite in_itv/= atan_gtNpi2 atan_ltpi2. -by rewrite in_itv/= oppr_cp0 divr_gt0 ?pi_gt0 // ltr0n. -Qed. - -Lemma atan1 : atan 1 = pi / 4%:R :> R. -Proof. -apply: tan_inj; first 2 last. -- by rewrite atanK tan_piquarter. -- by rewrite in_itv/= atan_gtNpi2 atan_ltpi2. -have v2_ge0 : 0 <= 2%:R :> R by rewrite ler0n. -have v2_gt0 : 0 < 2%:R :> R by rewrite ltr0n. -rewrite in_itv/= -mulNr (lt_trans _ (_ : 0 < _ )) /=; last 2 first. -- by rewrite mulNr oppr_cp0 divr_gt0 // pi_gt0. -- by rewrite divr_gt0 ?pi_gt0 // ltr0n. -rewrite (natrM _ 2 2) invfM mulrA lter_pdivrMr // divfK ?natr_eq0 //. - by rewrite ltr_pdivrMr // mulr_natr mulr2n -subr_gte0 addrK ?pi_gt0. -by case: ltgtP v2_gt0. -Qed. - -Lemma atanN (x : R) : atan (- x) = - atan x. -Proof. -apply: tan_inj; first by rewrite in_itv/= atan_ltpi2 atan_gtNpi2. - by rewrite in_itv/= ltrNl opprK ltrNl andbC atan_ltpi2 atan_gtNpi2. -by rewrite tanN !atanK. -Qed. -(* /NB: PR to analysis in progress *) - Lemma sin_half_angle a : `| sin (a / 2%:R) | = Num.sqrt ((1 - cos a) / 2%:R). Proof. move: (cosD (a / 2%:R) (a / 2%:R)). diff --git a/frame.v b/frame.v index 1b0829b..9e266b0 100644 --- a/frame.v +++ b/frame.v @@ -855,27 +855,29 @@ End framed_vector. End FramedVect. Notation fvec := FramedVect.t. -Notation "`[ v $ F ]" := (FramedVect.mk F v) - (at level 5, v, F at next level, format "`[ v $ F ]"). +Notation "''[' v $ F ]" := (FramedVect.mk F v) + (at level 5, v, F at next level, format "''[' v $ F ]") : frame_scope. Definition FramedVect_add (T : pzRingType) (F : tframe T) (a b : fvec F) : fvec F := - `[ FramedVect.v a + FramedVect.v b $ F ]. + '[ FramedVect.v a + FramedVect.v b $ F ]. Notation "a \+f b" := (FramedVect_add a b) (at level 39). -Lemma fv_eq (T : pzRingType) a b : a = b -> forall F : frame T, `[ a $ F ] = `[ b $ F ]. +Lemma fv_eq (T : pzRingType) a b : a = b -> forall F : frame T, '[ a $ F ] = '[ b $ F ]. Proof. by move=> ->. Qed. +Notation "a \+f b" := (FramedVect_add a b) (at level 39). + Section change_of_coordinate_by_rotation. Variable T : realType. Implicit Types A B : frame T. -Lemma FramedVectvK A (x : fvec A) : `[FramedVect.v x $ A] = x. +Lemma FramedVectvK A (x : fvec A) : '[FramedVect.v x $ A] = x. Proof. by case: x. Qed. (* change of coordinates: "rotation mapping" from frame A to frame B *) -Definition rmap A B (x : fvec A) : fvec B := `[FramedVect.v x *m (A _R^ B) $ B]. +Definition rmap A B (x : fvec A) : fvec B := '[FramedVect.v x *m (A _R^ B) $ B]. Lemma rmapK A B (x : fvec A) : rmap A (rmap B x) = x. Proof. @@ -885,15 +887,15 @@ by rewrite divrr ?noframe_is_unit // mulmx1 /= FramedVectvK. Qed. Lemma rmapE A B (x : 'rV[T]_3) : - rmap B `[x $ A] = `[x *m A (*A->can*) *m B^T(*can->B*) $ B]. + rmap B '[x $ A] = '[x *m A (*A->can*) *m B^T(*can->B*) $ B]. Proof. by rewrite /rmap FromToE noframe_inv mulmxA. Qed. Lemma rmapE_from_can A (x : 'rV[T]_3) : - rmap A `[x $ can_tframe T] = `[x *m A^T $ A]. + rmap A '[x $ can_tframe T] = '[x *m A^T $ A]. Proof. by rewrite rmapE can_frame_1 mulmx1. Qed. Lemma rmapE_to_can A (x : 'rV[T]_3) : - rmap (can_tframe T) `[x $ A] = `[x *m A $ can_tframe T]. + rmap (can_tframe T) '[x $ A] = '[x *m A $ can_tframe T]. Proof. by rewrite rmapE can_frame_1 trmx1 mulmx1. Qed. End change_of_coordinate_by_rotation. diff --git a/quaternion.v b/quaternion.v index f372ced..a1eda29 100644 --- a/quaternion.v +++ b/quaternion.v @@ -9,14 +9,15 @@ From mathcomp Require Import interval reals trigo. Require Import ssr_ext euclidean vec_angle frame rot. Require Import extra_trigo. -(******************************************************************************) -(* Quaternions *) +(**md**************************************************************************) +(* # Quaternions *) (* *) (* This file develops the theory of quaternions. It defines the type of *) (* quaternions and the type of unit quaternions and show that quaternions *) (* form a ZmodType, a RingType, a LmodType, a UnitRingType. It also defines *) (* polar coordinates and dual quaternions. *) (* *) +(* ``` *) (* quat R == type of quaternions over the ringType R *) (* x%:q == quaternion with scalar part x and vector part 0 *) (* x \is realq == the quaternion x has no vector part *) @@ -30,28 +31,36 @@ Require Import extra_trigo. (* normq x == norm of the quaternion x *) (* uquat R == type of unit quaternions, i.e., quaternions with norm 1 *) (* conjugation x == v |-> x v x^* *) +(* ``` *) (* *) (* Polar coordinates: *) +(* ``` *) (* polar_of_quat a == polar coordinates of the quaternion a *) (* quat_of_polar a u == quaternion corresponding to the polar coordinates *) (* angle a and vector u *) (* quat_rot x == snd \o conjugation x (rotation of angle 2a about *) (* vector v where a,v are the polar coordinates of x, *) (* a unit quaternion *) +(* ``` *) +(* *) (* Dual numbers: *) +(* ``` *) (* dual R == the type of dual numbers over a ringType R *) (* x.1 == left part of the dual number x *) (* x.2 == right part of the dual number x *) +(* ``` *) (* Dual numbers are equipped with a structure of ZmodType, RingType, and of *) (* LmodType when R is a ringType, of Com/UnitRingType when R is a *) (* Com/UnitRingType. *) (* *) (* Dual quaternions: *) +(* ``` *) (* x +ɛ* y == dual number formed by x and y *) (* dquat == type of dual quaternions *) (* x \is puredq == the dual quaternion x is pure *) (* a \is dnum == a has no vector part *) (* x^*dq == conjugate of dual quaternion x *) +(* ``` *) (* *) (******************************************************************************) diff --git a/rigid.v b/rigid.v index ffde32a..475d50a 100644 --- a/rigid.v +++ b/rigid.v @@ -1,4 +1,4 @@ -(* coq-robot (c) 2017 AIST and INRIA. License: LGPL-2.1-or-later. *) +(* coq-robot (c) 2025 AIST and INRIA. License: LGPL-2.1-or-later. *) From HB Require Import structures. From mathcomp Require Import all_ssreflect ssralg ssrint ssrnum rat poly. From mathcomp Require Import closed_field polyrcf matrix mxalgebra mxpoly zmodp. @@ -6,8 +6,8 @@ From mathcomp Require Import realalg complex finset fingroup perm. From mathcomp Require Import interval reals trigo. Require Import ssr_ext euclidean skew vec_angle rot frame extra_trigo. -(******************************************************************************) -(* Rigid Body Transformations *) +(**md**************************************************************************) +(* # Rigid Body Transformations *) (* *) (* This file develops the theory of isometries, proving basic properties such *) (* as the preservation of the cross-product by derivative maps, the facts *) @@ -16,6 +16,7 @@ Require Import ssr_ext euclidean skew vec_angle rot frame extra_trigo. (* body transformations are represented by elements of the special Euclidean *) (* group and are shown to preserve norms. *) (* *) +(* ``` *) (* 'Iso[T]_n == the type of isometries *) (* 'CIso[T]_n == the type of central isometries, i.e., isometries f such *) (* that f 0 = 0 *) @@ -38,6 +39,7 @@ Require Import ssr_ext euclidean skew vec_angle rot frame extra_trigo. (* homogeneous representation *) (* Adjoint g == adjoint transformation associated with the homogeneous *) (* matrix g *) +(* ``` *) (* *) (******************************************************************************) diff --git a/robot-rocq.opam b/robot-rocq.opam index 1d42fb4..10345cf 100644 --- a/robot-rocq.opam +++ b/robot-rocq.opam @@ -21,11 +21,11 @@ install: [make "install"] depends: [ "coq" { (>= "9.0" & < "9.2~") } "coq-hierarchy-builder" { (>= "1.9.1") } - "coq-mathcomp-ssreflect" { (>= "2.3.0") } - "coq-mathcomp-fingroup" { (>= "2.3.0") } - "coq-mathcomp-algebra" { (>= "2.3.0") } - "coq-mathcomp-solvable" { (>= "2.3.0") } - "coq-mathcomp-field" { (>= "2.3.0") } + "coq-mathcomp-ssreflect" { (>= "2.4.0") } + "coq-mathcomp-fingroup" { (>= "2.4.0") } + "coq-mathcomp-algebra" { (>= "2.4.0") } + "coq-mathcomp-solvable" { (>= "2.4.0") } + "coq-mathcomp-field" { (>= "2.4.0") } "coq-mathcomp-analysis" { (>= "1.11.0") } "coq-mathcomp-real-closed" { (>= "2.0.0") } "coq-mathcomp-algebra-tactics" { (>= "1.2.4") } diff --git a/rot.v b/rot.v index bb9ab56..cadcedf 100644 --- a/rot.v +++ b/rot.v @@ -10,17 +10,20 @@ From mathcomp Require Import sesquilinear. Require Import extra_trigo. Require Import ssr_ext euclidean skew vec_angle frame. -(******************************************************************************) -(* Rotations *) +(**md**************************************************************************) +(* # Rotations *) (* *) (* This file develops the theory of 3D rotations with results such as *) (* Rodrigues formula, the fact that any rotation matrix can be represented *) (* by its exponential coordinates, angle-axis representation, Euler angles, *) (* etc. See also quaternion.v for rotations using quaternions. *) (* *) +(* ``` *) (* RO a, RO' a == two dimensional rotations of angle a *) +(* ``` *) (* *) (* Elementary rotations (row vector convention): *) +(* ``` *) (* Rx a, Rx' a == rotations about axis x of angle a *) (* Ry a == rotation about axis y of angle a *) (* Rz a == rotation about axis z of angle a *) @@ -29,7 +32,9 @@ Require Import ssr_ext euclidean skew vec_angle frame. (* sample lemmas: *) (* all rotations around a vector of angle a have trace "1 + 2 * cos a" *) (* equivalence SO[R]_3 <-> Rot (isRot_SO, SO_is_Rot) *) +(* ``` *) (* *) +(* ``` *) (* `e(a, M) == specialized exponential map for angle a and matrix M *) (* `e^(a, w) == specialized exponential map for the matrix \S(w), i.e., the *) (* skew-symmetric matrix corresponding to vector w *) @@ -37,40 +42,52 @@ Require Import ssr_ext euclidean skew vec_angle frame. (* inverse of the exponential map, *) (* exponential map of a skew matrix is a rotation *) (* *) +(* ``` *) (* rodrigues u a w == linear combination of the vectors u, (u *d w)w, w *v u *) (* that provides an alternative expression for the vector *) (* u * e^(a,w) *) +(* ``` *) (* *) (* Angle-axis representation: *) +(* ``` *) (* Aa.angle M == angle of angle-axis representation for the matrix M *) (* Aa.vaxis M == axis of angle-axis representation for the matrix M *) (* sample lemma *) (* a rotation matrix has Aa.angle M and normalize (Aa.vaxis M) for *) (* exponential coordinates *) +(* ``` *) (* *) (* Composition of elementary rotations (row vector convention): *) +(* ``` *) (* Rzyz a b c == composition of a Rz rotation of angle c, a Ry rotation of *) (* angle b, and a Rz rotation of angle a *) (* Rxyz a b c == composition of a Rx rotation of angle c, a Ry rotation of *) (* angle b, and a Rz notation of angle a *) +(* ``` *) (* *) (* ZYZ angles given a rotation matrix M (ref: [sciavicco] 2.4.1): *) (* with zyz_b in ]0;pi[: *) +(* ``` *) (* zyz_a M == angle of the last Rz rotation *) (* zyz_b M == angle of the Ry rotation *) (* zyz_c M == angle of the first Rz rotation *) +(* ``` *) (* *) (* Roll-Pitch-Yaw (ZYX) angles given a rotation matrix M *) (* with pitch in ]-pi/2;pi/2[ (ref: [sciavicco] 2.4.2): *) +(* ``` *) (* rpy_a M == angle about axis z (roll) *) (* rpy_b M == angle about axis y (pitch) *) (* rpy_c M == angle about axis x (yaw) *) +(* ``` *) (* *) (* Alternative formulation of ZYX angles: *) (* (ref: [Gregory G. Slabaugh, Computer Euler angles from a rotation matrix]) *) +(* ``` *) (* euler_a == angle about z *) (* euler_b == angle about y *) (* euler_c == angle about x *) +(* ``` *) (******************************************************************************) Reserved Notation "'`e(' a ',' M ')'" (format "'`e(' a ',' M ')'"). @@ -328,8 +345,8 @@ Lemma RzE a : Rz a = (frame_of_SO (Rz_is_SO a)) _R^ (can_frame T). Proof. rewrite FromTo_to_can; by apply/matrix3P/and9P; split; rewrite !mxE. Qed. Lemma rmap_Rz_e0 a : - rmap (can_tframe T) `[ 'e_0 $ frame_of_SO (Rz_is_SO a) ] = - `[ row 0 (Rz a) $ can_tframe T ]. + rmap (can_tframe T) '[ 'e_0 $ frame_of_SO (Rz_is_SO a) ] = + '[ row 0 (Rz a) $ can_tframe T ]. Proof. by rewrite rmapE_to_can rowE [in RHS]RzE FromTo_to_can. Qed. Definition Rzy a b := col_mx3 diff --git a/scara.v b/scara.v index d7d61a2..aff606e 100644 --- a/scara.v +++ b/scara.v @@ -1,4 +1,4 @@ -(* coq-robot (c) 2017 AIST and INRIA. License: LGPL-2.1-or-later. *) +(* coq-robot (c) 2025 AIST and INRIA. License: LGPL-2.1-or-later. *) From mathcomp Require Import all_ssreflect ssralg ssrint ssrnum rat poly. From mathcomp Require Import closed_field polyrcf matrix mxalgebra mxpoly zmodp. From mathcomp Require Import realalg complex fingroup perm. @@ -8,18 +8,20 @@ Require Import ssr_ext euclidean skew vec_angle rot frame rigid screw. From mathcomp Require Import reals. Require Import extra_trigo. -(******************************************************************************) -(* SCARA Robot Manipulator *) +(**md**************************************************************************) +(* # SCARA Robot Manipulator *) (* *) (* This file addresses the forward kinematics problem for the SCARA robot *) (* manipulator in two ways: (1) it first provides the DH parameters, *) (* (2) using screw motions. *) (* *) +(* ``` *) (* B10,B21,B32,B43 == relative positions of the consecutive frames of the *) (* SCARA robot manipulator using DH parameters *) (* t1,t2,t3,t4 == twists of the SCARA robot manipulator *) (* g0 == position of the end-effector using twists *) (* g == orientation of the end-effector using twists *) +(* ``` *) (* *) (******************************************************************************) diff --git a/screw.v b/screw.v index 8c268c2..7883103 100644 --- a/screw.v +++ b/screw.v @@ -1,4 +1,4 @@ -(* coq-robot (c) 2017 AIST and INRIA. License: LGPL-2.1-or-later. *) +(* coq-robot (c) 2025 AIST and INRIA. License: LGPL-2.1-or-later. *) From HB Require Import structures. From mathcomp Require Import all_ssreflect ssralg ssrint ssrnum rat poly. From mathcomp Require Import closed_field polyrcf matrix mxalgebra mxpoly zmodp. @@ -7,8 +7,8 @@ From mathcomp Require Import sesquilinear. From mathcomp Require Import interval reals trigo. Require Import ssr_ext euclidean skew vec_angle frame rot rigid extra_trigo. -(******************************************************************************) -(* Screw Motions *) +(**md**************************************************************************) +(* # Screw Motions *) (* *) (* This file develops the theory of screws and twists. It establishes in *) (* particular Chasles' theorem (given a rigid body motion, it shows *) @@ -16,6 +16,7 @@ Require Import ssr_ext euclidean skew vec_angle frame rot rigid extra_trigo. (* rigid body motion), Mozzi-Chasles' theorem (it shows the existence of a *) (* set of points that undergo just a translation---this is the screw axis). *) (* *) +(* ``` *) (* Module sqLieAlgebra == square matrices form a Lie algebra *) (* 'se3[R] == the set of twists *) (* wedge t == form a twist in 'se3[R] given twist (coordinates) *) @@ -35,6 +36,7 @@ Require Import ssr_ext euclidean skew vec_angle frame rot rigid extra_trigo. (* `e$(a, t) == the exponential of a twist t with angle a *) (* rjoint_twist w p == twist of a revolute joint *) (* pjoint_twist v == twist of a prismatic joint *) +(* ``` *) (* *) (******************************************************************************) diff --git a/skew.v b/skew.v index 5a577f4..35add61 100644 --- a/skew.v +++ b/skew.v @@ -1,4 +1,4 @@ -(* coq-robot (c) 2017 AIST and INRIA. License: LGPL-2.1-or-later. *) +(* coq-robot (c) 2025 AIST and INRIA. License: LGPL-2.1-or-later. *) From HB Require Import structures. From mathcomp Require Import all_ssreflect ssralg ssrint ssrnum rat poly. From mathcomp Require Import closed_field polyrcf matrix mxalgebra mxpoly zmodp. @@ -7,12 +7,13 @@ From mathcomp Require Import realalg complex finset fingroup perm ring. Require Import ssr_ext euclidean vec_angle. From mathcomp Require Import reals. -(******************************************************************************) -(* Skew-symmetric matrices *) +(**md**************************************************************************) +(* # Skew-symmetric matrices *) (* *) (* This file develops the theory of skew-symmetric matrices to be used in *) (* particular to represent the exponential coordinates of rotation matrices. *) (* *) +(* ``` *) (* 'so[R]_n == the type of skew-symmetric matrices, i.e., matrices M such *) (* that M = -M^T *) (* \S(w) == the spin of vector w, i.e., the (row-vector convention) *) @@ -20,10 +21,13 @@ From mathcomp Require Import reals. (* symp A == symmetric part of matrix A *) (* antip A == antisymmetric part of matrix A *) (* spin_eigenvalues u == eigenvalues of \S(u) *) +(* ``` *) (* *) (* Cayley transform: *) +(* ``` *) (* cayley M == (1 - M)^-1 * (1 + M) *) (* uncayley M == (M - 1) * (M + 1)^-1 *) +(* ``` *) (* *) (******************************************************************************) diff --git a/ssr_ext.v b/ssr_ext.v index 25e7aee..5437800 100644 --- a/ssr_ext.v +++ b/ssr_ext.v @@ -1,19 +1,21 @@ -(* coq-robot (c) 2017 AIST and INRIA. License: LGPL-2.1-or-later. *) -From Stdlib Require Import NsatzTactic. +(* coq-robot (c) 2025 AIST and INRIA. License: LGPL-2.1-or-later. *) +Require Import NsatzTactic. From mathcomp Require Import all_ssreflect ssralg ssrnum ssrint rat poly. From mathcomp Require Import closed_field polyrcf matrix mxalgebra mxpoly zmodp. From mathcomp Require Import perm path fingroup complex. -(******************************************************************************) -(* Minor additions to MathComp libraries *) +(**md**************************************************************************) +(* # Minor additions to MathComp libraries *) (* *) (* This file contains minor additions ssrbool, ssralg, ssrnum, and complex *) (* and more. *) (* *) +(* ``` *) (* u``_i == the ith component of the row vector u *) (* 'e_0, 'e_1, 'e_2 == the canonical vectors *) (* Section Nsatz_rcfType == type classes for the Coq nsatz tactic *) (* (https://coq.inria.fr/refman/addendum/nsatz.html) *) +(* ``` *) (* *) (******************************************************************************) @@ -21,9 +23,11 @@ Reserved Notation "''e_' i" (format "''e_' i", at level 8, i at level 2). Reserved Notation "u '``_' i" (at level 3, i at level 2, left associativity, format "u '``_' i"). -(* TODO: overrides forms.v *) Notation "u '``_' i" := (u (@GRing.zero _) i) : ring_scope. +(* NB: like Damien's LaSalle *) +Notation "p ..[ i ]" := (p (@GRing.zero _) i) (at level 10, only parsing). + Set Implicit Arguments. Unset Strict Implicit. Unset Printing Implicit Defensive. diff --git a/tilt.v b/tilt.v new file mode 100644 index 0000000..69c6528 --- /dev/null +++ b/tilt.v @@ -0,0 +1,1928 @@ +From HB Require Import structures. +From mathcomp Require Import all_ssreflect all_algebra ring. +From mathcomp Require Import interval_inference. +From mathcomp Require Import boolp classical_sets functions reals order. +From mathcomp Require Import topology normedtype landau derive realfun. +From mathcomp Require Import matrix_normedtype. +Require Import ssr_ext euclidean rigid frame skew derive_matrix. +Require Import tilt_mathcomp tilt_analysis tilt_robot. + +(**md**************************************************************************) +(* # Tentative formalization of [1] *) +(* *) +(* ``` *) +(* posdefmx M == M is definite positive *) +(* locposdef V x == V is locally positive definite at x *) +(* is_Lyapunov_candidate V := locposdef V *) +(* locnegsemidef V x == V is locally negative semidefinite *) +(* 'D~(sol, x0) V == derivative of V along the solution sol *) +(* starting at x0 *) +(* is_sol f y == the function y satisfies y' = phi y *) +(* is_equilibrium_point f p := solves_equation f (cst p) *) +(* state_space f == the set points attainable by a solution *) +(* (in the sense of `is_sol`) *) +(* is_Lyapunov_stable_at f V x == Lyapunov stability *) +(* ``` *) +(* *) +(* Reference: *) +(* - [benallegue2023itac] *) +(* https://hal.science/hal-04271257v1/file/benallegue2019tac_October_2022.pdf *) +(* - [2]: Hassan K. Khalil, Nonlinear systems, 2002*) +(******************************************************************************) + +Reserved Notation "''D~(' sol , x ) f" (at level 10, sol, x, f at next level, + format "''D~(' sol , x ) f"). + +Set Implicit Arguments. +Unset Strict Implicit. +Unset Printing Implicit Defensive. + +Import Order.TTheory GRing.Theory Num.Def Num.Theory. +Import numFieldNormedType.Exports. +Local Open Scope ring_scope. + +(* additions to MathComp-Analysis *) + + +Lemma ball0_le0 (R : realDomainType) (V : pseudoMetricNormedZmodType R) (a : V) (r : R) : + ball a r = set0 -> r <= 0. +Proof. +rewrite -subset0 => ar0; rewrite leNgt; apply/negP => r0. +by have /(_ (ballxx _ r0)) := ar0 a. +Qed. + +Lemma le0_ball0 (R : realDomainType) (V : pseudoMetricNormedZmodType R) (a : V) (r : R) : + r <= 0 -> ball a r = set0. +Proof. +move=> r0; rewrite -subset0 => y. +rewrite -ball_normE /ball_/= ltNge => /negP; apply. +by rewrite (le_trans r0). +Qed. + +Lemma closed_ball0 (R : realDomainType) (V : pseudoMetricNormedZmodType R) (a : V) (r : R) : + r <= 0 -> closed_ball a r = set0. +Proof. +move=> r0; rewrite -subset0 => v. +by rewrite /closed_ball le0_ball0// closure0. +Qed. + +Lemma closed_ballAE {K : realType} n (e : K) (x : 'rV[K]_n) : + 0 < e -> closed_ball x e = closed_ball_ (@mx_norm _ _ _) x e. +Proof. +by move=> e0; rewrite closed_ballE. +Qed. + +Import Order.Def. + +Lemma maxE {K : realType} (x y : {nonneg K}) : + (max x%:num y%:num) = (max x y)%:num. +Proof. +rewrite /max; apply/esym. +case: ifPn => // xy. + case: ifPn => //. + rewrite -leNgt => yx. + by apply/eqP; rewrite eq_le yx/= ltW. +case: ifPn => // yx. +apply/eqP; rewrite eq_le (ltW yx)/=. +by rewrite -leNgt in xy. +Qed. + +Local Open Scope classical_set_scope. +(* NB: we are just mimicking the proofs for the real line already available in derive.v *) +Lemma EVT_max_rV (R : realType) n (f : 'rV[R]_n -> R) (A : set 'rV[R]_n) : + A !=set0 -> + compact A -> + {within A, continuous f} -> exists2 c, c \in A & + forall t, t \in A -> f t <= f c. +Proof. +move=> A0 compactA fcont; set imf := f @` A. +have imf_sup : has_sup imf. + split. + case: A0 => a Aa. + by exists (f a); apply/imageP. + have [M [Mreal imfltM]] : bounded_set (f @` A). + exact/compact_bounded/continuous_compact. + exists (M + 1) => y /imfltM yleM. + by rewrite (le_trans _ (yleM _ _)) ?ler_norm ?ltrDl. +have [|imf_ltsup] := pselect (exists2 c, c \in A & f c = sup imf). + move=> [c cab fceqsup]; exists c => // t tab; rewrite fceqsup. + apply/sup_upper_bound => //. + exact/imageP/set_mem. +have {}imf_ltsup t : t \in A -> f t < sup imf. + move=> tab; case: (ltrP (f t) (sup imf)) => // supleft. + rewrite falseE; apply: imf_ltsup; exists t => //; apply/eqP. + rewrite eq_le supleft andbT sup_upper_bound//. + exact/imageP/set_mem. +pose g t : R := (sup imf - f t)^-1. +have invf_continuous : {within A, continuous g}. + rewrite continuous_subspace_in => t tab; apply: cvgV => //=. + by rewrite subr_eq0 gt_eqF // imf_ltsup //; rewrite inE in tab. + by apply: cvgD; [exact: cst_continuous | apply: cvgN; exact: (fcont t)]. +have /ex_strict_bound_gt0 [k k_gt0 /= imVfltk] : bounded_set (g @` A). + by apply/compact_bounded/continuous_compact. +have [_ [t tab <-]] : exists2 y, imf y & sup imf - k^-1 < y. + by apply: sup_adherent => //; rewrite invr_gt0. +rewrite ltrBlDr -ltrBlDl. +suff : sup imf - f t > k^-1 by move=> /ltW; rewrite leNgt => /negbTE ->. +rewrite -[ltRHS]invrK ltf_pV2// ?qualifE/= ?invr_gt0 ?subr_gt0 ?imf_ltsup//; last first. + exact/mem_set. +by rewrite (le_lt_trans (ler_norm _) _) ?imVfltk//; exact: imageP. +Qed. + +Lemma EVT_min_rV (R : realType) n (f : 'rV[R]_n -> R) (A : set 'rV[R]_n) : + A !=set0 -> + compact A -> + {within A, continuous f} -> exists2 c, c \in A & + forall t, t \in A -> f c <= f t. +Proof. +move=> A0 cA fcont. +have /(EVT_max_rV A0 cA) [c clr fcmax] : {within A, continuous (- f)}. + by move=> ?; apply: continuousN => ?; exact: fcont. +by exists c => // ? /fcmax; rewrite lerN2. +Qed. +Local Close Scope classical_set_scope. + +(* TODO: rm with MCA 1.15.0 *) +Definition Jacobian n m (R : numFieldType) (f : 'rV[R]_n -> 'rV[R]_m) p := + lin1_mx ('d f p). + +Section gradient. + +Definition jacobian1 {R : numFieldType} n (f : 'rV[R]_n -> R) + : 'rV_n -> 'cV_n := + Jacobian (scalar_mx \o f). + +(* NB: not used*) +Definition partial {R : realType} {n : nat} (f : 'rV[R]_n -> R) (a : 'rV[R]_n) i := + lim (h^-1 * (f (a + h *: 'e_i) - f a) @[h --> 0^'])%classic. + +Lemma partial_diff {R : realType} n (f : 'rV[R]_n -> R) (a : 'rV[R]_n) + (i : 'I_n) : + derivable f a 'e_i -> + partial f a i = ('D_'e_i (@scalar_mx _ 1 \o f) a) 0 0. +Proof. +move=> fa1. +rewrite derive_mx ?mxE//=; last first. + exact: derivable_scalar_mx. +rewrite /partial. +under eq_fun do rewrite (addrC a). +by under [in RHS]eq_fun do rewrite !mxE/= !mulr1n. +Qed. + +(* NB: not used *) +Definition err_vec {R : pzRingType} n (i : 'I_n) : 'rV[R]_n := + \row_(j < n) (i == j)%:R. + +Lemma err_vecE {R : pzRingType} n (i : 'I_n) : + err_vec i = 'e_i :> 'rV[R]_n. +Proof. +apply/rowP => j. +by rewrite !mxE eqxx /= eq_sym. +Qed. + +Definition gradient_partial {R : realType} n (f : 'rV[R]_n -> R) (a : 'rV[R]_n) := + \row_(i < n) partial f a i. + +Lemma gradient_partial_sum {R : realType} n (f : 'rV[R]_n -> R) (a : 'rV[R]_n) : + gradient_partial f a = \sum_(i < n) partial f a i *: 'e_i. +Proof. +rewrite /gradient_partial [LHS]row_sum_delta. +by under eq_bigr do rewrite mxE. +Qed. + +(* TODO: generalize with MCA 1.15.0 *) +Lemma gradient_partial_jacobian1 {R : realType} n (f : 'rV[R]_n -> R) + (v : 'rV[R]_n) : differentiable f v -> + gradient_partial f v = (jacobian1 f v)^T. +Proof. +move=> fa; apply/rowP => i. +rewrite /gradient_partial mxE mxE /jacobian mxE -deriveE; last first. + apply: differentiable_comp => //. + exact: differentiable_scalar_mx. +rewrite partial_diff//. +exact/diff_derivable. +Qed. + +End gradient. + +(* spin and matrix/norm properties*) + +Lemma norm_spin {R : rcfType} (u : 'rV[R]_3) (v : 'rV[R]_3) : + (u *m \S(v - u) ^+ 2 *m (u)^T) 0 0 = - norm (u *m \S(v)) ^+ 2. +Proof. +rewrite spinD spinN -tr_spin mulmxA !mulmxDr mulmxDl !mul_tr_spin !addr0. +rewrite -dotmulvv /dotmul trmx_mul. +rewrite mxE [X in _ + X = _](_ : _ = 0) ?addr0; last first. + by rewrite tr_spin -mulmxA mulNmx spin_mul_tr mulmxN mulmx0 oppr0 mxE. +by rewrite tr_spin mulNmx mulmxN [in RHS]mxE opprK mulmxA. +Qed. + +Lemma sqr_spin {R : realType} (u : 'rV[R]_3) (norm_u1 : norm u = 1) : + \S(u) *m \S(u) = u^T *m u - 1%:M. +Proof. +have sqrspin : \S(u) ^+ 2 = u^T *m u - (norm u ^+ 2)%:A by rewrite sqr_spin. +rewrite expr2 norm_u1 expr2 mulr1 in sqrspin. +rewrite mulmxE sqrspin. + apply/matrixP => i j ; rewrite mxE /= [in RHS]mxE /=. + congr (_+_); rewrite mxE mxE /= mul1r. + by rewrite [in RHS]mxE [in RHS]mxE /= -mulNrn mxE -mulNrn. +Qed. + +Definition posdefmx {K : realType} m (M : 'M[K]_m) : Prop := + M \is sym m K /\ forall a, eigenvalue M a -> a > 0. + +(*From mathcomp Require Import spectral. +From mathcomp Require Import complex.*) + +Lemma posdefmxP {R : realType} m (M : 'M[R]_m) : + posdefmx M <-> (forall v : 'rV[R]_m, v != 0 -> (v *m M *m v^T) 0 0 > 0). +Proof. +split. +(* rewrite /posdefmx => -[symM eigen_gt0] v v0. +Local Open Scope complex_scope. + pose M' := map_mx (fun r => r%:C) M. + have : M' \is normalmx. + apply: symmetric_normalmx.*) + move => [Msym eigenM] x x_neq0. + apply/eigenM/eigenvalueP. + exists x => //=. +Admitted. + +Local Open Scope classical_set_scope. + +Section locdef. +Context {R : realType} {T : normedModType R}. +Implicit Types V : T -> R. + +Definition is_Lyapunov_candidate V (D : set T) (x : T) := + x \in D /\ V x = 0 /\ forall z, z \in D -> z != x -> V z > 0. + +(* TODO: useful? mettre dans un fichier wip.v? *) +Definition locnegdef V (x : T) := V x = 0 /\ \forall z \near x^', V z < 0. + +(* TODO: useful? mettre dans un fichier wip.v? *) +(* locally negative semidefinite *) +Definition locnegsemidef V (x : T) := V x = 0 /\ \forall z \near x^', V z <= 0. + +End locdef. + +(* derivation along the trajectory h *) +Definition derive_along {R : realType} {n : nat} + (V : 'rV[R]_n -> R) (f : R -> 'rV[R]_n) + (t : R) : R := + (jacobian1 V (f t))^T *d 'D_1 f t. + +Notation "''D~(' sol , x ) f" := (derive_along f (sol x)). + +Section derive_along. +Context {R : realType} {n : nat}. +Variable sol : 'rV[R]_n -> R -> 'rV[R]_n. +(* sol represents the solutions of a differential equation *) + +Lemma derive_along_derive (V : 'rV[R]_n -> R) (x0 : 'rV[R]_n) (t : R) : + differentiable V (sol x0 t) -> differentiable (sol x0) t -> + 'D~(sol, x0) V t = 'D_1 (V \o sol x0) t. +(* Warning: we are not representing the initial state at t = 0 of the trajectory x + see Khalil p.114 *) +Proof. +move => dif1 dif2. +rewrite /derive_along /=. +rewrite /jacobian1. +rewrite /jacobian. +rewrite /dotmul. +rewrite -trmx_mul. +rewrite mul_rV_lin1. +rewrite mxE. +rewrite -deriveE => //=; last first. + apply: differentiable_comp => //=. + exact/differentiable_scalar_mx. +rewrite derive_mx /=; last first. + apply: derivable_scalar_mx => //=. + exact: diff_derivable. +rewrite mxE. +rewrite [in RHS]deriveE => //=. +rewrite [in RHS]diff_comp => //=. +rewrite -![in RHS]deriveE => //=. +under eq_fun do rewrite mxE /= mulr1n /=. + by []. +exact: differentiable_comp. +Qed. + +Lemma derive_alongMl (f : 'rV_n -> R) (k : R) (x : 'rV[R]_n) t : + differentiable f (sol x t) -> differentiable (sol x) t -> + 'D~(sol, x) (k *: f) t = k *: 'D~(sol, x) f t. +Proof. +move=> dfx dpx. +rewrite derive_along_derive; last 2 first. + exact: differentiable_comp. + by []. +rewrite deriveZ/=; last first => //=. + apply: diff_derivable => //=. + rewrite -fctE. + exact: differentiable_comp. +congr (_ *: _). +by rewrite derive_along_derive. +Qed. + +Lemma derive_alongD (f g : 'rV_n -> R) (x : 'rV_n) t : + differentiable f (sol x t) -> differentiable g (sol x t) -> + differentiable (sol x) t -> + 'D~(sol, x) (f + g) t = 'D~(sol, x) f t + 'D~(sol, x) g t. +Proof. +move=> dfx dgx difp. +rewrite derive_along_derive; last 2 first. + exact: differentiableD. + by []. +rewrite deriveD/=; last 2 first. + apply: diff_derivable => //. + rewrite -fctE . + exact: differentiable_comp. + apply: diff_derivable => //. + rewrite -fctE . + exact: differentiable_comp. +rewrite derive_along_derive; [|by []..]. +by rewrite derive_along_derive. +Qed. + +Lemma derivative_derive_along_eq0 (f : 'rV_n -> R) (x : 'rV[R]_n) (t : R) : + differentiable f (sol x t) -> + 'D_1 (sol x) t = 0 -> 'D~(sol, x) f t = 0. +Proof. +move=> xt1 dtraj. +rewrite /derive_along /jacobian1 /dotmul dotmulP /dotmul -trmx_mul. +by rewrite dtraj mul0mx !mxE. +Qed. + +Lemma derive_along_norm_squared m (f : 'rV[R]_n -> 'rV_m) + (x : 'rV[R]_n) (t : R) : + differentiable f (sol x t) -> + differentiable (sol x) t -> + 'D~(sol, x) (fun y => norm (f y) ^+ 2) t = + (2 *: 'D_1 (f \o sol x) t *m (f (sol x t))^T) 0 0. +Proof. +move=> difff diffphi. +rewrite derive_along_derive => //=; last exact: differentiable_norm_squared. +rewrite fctE derive_norm_squared //=; last first. + by apply: diff_derivable=> //=; exact: differentiable_comp. +by rewrite mulrDl mul1r scalerDl scale1r mulmxDl [in RHS]mxE. +Qed. + +End derive_along. + +(* NB: not used, can be shown to be equivalent to derive_along *) +Definition derive_along_partial {R : realType} n (V : 'rV[R]_n -> R) + (a : R -> 'rV[R]_n) (t : R) : R := + \sum_(i < n) (partial V (a t) i * ('D_1 a t) ``_ i). + +Section ode. +Context {K : realType} {n : nat}. +Let T := 'rV[K]_n. + +Variable phi : (K -> T) -> K -> T. + +Definition is_sol (Init : set T) (x : K -> T) := + [/\ x 0 \in Init, (forall t, derivable x t 1) + & forall t, 'D_1 x t = phi x t]. +End ode. + +Section is_sol. +Context {K : realType} {n : nat}. +Let T := 'rV[K]_n. +Variable phi : (K -> T) -> K -> T. + +Lemma is_sol_subset y0 (A B : set T) (AB : A `<=` B) : + is_sol phi A y0 -> is_sol phi B y0. +Proof. +rewrite /is_sol inE => -[inD0 deriv tilt]; rewrite inE. +by split; [exact: AB|exact: deriv|exact: tilt]. +Qed. + +End is_sol. + +Section state_space. +Context {K : realType} {n : nat}. +Let T := 'rV[K]_n. +Variable phi : (K -> T) -> K -> T. + +Definition state_space (Init : set T) := + [set x | exists f, is_sol phi Init f /\ exists t, x = f t ]. + +End state_space. + +Section equilibrium_point. +Context {K : realType} {n : nat}. +Let T := 'rV[K]_n. +Variable phi : (K -> T) -> K -> T. +Variable Init : set T. + +Definition is_equilibrium_point (x : T) := is_sol phi Init (cst x). + +End equilibrium_point. + +Section equilibrium_point. +Context {K : realType} {n : nat}. +Let T := 'rV[K]_n. +Variable phi : (K -> T) -> K -> T. + +Lemma is_equilibrium_point_subset x (A B : set T) (AB : A `<=` B) : + is_equilibrium_point phi A x -> is_equilibrium_point phi B x. +Proof. +rewrite /is_equilibrium_point /is_sol inE => -[inD0 deriv tilt]. +by rewrite inE; split; [exact: AB|exact: deriv|exact: tilt]. +Qed. + +Definition equilibrium_points Init := + [set p : T | is_equilibrium_point phi Init p ]. + +End equilibrium_point. + +Section stability. +Context {K : realType} {n : nat}. +Let T := 'rV[K]_n. + +Definition is_stable_at (x : T) (z : K -> 'rV[K]_n) := + forall eps, eps > 0 -> exists2 d, d > 0 & + `| z 0 - x | < d -> forall t, t >= 0 -> `| z t - x | < eps. + +Definition is_asymptotically_stable_at (x : T) (z : K -> 'rV[K]_n) : Prop := + exists2 d, d > 0 & `| z 0 - x | < d -> z t @[t --> +oo] --> x. + +End stability. + +Definition existence_uniqueness {K : realType} {n} + (phi : (K -> 'rV[K]_n) -> K -> 'rV[K]_n) (Init : set 'rV[K]_n) + (sol : 'rV[K]_n -> K -> 'rV[K]_n) := + forall y, y 0 \in Init -> is_sol phi Init y <-> sol (y 0) = y. + +Definition initial_condition {K : realType} {n} + (sol : 'rV[K]_n -> K -> 'rV[K]_n) := + forall x0, sol x0 0 = x0. + +Section solutions_unique. +Context {K : realType} {n : nat}. +Variable phi : (K -> 'rV[K]_n) -> K -> 'rV[K]_n. +Variable Init : set 'rV[K]_n. + +Definition solutions_unique := forall (f g : K -> 'rV_n) (x0 : 'rV_n), + is_sol phi Init f -> + is_sol phi Init g -> + f 0 = x0 -> g 0 = x0 -> + f = g. + +End solutions_unique. + +Section solutions_unique_lemmas. +Context {K : realType} {n : nat}. +Variables (phi : (K -> 'rV[K]_n) -> K -> 'rV[K]_n) (Init : set 'rV[K]_n). + +Lemma existence_uniqueness_unique (sol : 'rV[K]_n -> K -> 'rV[K]_n) : + existence_uniqueness phi Init sol -> solutions_unique phi Init. +Proof. +move=> solP f g x0 solf solg f0 g0. +apply/funext => x. +case : (solf) => //=. +move => a0D Da fa. +have := solP _ a0D. +case. +move => /(_ solf). +move => a0a _. +case : (solg) => //=. +move => b0D Db fb. +have := solP _ b0D. +case. +move => /(_ solg). +move => b0b _. +by rewrite -b0b -a0a f0 g0. +Qed. + +Lemma existence_uniqueness_exists (sol : 'rV[K]_n -> K -> 'rV[K]_n) : + existence_uniqueness phi Init sol -> initial_condition sol -> + forall p, p \in Init -> is_sol phi Init (sol p). +Proof. +move=> solP sol0 p pD. +have H := solP (sol p). +apply H. + by rewrite sol0. +by rewrite sol0. +Qed. + +End solutions_unique_lemmas. + +Section sphere. +Context {K : realType} {n : nat}. + +Definition sphere r := [set x : 'rV[K]_n | `|x| = r]. + +Lemma sphere_nonempty r : n != 0 -> 0 < r -> sphere r !=set0. +Proof. +move=> n0. +move=> r_gt0. +rewrite /sphere. +exists (const_mx r). +rewrite /sphere /= /normr/=. +(* TODO: need lemma? *) +rewrite mx_normrE/=. +apply/eqP; rewrite eq_le; apply/andP; split. + apply: bigmax_le. + exact: ltW. + by move=> i _; rewrite mxE gtr0_norm. +under eq_bigr do rewrite mxE gtr0_norm//. +apply/le_bigmax => /=. +destruct n as [|n'] => //. +exact: (ord0, ord0). +Qed. + +Lemma compact_sphere r : compact (sphere r). +Proof. +apply: bounded_closed_compact. + suff : \forall M \near +oo, forall p, sphere r p -> forall i, `|p ord0 i| < M. + rewrite /bounded_set; apply: filter_app; near=> M0. + move=> Kbnd /= p /Kbnd ltpM0. + rewrite /normr/= mx_normrE. + apply/bigmax_leP; split => //= i _. + by rewrite ord1; exact/ltW/ltpM0. + near=> M => v. + rewrite /sphere /= => vr i. + rewrite (@le_lt_trans _ _ r)//. + rewrite -vr [leRHS]/normr/= mx_normE. + under eq_bigr do rewrite ord1. + rewrite -(pair_big xpredT xpredT (fun _ j => `|v ord0 j|%:nng))//=. + rewrite big_ord_recr/= big_ord0. + rewrite max_r; last exact/bigmax_ge_id. + rewrite (bigD1 i)//= -maxE le_max. + by apply/orP; left. + clear v vr i. + by near: M; apply: nbhs_pinfty_gt; rewrite num_real. +pose d := fun x : 'rV[K]_n => `|x| : K. +have contd : continuous d by move=> /= z; exact: norm_continuous. +rewrite [X in closed X](_ : _ = d @^-1` [set r]); last first. + by apply/seteqP; split. +by apply continuous_closedP. +Unshelve. all: by end_near. Qed. + +End sphere. + +Section Lyapunov_stability. +Context {K : realType} {n : nat}. +Variable phi : (K -> 'rV[K]_n.+1) -> K -> 'rV[K]_n.+1. +Variable Init : set 'rV[K]_n.+1. +Variable sol : 'rV[K]_n.+1 -> K -> 'rV[K]_n.+1. +Hypothesis sol0 : initial_condition sol. +Hypothesis solP : existence_uniqueness phi Init sol. +Hypothesis openD : open Init. (* D est forcement un ouvert *) +(* see Cohen Rouhling ITP 2017 Sect 3.2 *) + +Let B r := closed_ball_ (fun x => `|x|) (0 : 'rV[K]_n.+1) r. + +Let BE r : 0 < r -> B r = closed_ball 0 r. +Proof. by move=> r0; rewrite /B -closed_ballE. Qed. + +Variable V : 'rV[K]_n.+1 -> K. +Hypothesis Vdiff : forall t : 'rV[K]_n.+1, differentiable V t. +Hypothesis V'_le0 : forall x, x \in Init -> + forall t, t >= 0 -> 'D~(sol, x) V t <= 0. + +Let V_nincr a b : 0 <= a <= b -> + forall x, x \in Init -> V (sol x b) <= V (sol x a). +Proof. +move=> /andP[a_ge0 ab] x xD. +apply: (@ler0_derive1_le_cc _ (V \o sol x) 0 b) => //=. +- move=> y yb. + apply/diff_derivable/differentiable_comp; last exact: differentiable_comp. + rewrite -derivable1_diffP. + by have [] : is_sol phi Init (sol x) by apply solP; rewrite sol0. +- move=> y yb. + rewrite derive1E -derive_along_derive//. + + apply: V'_le0 => //. + by move : yb; rewrite in_itv/= => /andP[/ltW]. + + rewrite -derivable1_diffP. + by have [] : is_sol phi Init (sol x) by apply solP; rewrite sol0. +- apply: continuous_subspaceT => z. + apply: continuous_comp; last exact: differentiable_continuous. + apply: differentiable_continuous => //. + rewrite -derivable1_diffP. + by have [] : is_sol phi Init (sol x) by apply solP; rewrite sol0. +- by rewrite !in_itv/= lexx (le_trans a_ge0). +- by rewrite in_itv/= ab andbT. +Qed. + +(* khalil theorem 4.1 *) (* TODO: generalize to x != 0 *) +Theorem Lyapunov_stability (x : 'rV[K]_n.+1 := 0) : + is_Lyapunov_candidate V Init x -> + is_equilibrium_point phi Init x -> + is_stable_at x (sol x). +Proof. +move=> VDx eq /= eps eps0/=. +move: VDx => [/= xD [Vx0 DxV]]. +have [r [r_gt0 [r_eps BrD]]] : exists2 r : K, 0 < r & r <= eps /\ B r `<=` Init. + move: xD; rewrite inE => /(open_subball openD)[r0/= r0_gt0] q. + pose r := Num.min (r0 / 2) eps. + have r_gt0 : 0 < r by rewrite /r lt_min eps0 divr_gt0. + exists (r / 2); first by rewrite divr_gt0. + split; first by rewrite /r ler_pdivrMr// ge_min ler_pMr// ler1n orbT. + move=> v Brv; apply (q r) => //. + rewrite /ball/= sub0r normrN gtr0_norm//. + by rewrite /r gt_min ltr_pdivrMr// ltr_pMr// ltr1n. + by move: Brv; rewrite BE ?divr_gt0//; exact: subset_closure_half. +have alpha_min : {x : 'rV[K]_n.+1 | x \in sphere r /\ + forall y, y \in sphere r -> V x <= V y}. + have : {within sphere r, continuous V}. + apply: continuous_subspaceT => /= v. + by apply/differentiable_continuous; exact/Vdiff. + move/(EVT_min_rV (sphere_nonempty _ r_gt0) (@compact_sphere _ _ r)). + have m0 : n.+1 != 0 by []. + move=> /(_ m0). + by move=> /cid2[c sphere_r_c sphere_r_V]; exists c. +pose alpha := V (sval alpha_min). +have alpha_gt0 : 0 < alpha. + have sphere_pos y : y \in sphere r -> 0 < V y. + move=> yr; apply: DxV; last first. + rewrite gtr0_norm_neq0//. + by move: yr; rewrite inE /sphere/= => ->. + apply/mem_set/BrD. + move : yr; rewrite inE /sphere/= => <-. + by rewrite /B /closed_ball_/= sub0r normrN. + rewrite /alpha sphere_pos// /sphere inE/=. + by have [+ _] := svalP alpha_min; rewrite inE. +have [beta /andP[beta_gt0 beta_alpha]] : exists beta, 0 < beta < alpha. + by exists (alpha / 2); rewrite divr_gt0//= ltr_pdivrMr//= ltr_pMr// ltr1n. +set Omega_beta := [set x : 'rV[K]_n.+1 | B r x /\ V x <= beta]. +have Omega_beta_Br : Omega_beta `<=` (B r)°. + move=> y [Bry Vybeta]. + rewrite BE// interior_closed_ballE => //=. + have yr : `|y| <= r by move: Bry; rewrite /B /closed_ball_/= sub0r normrN. + have [{}yr | ry | {}yr] := ltgtP (`|y|) r. + - by rewrite mx_norm_ball /ball_/= sub0r normrN. + - by have := le_lt_trans yr ry; rewrite ltxx. + - have alphaVy : alpha <= V y. + by rewrite /alpha; case: (svalP alpha_min) => [_]; apply; rewrite inE. + by have := lt_le_trans beta_alpha (le_trans alphaVy Vybeta); rewrite ltxx. +(* any trajectory starting in Omega_beta at t = 0 + stays in Omega_beta for all t >= 0 *) +have Df_Omega_beta h : is_sol phi Init h -> + h 0 \in Omega_beta -> forall t, 0 <= t -> h t \in Omega_beta. + move=> sol_phi phi_Omega. + have /= V_nincr_consequence : forall t, 0 <= t -> forall u, 0 <= u <= t -> + 'D~(sol, h 0) V u <= 0 -> + V (sol (h 0) t) <= V (sol (h 0) 0) <= beta. + move=> /= t1 t10 u ut1 Vle0. + apply/andP; split. + move : phi_Omega; rewrite inE /Omega_beta/= => -[Brphi0 Vphi0beta]. + by apply: V_nincr; [rewrite lexx t10|rewrite inE; exact: BrD]. + by rewrite sol0; move: phi_Omega; rewrite inE => -[]. + move=> t t0. + rewrite inE; split; last first. + have : 'D~(sol, h 0) V t <= 0. + by apply: V'_le0 => //; case: sol_phi. + move/V_nincr_consequence => /(_ t). + rewrite lexx t0/= => /(_ isT isT). + have -> : sol (h 0) = h by apply solP => //; case: sol_phi. + by case/andP; exact: le_trans. + move: phi_Omega; rewrite inE /Omega_beta/= /B /closed_ball_/=. + rewrite !sub0r !normrN => -[phi0r Vphi0beta]. + rewrite leNgt; apply/negP => phi_t_r. + have [t1 [t1_ge0 phit1r]] : exists t0, t0 >= 0 /\ `|h t0| = r. + have norm_phi_cont : {within `[0, t]%classic, continuous (normr \o h)}. + apply: continuous_subspaceT => /= y. + apply: continuous_comp; last exact: norm_continuous. + apply: differentiable_continuous => //. + case : (sol_phi) => _ + _ => /(_ y). + by rewrite derivable1_diffP. + have : min `|h 0| `|h t| <= r <= max `|h 0| `|h t|. + by rewrite ge_min phi0r/= le_max (ltW phi_t_r) orbT. + move=> /(IVT t0 norm_phi_cont)[c cI norm_phi_c]. + by exists c; split => //; move/itvP: cI => ->. + have alphaVphit1 : alpha <= V (h t1). + rewrite {alpha_gt0 beta_alpha} /alpha; case: alpha_min => /=. + by move=> y [_ +]; apply; rewrite inE. + have : beta < V (h t1). + by rewrite (lt_le_trans _ alphaVphit1)//; case/andP : beta_alpha. + apply/negP; rewrite -leNgt. + have := V_nincr_consequence t1 t1_ge0 t1. + rewrite lexx t1_ge0 => /(_ isT). + have : 'D~(sol, h 0) V t1 <= 0 by apply: V'_le0 => //; case: sol_phi. + move=> /[swap] /[apply]. + have -> : sol (h 0) = h. + apply solP => //;rewrite inE; apply: BrD => //. + by rewrite /B /closed_ball_/= sub0r normrN. + by move=> /andP[]; exact: le_trans. +have _ : compact Omega_beta. + apply: bounded_closed_compact; rewrite /Omega_beta. + - rewrite /bounded_set /= /globally. + exists r; split => //= t rt v. + rewrite /B /closed_ball_/= sub0r normrN. + by move=> [/le_trans vr _]; rewrite vr// ltW. + - apply: closedI => /=. + by rewrite BE//; exact: closed_ball_closed. + rewrite [X in closed X](_ : _ = V @^-1` [set x | x <= beta]); last first. + by apply/seteqP; split. + apply: closed_comp => //= v _. + apply: continuous_comp; first by []. + exact: differentiable_continuous. +have [d0 d0_gt0 Vbeta] : exists2 d, d > 0 & forall x, `|x| <= d -> V x < beta. + have [d d_gt0 xdV] : exists2 d : K, 0 < d & + forall y, `|y - x| < d -> `|V y - V x| < beta. + have /cvgrPdist_lt /(_ _ beta_gt0) : V x @[x --> nbhs x] --> V x. + exact/differentiable_continuous/Vdiff. + rewrite nearE /= => /nbhs_ballP[d /= d_pos xdV]. + exists d => // y. + move: xdV; rewrite mx_norm_ball /ball_ /= distrC => /[apply]. + by rewrite distrC. + exists (d / 2); first exact: divr_gt0. + move=> v vd; have /(xdV v) : `|v - x| < d. + by rewrite subr0 (le_lt_trans vd)// ltr_pdivrMr // ltr_pMr // ltr1n. + by rewrite Vx0 subr0; apply: le_lt_trans; rewrite ler_normlW. +pose delta := Num.min d0 r. +have delta_gt0 : 0 < delta by rewrite /delta lt_min d0_gt0 r_gt0. +have deltaV y : `|y| <= delta -> V y < beta. + move=> /= ydelta. + have : `|y| <= d0 by rewrite (le_trans ydelta)// /delta ge_min lexx. + exact: Vbeta. +have B_delta_Omega_beta : B delta `<=` Omega_beta. + rewrite /Omega_beta => /= v. + rewrite /B /closed_ball_/= sub0r normrN => vdelta. + split; last exact/ltW/deltaV. + by rewrite (le_trans vdelta)// /delta ge_min lexx orbT. +have _ : (B delta) (sol x 0) -> + forall t, t >= 0 -> sol x t \in Omega_beta -> (B r) (sol x t). + by move => ball0 t1 t1_ge0; rewrite /Omega_beta inE => -[]. +rewrite /x !subr0. +exists delta => // sol0_delta t0 t0_ge0. +rewrite subr0. +have : sol x 0 \in Omega_beta. + rewrite inE; apply: B_delta_Omega_beta. + by rewrite /B /closed_ball_/= sub0r normrN; apply/ltW; exact: sol0_delta. +rewrite inE => -[+ _]. +rewrite /B /closed_ball_/= sub0r normrN => solx0r. +have : (B r)° (sol x t0). + apply: Omega_beta_Br; apply/set_mem. + apply: Df_Omega_beta => //. + by have [] : is_sol phi Init (sol x) by apply solP; rewrite sol0. + rewrite inE; split; first by rewrite /B /closed_ball_/= sub0r normrN. + have : B delta (sol x 0). + by rewrite /closed_ball_; apply: ltW; rewrite sub0r normrN. + by move/B_delta_Omega_beta => []. +rewrite BE//= interior_closed_ballE//=. +rewrite mx_norm_ball /ball_/= sub0r normrN => /lt_le_trans; exact. +Unshelve. all: by end_near. Qed. + +End Lyapunov_stability. + +(* see Appendix VII.A of + https://hal.science/hal-04271257v1/file/benallegue2019tac_October_2022.pdf *) +Section basic_facts. +Variable K : realType. + +Lemma fact212 (v w : 'rV[K]_3) : \S(v) * \S(w) = w^T *m v - (v *m w^T)``_0 *: 1. +Proof. +apply/matrix3P/and9P; split; apply/eqP; rewrite !(mxE,sum3E,spinij,sum1E); Simp.r. + ring. +by rewrite mulrC. +by rewrite mulrC. +by rewrite mulrC. +by rewrite !opprD; ring. +by rewrite mulrC. +by rewrite mulrC. +by rewrite mulrC. +by rewrite !opprD; ring. +Qed. + +Lemma fact213 (v w : 'rV[K]_3) : \S(v) * \S(w) * \S(v) = - (v *m w^T) ``_0 *: \S(v). +Proof. +rewrite fact212 mulrBl -mulmxE -mulmxA; have: v *m \S(v) = 0. + apply: trmx_inj. + by rewrite trmx_mul tr_spin mulNmx spin_mul_tr trmx0 oppr0. +move => ->. +by rewrite mulmx0 sub0r -mul_scalar_mx -mulNmx; congr (_ *m _); rewrite scalemx1 rmorphN. +Qed. + +Lemma fact215 ( v w : 'rV[K]_3) : \S(w *m \S(v)) = \S(w) * \S(v) - \S(v) * \S(w). +Proof. +by rewrite spinE spin_crossmul. +Qed. + +Lemma fact216 (v w : 'rV[K]_3): \S(w *m \S(v)) = v^T *m w - w^T *m v. +Proof. +by rewrite fact215 !fact212 -!/(_ *d _) dotmulC opprB addrA subrK. +Qed. +Lemma fact217 (v : 'rV[K]_3): \S(v) ^+ 3 = - (norm v ^+2) *: \S(v). + exact: spin3. +Qed. + +Lemma fact214 (R : 'M[K]_3) (v_ : seq 'rV[K]_3) : R \is 'SO[K]_3 -> + R^T * (\prod_(i <- v_) \S( i )) * R = (\prod_(i <- v_) \S( i *m R)). +Proof. +move => RSO. +elim/big_ind2 : _ => //. + by rewrite -!mulmxE mulmx1 rotation_tr_mul. +- move => a b c d H1 H2. + rewrite -H1 // -H2 // -!mulmxE -!rotation_inv // !mulmxA -[R^-1 *m b *m R *m R^-1]mulmxA. + rewrite mulmxV; last first. + rewrite unitmxE. + apply: orthogonal_unit. + exact: rotation_sub. + by rewrite -[R^-1 *m b *m 1%:M *m d]mulmxA mul1mx. +- move => i true. + exact: spin_similarity. +Qed. + +End basic_facts. + +Local Notation Left := (@lsubmx _ 1 3 3). +Local Notation Right := (@rsubmx _ 1 3 3). + +(* Modelization of the physical problem *) +Section ya. +(* mesure de l'accelerometre *) +Variable K : realType. +Variable R : K -> 'M[K]_3. (* L/W *) +Variable g0 : K. (*standard gravity constant*) +Let w t := ang_vel R t. (* local frame of the sensor (gyroscope) *) +Definition x2 t : 'rV_3 := 'e_2 *m R t. +Definition y_a x t := - x t *m \S(w t) + 'D_1 x t + g0 *: x2 t. (* world frame *) +Variable p : K -> 'rV[K]_3. +Let v := fun t : K => 'D_1 p t *m R t. +Hypothesis RisSO : forall t, R t \is 'SO[K]_3. + +Lemma y_aE t (derivableR : forall t, derivable R t 1) + (derivablep : forall t, derivable p t 1) + (derivableDp : forall t, derivable ('D_1 p) t 1) : + ('D_1 ('D_1 p) t + g0 *: 'e_2) *m R t = y_a v t. +Proof. +rewrite mulmxDl. +rewrite /y_a/= /= /x2. +congr +%R; last by rewrite scalemxAl. +rewrite -ang_vel_mxE/=; last 2 first. + move=> t0. + by rewrite rotation_sub. + exact : derivableR. +rewrite [in RHS]derive_mulmx => //. +rewrite derive1mx_ang_vel => //; last first. + by move=> t0; rewrite rotation_sub. +rewrite ang_vel_mxE// => //; last first. + by move=> t0; rewrite rotation_sub. +rewrite addrCA. +rewrite -mulmxE. +rewrite -mulNmx. +rewrite [X in _ = _ X]addrC. +rewrite !mulNmx. +by rewrite -mulmxA /= addrN addr0. +Qed. + +End ya. + +Definition S2 {K : realType} := [set x : 'rV[K]_3 | norm x = 1]. + +(* section III.A of [benallegue2023itac] *) +Section state_dynamics. +Variable K : realType. +Variable g0 : K. +Variable R : K -> 'M[K]_3. +Hypothesis RisSO : forall t, R t \is 'SO[K]_3. +Hypothesis derivableR : forall t, derivable R t 1. +Variable v : K -> 'rV[K]_3. +Let x1 t := v t. +Let x2 t : 'rV_3 := ('e_2) *m R t (* eqn (8) *). (* local frame ez ? *) +Let x1_dot t := 'D_1 x1 t. +Let x2_dot t := 'D_1 x2 t. +Let w t := ang_vel R t. + +Lemma x2_S2 t : x2 t \in S2. +Proof. +by rewrite /S2 /x2 inE/= orth_preserves_norm ?normeE ?rotation_sub. +Qed. + +(* not used but could be interesting *) +Lemma dRu t (u : K -> 'rV[K]_3) (T : K -> 'M[K]_3) (w' := ang_vel T) + : 'D_1 (fun t => u t *m T t) t = u t *m T t *m \S(w' t) + 'D_1 u t *m T t. +Proof. +rewrite derive_mulmx; last 2 first. + admit. + admit. +rewrite addrC. +congr(_+_). +rewrite -ang_vel_mxE; last 2 first. + admit. + admit. +rewrite -mulmxA. +rewrite mulmxE. +rewrite -derive1mx_ang_vel; last first. + admit. +by []. +Abort. + +(* eqn (10/11): we write x_1 * S(w) whereas it is - S(w) * x_1 in [benallegue2023itac] *) +Notation y_a := (y_a R g0). +Lemma derive_x1 t : 'D_1 x1 t = x1 t *m \S(w t) + y_a x1 t - g0 *: x2 t. +Proof. +rewrite /y_a/= -addrA addrK. +rewrite /x1. +rewrite addrCA addrA mulNmx /= /w. +by rewrite (addrC(-_)) subrr add0r. +Qed. + + (* eqn (11b): x_2 * S(w) instead of - S(w) * x_2 in [benallegue2023itac] *) +Lemma derive_x2 (t : K) : x2_dot t = x2 t *m \S( w t ). +Proof. +rewrite /w. +rewrite -ang_vel_mxE; last 2 first. + by move=> ?; rewrite rotation_sub. + by []. +rewrite /x2_dot. +rewrite /x2. +have ->: 'D_1 (fun t0 : K => 'e_2 *m (R t0)) t = + 'e_2 *m 'D_1 (fun t => (R t)) t. + move => n /=. + rewrite derive_mulmx//=. + by rewrite derive_cst mul0mx add0r. +rewrite derive1mx_ang_vel /=; last first. + by move=> ?; rewrite rotation_sub. +by rewrite mulmxA. +Qed. + +End state_dynamics. + +(* section III.A in [benallegue2023itac] *) +Section two_steps_first_order_estimator. +Context {K : realType}. +Variables gamma alpha1 : K. +Variable v : K -> 'rV[K]_3. +Variable R : K -> 'M[K]_3. +Hypothesis derivableR : forall t, derivable R t 1. +Let w t := ang_vel R t. +Variable x1_hat : K -> 'rV[K]_3. +Hypothesis derivable_x1_hat : forall t, derivable x1_hat t 1. +Variable x2_hat : K -> 'rV[K]_3. +Variable g0 : K. +Hypotheses g0_eq0 : g0 != 0. +Notation y_a := (y_a R g0 v). +Let x1 t := v t. +Let x2'_hat t := - (alpha1 / g0) *: (x1 t - x1_hat t). (* eqn (12b) *) +(* we write x^_1 * S(w) instead - S(w) * x^_1 in [benallegue2023itac] *) +Hypothesis eqn12a : forall t, + 'D_1 x1_hat t = x1_hat t *m \S(w t) + y_a t - g0 *: x2'_hat t. (* eqn (12a) *) +(* we write x^_2 * S(...) instead of - S(...) * x^_2 + and + gamma instead of - gamma in [benallegue2023itac] *) +Hypothesis eqn12c : forall t, + 'D_1 x2_hat t = x2_hat t *m \S(w t + gamma *: x2'_hat t *m \S(x2_hat t)). (* eqn (12c) *) +Hypothesis x2_hat_S2 : x2_hat 0 \in S2. +Hypothesis x2_hat_derivable : forall t, derivable x2_hat t 1. +Hypothesis v_derivable : forall t, derivable v t 1. +Notation x2 := (x2 R). +(* estimation error *) +Let error1 t := x2 t - x2'_hat t. (* p_1 in [benallegue2023ieeetac] *) +Let error2 t := x2 t - x2_hat t. (* \tilde{x_2} in [benallegue2023ieeetac] *) +Let error1_dot t := 'D_1 error1 t. +Let error2_dot t := 'D_1 error2 t. +Hypothesis RisSO : forall t, R t \is 'SO[K]_3. +(* projection from the local frame to the world frame(?) *) +Let error1_p t := error1 t *m (R t)^T (* z_p_1 in [benallegue2023ieeetac] *). +Let error2_p t := error2 t *m (R t)^T. +Hypothesis norm_x2_hat : forall t, norm (x2_hat t) = 1. + +Let error1E : error1 = fun t => x2 t + (alpha1 / g0) *: (x1 t - x1_hat t). +Proof. +apply/funext => ?. +rewrite /error1 /x2; congr +%R. +by rewrite /x2'_hat scaleNr opprK. +Qed. + +Let error2E t : error2 t = error2_p t *m R t. +Proof. +rewrite /error2 -mulmxA. +by rewrite orthogonal_tr_mul ?rotation_sub// mulmx1. +Qed. + +Let derivable_x2 t : derivable x2 t 1. Proof. exact: derivable_mulmx. Qed. + +Let derivable_x2'_hat t : derivable x2'_hat t 1. +Proof. by apply: derivableZ => /=; exact: derivableB. Qed. + +Let derivable_error1 t : derivable error1 t 1. Proof. exact: derivableB. Qed. + +Let derivable_error2 t : derivable error2 t 1. Proof. exact: derivableB. Qed. + +(* eqn (13a) *) +(* we write p_1 * S(w) instead of - S(w) * p1 in [benallegue2023itac] *) +Lemma derive_error1 t : + 'D_1 error1 t = error1 t *m \S(w t) - alpha1 *: error1 t. +Proof. +simpl in *. +rewrite error1E. +rewrite deriveD//=; last first. + by apply: derivableZ => /=; exact: derivableB. +rewrite deriveZ//=; last exact: derivableB. +rewrite deriveB//. +rewrite !(derive_x2) // -/(x2 t) /=. +rewrite (derive_x1 g0 R) //. +rewrite -/(x2 t) -/(v t) -/(x1 t) -/(w t). +rewrite eqn12a. +transitivity ((x2 t + (alpha1 / g0) *: (x1 t - x1_hat t)) *m \S(w t) + - alpha1 *: error1 t). + transitivity (x2 t *m \S(w t) + (alpha1 / g0) + *: (x1 t *m \S(w t) - g0 *: x2 t - + (x1_hat t *m \S(w t) - g0 *: x2'_hat t))). + do 2 f_equal. + rewrite -3![in LHS]addrA -[in RHS]addrA. + congr +%R. + rewrite opprD addrCA. + rewrite [in RHS]opprB [in RHS]addrA [in RHS]addrC. + congr +%R. + by rewrite opprD addrACA subrr add0r opprK. + rewrite (_ : x1 t *m \S(w t) - g0 *: x2 t - + (x1_hat t *m \S(w t) - g0 *: x2'_hat t) = + (x1 t - x1_hat t) *m \S(w t) - + g0 *: (x2 t - x2'_hat t)); last first. + rewrite mulmxBl scalerDr scalerN opprB addrA [LHS]addrC 2!addrA. + rewrite -addrA; congr +%R. + by rewrite addrC. + by rewrite opprB addrC. + rewrite -/(error1 t). + rewrite scalerDr addrA scalemxAl -mulmxDl scalerN scalerA. + by rewrite divfK. +by rewrite error1E. +Qed. + +(* eqn (13b) *) +(* we write x~_2 * S(w) instead of - S(w) * x~_2 in [benallegue2023itac] *) +Lemma derive_error2 t : + 'D_1 error2 t = error2 t *m \S(w t) + + gamma *: (error2 t - error1 t) *m \S(x2_hat t) ^+ 2. +Proof. +rewrite /error2. +rewrite [in LHS]deriveB//. +rewrite derive_x2//. +rewrite -/(x2 t) -/(w t) -/(error2 t). +rewrite eqn12c. +rewrite spinD. (*spinN*) +rewrite -scalemxAl (spinZ gamma). +rewrite mulmxDr opprD [LHS]addrA. +rewrite [in LHS]addrC addrA (addrC _ (x2 t *m \S(w t))). +rewrite addrAC. +rewrite -mulmxBl -/(error2 t). +simpl in *. +rewrite -[in RHS]opprB. +rewrite scalerN mulNmx. +congr (_ - _). +rewrite -scalemxAr -[RHS]scalemxAl. +congr (_ *: _). +rewrite /error2 /error1. +rewrite opprB addrCA. +rewrite (addrC (x2 t)) addrK. +rewrite mulmxBl. +rewrite [X in _ = X + _](_ : _ = 0) ?add0r; last first. + rewrite mulmxA. + rewrite -(mulmxA(x2_hat t)) sqr_spin //. + rewrite mulmxDr !mulmxA. + rewrite dotmul1 // mul1mx. + by rewrite mulmxN mulmx1 subrr. +rewrite expr2 -mulmxE fact215 -mulmxE -spin_crossmul. +rewrite [in RHS]mulmxA [in RHS]spinE spinE spinE. +by rewrite [LHS](@lieC _ (vec3 K)). +Qed. + +Lemma x2_hatR t : x2_hat t *m (R t)^T = 'e_2 - error2_p t. +Proof. +rewrite /error2_p /error2 mulmxBl opprB addrCA. +rewrite [X in _ + X](_ : _ = 0) ?addr0//. +rewrite /x2 -mulmxA. +by rewrite orthogonal_mul_tr ?rotation_sub// mulmx1 subrr. +Qed. + +(* eqn (14a) *) +Lemma derive_error1_p t : 'D_1 error1_p t = - alpha1 *: error1_p t. +Proof. +rewrite /error1. +rewrite derive_mulmx//=; last by rewrite derivable_trmx. +rewrite derive_error1. +rewrite mulmxBl addrAC. +apply/eqP. +rewrite subr_eq. +rewrite [in eqbRHS]addrC scaleNr scalemxAl subrr /=. +rewrite derive_trmx//. +rewrite derive1mx_ang_vel //; last by move => t0; rewrite rotation_sub. +rewrite ang_vel_mxE //; last by move => t1 ; rewrite rotation_sub. +rewrite -/(w t) -mulmxA -mulmxDr trmx_mul tr_spin. +by rewrite mulNmx subrr mulmx0. +Qed. + +Definition eqn14b_rhs x1 x2 := gamma *: (x2 - x1) *m \S('e_2 - x2) ^+ 2. + +(* eqn (14b) *) +Lemma derive_error2_p t : 'D_1 error2_p t = eqn14b_rhs (error1_p t) (error2_p t). +Proof. +rewrite /eqn14b_rhs. +rewrite [LHS]derive_mulmx//=; last by rewrite derivable_trmx. +simpl in *. +rewrite derive_trmx//. +rewrite derive1mx_ang_vel//=; last by move=> ?; rewrite rotation_sub. +rewrite !ang_vel_mxE//; last by move=> ?; rewrite rotation_sub. +rewrite trmx_mul mulmxA -mulmxDl. +rewrite derive_error2 /=. +rewrite -/(w t) tr_spin mulmxN. +rewrite -!addrA addrC addrA subrK. +rewrite -scalemxAl. +rewrite -!scalemxAl. +congr (_ *: _). +rewrite -x2_hatR. +rewrite -spin_similarity ?rotationV//. +rewrite trmxK. +rewrite [in RHS]expr2 -mulmxE !mulmxA. +rewrite -!mulNmx opprB. +congr (_ *m _ *m _). +rewrite -[in RHS]mulmxA. +rewrite orthogonal_tr_mul ?rotation_sub// mulmx1. +congr (_ *m _). +rewrite -/(error2 _). +rewrite error2E. +rewrite mulmxDl. +congr (_ + _)%R. +by rewrite /error1 -mulmxA orthogonal_tr_mul ?rotation_sub// mulmx1. +Qed. + +End two_steps_first_order_estimator. + +Definition state_space_tilt {K : realType} := + [set x : 'rV[K]_6 | norm ('e_2 - Right x) = 1]. + +Section tilt_eqn. +Context {K : realType}. +Variables alpha1 gamma : K. +Hypothesis gamma_gt0 : 0 < gamma. +Hypothesis alpha1_gt0 : 0 < alpha1. + +Definition tilt_eqn (f : K -> 'rV[K]_6) : K ->'rV[K]_6 := + let error1_p_dot := Left \o f in + let error2_p_dot := Right \o f in + fun t => row_mx + (- alpha1 *: error1_p_dot t) + (eqn14b_rhs gamma (error1_p_dot t) (error2_p_dot t)). + +Definition tilt_eqn_no_time (zp1_z2_point : 'rV[K]_6) : 'rV[K]_6 := + let zp1_point := Left zp1_z2_point in + let z2_point := Right zp1_z2_point in + row_mx (- alpha1 *: zp1_point) + (eqn14b_rhs gamma zp1_point z2_point). + +Lemma tilt_eqnE f t : tilt_eqn f t = tilt_eqn_no_time (f t). +Proof. by []. Qed. + +Lemma tilt_eqn_no_time_lipschitz : exists k, k.-lipschitz_setT tilt_eqn_no_time. +Proof. +near (pinfty_nbhs K) => k. +exists k => -[/= x x0] _. +rewrite /tilt_eqn_no_time. +set fx := row_mx (- alpha1 *: Left x) + (gamma *: (Right x - Left x) *m \S('e_2 - Right x) ^+ 2). +set fy := row_mx (- alpha1 *: Left x0) + (gamma *: (Right x0 - Left x0) *m \S('e_2 - Right x0) ^+ 2). +rewrite /Num.norm/=. +rewrite !mx_normrE. +apply: bigmax_le => /=. + rewrite mulr_ge0//. + apply: le_trans; last first. + exact: (le_bigmax _ _ (ord0, ord0)). + by []. +move=> -[a b] _. +rewrite /=. +rewrite [leRHS](_ : _ = + \big[maxr/0]_ij (maxr alpha1 gamma * `|(x - x0) ij.1 ij.2|)); last first. + admit. +rewrite (le_trans (@ler_peMl _ (maxr alpha1 gamma) _ _ _))//. + admit. +apply: le_trans; last first. + exact: (@le_bigmax _ _ _ 0 + (fun ij => maxr alpha1 gamma * `|(x - x0) ij.1 ij.2|) (a, b)). +rewrite /=. +apply: (@le_trans _ _ + (`|(maxr alpha1 gamma *: fx - maxr alpha1 gamma *: fy) a b|)). + admit. +apply: (@le_trans _ _ + (`|maxr alpha1 gamma *: x a b - maxr alpha1 gamma *: x0 a b|)); last first. +Abort. + +Lemma invariant_state_space_tilt p + (p33 : state_space tilt_eqn state_space_tilt p) : + let y := sval (cid p33) in + let t := sval (cid (svalP (cid p33)).2) in + forall Delta, Delta >= 0 -> + state_space tilt_eqn state_space_tilt (y (t + Delta)). +Proof. +case: p33 => /= x0 sol_y Delta Delta_ge0. +rewrite /state_space/=. +exists x0; split. + by case: sol_y. +case: cid => //= y' y'sol. +case: cid => t'/= pt'. +Abort. + +Lemma state_space_tiltS : + state_space tilt_eqn state_space_tilt `<=` state_space_tilt. +Proof. +- move=> p [y [[y0_init1]] deri y33 ] [t ->]. + rewrite /state_space_tilt. + have : derive1 (fun t=> ('e_2 - Right (y t)) *d (('e_2 - Right (y t)))) = 0. + transitivity (fun t => -2 * (Right(y^`()%classic t) *d ('e_2 - Right (y t)))). + apply/funext => x. + rewrite !derive1E. + rewrite derive_mx; last first. + by auto. + rewrite /dotmul. + under eq_fun do rewrite dotmulP /=. + rewrite dotmulP. + rewrite !mxE /= mulr1n. + under eq_fun do rewrite !mxE /= mulr1n. + rewrite !derive_dotmul/=; last 2 first. + by apply: derivableB => //=; exact : derivable_rsubmx => //=. + by apply: derivableB => //=; exact: derivable_rsubmx => //=. + rewrite /dotmul /=. + rewrite [in RHS]mulr2n [RHS]mulNr [in RHS]mulrDl. + rewrite !mul1r !dotmulP /= dotmulC [in RHS]dotmulC !linearD /=. + rewrite !mxE /= !mulr1n. + have -> : 'D_1 (fun x2 : K => 'e_2 - Right (y x2)) x = - Right ('D_1 y x). + rewrite deriveB /= ; last 2 first. + exact: derivable_cst. + exact: derivable_rsubmx. + rewrite derive_cst /= sub0r; congr (- _). + exact: derive_rsubmx. + rewrite -(_ : 'D_1 y x = + (\matrix_(i, j) 'D_1 (fun t0 : K => y t0 i j) x)); last first. + apply/matrixP => a b; rewrite !mxE. + by rewrite derive_mx//= ?mxE. + ring. + have Rsu t0 : Right (y^`()%classic t0) = + (gamma *: (Right (y t0) - Left (y t0)) *m \S('e_2 - Right (y t0)) ^+ 2). + rewrite derive1E. + rewrite y33. + by rewrite row_mxKr. + apply/funext => t0. + rewrite /dotmul. + transitivity (-2 * (gamma *: (Right (y t0) - + Left (y t0)) *m \S('e_2 - Right (y t0)) ^+ 2 *m + ('e_2 - Right (y t0))^T) 0 0). + by rewrite Rsu. + rewrite !mulmxA. + apply/eqP. + rewrite mulf_eq0 /= oppr_eq0 ?pnatr_eq0 /= -!mulmxA spin_mul_tr. + by rewrite !mulmx0 mxE. + under eq_fun do rewrite dotmulvv /=. (* derivee de la norme est egale a 0 *) + move => h. + have norm_constant : forall t, norm ('e_2 - Right (y t))^+2 = norm ('e_2 - Right (y 0))^+2. + move => t0. + have : forall x0, is_derive x0 (1:K) (fun x : K => norm ('e_2 - Right (y x)) ^+ 2) 0. + move => x0. + apply: DeriveDef. + apply/derivable_norm_squared => //=. + apply/derivableB => //=. + apply/derivable_rsubmx => //. + by rewrite -derive1E h. + rewrite /=. + move/is_derive_0_is_cst. + move/ (_ _ 0). + move => s0. + exact: s0. + suff: norm ('e_2 - Right (y t)) ^+ 2 = 1. + move => /(congr1 Num.sqrt). + rewrite sqrtr1 sqr_sqrtr //. + by rewrite dotmulvv sqr_ge0. + rewrite norm_constant. + move: y0_init1. + rewrite inE /state_space_tilt /= => ->. + by rewrite expr2 mulr1. +Qed. + +Definition point1 : 'rV[K]_6 := 0. +Definition point2 : 'rV[K]_6 := @row_mx _ _ 3 _ 0 (2 *: 'e_2). + +Lemma equilibrium_point1 : is_equilibrium_point tilt_eqn state_space_tilt point1. +Proof. +split => //=. +- rewrite inE /state_space_tilt /point1. + rewrite /=. + by rewrite rsubmx_const /= subr0 normeE. +- move=> t ; rewrite derive_cst /tilt_eqn /point1; apply/eqP. + rewrite eq_sym (@row_mx_eq0 _ 1 3 3); apply/andP. split. + rewrite scaler_eq0; apply/orP; right; apply/eqP/rowP; move => i. + rewrite /=. + by rewrite lsubmx_const. + apply/eqP/rowP; move => i; apply/eqP. + rewrite /eqn14b_rhs. + set N := (X in _ *: X *m _); have : N = 0. + rewrite /N /=; apply /rowP; move => a. + rewrite !mxE. + by rewrite subrr. + by move => n; rewrite n scaler0 mul0mx. +Qed. + +Lemma equilibrium_point2 : is_equilibrium_point tilt_eqn state_space_tilt point2. +Proof. +split => //. +- rewrite inE /state_space_tilt /point2 /=. + rewrite row_mxKr. + rewrite -[X in X - _ ]scale1r. + rewrite -scalerBl normZ normeE mulr1 distrC. + rewrite [X in _ - X](_:1 = 1%:R) //. + by rewrite -natrB //= normr1. +- move => t. rewrite derive_cst; apply/eqP. + rewrite eq_sym (@row_mx_eq0 _ 1 3 3); apply/andP. + set N := (X in _ *: X == 0 /\ _). + have N0 : N = 0. + apply/rowP; move => i; rewrite !mxE; case: splitP. + move => j _; by rewrite mxE. + move => k /= i3k. + have := ltn_ord i. + by rewrite i3k -ltn_subRL subnn. + split. + by rewrite scaler_eq0 N0 eqxx orbT. + rewrite /eqn14b_rhs. + rewrite -scalemxAl scalemx_eq0 gt_eqF//=. + rewrite -[Left point2]/N N0 subr0. + set M := (X in X *m _); rewrite -/M. + have ME : M = 2 *: 'e_2. + apply/rowP => i; rewrite !mxE eqxx/=. + case: splitP => [j ij|j]/=. + have := ltn_ord j. + by rewrite -ij. + move/eqP. + rewrite eqn_add2l => /eqP /ord_inj ->. + by rewrite !mxE eqxx/=. + rewrite ME -scalemxAl scalemx_eq0 pnatr_eq0/=. + rewrite [X in X *: _](_ : _ = 1 + 1)// scalerDl scale1r opprD addrA. + rewrite subrr sub0r spinN sqrrN expr2 -mulmxE mulmxA. + rewrite (_ : 'e_2 *m _ = 0) ?mul0mx//; apply: trmx_inj. + by rewrite trmx_mul trmx0 tr_spin mulNmx spin_mul_tr oppr0. +Qed. + +End tilt_eqn. +Arguments point1 {K}. + +(* technical section, skip on a first reading *) +Section u2. +Context {K : realType}. + +Definition u2 : 'M[K]_(2,2) := \matrix_(i < 2, j < 2) [eta (fun=> 0) with + (0,0) |-> 1, + (0,1) |-> -2^-1, + (1,0) |-> -2^-1, + (1,1) |-> 1] (i, j). + +Lemma u2neq0 : u2 != 0. +Proof. by apply/matrix0Pn; exists 1, 1; rewrite mxE /= oner_neq0. Qed. + +Lemma u2_sym : u2 \is sym 2 K. +Proof. +rewrite /= symE. +apply/eqP/matrixP. +move => i j. +rewrite !mxE/=. +case: ifPn => [/eqP[->{i} ->{j}//]|]. +case: ifPn => [/eqP[->{i} ->{j}//]|]. +case: ifPn => [/eqP[->{i} ->{j}//]|]. +case: ifPn => [/eqP[->{i} ->{j}//]|]. +by move: i j => [[|[|//]]] /= ? [[|[|]]]. +Qed. + +Lemma tr_u2 : \tr u2 = 2. +Proof. by rewrite /u2/= /mxtrace /= sum2E/= !mxE/=. Qed. + +Lemma det_u2 : \det u2 = 3/4. +Proof. by rewrite /u2 det_mx22 /= !mxE /=; field. Qed. + +Lemma posdefmxu2 : posdefmx u2. +Proof. +split; first exact: u2_sym. +move=> a. +move/eigenvalueP => [u] /[swap] u0 H. +have a_eigen : eigenvalue u2 a. + apply/eigenvalueP. + exists u. rewrite /u2. + exact: H. + exact: u0. +have : root (char_poly u2) a. + rewrite -eigenvalue_root_char. + exact : a_eigen. +rewrite char_poly2 tr_u2 det_u2 rootE => a_root . +have char_poly_fact : 'X^2 - 2%:P * 'X + (3/4)%:P = + ('X - (1%:R / 2)%:P) * ('X - (3%:R / 2)%:P) :> {poly K}. + rewrite mulrBr mulrBl -expr2 -!addrA; congr +%R. + rewrite mulrBl opprB addrCA addrC; congr +%R. + by rewrite -[RHS]polyCM; congr (_%:P); field. + rewrite [in RHS]mulrC -opprD -mulrDr mulrC; congr (- (_ * _)). + by rewrite -polyCD; congr (_%:P); by field. +move: a_root. +rewrite char_poly_fact hornerM !hornerXsubC. +by rewrite mulf_eq0 => /orP[|]; rewrite subr_eq0 => /eqP ->; rewrite divr_gt0. +Qed. + +End u2. + +Section V1. +Local Open Scope classical_set_scope. +Context {K : realType}. +Variables alpha1 gamma : K. +Hypothesis alpha1_gt0 : 0 < alpha1. +Hypothesis gamma_gt0 : 0 < gamma. + +Definition V1 (zp1_z2 : 'rV[K]_6) : K := + let zp1 := Left zp1_z2 in + let z2 := Right zp1_z2 in + (norm zp1)^+2 / (2 * alpha1) + (norm z2)^+2 / (2 * gamma). + +Lemma V1_is_Lyapunov_candidate : is_Lyapunov_candidate V1 [set: 'rV_6] point1. +Proof. +rewrite /V1 /point1; split; first by rewrite inE. +split. + by rewrite lsubmx_const rsubmx_const norm0 expr0n/= !mul0r add0r. +move=> /= z_near _ z0. +have /orP[lz0|rz0] : (Left z_near != 0) || (Right z_near != 0). + rewrite -negb_and. + apply: contra z0 => /andP[/eqP l0 /eqP r0]. + rewrite -[eqbLHS](@hsubmxK _ _ 3 3) l0 r0. + apply/eqP/rowP; move => i; rewrite !mxE /=; case: splitP => ? ?; + by rewrite mxE. +- set rsub := Right z_near. + have : norm rsub >= 0 by rewrite norm_ge0. + set lsub := Left z_near. + move=> nor. + have normlsub : norm lsub > 0 by rewrite norm_gt0. + rewrite ltr_pwDl//. + by rewrite divr_gt0 ?exprn_gt0// mulr_gt0. + by rewrite divr_ge0 ?exprn_ge0// mulr_ge0// ltW. +- rewrite ltr_pwDr//. + by rewrite divr_gt0 ?exprn_gt0 ?mulr_gt0// norm_gt0. + by rewrite divr_ge0 ?exprn_ge0 ?norm_ge0// mulr_ge0// ltW. +Unshelve. all: by end_near. Qed. + +Definition V1dot (zp1_z2 : 'rV[K]_6) : K := + let zp1 := Left zp1_z2 in + let z2 := Right zp1_z2 in + - (norm zp1)^+2 + (z2 *m (\S('e_2 - z2))^+2 *m z2^T + - z2 *m (\S('e_2 - z2))^+2 *m zp1^T)``_0. + +End V1. + +Section hurwitz. +Context {K : realType}. + +(* thm 4.6 p136*) +Definition hurwitz n (A : 'M[K]_n) : Prop := + (forall a, eigenvalue A a -> a < 0). + +(* thm 4.7 p139 + fact: it is exponentially stable*) +Definition locally_exponentially_stable_at n (eqn : 'rV[K]_n -> 'rV[K]_n) + (point : 'rV[K]_n) : Prop := + hurwitz (Jacobian eqn point). + +Lemma tilt_eqn_is_locally_exponentially_stable_at_0 alpha1 gamma : + locally_exponentially_stable_at (tilt_eqn_no_time alpha1 gamma) point1. +Proof. +rewrite /locally_exponentially_stable_at /jacobian /hurwitz. +move => a. +move/eigenvalueP => [u] /[swap] u0 H. +have a_eigen : eigenvalue (jacobian (tilt_eqn_no_time alpha1 gamma) point1) a. + apply/eigenvalueP. + exists u. + exact: H. + exact: u0. +have : root (char_poly (jacobian (tilt_eqn_no_time alpha1 gamma) point1)) a. + rewrite -eigenvalue_root_char. + exact : a_eigen. +rewrite /tilt_eqn_no_time /jacobian. +Abort. + +End hurwitz. + +Section tilt_eqn_Lyapunov. +Local Open Scope classical_set_scope. +Context {K : realType}. +Variable alpha1 : K. +Variable gamma : K. +Hypothesis alpha1_gt0 : 0 < alpha1. +Hypothesis gamma_gt0 : 0 < gamma. +Variable R : K -> 'M[K]_3. + +Lemma derive_zp1 (z : K) (sol : K -> 'rV_6) : + is_sol (tilt_eqn alpha1 gamma) state_space_tilt sol -> + 'D_1 (Left \o sol) z = - alpha1 *: Left (sol z). +Proof. +move=> [/= traj0 dtraj]. +move=> /(_ z)/(congr1 Left). +by rewrite row_mxKl => ?; rewrite derive_lsubmx//=. +Qed. + +Lemma derive_z2 (z : K) (sol : K -> 'rV_6) : + is_sol (tilt_eqn alpha1 gamma) state_space_tilt sol -> + 'D_1 (Right \o sol) z = + gamma *: (Right (sol z) - Left (sol z)) *m \S('e_2 - Right (sol z)) ^+ 2. +Proof. +move=> [/= traj0 dtraj]. +by move => /(_ z)/(congr1 Right); rewrite row_mxKr => ?; rewrite derive_rsubmx. +Qed. + +Lemma is_sol_state_space_tilt (sol : K -> 'rV_6) t : + is_sol (tilt_eqn alpha1 gamma) state_space_tilt sol -> + state_space_tilt (sol t). +Proof. +case => sol0 dsol deriv_sol. +apply: (@state_space_tiltS _ alpha1 gamma) => //=. +exists sol; split => //. +by exists t. +Qed. + +Lemma norm_e2z2 (sol : K -> 'rV_6) (z : K) + (z2 := Right \o sol) (zp1 := Left \o sol) (u := 'e_2 - z2 z) : + is_sol (tilt_eqn alpha1 gamma) state_space_tilt sol -> norm u = 1. +Proof. +move=> dtraj. +suff: state_space_tilt (row_mx (zp1 z) (z2 z)). + by rewrite /state_space_tilt/= row_mxKr. +rewrite /zp1 /z2 hsubmxK /=. +exact: is_sol_state_space_tilt. +Qed. + +Lemma angvel_sqr (traj : K -> 'rV_6) (z : K) (z2 := fun r : K => Right (traj r) : 'rV_3) + (w := (z2 z) *m \S('e_2)) (u := 'e_2 - z2 z) : + is_sol (tilt_eqn alpha1 gamma) state_space_tilt traj -> + (w *m \S(u)) *d (w *m \S(u)) = (w *d w) * (u *d u) - (w *d u) ^+ 2. +Proof. +move=> dtraj. +rewrite /dotmul !trmx_mul !tr_spin !mulNmx mulmxN opprK mulmxN !dotmulP. +have key_ortho : (z2 z *m \S('e_2)) *d u = 0. + by rewrite dotmulC; exact/ortho_spin. +rewrite key_ortho expr2. +rewrite [in RHS]mxE. +rewrite [X in _ = - (w *m (\S('e_2) *m (z2 z)^T)) 0 0 * (u *d u)%:M 0 0 - 0%:M 0 0 * X]mxE mulr1n mulr0 subr0/=. +rewrite /u -/w /dotmul. +have Hw_ortho : (w *d u) = 0 by rewrite /u dotmulC ortho_spin. +rewrite !mulmxA dotmulP dotmulvv norm_e2z2 // expr2 mulr1. +rewrite [X in _ = - (w *m \S('e_2) *m (z2 z)^T) 0 0 * X]mxE /= mulr1n /=. +rewrite [X in _ = - (w *m \S('e_2) *m (z2 z)^T) 0 0 * X]mxE /= mulr1. +have wu0 : w *m u^T *m u = 0 by rewrite dotmulP Hw_ortho mul_scalar_mx scale0r. +rewrite -[in LHS](mulmxA w) sqr_spin; last by rewrite -/u norm_e2z2. +rewrite [in LHS]mulmxBr mulmxA wu0 sub0r. +by rewrite 2!mulNmx mulmx1 mxE. +Qed. + +Lemma neg_spin (traj : K -> 'rV_6) (z : K) : + is_sol (tilt_eqn alpha1 gamma) state_space_tilt traj -> + norm (Right (traj z) *m \S('e_2) *m - \S('e_2 - Right (traj z))) = + norm (Right (traj z) *m \S('e_2)). +Proof. +move=> dtraj. +rewrite mulmxN normN. +pose zp1 := fun r => Left (traj r). +pose z2 := fun r => Right (traj r). +set w := (z2 z) *m \S('e_2). +have Gamma1_traj t : state_space_tilt (traj t) by apply/is_sol_state_space_tilt. +rewrite /norm. +rewrite !dotmulvv [RHS]sqrtr_sqr sqrtr_sqr. +have Hnorm_sq : norm (w *m \S('e_2 - Right (traj z))) ^+ 2 = norm w ^+ 2. + rewrite -!dotmulvv angvel_sqr// !dotmulvv norm_e2z2//=. + rewrite -!dotmulvv expr2 !mul1r mulr1. + have -> : w *d ('e_2 - Right (traj z)) = 0 by rewrite dotmulC ortho_spin. + by rewrite expr2 mul0r subr0. +rewrite !normr_norm. +by move/sqr_inj : Hnorm_sq => ->//; rewrite ?nnegrE ?norm_ge0. +Qed. + +Let c1 := 2^-1 / alpha1. +Let c2 := 2^-1 / gamma. + +Lemma V1dotE (z : K) (sol : K -> 'rV_6) + (zp1 := Left \o sol) (z2 := Right \o sol) : + is_sol (tilt_eqn alpha1 gamma) state_space_tilt sol -> + V1dot (sol z) = + c1 *: (2 *: 'D_1 zp1 z *m (Left (sol z))^T) 0 0 + + c2 *: (2 *: 'D_1 z2 z *m (Right (sol z))^T) 0 0. +Proof. +move=> ?. +rewrite -scalemxAl mxE (scalerA c1 2) mulrAC mulVf ?pnatr_eq0// div1r. +rewrite -scalemxAl [in X in _ + X]mxE (scalerA c2 2) mulrAC. +rewrite mulVf// div1r. +rewrite derive_zp1 // -scalemxAl mxE [X in X + _](mulrA (alpha1^-1) (- alpha1)). +rewrite mulrN mulVf ?gt_eqF// mulN1r. +rewrite derive_z2 // -scalemxAl mulmxA -scalemxAl [in X in _ + X]mxE. +rewrite scalerA mulVf ?gt_eqF// scale1r. +rewrite norm_squared /V1dot. +congr +%R. +rewrite -2![in LHS]mulmxA -mulmxBr -mulmxBr -linearB/=. +rewrite -[X in (X *m (_ *m _)) 0 0 = _]trmxK. +rewrite -[X in (_ *m (X *m _)) 0 0 = _]trmxK. +rewrite mulmxA -trmx_mul -trmx_mul [LHS]mxE. +rewrite -(mulmxA (Right (sol z) - (Left (sol z)))) mulmxE -expr2. +rewrite tr_sqr_spin. +by rewrite mulmxA. +Qed. + +Lemma derive_along_V1 (x : 'rV[K]_6) t sol : + is_sol (tilt_eqn alpha1 gamma) state_space_tilt (sol x) -> + (forall t, differentiable (sol x) t) -> + 'D~(sol, x) (V1 alpha1 gamma) t = V1dot (sol x t). +Proof. +rewrite /tilt_eqn => tilt_eqnx dif1. +rewrite /V1 derive_alongD; last 3 first. + apply/differentiableM => //=. + exact/differentiable_norm_squared/differentiable_lsubmx. + apply/differentiableM => //=. + exact/differentiable_norm_squared/differentiable_rsubmx. + exact: dif1. +under [X in derive_along X _ _ + _]eq_fun do rewrite mulrC. +under [X in _ + derive_along X _ _]eq_fun do rewrite mulrC. +rewrite derive_alongMl => //; last first. + exact/differentiable_norm_squared/differentiable_lsubmx. +rewrite derive_alongMl => //; last first. + exact/differentiable_norm_squared/differentiable_rsubmx. +rewrite -fctE /= !derive_along_norm_squared//=. +- rewrite V1dotE. + by rewrite /c1 /c2 !invfM. + rewrite /= in tilt_eqnx. + exact: tilt_eqnx. +- exact/differentiable_lsubmx. +- exact/differentiable_rsubmx. +Qed. + +Definition u1 (sol : K -> 'rV[K]_6) t + (zp1 := Left \o sol) (z2 := Right \o sol) + (w := z2 t *m \S('e_2)) : 'rV[K]_2 := + \row_(i < 2) [eta (fun=> 0) with 0 |-> norm (zp1 t), 1 |-> norm w] i. + +Lemma V1dot_ub (sol : K -> 'rV[K]_6) (zp1 := Left \o sol) (z2 := Right \o sol) : + is_sol (tilt_eqn alpha1 gamma) state_space_tilt sol -> + forall t, + V1dot (sol t) <= (- (u1 sol t) *m u2 *m (u1 sol t)^T) 0 0. +Proof. +move=> dtraj z. +set w := z2 z *m \S('e_2). +rewrite /V1dot. +rewrite mxE norm_spin mxE addrA expr2 mulmxA. +have -> : z2 z *m \S('e_2 - z2 z) = z2 z *m \S('e_2). + by rewrite spinD spinN -tr_spin !mulmxDr !mul_tr_spin !addr0. +rewrite -dotmulNv addrC -mulmxN -expr2. +have cauchy : ((w *m - \S('e_2 - z2 z) *d (zp1 z))%:M : 'rV_1) 0 0 <= + norm(w *m - (\S('e_2 - z2 z))) * norm(zp1 z). + rewrite mxE /= mulr1n (le_trans (ler_norm _)) //. + rewrite -ler_sqr // ; last first. + by rewrite nnegrE // mulr_ge0 ?norm_ge0. + by rewrite exprMn sqr_normr (le_trans (CauchySchwarz_vec _ _)) // !dotmulvv. +apply: (@le_trans _ _ (norm (w *m - \S('e_2 - z2 z)) * norm (zp1 z) + (- norm (zp1 z) ^+ 2 - norm w ^+ 2))). + rewrite lerD2r. + rewrite (le_trans _ (cauchy)) //. + by rewrite mxE eqxx mulr1n. +rewrite neg_spin /u1 /u2 //. +rewrite mxE. +rewrite !sum2E/= ![in leRHS]mxE !sum2E/= ![in leRHS]mxE /=. +rewrite !mulr1 mulrN mulNr opprK mulrDl mulNr -expr2. +rewrite [in leLHS] addrCA -!addrA lerD2l mulrDl (mulNr (norm w)). +rewrite -expr2 !addrA lerD2r !(mulrN , mulNr) opprK -mulrA. +rewrite [in leRHS](mulrC (_ / 2)) (mulrC 2^-1) -mulrDr -splitr. +by rewrite [leRHS]mulrC. +Qed. + +(* TODO: rework of this proof is needed *) +(* NB: unused *) +Lemma derive_along_Left_Right_le0 sol (x : 'rV[K]_6) : + is_sol (tilt_eqn alpha1 gamma) state_space_tilt (sol x) -> + sol x 0 = point1 -> + \forall z \near 0^', + ('D~(sol, x) (fun x => norm (Left x) ^+ 2 / (2 * alpha1)) + + 'D~(sol, x) (fun x => norm (Right x) ^+ 2 / (2 * gamma))) z <= 0. +Proof. +move=> dtraj traj0. +rewrite fctE !invfM /=. +near=> z. +under [X in derive_along X _ _ + _]eq_fun do rewrite mulrC. +under [X in _ + derive_along X _ _]eq_fun do rewrite mulrC. +move: dtraj => [H0 Hderiv Htilt]. +have Hz_derivable : derivable (sol x) z 1. + by apply: Hderiv. +rewrite derive_alongMl; last 2 first. + exact/differentiable_norm_squared/differentiable_lsubmx. + apply derivable1_diffP. + exact: Hderiv. +rewrite derive_alongMl; last 2 first. + exact/differentiable_norm_squared/differentiable_rsubmx. + exact/derivable1_diffP. +rewrite /= !derive_along_norm_squared; last 4 first. + exact/differentiable_rsubmx. + exact/derivable1_diffP. + exact/differentiable_lsubmx. + exact/derivable1_diffP. +rewrite -V1dotE //. +pose zp1 := Left \o sol x. +pose z2 := Right \o sol x. +set w := (z2 z) *m \S('e_2). +pose u1 : 'rV[K]_2 := + \row_(i < 2) [eta (fun=> 0) with 0 |-> norm (zp1 z), 1 |-> norm w] i. +apply: (@le_trans _ _ ((- u1 *m u2 *m u1^T) ``_ 0)). + exact: V1dot_ub. +have := @posdefmxu2 K. +rewrite posdefmxP => def. +have [->|H] := eqVneq u1 0. + by rewrite mulNmx mul0mx mulNmx mul0mx mxE mxE oppr0. +have Hpos := def u1 H. +rewrite -oppr_ge0 -oppr_le0 opprK ltW//. +by rewrite -oppr_gt0 mulNmx !mulNmx mxE opprK Hpos. +Unshelve. all: try by end_near. Qed. + +(* NB: should be completed to prove asymptotic stability *) +Lemma locnegsemidef_derive_alone_V1 sol (x : 'rV[K]_6) : + is_sol (tilt_eqn alpha1 gamma) state_space_tilt (sol x) -> + sol x 0 = point1 -> + locnegsemidef ('D~(sol, x) (V1 alpha1 gamma)) 0. +Proof. +move=> [y033] dy dtraj traj0. +rewrite /locnegsemidef /V1. +rewrite derive_alongD /=; last 3 first. + apply: differentiableM => /=; last exact: differentiable_cst. + exact/differentiable_norm_squared/differentiable_lsubmx. + apply: differentiableM; last exact: differentiable_cst. + exact/differentiable_norm_squared/differentiable_rsubmx. + exact/derivable1_diffP. +split; last first. + near=> z. + rewrite derive_along_derive //; last exact/derivable1_diffP. + admit. (* TODO: lynda *) + admit. (* TODO: lynda *) +under [X in derive_along X _ _ + _]eq_fun do rewrite mulrC. +under [X in _ + derive_along X _ _]eq_fun do rewrite mulrC. +rewrite derive_alongMl; last 2 first. + exact/differentiable_norm_squared/differentiable_lsubmx. + exact/derivable1_diffP. +rewrite derive_alongMl; last 2 first. + exact/differentiable_norm_squared/differentiable_rsubmx. + exact/derivable1_diffP. +rewrite /= !derivative_derive_along_eq0; last 4 first. + exact/differentiable_norm_squared/differentiable_rsubmx. + rewrite [LHS]dtraj /tilt_eqn/= traj0 /point1. + rewrite /eqn14b_rhs. + by rewrite rsubmx_const lsubmx_const !subr0 !scaler0 mul0mx row_mx0. + exact/differentiable_norm_squared/differentiable_lsubmx. + rewrite [LHS]dtraj /tilt_eqn/= traj0 /point1. + rewrite /eqn14b_rhs. + by rewrite rsubmx_const lsubmx_const !subr0 !scaler0 mul0mx row_mx0. +by rewrite scaler0 scaler0 add0r. +Abort. + +Lemma locnegdef_derive_along_V1 sol (x : 'rV[K]_6) + (zp1 := Left \o sol x) (z2 := Right \o sol x) : + is_sol (tilt_eqn alpha1 gamma) state_space_tilt (sol x) -> + (forall t : K, state_space_tilt (sol x t)) -> + sol x 0 = point1 -> + locnegdef ('D~(sol, x) (V1 alpha1 gamma)) 0. +Proof. +move=> solves state y0. +split. + rewrite /is_sol in solves. + rewrite /= derivative_derive_along_eq0 => //; last first. + admit. + rewrite /V1. + apply: differentiableD => //; last first. + apply: differentiableM; last exact: differentiable_cst. + exact/differentiable_norm_squared/differentiable_rsubmx. + apply: differentiableM => //. + exact/differentiable_norm_squared/differentiable_lsubmx. +near=> z0. +rewrite derive_along_V1. +- have V1dot_le := V1dot_ub solves z0 => //. + have := @posdefmxu2 K. + rewrite posdefmxP => def. + set w := z2 z0 *m \S('e_2). + set u1 : 'rV[K]_2 := \row_(i < 2) + [eta (fun=> 0) with 0 |-> norm (zp1 z0), 1 |-> norm w] i. + have Hpos : 0 < (u1 *m u2 *m u1^T) 0 0. + apply: def. + rewrite /u1. + admit. + have Hneg : - (u1 *m u2 *m u1^T) 0 0 < 0 by rewrite oppr_lt0. + rewrite lt_neqAle. + apply/andP; split; last first. + apply: (@le_trans _ _ ((- u1 *m u2 *m u1^T) ``_ 0)). + by []. + have -> : (- u1 *m u2 *m u1^T) 0 0 = - (u1 *m u2 *m u1^T) 0 0. + rewrite !mxE -sumrN. + under [in RHS]eq_bigr do rewrite -mulNr. + by under eq_bigr do rewrite mulNmx mxE. + exact/ltW. + rewrite /V1dot. + rewrite mxE/=. + apply/eqP => Habs. + admit. +- by []. +- move => t. + apply/derivable1_diffP => //. + move : solves; rewrite /is_sol. + by case. +Unshelve. all: by end_near. +Abort. + +(*Definition is_Lyapunov_stable_at {K : realType} {n} + (f : (K -> 'rV[K]_n.+1) -> K -> 'rV[K]_n.+1) + (A : set 'rV[K]_n.+1) + (V : 'rV[K]_n.+1 -> K) + (x0 : 'rV[K]_n.+1) : Prop := + [/\ is_equilibrium_point f x0 A, + is_Lyapunov_candidate V setT x0 & + forall traj1 traj2 : (K -> 'rV[K]_n.+1), + is_sol f traj1 A -> + traj1 0 = x0 -> + locnegsemidef (derive_along V (fun a => traj1) 0 ) 0].*) + +(*Lemma V1_is_Lyapunov_stable : + is_Lyapunov_stable_at (tilt_eqn alpha1 gamma) state_space_tilt (V1 alpha1 gamma) point1. +Proof. +split. +- exact: equilibrium_point1. +- exact: V1_is_Lyapunov_candidate. +(*- by move=> traj1 ? ?; exact: V1_point_is_lnsd. +Qed.*) Abort.*) + +Lemma derive_along_V1_le0 sol (x : 'rV[K]_6) : + is_sol (tilt_eqn alpha1 gamma) state_space_tilt (sol x) -> + (forall t, differentiable (sol x) t) -> + forall t : K, 0 <= t -> + 'D~(sol, x) (V1 alpha1 gamma) t <= 0. +Proof. +move=> solves diff t t0. +rewrite derive_along_V1//. +have Hub := V1dot_ub solves t. +have := @posdefmxu2 K. +rewrite posdefmxP => def. +apply: (le_trans Hub). +have Hquad : let u1 := \row_i [eta fun=> 0 + with 0 |-> norm ((Left \o sol x) t), + 1 |-> norm ((Right \o sol x) t *m \S('e_2))] + i in 0 <= (u1 *m u2 *m u1^T) 0 0. +set u1 := \row_i [eta fun=> 0 + with 0 |-> norm ((Left \o sol x) t), + 1 |-> norm ((Right \o sol x) t *m \S('e_2))] + i. +rewrite /=. +case: (u1 =P 0) => [->|/eqP u1_neq0]. + by rewrite !mul0mx mxE. + apply: ltW. + exact: (def u1 u1_neq0). +rewrite -oppr_ge0. +by rewrite !mulNmx mxE opprK Hquad. +Qed. + +End tilt_eqn_Lyapunov. + +Section equilibrium_zero_stable. +Context {K : realType}. +Variables gamma alpha1 : K. +Hypothesis gamma_gt0 : 0 < gamma. +Hypothesis alpha1_gt0 : 0 < alpha1. +Let phi := tilt_eqn alpha1 gamma. +Variable Init : set 'rV[K]_6. +Variable sol : 'rV[K]_6 -> K -> 'rV[K]_6. +Hypothesis solP : existence_uniqueness phi Init sol. +Hypothesis sol0 : initial_condition sol. + +Hypothesis y0 : 0 \in Init. + +Notation is_sol := (is_sol phi Init). + +Hypothesis y_sol : is_sol (sol 0). +Hypothesis y00 : sol 0 0 = 0. + +Lemma is_equilibrium_subset : + is_equilibrium_point phi state_space_tilt 0 -> + is_equilibrium_point phi Init 0. +Proof. +rewrite /is_equilibrium_point. +rewrite /is_sol/= inE => -[inD0 deriv tilt]. +by split => //; exact/set_mem. +Qed. + +Lemma equilibrium_zero_stable : + open Init -> 0 \in Init -> Init `<=` state_space_tilt -> + is_stable_at point1 (sol 0). +Proof. +move=> openInit Init0 Init_in_state. +apply: (@Lyapunov_stability K _ phi Init sol sol0 solP openInit (V1 alpha1 gamma)). +- move=> t. + apply/differentiableD => //. + apply/differentiableM => //. + exact/differentiable_norm_squared/differentiable_lsubmx. + apply/differentiableM => //. + exact/differentiable_norm_squared/differentiable_rsubmx. +- move=> z zD t t0; apply: derive_along_V1_le0; [by []|by []| | |]. + + apply: (is_sol_subset Init_in_state). + by apply solP; rewrite sol0. + + move=> t1. + rewrite -derivable1_diffP. + have : is_sol (sol z) by apply solP; rewrite sol0. + by case. +- assumption. +- have := V1_is_Lyapunov_candidate alpha1_gt0 gamma_gt0. + rewrite /is_Lyapunov_candidate /point1 => Hpos. + rewrite /V1 lsubmx_const rsubmx_const; split => //. + split. + by rewrite !expr2 !norm0 !mulr0 !mul0r add0r. + move=> z zin z_neq0. + case : Hpos => // _ [_]. + by apply => //; rewrite inE. +- exact/is_equilibrium_subset/equilibrium_point1. +Qed. + +End equilibrium_zero_stable. diff --git a/tilt_analysis.v b/tilt_analysis.v new file mode 100644 index 0000000..841c33c --- /dev/null +++ b/tilt_analysis.v @@ -0,0 +1,353 @@ +From mathcomp Require Import all_ssreflect all_algebra ring. +From mathcomp Require Import boolp classical_sets functions reals. +From mathcomp Require Import topology normedtype derive realfun landau. +From HB Require Import structures. +Require Import ssr_ext euclidean rigid frame skew derive_matrix. + +Set Implicit Arguments. +Unset Strict Implicit. +Unset Printing Implicit Defensive. + +Import Order.TTheory GRing.Theory Num.Def Num.Theory. +Import numFieldNormedType.Exports. +Local Open Scope ring_scope. + (* is already in realfun.v*) +Global Instance is_derive1_sqrt {K : realType} (x : K) : 0 < x -> + is_derive x 1 Num.sqrt (2 * Num.sqrt x)^-1. +Proof. +move=> x_gt0. +have sqrtK : {in Num.pos, cancel (@Num.sqrt K) (fun x => x ^+ 2)}. + by move=> a a0; rewrite sqr_sqrtr// ltW. +rewrite -[x]sqrtK//. +apply: (@is_derive_inverse K (fun x => x ^+ 2)). +- near=> z. + rewrite sqrtr_sqr gtr0_norm//. + have [xz|zx|->] := ltgtP z (Num.sqrt x); last first. + + by rewrite sqrtr_gt0. + + by rewrite (lt_trans _ zx)// sqrtr_gt0. + + move: xz. + near: z. + exists (Num.sqrt x / 2). + rewrite /=. + rewrite mulr_gt0 //. + by rewrite sqrtr_gt0 x_gt0. + rewrite invr_gt0. + by []. + move=> r/=. + move=> /[swap] rx. + rewrite gtr0_norm ?subr_gt0//. + rewrite ltrBlDl. + rewrite -ltrBlDr. + apply: le_lt_trans. + rewrite subr_ge0. + rewrite ger_pMr. + rewrite invf_le1. + by rewrite ler1n. + by []. + by rewrite sqrtr_gt0. +- near=> z. + exact: exprn_continuous. +- rewrite !sqrtK//; split. + exact: exprn_derivable (* TODO: renaming, see https://github.com/math-comp/analysis/issues/1677 *). + by rewrite exp_derive (* TODO: renaming -> issue *) expr1 scaler1. +- by rewrite mulf_neq0 ?pnatr_eq0// gt_eqF// sqrtr_gt0 exprn_gt0// sqrtr_gt0. +Unshelve. all: by end_near. Qed. + +Lemma derive_sqrt {K : realType} (r : K) : 0 < r -> + (Num.sqrt^`())%classic r = (2 * Num.sqrt r)^-1 :> K. +Proof. +move=> r0. +rewrite derive1E. +apply: derive_val. +exact: is_derive1_sqrt. +Qed. + +Lemma differentiable_scalar_mx {R : realType} n (r : R) : + differentiable (@scalar_mx _ n) r. +Proof. +apply/derivable1_diffP/cvg_ex => /=. +exists 1; apply/cvgrPdist_le => /= e e0. +near=> t. +Search (_%:A). +rewrite scaler1 -raddfB/= addrK (scale_scalar_mx _ t^-1) mulVf. + by rewrite subrr normr0 ltW. +by near: t; exact: nbhs_dnbhs_neq. +Unshelve. all: by end_near. Qed. + +(*Lemma derivable_norm_squared {K : realType} n (f : K -> 'rV[K]_n) (x0 : K) : + derivable f x0 1 -> + derivable (fun x => norm (f x) ^+ 2) x0 1. +Proof. +move => dif1. +apply/diff_derivable. +rewrite /=. +under eq_fun do rewrite -dotmulvv dotmulE. +have -> : (fun x : K => \sum_k (f x)``_k * (f x)``_k) = + \sum_k (fun x => (f x)``_k * (f x)``_k ). + apply/funext => x => //=. + by rewrite fct_sumE. +apply/differentiable_sum => k => //=. +apply/differentiableM => //=. + apply/derivable1_diffP. + by apply/derivable_coord => //. +apply/derivable1_diffP. +by apply/derivable_coord => //. +Qed.*) + +(*Lemma derive_norm_squared {K : realType} n (u : K -> 'rV[K]_n) (t : K) : + derivable u t 1 -> + 'D_1 (fun x => norm (u x) ^+ 2) t = + 2 * ('D_1 u t *m (u t)^T)``_0. +Proof. +move=> ut1. +under eq_fun do rewrite -dotmulvv. +rewrite dotmulP mxE /= mulr1n derive_dotmul// dotmulC. +by field. +Qed.*) + +Lemma derivable_sqrt {K: realType} (u : K) : u > 0 -> derivable Num.sqrt (u) 1. +Proof. +move => gt0. +apply: ex_derive. +by apply: (is_derive1_sqrt gt0). +Qed. +(* should go to tilt_robot*) +(*Lemma differentiable_norm {K : realType} m n (f : 'rV[K]_m -> 'rV_n) + (g : K -> 'rV[K]_m) t : + differentiable f (g t) -> f (g t) != 0 -> + differentiable (fun x => norm (f x)) (g t) . +Proof. +move=> fgt fgt0; rewrite /norm -fctE. +apply: differentiable_comp. + exact: differentiable_dotmul. +apply/derivable1_diffP/derivable_sqrt. +by rewrite dotmulvv expr2 mulr_gt0 //= !norm_gt0. +Qed.*) + +(*Lemma differentiable_norm_squared {R : rcfType} m n + (f : 'rV[R]_m -> 'rV[R]_n) (v : 'rV[R]_m) : + differentiable f v -> + differentiable (fun x => norm (f x) ^+ 2) v. +Proof. +move=> dif1. +under eq_fun do rewrite -dotmulvv. +exact: differentiable_dotmul. +Qed.*) +(* this one too *) +(*Lemma derivable_rsubmx {R : realFieldType} {V : normedModType R} {n1 n2} + (f : V -> 'rV[R]_(n1 + n2)) t v : + (forall x, derivable f x v) -> derivable (fun x => rsubmx (f x)) t v. +Proof. +move=> /= => df1. +apply/derivable_mxP => i j/=. +rewrite (ord1 i). +have /cvg_ex[/= r Hr]:= df1 t. +apply/cvg_ex => /=; exists (r``_(rshift n1 j)). +apply/cvgrPdist_le => /= e e0. +move/cvgrPdist_le : Hr => /(_ _ e0). +apply: filterS => x. +apply: le_trans. +rewrite [in leRHS]/Num.Def.normr/= mx_normrE. +apply: le_trans; last first. + exact: (le_bigmax _ _ (ord0, rshift n1 j)). +by rewrite !mxE. +Qed.*) + +(*Lemma derive_rsubmx {R : realFieldType} {V : normedModType R} {n1 n2} + (f : V -> 'rV[R]_(n1 + n2)) t v : + (forall x, derivable f x v) -> + 'D_v (fun x => rsubmx (f x)) t = @rsubmx _ _ n1 _ ('D_v f t). +Proof. +move=> df1; apply/matrixP => i j; rewrite !mxE /=. +rewrite derive_mx ?mxE//=; last exact: derivable_rsubmx. +rewrite derive_mx ?mxE//=; congr ('D_v _ t). +by apply/funext => x; rewrite !mxE. +Qed.*) +(*DONE*) +Lemma differentiable_rsubmx0 {R : realFieldType} {V : normedModType R} {n1 n2} t : + differentiable (@rsubmx R 1 n1 n2) t. +Proof. +have lin_rsubmx : linear (@rsubmx R 1 n1 n2). + move=> a b c. + by rewrite linearD//= linearZ. +pose build_lin_rsubmx := GRing.isLinear.Build _ _ _ _ _ lin_rsubmx. +pose Rsubmx : {linear 'rV[R^o]_(n1 + n2) -> 'rV[R^o]_n2} := HB.pack (@rsubmx R _ _ _) build_lin_rsubmx. +apply: (@linear_differentiable _ _ _ Rsubmx). +move=> /= u A /=. +move/nbhs_ballP=> [e /= e0 eA]. +apply/nbhs_ballP; exists e => //= v [? uv]. +apply: eA; split => //. +(* TODO: lemma *) +move: uv; rewrite /ball/= /mx_ball/ball /= => uv i j. +apply: (le_lt_trans _ (uv i (rshift n1 j))). +by rewrite !mxE. +Qed. +(*DONE*) +Lemma differentiable_rsubmx {R : realFieldType} (V : normedModType R) {n1 n2} + (f : V -> 'rV[R]_(n1 + n2)) t : + (forall x, differentiable f x) -> + differentiable (fun x => rsubmx (f x)) t. +Proof. +move=> /= => df1. +apply: differentiable_comp => //. +exact: differentiable_rsubmx0. +Qed. +(*TODO*) +Lemma derivable_lsubmx {R : realFieldType} {V : normedModType R} {n1 n2} + (f : V -> 'rV[R]_(n1 + n2)) t v : + (forall x, derivable f x v) -> derivable (fun x => lsubmx (f x)) t v. +Proof. +move=> /= => df1. +apply/derivable_mxP => i j/=. +rewrite (ord1 i). +have /cvg_ex[/= l Hl]:= df1 t. +apply/cvg_ex => /=; exists (l``_(lshift n2 j)). +apply/cvgrPdist_le => /= e e0. +move/cvgrPdist_le : Hl => /(_ _ e0). +apply: filterS => x. +apply: le_trans. +rewrite [in leRHS]/Num.Def.normr/= mx_normrE. +apply: le_trans; last first. + exact: (le_bigmax _ _ (ord0, lshift n2 j)). +by rewrite !mxE. +Qed. + +Lemma derive_lsubmx {R : realFieldType} {V : normedModType R} {n1 n2} + (f : V -> 'rV[R]_(n1 + n2)) t v : + (forall x, derivable f x v) -> + 'D_v (fun x => lsubmx (f x)) t = @lsubmx _ _ n1 _ ('D_v f t). +Proof. +move=> df1; apply/matrixP => i j; rewrite !mxE /=. +rewrite derive_mx ?mxE//=; last exact: derivable_lsubmx. +rewrite derive_mx ?mxE//=; congr ('D_v _ t). +by apply/funext => x; rewrite !mxE. +Qed. +(*DONE*) +Lemma differentiable_lsubmx0 {R : realFieldType} {V : normedModType R} {n1 n2} t : + differentiable (@lsubmx R 1 n1 n2) t. +Proof. +have lin_lsubmx : linear (@lsubmx R 1 n1 n2). + move=> a b c. + by rewrite linearD//= linearZ. +pose build_lin_lsubmx := GRing.isLinear.Build _ _ _ _ _ lin_lsubmx. +pose Lsubmx : {linear 'rV[R^o]_(n1 + n2) -> 'rV[R^o]_n1} := + HB.pack (@lsubmx R _ _ _) build_lin_lsubmx. +apply: (@linear_differentiable _ _ _ Lsubmx). +move=> /= u A /=. +move/nbhs_ballP=> [e /= e0 eA]. +apply/nbhs_ballP; exists e => //= v [? uv]. +apply: eA; split => //. +(* TODO: lemma *) +move: uv; rewrite /ball/= /mx_ball/ball /= => uv i j. +apply: (le_lt_trans _ (uv i (lshift n2 j))). +by rewrite !mxE. +Qed. + +(*Global Instance is_diff_lsubmx {R : realFieldType} {V : normedModType R} {n1 n2} + (f df : V -> 'rV[R]_(n1 + n2)) t : + is_diff t f df -> + is_diff t (fun x => lsubmx (f x)) (fun x => lsubmx (df x)). +Proof. +case=> diff_f dfE. +apply: DiffDef. + by apply: differentiable_comp => //; exact: differentiable_lsubmx0. +apply/funext => v. +rewrite -dfE. +rewrite -[LHS]deriveE; last first. + by apply: differentiable_comp => //; exact: differentiable_lsubmx0. +rewrite -[in RHS]deriveE; last first. + by []. +rewrite derive_lsubmx//. +Abort.*) +(*DONE*) +Lemma differentiable_lsubmx {R : realFieldType} (V : normedModType R) {n1 n2} + (f : V -> 'rV[R]_(n1 + n2)) t : + (forall x, differentiable f x) -> + differentiable (fun x => lsubmx (f x)) t. +Proof. +move=> /= => df1. +apply: differentiable_comp => //. +exact: differentiable_lsubmx0. +Qed. + +(*Lemma derivable_row_mx {R : realFieldType} {n1 n2 : nat} + (f : R -> 'rV[R]_n1) (g : R -> 'rV[R]_n2) t v : + (forall x, derivable f x v) -> (forall x, derivable g x v) -> + derivable (fun x : R => row_mx (f x) (g x)) t v. +Proof. +move=> /= fv gv; apply/derivable_mxP => i j. +rewrite (ord1 i)/=. +have /cvg_ex[/= l Hl]:= fv t. +have /cvg_ex[/= k Hk]:= gv t. +apply/cvg_ex => /=; exists (row_mx l k)``_j. +apply/cvgrPdist_le => /= e e0. +move/cvgrPdist_le : Hl => /(_ _ e0) Hl. +move/cvgrPdist_le : Hk => /(_ _ e0) Hk. +move: Hl Hk; apply: filterS2 => x Hl Hk. +rewrite !mxE. +case: fintype.splitP => j1 jj1. + apply: le_trans Hl. + rewrite [in leRHS]/Num.Def.normr/= mx_normrE. + apply: le_trans; last first. + exact: (le_bigmax _ _ (ord0, j1)). + by rewrite !mxE/=. +apply: le_trans Hk. +rewrite [in leRHS]/Num.Def.normr/= mx_normrE. +apply: le_trans; last first. + exact: (le_bigmax _ _ (ord0, j1)). +by rewrite !mxE/=. +Qed.*) + +(* used in derive_along_derive*) +(*TODO*) +Lemma derivable_scalar_mx {R : realFieldType} n (f : 'rV[R]_n -> R) + (a : 'rV[R]_n) v : + derivable f a v -> + derivable (@scalar_mx _ 1 \o f) a v. +Proof. +move=> /cvg_ex[/= l fav]. +apply/cvg_ex => /=. +exists (\col_(i < 1) l). +apply/cvgrPdist_le => /= e e0. +move/cvgrPdist_le : fav => /(_ _ e0). +apply: filterS => x. +apply: le_trans. +rewrite [in leLHS]/Num.Def.normr/= !mx_normrE/=. +apply: bigmax_le => //= -[i j] _. +rewrite !mxE/=. +by rewrite !ord1 eqxx !mulr1n. +Qed. + +(* not used? *) +(*Lemma derive_row_mx {R : realFieldType} {n1 n2 : nat} + (f : R -> 'rV[R]_n1) (g : R -> 'rV[R]_n2) t v : + (forall x : R, derivable f x v) -> + (forall x : R, derivable g x v) -> + 'D_v (fun x => row_mx (f x) (g x)) t = row_mx ('D_v f t) ('D_v g t). +Proof. +move=> fv gv. +apply/matrixP => i j. +rewrite derive_mx ?mxE//=; last first. + by apply: derivable_row_mx; [exact: fv|exact: gv]. +do 2 rewrite derive_mx ?mxE//=. +case: fintype.split_ordP => /= j1 jj1; rewrite !mxE; congr ('D_v _ t). + apply/funext => x; rewrite !mxE. + case: fintype.split_ordP => k jE. + congr (f x i _). + move: jE. + by rewrite jj1 => /(congr1 val) => /= /val_inj. + move: jE. + rewrite jj1 => /(congr1 val)/=. + have /[swap] -> := ltn_ord j1. + by rewrite ltnNge/= leq_addr. +apply/funext => x; rewrite !mxE. +case: fintype.split_ordP => k jE. + move: jE. + rewrite jj1 => /(congr1 val)/=. + have /[swap] <- := ltn_ord k. + by rewrite ltnNge/= leq_addr. +congr (g x i _). +move: jE. +rewrite jj1 => /(congr1 val) => /= /eqP. +by rewrite eqn_add2l => /eqP /val_inj. +Qed.*) diff --git a/tilt_mathcomp.v b/tilt_mathcomp.v new file mode 100644 index 0000000..3eeca90 --- /dev/null +++ b/tilt_mathcomp.v @@ -0,0 +1,47 @@ +From mathcomp Require Import all_ssreflect all_algebra ring. +Require Import ssr_ext euclidean rigid frame skew. + +Set Implicit Arguments. +Unset Strict Implicit. +Unset Printing Implicit Defensive. + +Import Order.TTheory GRing.Theory Num.Def Num.Theory. +Local Open Scope ring_scope. + +(* to appear in MathComp 2.5.0 *) +Lemma lsubmx_const {R : nmodType} (r : R) m n1 n2 : + lsubmx (const_mx r : 'M_(m, n1 + n2)) = const_mx r. +Proof. by apply/matrixP => i j; rewrite !mxE. Qed. + +(* to appear in MathComp 2.5.0 *) +Lemma rsubmx_const {R : nmodType} (r : R) m n1 n2 : + rsubmx (const_mx r : 'M_(m, n1 + n2)) = const_mx r. +Proof. by apply/matrixP => i j; rewrite !mxE. Qed. + +Lemma sqr_inj {R : rcfType} : {in Num.nneg &, injective (fun x : R => x ^+ 2)}. +Proof. +by move=> x y x0 y0 /(congr1 (@Num.sqrt R)); rewrite !sqrtr_sqr! ger0_norm. +Qed. + +(* PR to MathComp *) +(* det_mx22 depend de robot*) +(*Lemma char_poly2 (R : numFieldType) (M : 'M[R]_2) : char_poly M = 'X^2 - (\tr M)%:P * 'X + (\det M)%:P. +Proof. +set P := (RHS). +apply/polyP => -[|[|[|i]]]; last first. +- have := (rwP (leq_sizeP (char_poly M) i.+3)).2. + rewrite size_char_poly => /(_ erefl) /(_ i.+3) => ->//. + rewrite (rwP (leq_sizeP P i.+3)).2//. + rewrite /P -addrA size_addl ?size_polyXn//. + rewrite -mulNr size_MXaddC; case: ifPn => // _. + by rewrite ltnS -polyCN size_polyC; case: (_ == _). +- rewrite /P -[in RHS]addrA [RHS]coefD coefXn/= coefD -mulrN coefCM coefC/= coefN coefX/= oppr0 mulr0 !addr0. + rewrite /char_poly det_mx22//. + rewrite /char_poly_mx !mxE/= mulr1n mulr0n sub0r mulNr opprK sub0r mulrN. + rewrite coefD coefN coefCM coefC/= mulr0 subr0. + by rewrite coefM sum3E !coefE/= !(subr0,mul0r,mulr0,addr0,mulr1,add0r). +- rewrite char_poly_trace//. + by rewrite /P -addrA addrCA !coefD coefN coefCM coefX/= mulr1 coefC/= addr0 coefXn addr0. +- rewrite char_poly_det sqrrN expr1n mul1r. + by rewrite /P !coefD coefC/= coefN coefCM coefX mulr0 subr0 coefXn/= add0r. +Qed.*) diff --git a/tilt_robot.v b/tilt_robot.v new file mode 100644 index 0000000..5e01c68 --- /dev/null +++ b/tilt_robot.v @@ -0,0 +1,405 @@ +From HB Require Import structures. +From mathcomp Require Import all_ssreflect all_algebra ring. +From mathcomp Require Import interval_inference. +From mathcomp Require Import boolp classical_sets functions reals. +From mathcomp Require Import topology normedtype derive. +Require Import ssr_ext euclidean rigid frame skew derive_matrix tilt_analysis. + +Set Implicit Arguments. +Unset Strict Implicit. +Unset Printing Implicit Defensive. + +Import Order.TTheory GRing.Theory Num.Def Num.Theory. +Import numFieldNormedType.Exports. +Local Open Scope ring_scope. + +(* spin and matrix/norm properties *) + +Lemma tr_sqr_spin {R : realFieldType} (u : 'rV[R]_3) : + (\S(u) ^+ 2)^T = \S(u) ^+ 2. +Proof. by apply/esym/eqP; rewrite -symE; exact: sqr_spin_is_sym. Qed. + +Lemma mul_tr_spin {R : comNzRingType} (u : 'rV[R]_3) : u *m \S(u)^T = 0. +Proof. by apply: trmx_inj; rewrite trmx_mul trmxK spin_mul_tr trmx0. Qed. + +Lemma CauchySchwarz_vec {R : rcfType} {n : nat} (a b : 'rV[R]_n) : + (a *d b)^+2 <= (a *d a) * (b *d b). +Proof. +suffices: 0 <= (b *d b) * (a *d a) - (a *d b) ^+ 2. + rewrite subr_ge0. + by rewrite mulrC. +rewrite subr_ge0 expr2 mulrC !dotmulvv /= -expr2. +have [->|hb] := eqVneq b 0. + rewrite dotmulv0 expr0n. + rewrite norm0. + rewrite expr0n mul0r //=. +pose t := (a *d b) / (norm b ^+ 2). +have h : 0 <= norm (a - t *: b) ^+ 2. + rewrite exprn_ge0 //. + by rewrite norm_ge0. +rewrite -(dotmulvv (a - t *: b)) in h. +rewrite dotmulBl dotmulBr dotmulvv in h. +rewrite dotmulvZ in h. +rewrite -dotmulvv in h. +rewrite /t in h. +have h1 : 0 <= a *d a - (a *d b) ^+ 2 / norm b ^+ 2. + move: h. + rewrite dotmulBr dotmulvZ. + rewrite (dotmulC ((a *d b / norm b ^+ 2) *: b) a). + rewrite dotmulvZ dotmulC dotmulvv /t expr2 -!expr2 dotmulZv dotmulvv. + rewrite divfK /=; last first. + by rewrite sqrf_eq0 norm_eq0. + by rewrite subrr subr0 !expr2 mulrAC. +have h2 : 0 <= norm b ^+ 2 * (a *d a) - (a *d b) ^+ 2. + have pos: 0 < norm b ^+ 2. + by rewrite exprn_gt0 // norm_gt0. + suff: norm b ^+ 2 * (a *d a - (a *d b) ^+ 2 / norm b ^+ 2) = + norm b ^+ 2 * (a *d a) - (a *d b) ^+ 2. + move=> eq_step. + rewrite -eq_step. + by apply: mulr_ge0; [rewrite ltW | exact h1]. + rewrite mulrBr. + congr (_ - _)%R. + by rewrite mulrCA divff ?mulr1// sqrf_eq0 norm_eq0. +rewrite -subr_ge0 mulrC. +by rewrite dotmulvv mulrC in h2. +Qed. + +(* not used *) +Lemma young_inequality_vec {R : rcfType} {n : nat} (a b : 'rV[R]_n) : + (a *d b) <= (2^-1 * (norm a)^+2) + (2^-1 * (norm b)^+2). +Proof. +have normage0 : 0 <= (norm a)^+2. + rewrite expr2. + by rewrite mulr_ge0 // norm_ge0. +have normbge0 : 0 <= (norm(b))^+2. + rewrite expr2. + by rewrite mulr_ge0 // norm_ge0. +rewrite -!dotmulvv. +have: 0 <= norm(a - b)^+2. + rewrite expr2. + by rewrite mulr_ge0 // norm_ge0. +rewrite -dotmulvv dotmulD !dotmulvv. +move => h. +rewrite -mulr_natl in h. +have h2 : 2 * (a *d b) <= norm a ^+ 2 + norm (- b) ^+ 2. + rewrite -subr_ge0. + rewrite dotmulvN mulrN in h. + by rewrite addrAC. +rewrite -ler_pdivlMl// in h2. +rewrite -mulrDr. +by rewrite normN in h2. +Qed. + +Lemma dotmulspin1 {R : numFieldType} (u : 'rV[R]_3) (v : 'rV[R]_3) : + (u *m \S(v)) *d v = 0. +Proof. +apply/eqP. +rewrite dotmulC dotmul_trmx -normalvv normal_sym mul_tr_spin normalvv. +by rewrite dotmulv0. +Qed. + +Lemma dotmulspin2 {R : numFieldType} (u : 'rV[R]_3) (v : 'rV[R]_3) : + (u *m \S(v)) *d u = 0. +Proof. +apply/eqP. +rewrite -normalvv normal_sym spinE -normalmN (@lieC _ (vec3 R)) /= opprK. +by rewrite crossmul_normal. +Qed. + +Lemma ortho_spin {R : numFieldType} (u : 'rV[R]_3) (v : 'rV[R]_3) : + (u - v) *d (v *m \S(u))= 0. +Proof. by rewrite dotmulBl dotmulC dotmulspin1 dotmulC dotmulspin2 subr0. Qed. + +Lemma norm_squared {R : rcfType} n (u : 'rV[R]_n) : + (u *m (u)^T) 0 0 = norm u ^+2. +Proof. by rewrite -dotmulvv /dotmul. Qed. + (* TODO in tilt_analysis.v *) +Lemma derivable_rsubmx {R : realFieldType} {V : normedModType R} {n1 n2} + (f : V -> 'rV[R]_(n1 + n2)) t v : + (forall x, derivable f x v) -> derivable (fun x => rsubmx (f x)) t v. +Proof. +move=> /= => df1. +apply/derivable_mxP => i j/=. +rewrite (ord1 i). +have /cvg_ex[/= r Hr]:= df1 t. +apply/cvg_ex => /=; exists (r``_(rshift n1 j)). +apply/cvgrPdist_le => /= e e0. +move/cvgrPdist_le : Hr => /(_ _ e0). +apply: filterS => x. +apply: le_trans. +rewrite [in leRHS]/Num.Def.normr/= mx_normrE. +apply: le_trans; last first. + exact: (le_bigmax _ _ (ord0, rshift n1 j)). +by rewrite !mxE. +Qed. + +Lemma derive_rsubmx {R : realFieldType} {V : normedModType R} {n1 n2} + (f : V -> 'rV[R]_(n1 + n2)) t v : + (forall x, derivable f x v) -> + 'D_v (fun x => rsubmx (f x)) t = @rsubmx _ _ n1 _ ('D_v f t). +Proof. +move=> df1; apply/matrixP => i j; rewrite !mxE /=. +rewrite derive_mx ?mxE//=; last exact: derivable_rsubmx. +rewrite derive_mx ?mxE//=; congr ('D_v _ t). +by apply/funext => x; rewrite !mxE. +Qed. + +Lemma differentiable_rsubmx0 {R : realFieldType} {V : normedModType R} {n1 n2} t : + differentiable (@rsubmx R 1 n1 n2) t. +Proof. +have lin_rsubmx : linear (@rsubmx R 1 n1 n2). + move=> a b c. + by rewrite linearD//= linearZ. +pose build_lin_rsubmx := GRing.isLinear.Build _ _ _ _ _ lin_rsubmx. +pose Rsubmx : {linear 'rV[R^o]_(n1 + n2) -> 'rV[R^o]_n2} := HB.pack (@rsubmx R _ _ _) build_lin_rsubmx. +apply: (@linear_differentiable _ _ _ Rsubmx). +move=> /= u A /=. +move/nbhs_ballP=> [e /= e0 eA]. +apply/nbhs_ballP; exists e => //= v [? uv]. +apply: eA; split => //. +(* TODO: lemma *) +move: uv; rewrite /ball/= /mx_ball/ball /= => uv i j. +apply: (le_lt_trans _ (uv i (rshift n1 j))). +by rewrite !mxE. +Qed. + +Global Instance is_diff_rsubmx {R : realFieldType} {V : normedModType R} {n1 n2} + (f df : V -> 'rV[R]_(n1 + n2)) t : + is_diff t f df -> + is_diff t (fun x => rsubmx (f x)) (fun x => rsubmx (df x)). +Proof. +case=> diff_f dfE. +apply: DiffDef. + by apply: differentiable_comp => //; exact: differentiable_rsubmx0. +apply/funext => v. +rewrite -dfE. +rewrite -[LHS]deriveE; last first. + by apply: differentiable_comp => //; exact: differentiable_rsubmx0. +rewrite -[in RHS]deriveE; last first. + by []. +rewrite derive_rsubmx//. +Abort. + +Lemma differentiable_rsubmx {R : realFieldType} (V : normedModType R) {n1 n2} + (f : V -> 'rV[R]_(n1 + n2)) t : + (forall x, differentiable f x) -> + differentiable (fun x => rsubmx (f x)) t. +Proof. +move=> /= => df1. +apply: differentiable_comp => //. +exact: differentiable_rsubmx0. +Qed. + +Lemma derivable_lsubmx {R : realFieldType} {V : normedModType R} {n1 n2} + (f : V -> 'rV[R]_(n1 + n2)) t v : + (forall x, derivable f x v) -> derivable (fun x => lsubmx (f x)) t v. +Proof. +move=> /= => df1. +apply/derivable_mxP => i j/=. +rewrite (ord1 i). +have /cvg_ex[/= l Hl]:= df1 t. +apply/cvg_ex => /=; exists (l``_(lshift n2 j)). +apply/cvgrPdist_le => /= e e0. +move/cvgrPdist_le : Hl => /(_ _ e0). +apply: filterS => x. +apply: le_trans. +rewrite [in leRHS]/Num.Def.normr/= mx_normrE. +apply: le_trans; last first. + exact: (le_bigmax _ _ (ord0, lshift n2 j)). +by rewrite !mxE. +Qed. + +Lemma derive_lsubmx {R : realFieldType} {V : normedModType R} {n1 n2} + (f : V -> 'rV[R]_(n1 + n2)) t v : + (forall x, derivable f x v) -> + 'D_v (fun x => lsubmx (f x)) t = @lsubmx _ _ n1 _ ('D_v f t). +Proof. +move=> df1; apply/matrixP => i j; rewrite !mxE /=. +rewrite derive_mx ?mxE//=; last exact: derivable_lsubmx. +rewrite derive_mx ?mxE//=; congr ('D_v _ t). +by apply/funext => x; rewrite !mxE. +Qed. + +Lemma differentiable_lsubmx0 {R : realFieldType} {V : normedModType R} {n1 n2} t : + differentiable (@lsubmx R 1 n1 n2) t. +Proof. +have lin_lsubmx : linear (@lsubmx R 1 n1 n2). + move=> a b c. + by rewrite linearD//= linearZ. +pose build_lin_lsubmx := GRing.isLinear.Build _ _ _ _ _ lin_lsubmx. +pose Lsubmx : {linear 'rV[R^o]_(n1 + n2) -> 'rV[R^o]_n1} := + HB.pack (@lsubmx R _ _ _) build_lin_lsubmx. +apply: (@linear_differentiable _ _ _ Lsubmx). +move=> /= u A /=. +move/nbhs_ballP=> [e /= e0 eA]. +apply/nbhs_ballP; exists e => //= v [? uv]. +apply: eA; split => //. +(* TODO: lemma *) +move: uv; rewrite /ball/= /mx_ball/ball /= => uv i j. +apply: (le_lt_trans _ (uv i (lshift n2 j))). +by rewrite !mxE. +Qed. + +(*Global Instance is_diff_lsubmx {R : realFieldType} {V : normedModType R} {n1 n2} + (f df : V -> 'rV[R]_(n1 + n2)) t : + is_diff t f df -> + is_diff t (fun x => lsubmx (f x)) (fun x => lsubmx (df x)). +Proof. +case=> diff_f dfE. +apply: DiffDef. + by apply: differentiable_comp => //; exact: differentiable_lsubmx0. +apply/funext => v. +rewrite -dfE. +rewrite -[LHS]deriveE; last first. + by apply: differentiable_comp => //; exact: differentiable_lsubmx0. +rewrite -[in RHS]deriveE; last first. + by []. +rewrite derive_lsubmx//. +Abort.*) + +Lemma differentiable_lsubmx {R : realFieldType} (V : normedModType R) {n1 n2} + (f : V -> 'rV[R]_(n1 + n2)) t : + (forall x, differentiable f x) -> + differentiable (fun x => lsubmx (f x)) t. +Proof. +move=> /= => df1. +apply: differentiable_comp => //. +exact: differentiable_lsubmx0. +Qed. + +Lemma derivable_row_mx {R : realFieldType} {n1 n2 : nat} + (f : R -> 'rV[R]_n1) (g : R -> 'rV[R]_n2) t v : + (forall x, derivable f x v) -> (forall x, derivable g x v) -> + derivable (fun x : R => row_mx (f x) (g x)) t v. +Proof. +move=> /= fv gv; apply/derivable_mxP => i j. +rewrite (ord1 i)/=. +have /cvg_ex[/= l Hl]:= fv t. +have /cvg_ex[/= k Hk]:= gv t. +apply/cvg_ex => /=; exists (row_mx l k)``_j. +apply/cvgrPdist_le => /= e e0. +move/cvgrPdist_le : Hl => /(_ _ e0) Hl. +move/cvgrPdist_le : Hk => /(_ _ e0) Hk. +move: Hl Hk; apply: filterS2 => x Hl Hk. +rewrite !mxE. +case: fintype.splitP => j1 jj1. + apply: le_trans Hl. + rewrite [in leRHS]/Num.Def.normr/= mx_normrE. + apply: le_trans; last first. + exact: (le_bigmax _ _ (ord0, j1)). + by rewrite !mxE/=. +apply: le_trans Hk. +rewrite [in leRHS]/Num.Def.normr/= mx_normrE. +apply: le_trans; last first. + exact: (le_bigmax _ _ (ord0, j1)). +by rewrite !mxE/=. +Qed. + +Lemma derive_row_mx {R : realFieldType} {n1 n2 : nat} + (f : R -> 'rV[R]_n1) (g : R -> 'rV[R]_n2) t v : + (forall x : R, derivable f x v) -> + (forall x : R, derivable g x v) -> + 'D_v (fun x => row_mx (f x) (g x)) t = row_mx ('D_v f t) ('D_v g t). +Proof. +move=> fv gv. +apply/matrixP => i j. +rewrite derive_mx ?mxE//=; last first. + by apply: derivable_row_mx; [exact: fv|exact: gv]. +do 2 rewrite derive_mx ?mxE//=. +case: fintype.split_ordP => /= j1 jj1; rewrite !mxE; congr ('D_v _ t). + apply/funext => x; rewrite !mxE. + case: fintype.split_ordP => k jE. + congr (f x i _). + move: jE. + by rewrite jj1 => /(congr1 val) => /= /val_inj. + move: jE. + rewrite jj1 => /(congr1 val)/=. + have /[swap] -> := ltn_ord j1. + by rewrite ltnNge/= leq_addr. +apply/funext => x; rewrite !mxE. +case: fintype.split_ordP => k jE. + move: jE. + rewrite jj1 => /(congr1 val)/=. + have /[swap] <- := ltn_ord k. + by rewrite ltnNge/= leq_addr. +congr (g x i _). +move: jE. +rewrite jj1 => /(congr1 val) => /= /eqP. +by rewrite eqn_add2l => /eqP /val_inj. +Qed. + + +Lemma char_poly2 (R : numFieldType) (M : 'M[R]_2) : char_poly M = 'X^2 - (\tr M)%:P * 'X + (\det M)%:P. +Proof. +set P := (RHS). +apply/polyP => -[|[|[|i]]]; last first. +- have := (rwP (leq_sizeP (char_poly M) i.+3)).2. + rewrite size_char_poly => /(_ erefl) /(_ i.+3) => ->//. + rewrite (rwP (leq_sizeP P i.+3)).2//. + rewrite /P -addrA size_addl ?size_polyXn//. + rewrite -mulNr size_MXaddC; case: ifPn => // _. + by rewrite ltnS -polyCN size_polyC; case: (_ == _). +- rewrite /P -[in RHS]addrA [RHS]coefD coefXn/= coefD -mulrN coefCM coefC/= coefN coefX/= oppr0 mulr0 !addr0. + rewrite /char_poly det_mx22//. + rewrite /char_poly_mx !mxE/= mulr1n mulr0n sub0r mulNr opprK sub0r mulrN. + rewrite coefD coefN coefCM coefC/= mulr0 subr0. + by rewrite coefM sum3E !coefE/= !(subr0,mul0r,mulr0,addr0,mulr1,add0r). +- rewrite char_poly_trace//. + by rewrite /P -addrA addrCA !coefD coefN coefCM coefX/= mulr1 coefC/= addr0 coefXn addr0. +- rewrite char_poly_det sqrrN expr1n mul1r. + by rewrite /P !coefD coefC/= coefN coefCM coefX mulr0 subr0 coefXn/= add0r. +Qed. + +Lemma differentiable_norm {K : realType} m n (f : 'rV[K]_m -> 'rV_n) + (g : K -> 'rV[K]_m) t : + differentiable f (g t) -> f (g t) != 0 -> + differentiable (fun x => norm (f x)) (g t) . +Proof. +move=> fgt fgt0; rewrite /norm -fctE. +apply: differentiable_comp. + exact: differentiable_dotmul. +apply/derivable1_diffP/derivable_sqrt. +by rewrite dotmulvv expr2 mulr_gt0 //= !norm_gt0. +Qed. + +Lemma derivable_norm_squared {K : realType} n (f : K -> 'rV[K]_n) (x0 : K) : + derivable f x0 1 -> + derivable (fun x => norm (f x) ^+ 2) x0 1. +Proof. +move => dif1. +apply/diff_derivable. +rewrite /=. +under eq_fun do rewrite -dotmulvv dotmulE. +have -> : (fun x : K => \sum_k (f x)``_k * (f x)``_k) = + \sum_k (fun x => (f x)``_k * (f x)``_k ). + apply/funext => x => //=. + by rewrite fct_sumE. +apply/differentiable_sum => k => //=. +apply/differentiableM => //=. + apply/derivable1_diffP. + by apply/derivable_coord => //. +apply/derivable1_diffP. +by apply/derivable_coord => //. +Qed. + +Lemma derive_norm_squared {K : realType} n (u : K -> 'rV[K]_n) (t : K) : + derivable u t 1 -> + 'D_1 (fun x => norm (u x) ^+ 2) t = + 2 * ('D_1 u t *d u t). +Proof. +move=> ut1. +under eq_fun do rewrite -dotmulvv. +rewrite derive_dotmul// dotmulC. +by field. +Qed. + +Lemma differentiable_norm_squared {R : rcfType} m n + (f : 'rV[R]_m -> 'rV[R]_n) (v : 'rV[R]_m) : + differentiable f v -> + differentiable (fun x => norm (f x) ^+ 2) v. +Proof. +move=> dif1. +under eq_fun do rewrite -dotmulvv. +exact: differentiable_dotmul. +Qed.