-
Notifications
You must be signed in to change notification settings - Fork 2
/
scara.v
175 lines (143 loc) · 5.92 KB
/
scara.v
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
(* coq-robot (c) 2017 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.
From mathcomp Require Import interval reals trigo.
Require Import ssr_ext euclidean skew vec_angle rot frame rigid screw.
From mathcomp.analysis Require Import reals forms.
Require Import extra_trigo.
(******************************************************************************)
(* 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 *)
(* *)
(******************************************************************************)
Set Implicit Arguments.
Unset Strict Implicit.
Unset Printing Implicit Defensive.
Import GRing.Theory.
Import Num.Theory.
Local Open Scope ring_scope.
Section scara.
Variable R : realType.
Let vector := 'rV[R]_3.
Variable theta1 : R.
Variable a1 : R.
Variable theta2 : R.
Variable a2 : R.
Variable d3 : R.
Variable theta4 : R.
Variable d4 : R.
Definition scara_rot := Rz (theta1 + theta2 + theta4).
Definition scara_trans := row3
(a2 * cos (theta2 + theta1) + a1 * cos theta1)
(a2 * sin (theta2 + theta1) + a1 * sin theta1)
(d4 + d3).
(* [spong] p. 81, eqn. 3.49 *)
Section hom_scara.
Definition A10 := hom (Rz theta1) (row3 (a1 * cos theta1) (a1 * sin theta1) 0).
Definition A21 := hom (Rz theta2) (row3 (a2 * cos theta2) (a2 * sin theta2) 0).
Definition A32 := hTz d3.
Definition A43 := hom (Rz theta4) (row3 0 0 d4).
Lemma hom_SCARA_forward : A43 * A32 * A21 * A10 = hom scara_rot scara_trans.
Proof.
rewrite /A43 /A32.
rewrite homM mulr1 mulmx1 row3D. Simp.r.
rewrite /A21.
rewrite homM RzM mulmx_row3_col3 !scale0r !add0r e2row !row3Z row3D. Simp.r.
rewrite homM RzM addrC (addrC _ theta2) addrA; congr hom.
rewrite mulmx_row3_col3 e2row !row3Z !row3D. Simp.r.
by rewrite -!mulrA -mulrBr -cosD -mulrDr (addrC (_ * sin theta1)) -sinD/= add0r.
Qed.
End hom_scara.
Section dh_scara.
Definition B10 := hTx a1 * hRz theta1.
Definition B21 := hTx a2 * hRz theta2.
Definition B32 := hTz d3.
Definition B43 := hTz d4 * hRz theta4.
Lemma dh_SCARA_forward : B43 * B32 * B21 * B10 = hom scara_rot scara_trans.
Proof.
rewrite -hom_SCARA_forward /B10 hTxRz -/A10 /B21 hTxRz -/A21.
by rewrite /B32 -/A32 /B43 /A43 hTzRz.
Qed.
End dh_scara.
(* [murray] example 3.1, p.87 *)
Section screw_scara.
(* initial configuration *)
Definition g0 := hom 1 (row3 (a1 + a2) 0 d4).
Definition w1 : vector := 'e_2%:R.
Definition w2 : vector := 'e_2%:R.
Definition v3 : vector := 'e_2%:R.
Definition w4 : vector := 'e_2%:R.
(* axis points *)
Definition q1 : vector := 0.
Definition q2 : vector := row3 a1 0 0.
Definition q4 : vector := row3 (a1 + a2) 0 0.
Definition t1 := rjoint_twist w1 q1.
Definition t2 := rjoint_twist w2 q2.
Definition t3 := pjoint_twist v3.
Definition t4 := rjoint_twist w4 q4.
Definition g := g0 * `e$(theta4, t4) *
`e$(d3, t3) * `e$(theta2, t2) * `e$(theta1, t1).
Lemma S1 : `e$(theta1, t1) = hRz theta1.
Proof.
rewrite /t1 /rjoint_twist linearNl /= linear0r oppr0 etwist_Rz; last first.
by rewrite -norm_eq0 normeE oner_eq0.
by rewrite -Rz_eskew.
Qed.
(* TODO: generalize *)
Lemma point_axis_twist (d : R) :
\pt( axis \T((- 'e_2%:R *v row3 d 0 0), 'e_2%:R) ) = row3 d 0 0.
Proof.
rewrite {1}/axis ang_tcoorE (negbTE (norm1_neq0 (normeE _ _))) /=.
rewrite normeE expr1n invr1 scale1r lin_tcoorE linearNl linearNr /=.
rewrite double_crossmul dotmulvv normeE expr1n scale1r /w2 /q2 e2row.
rewrite dotmulE sum3E !mxE /=. by Simp.r.
Qed.
Lemma S2_helper th d :
`e$(th, \T(- w2 *v row3 d 0 0, w2)) =
hom (Rz th) (row3 (d * (1 - cos th)) (- d * sin th) 0).
Proof.
rewrite etwistE (negbTE (norm1_neq0 (normeE _ _))).
rewrite pitch_perp ?normeE // mulr0 scale0r add0r.
rewrite point_axis_twist -Rz_eskew; congr hom.
rewrite mulmxBr mulmx1 mulmx_row3_col3 !scale0r !addr0 row3Z row3N row3D.
Simp.r.
by rewrite mulrBr mulr1.
Qed.
Lemma S2 : `e$(theta2, t2) =
hom (Rz theta2) (row3 (a1 * (1 - cos theta2)) (- a1 * sin theta2) 0).
Proof. by rewrite /t2 S2_helper. Qed.
Lemma S3 : `e$(d3, t3) = hTz d3.
Proof.
rewrite etwistE eqxx eskew_v0.
rewrite /hTz /v3 e2row row3Z. by Simp.r.
Qed.
Lemma S4 : `e$(theta4, t4) = hom (Rz theta4)
(row3 ((a1 + a2) * (1 - cos theta4)) (- (a1 + a2) * sin theta4) 0).
Proof. by rewrite /t4 /q4 S2_helper. Qed.
Lemma screw_SCARA_forward : g = hom scara_rot scara_trans.
Proof.
rewrite /g /g0 S1 S2 S3 S4 homM mul1r.
rewrite mulmx_row3_col3 scale0r addr0 e2row 2!row3Z 2!row3D. Simp.r.
rewrite mulrBr mulr1 addrCA subrr addr0 subrr.
rewrite homM mulr1 mulmx1 row3D. Simp.r.
rewrite homM RzM mulmx_row3_col3 e2row !row3Z !row3D. Simp.r.
rewrite (addrC _ (a1 * (1 - cos theta2))) mulrBr mulr1 mulrDl !addrA subrK.
rewrite mulrDl addrAC subrr add0r.
rewrite homM RzM mulmx_row3_col3 e2row !row3Z !row3D. Simp.r.
rewrite addrC (addrC theta4) addrA; congr hom.
rewrite /scara_trans/= !add0r; congr row3.
- by rewrite mulrDl -addrA -!mulrA -mulrBr -cosD addrC.
- by rewrite mulrDl -addrA -!mulrA -mulrDr (addrC (cos theta2 * _)) -sinD addrC.
Qed.
End screw_scara.
End scara.