release: v0.1 initial version
This commit is contained in:
lovez
554
MJDATA.TXT
Normal file
554
MJDATA.TXT
Normal file
@@ -0,0 +1,554 @@
|
|||||||
|
SIZES
|
||||||
|
narena 13631488
|
||||||
|
nbuffer 13952
|
||||||
|
nplugin 0
|
||||||
|
maxuse_stack 2388
|
||||||
|
maxuse_arena 29552
|
||||||
|
maxuse_con 1
|
||||||
|
maxuse_efc 4
|
||||||
|
solver_nisland 1
|
||||||
|
ne 0
|
||||||
|
nf 0
|
||||||
|
nl 0
|
||||||
|
nefc 0
|
||||||
|
nnzJ 0
|
||||||
|
ncon 0
|
||||||
|
nisland 0
|
||||||
|
threadpool 0
|
||||||
|
|
||||||
|
TIMER
|
||||||
|
0: duration = 0.0029 number = 1
|
||||||
|
1: duration = 0.0055 number = 2
|
||||||
|
2: duration = 0 number = 0
|
||||||
|
3: duration = 0.0039 number = 2
|
||||||
|
4: duration = 0.0009 number = 2
|
||||||
|
5: duration = 0.00017 number = 2
|
||||||
|
6: duration = 6e-05 number = 2
|
||||||
|
7: duration = 8e-05 number = 1
|
||||||
|
8: duration = 0.001 number = 2
|
||||||
|
9: duration = 0.00032 number = 2
|
||||||
|
10: duration = 0.0018 number = 2
|
||||||
|
11: duration = 0.00029 number = 2
|
||||||
|
12: duration = 6e-05 number = 2
|
||||||
|
13: duration = 0.0014 number = 2
|
||||||
|
14: duration = 0.00031 number = 2
|
||||||
|
|
||||||
|
ENERGY = 0 0
|
||||||
|
|
||||||
|
TIME = 3.7e+04
|
||||||
|
|
||||||
|
QPOS
|
||||||
|
0.6
|
||||||
|
0.51
|
||||||
|
-27
|
||||||
|
-99
|
||||||
|
-99
|
||||||
|
-7.3e+02
|
||||||
|
1
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
|
||||||
|
QVEL
|
||||||
|
-1.1e-322
|
||||||
|
-1.1e-322
|
||||||
|
-2e-323
|
||||||
|
0
|
||||||
|
0
|
||||||
|
-3.7e+05
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
|
||||||
|
QACC_WARMSTART
|
||||||
|
1.2e-321
|
||||||
|
1.2e-321
|
||||||
|
8.8e-322
|
||||||
|
0
|
||||||
|
0
|
||||||
|
-9.8
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
|
||||||
|
CTRL
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
|
||||||
|
QFRC_APPLIED
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
|
||||||
|
XFRC_APPLIED
|
||||||
|
0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0
|
||||||
|
|
||||||
|
QACC
|
||||||
|
1.2e-321
|
||||||
|
1.2e-321
|
||||||
|
8.8e-322
|
||||||
|
0
|
||||||
|
0
|
||||||
|
-9.8
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
|
||||||
|
XPOS
|
||||||
|
0 0 0
|
||||||
|
0.6 0.51 0.35
|
||||||
|
-99 -99 0.1
|
||||||
|
1.5 0 0.25
|
||||||
|
-1.2 1 0.2
|
||||||
|
0.8 -1 0.15
|
||||||
|
-0.5 -0.8 0.2
|
||||||
|
1 0.8 0.25
|
||||||
|
|
||||||
|
XQUAT
|
||||||
|
1 0 0 0
|
||||||
|
0.74 0 0 -0.67
|
||||||
|
1 0 0 0
|
||||||
|
1 0 0 0
|
||||||
|
1 0 0 0
|
||||||
|
1 0 0 0
|
||||||
|
1 0 0 0
|
||||||
|
1 0 0 0
|
||||||
|
|
||||||
|
XMAT
|
||||||
|
1 0 0 0 1 0 0 0 1
|
||||||
|
0.096 1 0 -1 0.096 -0 -0 0 1
|
||||||
|
1 0 0 0 1 0 0 0 1
|
||||||
|
1 0 0 0 1 0 0 0 1
|
||||||
|
1 0 0 0 1 0 0 0 1
|
||||||
|
1 0 0 0 1 0 0 0 1
|
||||||
|
1 0 0 0 1 0 0 0 1
|
||||||
|
1 0 0 0 1 0 0 0 1
|
||||||
|
|
||||||
|
XIPOS
|
||||||
|
0 0 0
|
||||||
|
0.61 0.48 0.36
|
||||||
|
-99 -99 0.1
|
||||||
|
1.5 0 0.25
|
||||||
|
-1.2 1 0.2
|
||||||
|
0.8 -1 0.15
|
||||||
|
-0.5 -0.8 0.2
|
||||||
|
1 0.8 0.25
|
||||||
|
|
||||||
|
XIMAT
|
||||||
|
1 0 0 0 1 0 0 0 1
|
||||||
|
0.024 -1 0.049 -0.021 -0.05 -1 1 0.023 -0.022
|
||||||
|
1 0 0 0 1 0 0 0 1
|
||||||
|
1 0 0 0 1 0 0 0 1
|
||||||
|
1 0 0 0 1 0 0 0 1
|
||||||
|
1 0 0 0 1 0 0 0 1
|
||||||
|
1 0 0 0 1 0 0 0 1
|
||||||
|
1 0 0 0 1 0 0 0 1
|
||||||
|
|
||||||
|
XANCHOR
|
||||||
|
0 0 0.35
|
||||||
|
0.6 0 0.35
|
||||||
|
0.6 0.51 0.35
|
||||||
|
-99 -99 0.1
|
||||||
|
|
||||||
|
XAXIS
|
||||||
|
1 0 0
|
||||||
|
0 1 0
|
||||||
|
0 0 1
|
||||||
|
0 0 1
|
||||||
|
|
||||||
|
GEOM_XPOS
|
||||||
|
0 0 0
|
||||||
|
0.6 0.51 0.35
|
||||||
|
0.62 0.32 0.35
|
||||||
|
0.73 0.52 0.35
|
||||||
|
0.6 0.51 0.45
|
||||||
|
-99 -99 0.1
|
||||||
|
1.5 0 0.25
|
||||||
|
-1.2 1 0.2
|
||||||
|
0.8 -1 0.15
|
||||||
|
-0.5 -0.8 0.2
|
||||||
|
1 0.8 0.25
|
||||||
|
|
||||||
|
GEOM_XMAT
|
||||||
|
1 0 0 0 1 0 0 0 1
|
||||||
|
0.096 1 0 -1 0.096 -0 -0 0 1
|
||||||
|
-2.8e-17 1 -0.096 -5.6e-17 0.096 1 1 5.6e-17 5.6e-17
|
||||||
|
0.096 5.6e-17 -1 -1 -5.6e-17 -0.096 -5.6e-17 1 5.6e-17
|
||||||
|
0.096 -1 -1.2e-16 -1 -0.096 -1.2e-17 0 1.2e-16 -1
|
||||||
|
1 0 0 0 1 0 0 0 1
|
||||||
|
1 0 0 0 1 0 0 0 1
|
||||||
|
1 0 0 0 1 0 0 0 1
|
||||||
|
1 0 0 0 1 0 0 0 1
|
||||||
|
1 0 0 0 1 0 0 0 1
|
||||||
|
1 0 0 0 1 0 0 0 1
|
||||||
|
|
||||||
|
SITE_XPOS
|
||||||
|
0.63 0.26 0.35
|
||||||
|
|
||||||
|
SITE_XMAT
|
||||||
|
0.096 1 0 -1 0.096 -0 -0 0 1
|
||||||
|
|
||||||
|
CAM_XPOS
|
||||||
|
0 0 5
|
||||||
|
|
||||||
|
CAM_XMAT
|
||||||
|
1 0 0 0 1 0 0 0 1
|
||||||
|
|
||||||
|
SUBTREE_COM
|
||||||
|
-1.4 -2.1 0.23
|
||||||
|
0.61 0.48 0.36
|
||||||
|
-99 -99 0.1
|
||||||
|
1.5 0 0.25
|
||||||
|
-1.2 1 0.2
|
||||||
|
0.8 -1 0.15
|
||||||
|
-0.5 -0.8 0.2
|
||||||
|
1 0.8 0.25
|
||||||
|
|
||||||
|
CDOF
|
||||||
|
0 0 0 1 0 0
|
||||||
|
0 0 0 0 1 0
|
||||||
|
0 0 1 0.037 0.012 -0
|
||||||
|
0 0 0 1 0 0
|
||||||
|
0 0 0 0 1 0
|
||||||
|
0 0 0 0 0 1
|
||||||
|
1 0 0 0 0 0
|
||||||
|
0 1 0 0 0 0
|
||||||
|
0 0 1 0 0 0
|
||||||
|
|
||||||
|
CINERT
|
||||||
|
0 0 0 0 0 0 0 0 0 0
|
||||||
|
0.15 0.1 0.17 0.0025 0.00047 -0.0014 0 0 0 7.3
|
||||||
|
0.042 0.042 0.042 0 0 0 0 0 0 7.2
|
||||||
|
9.2 9.2 11 0 0 0 0 0 0 1.8e+02
|
||||||
|
0.54 0.54 0.32 0 0 0 0 0 0 28
|
||||||
|
0.13 0.13 0.13 0 0 0 0 0 -3.9e-16 14
|
||||||
|
3.4 3.4 4.2 0 0 0 0 0 0 1e+02
|
||||||
|
0.55 0.55 0.16 0 0 0 0 0 0 23
|
||||||
|
|
||||||
|
ACTUATOR_LENGTH
|
||||||
|
0.6
|
||||||
|
0.51
|
||||||
|
-27
|
||||||
|
|
||||||
|
ACTUATOR_MOMENT
|
||||||
|
1 0 0 0 0 0 0 0 0
|
||||||
|
0 1 0 0 0 0 0 0 0
|
||||||
|
0 0 1 0 0 0 0 0 0
|
||||||
|
|
||||||
|
CRB
|
||||||
|
0 0 0 0 0 0 0 0 0 0
|
||||||
|
0.15 0.1 0.17 0.0025 0.00047 -0.0014 0 0 0 7.3
|
||||||
|
0.042 0.042 0.042 0 0 0 0 0 0 7.2
|
||||||
|
9.2 9.2 11 0 0 0 0 0 0 1.8e+02
|
||||||
|
0.54 0.54 0.32 0 0 0 0 0 0 28
|
||||||
|
0.13 0.13 0.13 0 0 0 0 0 -3.9e-16 14
|
||||||
|
3.4 3.4 4.2 0 0 0 0 0 0 1e+02
|
||||||
|
0.55 0.55 0.16 0 0 0 0 0 0 23
|
||||||
|
|
||||||
|
QM
|
||||||
|
7.3 0 0.27 0 0 0 0 0 0
|
||||||
|
0 7.3 0.088 0 0 0 0 0 0
|
||||||
|
0.27 0.088 0.18 0 0 0 0 0 0
|
||||||
|
0 0 0 7.2 0 0 0 0 0
|
||||||
|
0 0 0 0 7.2 0 0 0 0
|
||||||
|
0 0 0 0 0 7.2 0 0 0
|
||||||
|
0 0 0 0 0 0 0.042 0 0
|
||||||
|
0 0 0 0 0 0 0 0.042 0
|
||||||
|
0 0 0 0 0 0 0 0 0.042
|
||||||
|
|
||||||
|
QLD
|
||||||
|
6.9 -0.018 1.5 0 0 0 0 0 0
|
||||||
|
-0.018 7.3 0.48 0 0 0 0 0 0
|
||||||
|
1.5 0.48 0.18 0 0 0 0 0 0
|
||||||
|
0 0 0 7.2 0 0 0 0 0
|
||||||
|
0 0 0 0 7.2 0 0 0 0
|
||||||
|
0 0 0 0 0 7.2 0 0 0
|
||||||
|
0 0 0 0 0 0 0.042 0 0
|
||||||
|
0 0 0 0 0 0 0 0.042 0
|
||||||
|
0 0 0 0 0 0 0 0 0.042
|
||||||
|
|
||||||
|
QLDIAGINV
|
||||||
|
0.14
|
||||||
|
0.14
|
||||||
|
5.5
|
||||||
|
0.14
|
||||||
|
0.14
|
||||||
|
0.14
|
||||||
|
24
|
||||||
|
24
|
||||||
|
24
|
||||||
|
|
||||||
|
QLDIAGSQRTINV
|
||||||
|
0.38
|
||||||
|
0.37
|
||||||
|
2.3
|
||||||
|
0.37
|
||||||
|
0.37
|
||||||
|
0.37
|
||||||
|
4.9
|
||||||
|
4.9
|
||||||
|
4.9
|
||||||
|
|
||||||
|
B_rownnz 9 3 6 0 0 0 0 0
|
||||||
|
|
||||||
|
B_rowadr 0 9 12 18 18 18 18 18
|
||||||
|
|
||||||
|
B_colind 0 1 2 3 4 5 6 7 8 0 1 2 3 4 5 6 7 8
|
||||||
|
|
||||||
|
C_rownnz 3 3 3 1 1 1 1 1 1
|
||||||
|
|
||||||
|
C_rowadr 0 3 6 9 10 11 12 13 14
|
||||||
|
|
||||||
|
C_colind 0 1 2 0 1 2 0 1 2 3 4 5 6 7 8
|
||||||
|
|
||||||
|
mapM2C 0 2 5 2 1 4 5 4 3 6 7 9 12 16 21
|
||||||
|
|
||||||
|
D_rownnz 3 3 3 6 6 6 6 6 6
|
||||||
|
|
||||||
|
D_rowadr 0 3 6 9 15 21 27 33 39
|
||||||
|
|
||||||
|
D_colind 0 1 2 0 1 2 0 1 2 3 4 5 6 7 8 3 4 5 6 7 8 3 4 5 6 7 8 3 4 5 6 7 8 3 4 5 6 7 8 3 4 5 6 7 8
|
||||||
|
|
||||||
|
mapM2D 0 2 5 2 1 4 5 4 3 6 8 11 15 20 26 8 7 10 14 19 25 11 10 9 13 18 24 15 14 13 12 17 23 20 19 18 17 16 22 26 25 24 23 22 21
|
||||||
|
|
||||||
|
mapD2M 0 4 3 8 7 6 9 16 15 23 22 21 30 29 28 27 37 36 35 34 33 44 43 42 41 40 39
|
||||||
|
|
||||||
|
QDERIV
|
||||||
|
0 0 0 0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0 0 0 0
|
||||||
|
|
||||||
|
QLU
|
||||||
|
0 0 0 0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0 0 0 0
|
||||||
|
|
||||||
|
CONTACT
|
||||||
|
ACTUATOR_VELOCITY
|
||||||
|
-1.1e-322
|
||||||
|
-1.1e-322
|
||||||
|
-2e-323
|
||||||
|
|
||||||
|
CVEL
|
||||||
|
0 0 0 0 0 0
|
||||||
|
0 0 -2e-323 -1.1e-322 -1.1e-322 0
|
||||||
|
0 0 0 0 0 -3.7e+05
|
||||||
|
0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0
|
||||||
|
|
||||||
|
CDOF_DOT
|
||||||
|
0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0
|
||||||
|
0 0 0 -1.1e-322 1.1e-322 0
|
||||||
|
0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0
|
||||||
|
0 0 0 0 -3.7e+05 0
|
||||||
|
0 0 0 3.7e+05 0 0
|
||||||
|
0 0 0 0 0 0
|
||||||
|
|
||||||
|
QFRC_BIAS
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
71
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
|
||||||
|
QFRC_SPRING
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
|
||||||
|
QFRC_DAMPER
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
|
||||||
|
QFRC_GRAVCOMP
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
|
||||||
|
QFRC_FLUID
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
|
||||||
|
QFRC_PASSIVE
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
|
||||||
|
SUBTREE_LINVEL
|
||||||
|
0 0 0
|
||||||
|
0 0 0
|
||||||
|
0 0 0
|
||||||
|
0 0 0
|
||||||
|
0 0 0
|
||||||
|
0 0 0
|
||||||
|
0 0 0
|
||||||
|
0 0 0
|
||||||
|
|
||||||
|
SUBTREE_ANGMOM
|
||||||
|
0 0 0
|
||||||
|
0 0 0
|
||||||
|
0 0 0
|
||||||
|
0 0 0
|
||||||
|
0 0 0
|
||||||
|
0 0 0
|
||||||
|
0 0 0
|
||||||
|
0 0 0
|
||||||
|
|
||||||
|
ACTUATOR_FORCE
|
||||||
|
9.1e-321
|
||||||
|
9.1e-321
|
||||||
|
5.9e-322
|
||||||
|
|
||||||
|
QFRC_ACTUATOR
|
||||||
|
9.1e-321
|
||||||
|
9.1e-321
|
||||||
|
5.9e-322
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
|
||||||
|
QFRC_SMOOTH
|
||||||
|
9.1e-321
|
||||||
|
9.1e-321
|
||||||
|
5.9e-322
|
||||||
|
0
|
||||||
|
0
|
||||||
|
-71
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
|
||||||
|
QACC_SMOOTH
|
||||||
|
1.2e-321
|
||||||
|
1.2e-321
|
||||||
|
8.8e-322
|
||||||
|
0
|
||||||
|
0
|
||||||
|
-9.8
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
|
||||||
|
QFRC_CONSTRAINT
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
|
||||||
|
QFRC_INVERSE
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
0
|
||||||
|
|
||||||
|
CACC
|
||||||
|
0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0
|
||||||
|
|
||||||
|
CFRC_INT
|
||||||
|
0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0
|
||||||
|
|
||||||
|
CFRC_EXT
|
||||||
|
0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0
|
||||||
|
0 0 0 0 0 0
|
||||||
|
|
||||||
@@ -1,197 +1,303 @@
|
|||||||
#!/usr/bin/env python3
|
#!/usr/bin/env python3
|
||||||
"""
|
"""
|
||||||
ROS1 + MuJoCo VLA 3D 导航 — 全向移动 + 前向扫描 + 避障 + 目标点导航
|
MuJoCo 全向小车导航（前向射线扫描 + 势场避障 + 目标点导航）
|
||||||
|
- vx, vy, wz 全向移动（车体系输出 -> 世界系执行器）
|
||||||
圆柱体小车: vx, vy, wz 全向移动
|
- mj_ray 扇形前向扫描
|
||||||
前向范围扫描: mj_ray 射线检测
|
- 避障：排斥 + 朝开阔方向绕行
|
||||||
避障: 基于距离的势场/向量场
|
- 卡住检测：连续位移太小 -> 强制脱困策略
|
||||||
"""
|
"""
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import os
|
||||||
|
from dataclasses import dataclass
|
||||||
|
from typing import Optional, Tuple, List
|
||||||
|
|
||||||
import mujoco
|
import mujoco
|
||||||
import mujoco.viewer
|
import mujoco.viewer
|
||||||
import numpy as np
|
import numpy as np
|
||||||
import os
|
|
||||||
|
|
||||||
SCENE_PATH = os.path.join(os.path.dirname(__file__), "..", "mujoco_scenes", "cylinder_obstacles.xml")
|
SCENE_PATH = os.path.join(os.path.dirname(__file__), "..", "mujoco_scenes", "cylinder_obstacles.xml")
|
||||||
|
|
||||||
# ============ 可配置参数 ============
|
|
||||||
NUM_RAYS = 19 # 前向扫描射线数量（-90 ~ +90 deg）
|
# ===================== 参数配置 =====================
|
||||||
RAY_ANGLE_SPAN = np.pi # 扫描角度跨度 (rad)
|
@dataclass
|
||||||
OBSTACLE_THRESHOLD = 0.45 # 障碍判定距离 (m)
|
class NavCfg:
|
||||||
SAFE_DISTANCE = 0.65 # 安全距离
|
# ray scan
|
||||||
GOAL_THRESHOLD = 0.2 # 到达目标判定距离
|
num_rays: int = 19
|
||||||
MAX_LINEAR = 0.6 # 最大线速度
|
ray_angle_span: float = np.pi # -span/2 ~ +span/2
|
||||||
MAX_ANGULAR = 0.8 # 最大角速度
|
|
||||||
K_ATTRACT = 1.0 # 目标吸引力
|
# distances
|
||||||
K_REPEL = 0.6 # 障碍排斥力
|
obstacle_threshold: float = 0.45
|
||||||
K_YAW_GOAL = 0.8 # 目标对准角速度增益
|
safe_distance: float = 0.65
|
||||||
MIN_LINEAR = 0.18 # 最小线速度，避免卡住
|
goal_threshold: float = 0.20
|
||||||
STUCK_THRESH = 0.03 # 位移阈值，低于此认为卡住 (m)
|
|
||||||
STUCK_STEPS = 100 # 连续卡住步数触发绕行
|
# limits
|
||||||
GOAL_POINTS = [] # 初始无目标
|
max_linear: float = 0.60
|
||||||
GOAL_AHEAD_DIST = 2.0 # G 键：目标 = 车头前方 N 米
|
max_angular: float = 0.80
|
||||||
GOAL_MAX_R = 5.0 # 目标点边界：距离原点不超过此值 (m)
|
min_linear: float = 0.18 # 防止“只转不走”或输出趋零卡死
|
||||||
|
|
||||||
|
# gains
|
||||||
|
k_attract: float = 1.0
|
||||||
|
k_repel: float = 0.6
|
||||||
|
k_yaw_goal: float = 0.8
|
||||||
|
|
||||||
|
# stuck detect
|
||||||
|
stuck_thresh: float = 0.03
|
||||||
|
stuck_steps: int = 100
|
||||||
|
|
||||||
|
# goal set
|
||||||
|
goal_ahead_dist: float = 2.0
|
||||||
|
goal_max_r: float = 5.0
|
||||||
|
|
||||||
|
# output smoothing
|
||||||
|
ema_alpha: float = 0.70
|
||||||
|
max_dv: float = 0.18
|
||||||
|
max_dw: float = 0.20
|
||||||
|
max_dv_stuck: float = 0.25
|
||||||
|
max_dw_stuck: float = 0.28
|
||||||
|
|
||||||
|
|
||||||
def _clamp_goal(gx, gy):
|
CFG = NavCfg()
|
||||||
"""目标点边界：限制在距离原点 GOAL_MAX_R 以内"""
|
|
||||||
r = np.sqrt(gx * gx + gy * gy) + 1e-8
|
|
||||||
if r > GOAL_MAX_R:
|
|
||||||
scale = GOAL_MAX_R / r
|
|
||||||
return (float(gx * scale), float(gy * scale))
|
|
||||||
return (float(gx), float(gy))
|
|
||||||
|
|
||||||
|
|
||||||
def get_agent_state(data, body_id):
|
# ===================== 工具函数 =====================
|
||||||
"""获取小车位姿: x, y, yaw"""
|
def clamp_goal(gx: float, gy: float, max_r: float) -> Tuple[float, float]:
|
||||||
|
r = float(np.hypot(gx, gy)) + 1e-9
|
||||||
|
if r > max_r:
|
||||||
|
s = max_r / r
|
||||||
|
return float(gx * s), float(gy * s)
|
||||||
|
return float(gx), float(gy)
|
||||||
|
|
||||||
|
|
||||||
|
def wrap_pi(a: float) -> float:
|
||||||
|
# 更稳定的 [-pi, pi]
|
||||||
|
return float(np.arctan2(np.sin(a), np.cos(a)))
|
||||||
|
|
||||||
|
|
||||||
|
def get_agent_state(data: mujoco.MjData) -> Tuple[float, float, float]:
|
||||||
|
# 假设 qpos: x, y, yaw 在 0,1,2
|
||||||
qpos = data.qpos
|
qpos = data.qpos
|
||||||
x, y = qpos[0], qpos[1]
|
return float(qpos[0]), float(qpos[1]), float(qpos[2])
|
||||||
yaw = qpos[2]
|
|
||||||
return x, y, yaw
|
|
||||||
|
|
||||||
|
|
||||||
def forward_ray_scan(model, data, body_id, site_id, num_rays, angle_span):
|
def goal_from_camera(viewer_handle) -> Optional[Tuple[float, float]]:
|
||||||
|
"""相机视线与 z=0 平面交点（旋转相机后按 C）。"""
|
||||||
|
cam = viewer_handle.cam
|
||||||
|
lookat = np.array(cam.lookat, dtype=float)
|
||||||
|
dist = float(cam.distance)
|
||||||
|
az = np.deg2rad(float(cam.azimuth))
|
||||||
|
el = np.deg2rad(float(cam.elevation))
|
||||||
|
|
||||||
|
# MuJoCo viewer 的相机方向约定：这里延续你原实现
|
||||||
|
fx = np.cos(el) * np.sin(az)
|
||||||
|
fy = -np.cos(el) * np.cos(az)
|
||||||
|
fz = np.sin(el)
|
||||||
|
|
||||||
|
cam_pos = lookat - dist * np.array([fx, fy, fz], dtype=float)
|
||||||
|
|
||||||
|
if abs(fz) < 1e-8:
|
||||||
|
return None
|
||||||
|
t = -cam_pos[2] / fz
|
||||||
|
if t < 0:
|
||||||
|
return None
|
||||||
|
pt = cam_pos + t * np.array([fx, fy, fz], dtype=float)
|
||||||
|
return float(pt[0]), float(pt[1])
|
||||||
|
|
||||||
|
|
||||||
|
# ===================== 射线扫描（预计算优化） =====================
|
||||||
|
class RayScanner:
|
||||||
"""
|
"""
|
||||||
前向范围扫描: 在车体前方扇形区域内发射射线
|
预计算扇形扫描的 body-frame 方向（基于 forward=[1,0,0], left=[0,1,0]），
|
||||||
返回: (距离数组, 角度数组)
|
每帧用 body rotation (xmat) 转到 world，减少每帧 cos/sin + 临时数组。
|
||||||
"""
|
"""
|
||||||
site_xpos = data.site_xpos[site_id].copy()
|
|
||||||
xmat = np.array(data.xmat[body_id]).reshape(3, 3)
|
|
||||||
forward = xmat[:, 0] # 前向 (body +X)
|
|
||||||
|
|
||||||
angles = np.linspace(-angle_span / 2, angle_span / 2, num_rays)
|
def __init__(self, num_rays: int, angle_span: float):
|
||||||
distances = np.full(num_rays, 10.0)
|
self.num_rays = int(num_rays)
|
||||||
|
self.angle_span = float(angle_span)
|
||||||
|
|
||||||
for i, theta in enumerate(angles):
|
self.angles = np.linspace(-self.angle_span / 2, self.angle_span / 2, self.num_rays)
|
||||||
c, s = np.cos(theta), np.sin(theta)
|
c = np.cos(self.angles)
|
||||||
ray_dir = c * forward + s * xmat[:, 1] # 绕 body Z 旋转
|
s = np.sin(self.angles)
|
||||||
ray_dir = ray_dir / (np.linalg.norm(ray_dir) + 1e-8)
|
|
||||||
|
|
||||||
geomid = np.array([-1], dtype=np.int32)
|
# body-frame ray directions: dir_b = c*[1,0,0] + s*[0,1,0]
|
||||||
d = mujoco.mj_ray(model, data, site_xpos, ray_dir, None, 1, body_id, geomid)
|
# shape (N,3)
|
||||||
|
self.ray_dirs_body = np.stack([c, s, np.zeros_like(c)], axis=1).astype(np.float64)
|
||||||
|
|
||||||
|
# buffers to avoid per-step alloc
|
||||||
|
self.distances = np.full(self.num_rays, 10.0, dtype=np.float64)
|
||||||
|
self._geomid = np.array([-1], dtype=np.int32)
|
||||||
|
|
||||||
|
def scan(
|
||||||
|
self,
|
||||||
|
model: mujoco.MjModel,
|
||||||
|
data: mujoco.MjData,
|
||||||
|
body_id: int,
|
||||||
|
site_id: int,
|
||||||
|
) -> Tuple[np.ndarray, np.ndarray]:
|
||||||
|
site_xpos = data.site_xpos[site_id] # view, no copy
|
||||||
|
xmat = np.array(data.xmat[body_id], dtype=np.float64).reshape(3, 3)
|
||||||
|
|
||||||
|
# world dirs: dir_w = xmat @ dir_b
|
||||||
|
# shape (N,3)
|
||||||
|
ray_dirs_world = (xmat @ self.ray_dirs_body.T).T
|
||||||
|
|
||||||
|
# normalize (理论上已单位，但数值上更稳一点)
|
||||||
|
norms = np.linalg.norm(ray_dirs_world, axis=1) + 1e-9
|
||||||
|
ray_dirs_world /= norms[:, None]
|
||||||
|
|
||||||
|
self.distances.fill(10.0)
|
||||||
|
for i in range(self.num_rays):
|
||||||
|
d = mujoco.mj_ray(
|
||||||
|
model, data,
|
||||||
|
site_xpos, ray_dirs_world[i],
|
||||||
|
None, 1, body_id, self._geomid
|
||||||
|
)
|
||||||
if d >= 0:
|
if d >= 0:
|
||||||
distances[i] = d
|
self.distances[i] = float(d)
|
||||||
|
|
||||||
return distances, angles
|
return self.distances, self.angles
|
||||||
|
|
||||||
|
|
||||||
def obstacle_avoidance_vel(distances, angles):
|
# ===================== 速度融合与滤波 =====================
|
||||||
"""
|
class VelocityFilter:
|
||||||
避障速度 + 绕行分量。被挡时朝开阔方向加速，而非只排斥
|
"""EMA + 速率限制，输出更平滑，避免 ctrl 抖动。"""
|
||||||
返回: (vx_avoid, vy_avoid, wz_avoid)
|
|
||||||
"""
|
|
||||||
min_d = np.min(distances)
|
|
||||||
if min_d > OBSTACLE_THRESHOLD:
|
|
||||||
return 0.0, 0.0, 0.0
|
|
||||||
|
|
||||||
vx, vy, wz = 0.0, 0.0, 0.0
|
def __init__(self, alpha: float, max_dv: float, max_dw: float):
|
||||||
safe_sector = np.argmax(distances)
|
self.alpha = float(alpha)
|
||||||
best_theta = angles[safe_sector]
|
self.max_dv = float(max_dv)
|
||||||
|
self.max_dw = float(max_dw)
|
||||||
|
self.vx = 0.0
|
||||||
|
self.vy = 0.0
|
||||||
|
self.wz = 0.0
|
||||||
|
|
||||||
for d, theta in zip(distances, angles):
|
def reset(self):
|
||||||
if d < OBSTACLE_THRESHOLD:
|
self.vx = self.vy = self.wz = 0.0
|
||||||
ratio = 1.0 - d / OBSTACLE_THRESHOLD
|
|
||||||
strength = K_REPEL * (ratio ** 2)
|
|
||||||
vx -= strength * np.cos(theta)
|
|
||||||
vy -= strength * np.sin(theta)
|
|
||||||
elif d < SAFE_DISTANCE:
|
|
||||||
ratio = (SAFE_DISTANCE - d) / (SAFE_DISTANCE - OBSTACLE_THRESHOLD)
|
|
||||||
strength = K_REPEL * 0.15 * ratio
|
|
||||||
wz += strength * np.clip(best_theta - theta, -1, 1)
|
|
||||||
|
|
||||||
# 绕行：朝最开阔方向加正向速度，避免只退不绕
|
def update(self, vx_cmd: float, vy_cmd: float, wz_cmd: float) -> Tuple[float, float, float]:
|
||||||
|
vx_f = self.alpha * self.vx + (1.0 - self.alpha) * vx_cmd
|
||||||
|
vy_f = self.alpha * self.vy + (1.0 - self.alpha) * vy_cmd
|
||||||
|
wz_f = self.alpha * self.wz + (1.0 - self.alpha) * wz_cmd
|
||||||
|
|
||||||
|
dvx = float(np.clip(vx_f - self.vx, -self.max_dv, self.max_dv))
|
||||||
|
dvy = float(np.clip(vy_f - self.vy, -self.max_dv, self.max_dv))
|
||||||
|
dwz = float(np.clip(wz_f - self.wz, -self.max_dw, self.max_dw))
|
||||||
|
|
||||||
|
self.vx += dvx
|
||||||
|
self.vy += dvy
|
||||||
|
self.wz += dwz
|
||||||
|
return self.vx, self.vy, self.wz
|
||||||
|
|
||||||
|
|
||||||
|
def goal_attraction_vel(x: float, y: float, yaw: float, goal_x: float, goal_y: float, cfg: NavCfg):
|
||||||
|
dx = goal_x - x
|
||||||
|
dy = goal_y - y
|
||||||
|
dist = float(np.hypot(dx, dy)) + 1e-6
|
||||||
|
|
||||||
|
# 世界系单位方向
|
||||||
|
gx_w = dx / dist
|
||||||
|
gy_w = dy / dist
|
||||||
|
|
||||||
|
# 转车体系（只关心 yaw）
|
||||||
|
c = np.cos(-yaw)
|
||||||
|
s = np.sin(-yaw)
|
||||||
|
gx_b = c * gx_w - s * gy_w
|
||||||
|
gy_b = s * gx_w + c * gy_w
|
||||||
|
|
||||||
|
# 线速度（随距离平滑）
|
||||||
|
scale = float(np.tanh(dist) * 0.8 + 0.2)
|
||||||
|
vx = cfg.k_attract * scale * gx_b
|
||||||
|
vy = cfg.k_attract * scale * gy_b
|
||||||
|
|
||||||
|
# 角速度（弱对准）
|
||||||
|
target_yaw = float(np.arctan2(dy, dx))
|
||||||
|
yaw_err = wrap_pi(target_yaw - yaw)
|
||||||
|
wz = cfg.k_yaw_goal * float(np.tanh(yaw_err))
|
||||||
|
return vx, vy, wz
|
||||||
|
|
||||||
|
|
||||||
|
def obstacle_avoidance_vel(distances: np.ndarray, angles: np.ndarray, cfg: NavCfg):
|
||||||
|
min_d = float(np.min(distances))
|
||||||
|
if min_d > cfg.obstacle_threshold:
|
||||||
|
return 0.0, 0.0, 0.0, min_d
|
||||||
|
|
||||||
|
# 最开阔方向
|
||||||
|
safe_i = int(np.argmax(distances))
|
||||||
|
best_theta = float(angles[safe_i])
|
||||||
|
|
||||||
|
vx = 0.0
|
||||||
|
vy = 0.0
|
||||||
|
wz = 0.0
|
||||||
|
|
||||||
|
# 排斥 + 轻微“扭转”朝开阔方向
|
||||||
|
# （这里保持你原逻辑，但数值上更稳一点）
|
||||||
|
for d, th in zip(distances, angles):
|
||||||
|
d = float(d)
|
||||||
|
th = float(th)
|
||||||
|
if d < cfg.obstacle_threshold:
|
||||||
|
ratio = 1.0 - d / cfg.obstacle_threshold
|
||||||
|
strength = cfg.k_repel * (ratio * ratio)
|
||||||
|
vx -= strength * np.cos(th)
|
||||||
|
vy -= strength * np.sin(th)
|
||||||
|
elif d < cfg.safe_distance:
|
||||||
|
ratio = (cfg.safe_distance - d) / (cfg.safe_distance - cfg.obstacle_threshold)
|
||||||
|
strength = cfg.k_repel * 0.15 * ratio
|
||||||
|
wz += strength * float(np.clip(best_theta - th, -1.0, 1.0))
|
||||||
|
|
||||||
|
# 绕行：朝最开阔方向“推一把”
|
||||||
bypass = 0.35
|
bypass = 0.35
|
||||||
vx += bypass * np.cos(best_theta)
|
vx += bypass * np.cos(best_theta)
|
||||||
vy += bypass * np.sin(best_theta)
|
vy += bypass * np.sin(best_theta)
|
||||||
wz += 0.4 * best_theta
|
wz += 0.4 * best_theta
|
||||||
|
|
||||||
return vx, vy, wz
|
return vx, vy, wz, min_d
|
||||||
|
|
||||||
|
|
||||||
def goal_attraction_vel(x, y, yaw, goal_x, goal_y):
|
def blend_and_clamp(
|
||||||
"""
|
vx_a: float, vy_a: float, wz_a: float,
|
||||||
目标吸引力: 在车体坐标系下计算朝目标的 vx, vy, wz
|
vx_g: float, vy_g: float, wz_g: float,
|
||||||
优先保持线速度，角速度仅用于微调朝向
|
dist_to_goal: float,
|
||||||
"""
|
stuck: bool,
|
||||||
dx = goal_x - x
|
cfg: NavCfg
|
||||||
dy = goal_y - y
|
):
|
||||||
dist = np.sqrt(dx * dx + dy * dy) + 1e-6
|
|
||||||
|
|
||||||
# 世界系下期望方向
|
|
||||||
gx_w = dx / dist
|
|
||||||
gy_w = dy / dist
|
|
||||||
|
|
||||||
# 转到车体系
|
|
||||||
c, s = np.cos(-yaw), np.sin(-yaw)
|
|
||||||
gx_b = c * gx_w - s * gy_w
|
|
||||||
gy_b = s * gx_w + c * gy_w
|
|
||||||
|
|
||||||
# 线速度：朝目标，随距离平滑
|
|
||||||
scale = np.tanh(dist) * 0.8 + 0.2
|
|
||||||
vx = K_ATTRACT * scale * gx_b
|
|
||||||
vy = K_ATTRACT * scale * gy_b
|
|
||||||
|
|
||||||
# 角速度：弱增益，避免只转不走
|
|
||||||
target_yaw = np.arctan2(dy, dx)
|
|
||||||
yaw_err = np.arctan2(np.sin(target_yaw - yaw), np.cos(target_yaw - yaw))
|
|
||||||
wz = K_YAW_GOAL * np.tanh(yaw_err)
|
|
||||||
|
|
||||||
return vx, vy, wz
|
|
||||||
|
|
||||||
|
|
||||||
def blend_and_clamp(vx_a, vy_a, wz_a, vx_g, vy_g, wz_g, dist_to_goal, min_d, stuck):
|
|
||||||
"""融合避障与目标速度，限幅，卡住时强制最小速度"""
|
|
||||||
vx = vx_a + vx_g
|
vx = vx_a + vx_g
|
||||||
vy = vy_a + vy_g
|
vy = vy_a + vy_g
|
||||||
wz = wz_a + wz_g
|
wz = wz_a + wz_g
|
||||||
|
|
||||||
lin = np.sqrt(vx * vx + vy * vy)
|
lin = float(np.hypot(vx, vy))
|
||||||
# 未到目标且线速度过小：强制最小速度
|
|
||||||
if dist_to_goal > GOAL_THRESHOLD and lin < MIN_LINEAR:
|
# 强制最小速度：未到目标且输出太小
|
||||||
|
if dist_to_goal > cfg.goal_threshold and lin < cfg.min_linear:
|
||||||
if lin > 1e-6:
|
if lin > 1e-6:
|
||||||
vx *= MIN_LINEAR / lin
|
s = cfg.min_linear / lin
|
||||||
vy *= MIN_LINEAR / lin
|
vx *= s
|
||||||
|
vy *= s
|
||||||
else:
|
else:
|
||||||
# 融合后接近零：优先朝目标，被挡时朝避障方向
|
# 彻底趋零时：优先用目标方向（若也为零就不动）
|
||||||
ax, ay = vx_g + vx_a, vy_g + vy_a
|
ax = vx_g + vx_a
|
||||||
anorm = np.sqrt(ax * ax + ay * ay) + 1e-8
|
ay = vy_g + vy_a
|
||||||
vx = MIN_LINEAR * ax / anorm
|
an = float(np.hypot(ax, ay)) + 1e-9
|
||||||
vy = MIN_LINEAR * ay / anorm
|
vx = cfg.min_linear * ax / an
|
||||||
lin = np.sqrt(vx * vx + vy * vy)
|
vy = cfg.min_linear * ay / an
|
||||||
if lin > MAX_LINEAR:
|
|
||||||
scale = MAX_LINEAR / lin
|
# 卡住脱困：额外加一点“侧向+偏航”，更容易摆脱夹角/凹障碍
|
||||||
vx *= scale
|
if stuck:
|
||||||
vy *= scale
|
# 让它更愿意横移并转一点（避免只顶着障碍）
|
||||||
wz = np.clip(wz, -MAX_ANGULAR, MAX_ANGULAR)
|
vx *= 0.85
|
||||||
|
vy = vy * 1.15 + 0.06 * np.sign(vy if abs(vy) > 1e-3 else 1.0)
|
||||||
|
wz += 0.10 * np.sign(wz if abs(wz) > 1e-3 else 1.0)
|
||||||
|
|
||||||
|
# 限幅
|
||||||
|
lin = float(np.hypot(vx, vy))
|
||||||
|
if lin > cfg.max_linear:
|
||||||
|
s = cfg.max_linear / lin
|
||||||
|
vx *= s
|
||||||
|
vy *= s
|
||||||
|
wz = float(np.clip(wz, -cfg.max_angular, cfg.max_angular))
|
||||||
return vx, vy, wz
|
return vx, vy, wz
|
||||||
|
|
||||||
|
|
||||||
class VelocityFilter:
|
# ===================== 主程序 =====================
|
||||||
"""EMA 滤波 + 速率限制，平滑控制输出"""
|
|
||||||
|
|
||||||
def __init__(self, alpha=0.75, max_dv=0.15, max_dw=0.2):
|
|
||||||
self.alpha = alpha # 滤波系数，越大越平滑
|
|
||||||
self.max_dv = max_dv # 每步最大线速度变化
|
|
||||||
self.max_dw = max_dw # 每步最大角速度变化
|
|
||||||
self.vx, self.vy, self.wz = 0.0, 0.0, 0.0
|
|
||||||
|
|
||||||
def update(self, vx_cmd, vy_cmd, wz_cmd):
|
|
||||||
# 1. EMA 滤波
|
|
||||||
vx_f = self.alpha * self.vx + (1 - self.alpha) * vx_cmd
|
|
||||||
vy_f = self.alpha * self.vy + (1 - self.alpha) * vy_cmd
|
|
||||||
wz_f = self.alpha * self.wz + (1 - self.alpha) * wz_cmd
|
|
||||||
|
|
||||||
# 2. 速率限制
|
|
||||||
dvx = np.clip(vx_f - self.vx, -self.max_dv, self.max_dv)
|
|
||||||
dvy = np.clip(vy_f - self.vy, -self.max_dv, self.max_dv)
|
|
||||||
dwz = np.clip(wz_f - self.wz, -self.max_dw, self.max_dw)
|
|
||||||
|
|
||||||
self.vx += dvx
|
|
||||||
self.vy += dvy
|
|
||||||
self.wz += dwz
|
|
||||||
|
|
||||||
return self.vx, self.vy, self.wz
|
|
||||||
|
|
||||||
|
|
||||||
def main():
|
def main():
|
||||||
model = mujoco.MjModel.from_xml_path(SCENE_PATH)
|
model = mujoco.MjModel.from_xml_path(SCENE_PATH)
|
||||||
data = mujoco.MjData(model)
|
data = mujoco.MjData(model)
|
||||||
@@ -199,160 +305,142 @@ def main():
|
|||||||
body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "cylinder_agent")
|
body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "cylinder_agent")
|
||||||
site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ray_origin")
|
site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ray_origin")
|
||||||
|
|
||||||
# 执行器索引
|
# actuators
|
||||||
act_ids = [
|
act_vel_x = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "vel_x")
|
||||||
mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "vel_x"),
|
act_vel_y = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "vel_y")
|
||||||
mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "vel_y"),
|
act_vel_yaw = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "vel_yaw")
|
||||||
mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "vel_yaw"),
|
|
||||||
]
|
# goal marker joint
|
||||||
|
goal_joint_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "goal_joint")
|
||||||
|
goal_qposadr = int(model.jnt_qposadr[goal_joint_id])
|
||||||
|
|
||||||
|
def hide_goal_marker():
|
||||||
|
data.qpos[goal_qposadr: goal_qposadr + 3] = [-99.0, -99.0, 0.1]
|
||||||
|
|
||||||
|
def set_goal_marker(gx: float, gy: float):
|
||||||
|
data.qpos[goal_qposadr] = gx
|
||||||
|
data.qpos[goal_qposadr + 1] = gy
|
||||||
|
data.qpos[goal_qposadr + 2] = 0.15
|
||||||
|
|
||||||
|
hide_goal_marker()
|
||||||
|
|
||||||
|
scanner = RayScanner(CFG.num_rays, CFG.ray_angle_span)
|
||||||
|
vel_filter = VelocityFilter(alpha=CFG.ema_alpha, max_dv=CFG.max_dv, max_dw=CFG.max_dw)
|
||||||
|
|
||||||
|
goals: List[Tuple[float, float]] = []
|
||||||
|
goal_active = False
|
||||||
|
|
||||||
goals = GOAL_POINTS.copy()
|
|
||||||
goal_idx = 0 if goals else -1
|
|
||||||
vel_filter = VelocityFilter(alpha=0.70, max_dv=0.18, max_dw=0.2)
|
|
||||||
last_x, last_y = 0.0, 0.0
|
last_x, last_y = 0.0, 0.0
|
||||||
stuck_cnt = 0
|
stuck_cnt = 0
|
||||||
|
|
||||||
goal_joint_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "goal_joint")
|
# key flags
|
||||||
goal_qposadr = model.jnt_qposadr[goal_joint_id]
|
add_goal_ahead = False
|
||||||
data.qpos[goal_qposadr : goal_qposadr + 3] = [-99, -99, 0.1] # 初始隐藏
|
add_goal_camera = False
|
||||||
|
|
||||||
print("=" * 55)
|
print("=" * 60)
|
||||||
print("VLA 3D 导航 - 全向移动 + 前向扫描 + 避障")
|
print("MuJoCo 全向导航：射线扫描 + 避障 + 目标点")
|
||||||
print("=" * 55)
|
print(f"G: 车头前方 {CFG.goal_ahead_dist:.1f}m | C: 相机视线落点 | 目标边界 R<{CFG.goal_max_r:.1f}m")
|
||||||
print("G: 车头前方 {:.0f}m | C: 相机视线 | 目标边界 R<{:.0f}m".format(GOAL_AHEAD_DIST, GOAL_MAX_R))
|
|
||||||
print("ESC 退出")
|
print("ESC 退出")
|
||||||
print("=" * 55)
|
print("=" * 60)
|
||||||
|
|
||||||
add_goal_ahead = [False]
|
def key_cb(keycode: int):
|
||||||
add_goal_camera = [False]
|
nonlocal add_goal_ahead, add_goal_camera
|
||||||
|
|
||||||
def key_cb(keycode):
|
|
||||||
if keycode == 71: # G
|
if keycode == 71: # G
|
||||||
add_goal_ahead[0] = True
|
add_goal_ahead = True
|
||||||
elif keycode == 67: # C: 相机视线与地面交点
|
elif keycode == 67: # C
|
||||||
add_goal_camera[0] = True
|
add_goal_camera = True
|
||||||
|
|
||||||
def _goal_from_camera(viewer_handle):
|
def stop_output():
|
||||||
"""相机视线与 z=0 地面交点（旋转相机后按 C）"""
|
data.ctrl[act_vel_x] = 0.0
|
||||||
cam = viewer_handle.cam
|
data.ctrl[act_vel_y] = 0.0
|
||||||
lookat = np.array(cam.lookat)
|
data.ctrl[act_vel_yaw] = 0.0
|
||||||
dist = float(cam.distance)
|
|
||||||
az = np.deg2rad(float(cam.azimuth))
|
|
||||||
el = np.deg2rad(float(cam.elevation))
|
|
||||||
# 相机位置 = lookat - dist * 前向单位向量
|
|
||||||
fx = np.cos(el) * np.sin(az)
|
|
||||||
fy = -np.cos(el) * np.cos(az)
|
|
||||||
fz = np.sin(el)
|
|
||||||
cam_pos = lookat - dist * np.array([fx, fy, fz])
|
|
||||||
# 射线与 z=0 交点: cam_pos + t*[fx,fy,fz], 令 z=0
|
|
||||||
if abs(fz) < 1e-6:
|
|
||||||
return None
|
|
||||||
t = -cam_pos[2] / fz
|
|
||||||
if t < 0:
|
|
||||||
return None
|
|
||||||
pt = cam_pos + t * np.array([fx, fy, fz])
|
|
||||||
return (float(pt[0]), float(pt[1]))
|
|
||||||
|
|
||||||
def control_callback():
|
|
||||||
nonlocal goal_idx, last_x, last_y, stuck_cnt
|
|
||||||
if add_goal_ahead[0]:
|
|
||||||
add_goal_ahead[0] = False
|
|
||||||
x, y, yaw = get_agent_state(data, body_id)
|
|
||||||
gx = x + GOAL_AHEAD_DIST * np.cos(yaw)
|
|
||||||
gy = y + GOAL_AHEAD_DIST * np.sin(yaw)
|
|
||||||
gx, gy = _clamp_goal(gx, gy)
|
|
||||||
goals.clear()
|
|
||||||
goals.append((gx, gy))
|
|
||||||
goal_idx = 0
|
|
||||||
vel_filter.vx = vel_filter.vy = vel_filter.wz = 0.0
|
|
||||||
print(" 目标: ({:.2f}, {:.2f}) [G=车头前方]".format(gx, gy))
|
|
||||||
elif add_goal_camera[0]:
|
|
||||||
add_goal_camera[0] = False
|
|
||||||
g = _goal_from_camera(viewer)
|
|
||||||
if g is not None:
|
|
||||||
g = _clamp_goal(g[0], g[1])
|
|
||||||
goals.clear()
|
|
||||||
goals.append(g)
|
|
||||||
goal_idx = 0
|
|
||||||
vel_filter.vx = vel_filter.vy = vel_filter.wz = 0.0
|
|
||||||
print(" 目标: ({:.2f}, {:.2f}) [C=相机视线]".format(g[0], g[1]))
|
|
||||||
else:
|
|
||||||
print(" [C] 相机未指向地面，请调整视角后重试")
|
|
||||||
|
|
||||||
if goal_idx < 0 or not goals:
|
|
||||||
data.ctrl[act_ids[0]] = 0
|
|
||||||
data.ctrl[act_ids[1]] = 0
|
|
||||||
data.ctrl[act_ids[2]] = 0
|
|
||||||
vel_filter.vx = vel_filter.vy = vel_filter.wz = 0.0
|
|
||||||
data.qpos[goal_qposadr : goal_qposadr + 3] = [-99, -99, 0.1] # 藏起标记
|
|
||||||
return
|
|
||||||
|
|
||||||
x, y, yaw = get_agent_state(data, body_id)
|
|
||||||
goal_x, goal_y = goals[goal_idx]
|
|
||||||
|
|
||||||
dist_to_goal = np.sqrt((goal_x - x) ** 2 + (goal_y - y) ** 2)
|
|
||||||
if dist_to_goal < GOAL_THRESHOLD:
|
|
||||||
goals.clear()
|
|
||||||
goal_idx = -1
|
|
||||||
vel_filter.vx = vel_filter.vy = vel_filter.wz = 0.0
|
|
||||||
print(" 已到达，停止。按 G 设置新目标")
|
|
||||||
data.ctrl[act_ids[0]] = 0
|
|
||||||
data.ctrl[act_ids[1]] = 0
|
|
||||||
data.ctrl[act_ids[2]] = 0
|
|
||||||
data.qpos[goal_qposadr : goal_qposadr + 3] = [-99, -99, 0.1]
|
|
||||||
return
|
|
||||||
|
|
||||||
# 更新目标点标记位置
|
|
||||||
data.qpos[goal_qposadr] = goal_x
|
|
||||||
data.qpos[goal_qposadr + 1] = goal_y
|
|
||||||
data.qpos[goal_qposadr + 2] = 0.15
|
|
||||||
|
|
||||||
# 1. 前向范围扫描
|
|
||||||
distances, angles = forward_ray_scan(
|
|
||||||
model, data, body_id, site_id, NUM_RAYS, RAY_ANGLE_SPAN
|
|
||||||
)
|
|
||||||
min_d = np.min(distances)
|
|
||||||
|
|
||||||
# 2. 卡住检测
|
|
||||||
moved = np.sqrt((x - last_x) ** 2 + (y - last_y) ** 2)
|
|
||||||
if moved < STUCK_THRESH and dist_to_goal > GOAL_THRESHOLD:
|
|
||||||
stuck_cnt += 1
|
|
||||||
else:
|
|
||||||
stuck_cnt = 0
|
|
||||||
last_x, last_y = x, y
|
|
||||||
stuck = stuck_cnt > STUCK_STEPS
|
|
||||||
|
|
||||||
# 3. 避障速度（含绕行分量）
|
|
||||||
vx_a, vy_a, wz_a = obstacle_avoidance_vel(distances, angles)
|
|
||||||
|
|
||||||
# 4. 目标吸引速度
|
|
||||||
vx_g, vy_g, wz_g = goal_attraction_vel(x, y, yaw, goal_x, goal_y)
|
|
||||||
|
|
||||||
# 5. 融合、最小速度、限幅
|
|
||||||
vx, vy, wz = blend_and_clamp(
|
|
||||||
vx_a, vy_a, wz_a, vx_g, vy_g, wz_g, dist_to_goal, min_d, stuck
|
|
||||||
)
|
|
||||||
|
|
||||||
# 6. 滤波 + 速率限制（卡住时放宽）
|
|
||||||
max_dv = 0.25 if stuck else 0.18
|
|
||||||
max_dw = 0.28 if stuck else 0.2
|
|
||||||
vel_filter.max_dv = max_dv
|
|
||||||
vel_filter.max_dw = max_dw
|
|
||||||
vx, vy, wz = vel_filter.update(vx, vy, wz)
|
|
||||||
|
|
||||||
# 车体系 -> 世界系（slide_x/y 沿世界轴）
|
|
||||||
c, s = np.cos(yaw), np.sin(yaw)
|
|
||||||
vx_w = vx * c - vy * s
|
|
||||||
vy_w = vx * s + vy * c
|
|
||||||
|
|
||||||
data.ctrl[act_ids[0]] = vx_w
|
|
||||||
data.ctrl[act_ids[1]] = vy_w
|
|
||||||
data.ctrl[act_ids[2]] = wz
|
|
||||||
|
|
||||||
with mujoco.viewer.launch_passive(model, data, key_callback=key_cb) as viewer:
|
with mujoco.viewer.launch_passive(model, data, key_callback=key_cb) as viewer:
|
||||||
while viewer.is_running():
|
while viewer.is_running():
|
||||||
mujoco.mj_forward(model, data)
|
mujoco.mj_forward(model, data)
|
||||||
control_callback()
|
|
||||||
|
# ========== goal set ==========
|
||||||
|
nonlocal_vars = None # 仅为了强调这里不用闭包 nonlocal
|
||||||
|
if add_goal_ahead:
|
||||||
|
add_goal_ahead = False
|
||||||
|
x, y, yaw = get_agent_state(data)
|
||||||
|
gx = x + CFG.goal_ahead_dist * float(np.cos(yaw))
|
||||||
|
gy = y + CFG.goal_ahead_dist * float(np.sin(yaw))
|
||||||
|
gx, gy = clamp_goal(gx, gy, CFG.goal_max_r)
|
||||||
|
goals[:] = [(gx, gy)]
|
||||||
|
goal_active = True
|
||||||
|
vel_filter.reset()
|
||||||
|
print(f" 目标: ({gx:.2f}, {gy:.2f}) [G=车头前方]")
|
||||||
|
elif add_goal_camera:
|
||||||
|
add_goal_camera = False
|
||||||
|
g = goal_from_camera(viewer)
|
||||||
|
if g is None:
|
||||||
|
print(" [C] 相机未指向地面，请调整视角后重试")
|
||||||
|
else:
|
||||||
|
gx, gy = clamp_goal(g[0], g[1], CFG.goal_max_r)
|
||||||
|
goals[:] = [(gx, gy)]
|
||||||
|
goal_active = True
|
||||||
|
vel_filter.reset()
|
||||||
|
print(f" 目标: ({gx:.2f}, {gy:.2f}) [C=相机视线]")
|
||||||
|
|
||||||
|
if not goal_active or not goals:
|
||||||
|
stop_output()
|
||||||
|
vel_filter.reset()
|
||||||
|
hide_goal_marker()
|
||||||
|
mujoco.mj_step(model, data)
|
||||||
|
viewer.sync()
|
||||||
|
continue
|
||||||
|
|
||||||
|
# ========== state ==========
|
||||||
|
x, y, yaw = get_agent_state(data)
|
||||||
|
goal_x, goal_y = goals[0]
|
||||||
|
|
||||||
|
dist_to_goal = float(np.hypot(goal_x - x, goal_y - y))
|
||||||
|
if dist_to_goal < CFG.goal_threshold:
|
||||||
|
print(" 已到达，停止。按 G / C 设置新目标")
|
||||||
|
goal_active = False
|
||||||
|
goals.clear()
|
||||||
|
vel_filter.reset()
|
||||||
|
stop_output()
|
||||||
|
hide_goal_marker()
|
||||||
|
mujoco.mj_step(model, data)
|
||||||
|
viewer.sync()
|
||||||
|
continue
|
||||||
|
|
||||||
|
set_goal_marker(goal_x, goal_y)
|
||||||
|
|
||||||
|
# ========== scan ==========
|
||||||
|
distances, angles = scanner.scan(model, data, body_id, site_id)
|
||||||
|
|
||||||
|
# ========== stuck detect ==========
|
||||||
|
moved = float(np.hypot(x - last_x, y - last_y))
|
||||||
|
if moved < CFG.stuck_thresh and dist_to_goal > CFG.goal_threshold:
|
||||||
|
stuck_cnt += 1
|
||||||
|
else:
|
||||||
|
stuck_cnt = 0
|
||||||
|
last_x, last_y = x, y
|
||||||
|
stuck = stuck_cnt > CFG.stuck_steps
|
||||||
|
|
||||||
|
# ========== velocities ==========
|
||||||
|
vx_a, vy_a, wz_a, _min_d = obstacle_avoidance_vel(distances, angles, CFG)
|
||||||
|
vx_g, vy_g, wz_g = goal_attraction_vel(x, y, yaw, goal_x, goal_y, CFG)
|
||||||
|
vx_b, vy_b, wz_b = blend_and_clamp(vx_a, vy_a, wz_a, vx_g, vy_g, wz_g, dist_to_goal, stuck, CFG)
|
||||||
|
|
||||||
|
# ========== filter ==========
|
||||||
|
vel_filter.max_dv = CFG.max_dv_stuck if stuck else CFG.max_dv
|
||||||
|
vel_filter.max_dw = CFG.max_dw_stuck if stuck else CFG.max_dw
|
||||||
|
vx, vy, wz = vel_filter.update(vx_b, vy_b, wz_b)
|
||||||
|
|
||||||
|
# ========== body -> world for actuator ==========
|
||||||
|
c = float(np.cos(yaw))
|
||||||
|
s = float(np.sin(yaw))
|
||||||
|
vx_w = vx * c - vy * s
|
||||||
|
vy_w = vx * s + vy * c
|
||||||
|
|
||||||
|
data.ctrl[act_vel_x] = vx_w
|
||||||
|
data.ctrl[act_vel_y] = vy_w
|
||||||
|
data.ctrl[act_vel_yaw] = wz
|
||||||
|
|
||||||
mujoco.mj_step(model, data)
|
mujoco.mj_step(model, data)
|
||||||
viewer.sync()
|
viewer.sync()
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user