Guest User

Untitled

a guest
Jan 22nd, 2019
72
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 21.25 KB | None | 0 0
  1. ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
  2. ;; util
  3. ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
  4.  
  5. (defun make-default-robot-link
  6. (len
  7. radius
  8. axis
  9. name
  10. &key
  11. (extbody)
  12. (visualize-cylinder? t)
  13. )
  14. (let (bs b0 b1 c a (2r (* radius 2)))
  15. (setq b0 (make-cylinder (* 1.4 radius) (* 4 radius)))
  16. (setq b1 (make-cube 2r 2r len))
  17.  
  18. (setq c (make-cascoords))
  19. (case axis
  20. (:x (setq a #f(1 0 0)))
  21. (:y (setq a #f(0 1 0)))
  22. (:z (setq a #f(0 0 1)))
  23. (:-x (setq a #f(-1 0 0)))
  24. (:-y (setq a #f(0 -1 0)))
  25. (:-z (setq a #f(0 0 -1)))
  26. (t (setq a axis)))
  27. (if (> (norm (v* a #f(0 0 -1))) 0)
  28. (send c :orient (acos (v. a #f(0 0 -1))) (v* a #f(0 0 -1)) :world))
  29. (when visualize-cylinder?
  30. (send b0 :transform c)
  31. (send b0 :translate (float-vector 0 0 (- 2r)))
  32. (send b0 :set-color :red)
  33. )
  34. (send b1 :translate (float-vector 0 0 (/ len -2)) :locate)
  35. (send b1 :set-color :green)
  36. (setq bs (append (if visualize-cylinder? (list b0 b1) (list b1)) extbody))
  37. (dolist (b (cdr bs))
  38. (send (car bs) :assoc b))
  39. (send-all bs :worldcoords) ;; for update centroid
  40. ;; set a mass center of default-robot-link as a volume center
  41. (let* ((valid-bodies
  42. (remove-if #'(lambda (x)
  43. (and (> (send x :volume) 0) (< (send x :volume) 0))) ;; nan check
  44. bs))
  45. (bodies-centroid
  46. (cond ((= (length valid-bodies) 0)
  47. (float-vector 0 0 0)
  48. )
  49. ((= (length valid-bodies) 1)
  50. (send (car valid-bodies) :centroid)
  51. )
  52. (t
  53. (scale (/ 1.0 (reduce #'+ (mapcar #'(lambda (x) (send x :volume)) valid-bodies)))
  54. (reduce #'v+ (mapcar #'(lambda (x) (scale (send x :volume) (send x :centroid))) valid-bodies)))
  55. )))
  56. )
  57. (instance bodyset-link :init (make-cascoords)
  58. :bodies bs :name name :centroid bodies-centroid)
  59. )))
  60.  
  61.  
  62. ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
  63. ;; two-stretchable-links-limb
  64. ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
  65.  
  66. (defclass two-stretchable-links-limb
  67. :super robot-model
  68. :slots (root-coords
  69. middle-coords
  70. end-coords
  71. superior-link-length
  72. inferior-link-length
  73. joint-superior-0
  74. joint-superior-1
  75. joint-superior-2
  76. joint-linear-superior
  77. joint-middle
  78. joint-linear-inferior
  79. joint-inferior-0
  80. joint-inferior-1
  81. joint-inferior-2
  82. link-radius
  83. l-min-max-x-y
  84. )
  85. )
  86.  
  87. (defmethod two-stretchable-links-limb
  88. (:init
  89. (&key
  90. (limb-name "default-limb")
  91. ((:link-radius tmp-link-radius) 12.5)
  92. ((:superior-link-length tmp-superior-link-length) 200.0)
  93. ((:inferior-link-length tmp-inferior-link-length) 200.0)
  94. (superior-link-weight 10.0)
  95. (inferior-link-weight 10.0)
  96. (eef-link-weight 2.0)
  97. (eef-body-lx 75.0)
  98. (eef-body-ly 50.0)
  99. (eef-body-lz 5.0)
  100. (eef-body-pos-offset (float-vector 0 0 0))
  101. (end-coords-offset (float-vector 0 0 0))
  102. )
  103. (send-super :init :name limb-name)
  104. (setq link-radius tmp-link-radius)
  105. (setq superior-link-length tmp-superior-link-length)
  106. (setq inferior-link-length tmp-inferior-link-length)
  107. (let* ((limb-root-link
  108. (instance bodyset-link :init (make-cascoords) :bodies (list (make-cube 1 1 1))
  109. :name (read-from-string (format nil "~A-root-link" limb-name))))
  110. (limb-superior-parent-pre-1-link
  111. (make-default-robot-link 0 link-radius :z
  112. (read-from-string (format nil "~A-superior-parent-pre-1-link" limb-name))))
  113. (limb-superior-parent-pre-2-link
  114. (make-default-robot-link 0 link-radius :y
  115. (read-from-string (format nil "~A-superior-parent-pre-2-link" limb-name))))
  116. (limb-superior-parent-link
  117. (make-default-robot-link superior-link-length link-radius :z
  118. (read-from-string (format nil "~A-superior-parent-link" limb-name))))
  119. (limb-superior-child-link
  120. (make-default-robot-link superior-link-length link-radius :z
  121. (read-from-string (format nil "~A-superior-child-link" limb-name))
  122. :visualize-cylinder? nil))
  123. (limb-inferior-parent-link
  124. (make-default-robot-link inferior-link-length link-radius :y
  125. (read-from-string (format nil "~A-inferior-parent-link" limb-name))))
  126. (limb-inferior-child-link
  127. (make-default-robot-link inferior-link-length link-radius :z
  128. (read-from-string (format nil "~A-inferior-child-link" limb-name))
  129. :visualize-cylinder? nil))
  130. (limb-inferior-child-post-1-link
  131. (make-default-robot-link 0 link-radius :z
  132. (read-from-string (format nil "~A-inferior-child-post-1-link" limb-name))
  133. :visualize-cylinder? nil))
  134. (limb-inferior-child-post-2-link
  135. (make-default-robot-link 0 link-radius :y
  136. (read-from-string (format nil "~A-inferior-child-post-2-link" limb-name))
  137. :visualize-cylinder? nil))
  138. (limb-eef-link)
  139. (limb-eef-body
  140. (make-cube eef-body-lx eef-body-ly eef-body-lz))
  141. )
  142. (send limb-eef-body :set-color :red)
  143. (send limb-eef-body :translate eef-body-pos-offset :local)
  144. (send limb-eef-body :worldcoords) ;; for update centroid
  145. (setq limb-eef-link
  146. (instance bodyset-link :init (make-cascoords) :bodies (list limb-eef-body)
  147. :name (read-from-string (format nil "~A-eef-link" limb-name)) :centroid (send limb-eef-body :centroid)))
  148. (setq links
  149. (list limb-root-link
  150. limb-superior-parent-pre-1-link
  151. limb-superior-parent-pre-2-link
  152. limb-superior-parent-link
  153. limb-superior-child-link
  154. limb-inferior-parent-link
  155. limb-inferior-child-link
  156. limb-inferior-child-post-1-link
  157. limb-inferior-child-post-2-link
  158. limb-eef-link
  159. ))
  160. ;; coords
  161. (setq root-coords (make-cascoords :rpy (list 0 0 pi) :parent limb-root-link))
  162. (setq middle-coords (make-cascoords :rpy (list 0 0 pi/2) :parent limb-inferior-parent-link))
  163. (setq end-coords (make-cascoords :name (read-from-string (format nil "~A-end-coords" limb-name))
  164. :pos end-coords-offset
  165. ;; :pos (v+ end-coords-offset (float-vector 0 0 (* -0.5 eef-body-lz))) ;; [ToDo] support eef offset
  166. :parent limb-eef-link))
  167. ;; assoc
  168. (send limb-inferior-child-post-2-link :assoc limb-eef-link)
  169. (send limb-inferior-child-post-1-link :assoc limb-inferior-child-post-2-link)
  170. (send limb-inferior-child-post-1-link :translate (float-vector 0 0 (- inferior-link-length)) :local)
  171. (send limb-inferior-child-link :assoc limb-inferior-child-post-1-link)
  172. (send limb-inferior-parent-link :assoc limb-inferior-child-link)
  173. (send limb-inferior-parent-link :translate (float-vector 0 0 (- superior-link-length)) :local)
  174. (send limb-superior-child-link :assoc limb-inferior-parent-link)
  175. (send limb-superior-parent-link :assoc limb-superior-child-link)
  176. (send limb-superior-parent-pre-2-link :assoc limb-superior-parent-link)
  177. (send limb-superior-parent-pre-1-link :assoc limb-superior-parent-pre-2-link)
  178. (send limb-root-link :assoc limb-superior-parent-pre-1-link)
  179. (send limb-root-link :rotate pi :x)
  180. (send self :assoc limb-root-link)
  181. ;; joint
  182. (setq joint-superior-0
  183. (instance rotational-joint :init :parent-link limb-root-link :child-link limb-superior-parent-pre-1-link
  184. :name (send limb-superior-parent-pre-1-link :name) :axis :-z :min -180 :max 180))
  185. (setq joint-superior-1
  186. (instance rotational-joint :init :parent-link limb-superior-parent-pre-1-link :child-link limb-superior-parent-pre-2-link
  187. :name (send limb-superior-parent-pre-2-link :name) :axis :-y :min -180 :max 180))
  188. (setq joint-superior-2
  189. (instance rotational-joint :init :parent-link limb-superior-parent-pre-2-link :child-link limb-superior-parent-link
  190. :name (send limb-superior-parent-link :name) :axis :-z :min -180 :max 180))
  191. (setq joint-linear-superior
  192. (instance linear-joint :init :parent-link limb-superior-parent-link :child-link limb-superior-child-link
  193. :name (send limb-superior-child-link :name) :axis :-z :min *-inf* :max *inf*))
  194. (setq joint-middle
  195. (instance rotational-joint :init :parent-link limb-superior-child-link :child-link limb-inferior-parent-link
  196. :name (send limb-inferior-parent-link :name) :axis :-y :min -180 :max 180))
  197. (setq joint-linear-inferior
  198. (instance linear-joint :init :parent-link limb-inferior-parent-link :child-link limb-inferior-child-link
  199. :name (send limb-inferior-child-link :name) :axis :-z :min *-inf* :max *inf*))
  200. (setq joint-inferior-0
  201. (instance rotational-joint :init :parent-link limb-inferior-child-link :child-link limb-inferior-child-post-1-link
  202. :name (send limb-inferior-child-post-1-link :name) :axis :-z :min -180 :max 180))
  203. (setq joint-inferior-1
  204. (instance rotational-joint :init :parent-link limb-inferior-child-post-1-link :child-link limb-inferior-child-post-2-link
  205. :name (send limb-inferior-child-post-2-link :name) :axis :-y :min -180 :max 180))
  206. (setq joint-inferior-2
  207. (instance rotational-joint :init :parent-link limb-inferior-child-post-2-link :child-link limb-eef-link
  208. :name (send limb-eef-link :name) :axis :-z :min -180 :max 180))
  209. (setq joint-list
  210. (list joint-superior-0 joint-superior-1 joint-superior-2
  211. joint-linear-superior joint-middle joint-linear-inferior
  212. joint-inferior-0 joint-inferior-1 joint-inferior-2))
  213. ;; weight
  214. (send limb-superior-parent-link :weight (* 0.5 superior-link-weight))
  215. (send limb-superior-child-link :weight (* 0.5 superior-link-weight))
  216. (send limb-inferior-parent-link :weight (* 0.5 inferior-link-weight))
  217. (send limb-inferior-child-link :weight (* 0.5 inferior-link-weight))
  218. (send limb-eef-link :weight eef-link-weight)
  219. (send self :_set-l-min-max-x-y limb-eef-link)
  220. ;; end
  221. (send self :init-ending)
  222. self
  223. ))
  224. (:_set-l-min-max-x-y
  225. (limb-eef-link)
  226. (let* ((limb-eef-link-vertices
  227. (mapcar #'(lambda (v) (send end-coords :inverse-transform-vector v))
  228. (flatten (send-all (send limb-eef-link :bodies) :vertices))))
  229. )
  230. (setq l-min-max-x-y
  231. (list :l-min-x (elt (find-extream limb-eef-link-vertices #'(lambda (v) (elt v 0)) #'<) 0)
  232. :l-max-x (elt (find-extream limb-eef-link-vertices #'(lambda (v) (elt v 0)) #'>) 0)
  233. :l-min-y (elt (find-extream limb-eef-link-vertices #'(lambda (v) (elt v 1)) #'<) 1)
  234. :l-max-y (elt (find-extream limb-eef-link-vertices #'(lambda (v) (elt v 1)) #'>) 1)
  235. ))
  236. ))
  237. (:l-min-max-x-y
  238. ()
  239. l-min-max-x-y
  240. )
  241. (:root-coords
  242. ()
  243. root-coords
  244. )
  245. (:middle-coords
  246. ()
  247. middle-coords
  248. )
  249. (:end-coords
  250. ()
  251. end-coords
  252. )
  253. (:arm-plane-normal
  254. ()
  255. (let* ((root-pos (send (send self :worldcoords) :worldpos))
  256. (middle-pos (send middle-coords :worldpos))
  257. (end-pos (send end-coords :worldpos))
  258. (outer-product-vec (v* (v- middle-pos root-pos) (v- end-pos middle-pos)))
  259. )
  260. (when (< (norm outer-product-vec) 1e-10)
  261. (warning-message 1 "[~a] superior-link and inferior-link are parallel.~%"
  262. (send (class self) :name))
  263. (return-from :arm-plane-normal nil)
  264. )
  265. (normalize-vector outer-product-vec)
  266. ))
  267. (:superior-link-stretched-length
  268. ()
  269. (distance (send root-coords :worldpos) (send middle-coords :worldpos))
  270. )
  271. (:inferior-link-stretched-length
  272. ()
  273. (distance (send end-coords :worldpos) (send middle-coords :worldpos))
  274. )
  275. )
  276.  
  277. (defmethod two-stretchable-links-limb
  278. (:inverse-kinematics-analytical
  279. (tc)
  280. (send self :init-pose)
  281. (let* ((target-length (distance (send tc :worldpos) (send root-coords :worldpos)))
  282. (superior-length superior-link-length) (inferior-length inferior-link-length)
  283. (linear-angle-list) (middle-angle) (superior-angle-list) (inferior-angle-list)
  284. )
  285. (setq linear-angle-list
  286. (send self :_inverse-kinematics-analytical-length tc
  287. :target-length target-length))
  288. (unless (= (elt linear-angle-list 0) 0)
  289. (send joint-linear-superior :joint-angle (elt linear-angle-list 0))
  290. (setq superior-length (send self :superior-link-stretched-length))
  291. )
  292. (unless (= (elt linear-angle-list 1) 0)
  293. (send joint-linear-inferior :joint-angle (elt linear-angle-list 1))
  294. (setq inferior-length (send self :inferior-link-stretched-length))
  295. )
  296.  
  297. (setq middle-angle
  298. (send self :_inverse-kinematics-analytical-middle tc
  299. :target-length target-length :superior-length superior-length :inferior-length inferior-length))
  300. (send joint-middle :joint-angle middle-angle)
  301.  
  302. (setq superior-angle-list
  303. (send self :_inverse-kinematics-analytical-superior tc
  304. :target-length target-length :superior-length superior-length :inferior-length inferior-length))
  305. (send joint-superior-0 :joint-angle (elt superior-angle-list 0))
  306. (send joint-superior-1 :joint-angle (elt superior-angle-list 1))
  307.  
  308. (setq inferior-angle-list
  309. (send self :_inverse-kinematics-analytical-inferior tc))
  310. (send joint-inferior-0 :joint-angle (elt inferior-angle-list 0))
  311. (send joint-inferior-1 :joint-angle (elt inferior-angle-list 1))
  312. (send joint-inferior-2 :joint-angle (elt inferior-angle-list 2))
  313. (send self :angle-vector)
  314. ))
  315. (:_inverse-kinematics-analytical-length
  316. (tc
  317. &key
  318. (target-length (distance (send tc :worldpos) (send root-coords :worldpos)))
  319. )
  320. (let* ((linear-superior-angle 0)
  321. (linear-inferior-angle 0)
  322. )
  323. (cond ((and (< target-length (+ superior-link-length inferior-link-length))
  324. (> target-length (abs (- superior-link-length inferior-link-length))))
  325. )
  326. ((<= target-length (abs (- superior-link-length inferior-link-length)))
  327. (cond ((> superior-link-length inferior-link-length)
  328. (setq linear-superior-angle
  329. (- target-length (abs (- superior-link-length inferior-link-length))))
  330. )
  331. (t
  332. (setq linear-inferior-angle
  333. (- target-length (abs (- superior-link-length inferior-link-length))))
  334. ))
  335. )
  336. ((>= target-length (+ superior-link-length inferior-link-length))
  337. (setq linear-inferior-angle (* 0.5 (- target-length (+ superior-link-length inferior-link-length))))
  338. (setq linear-superior-angle (* 0.5 (- target-length (+ superior-link-length inferior-link-length))))
  339. ))
  340. (list linear-superior-angle linear-inferior-angle)
  341. ))
  342. (:_inverse-kinematics-analytical-middle
  343. (tc
  344. &key
  345. (target-length (distance (send tc :worldpos) (send root-coords :worldpos)))
  346. (superior-length (send self :superior-link-stretched-length))
  347. (inferior-length (send self :inferior-link-stretched-length))
  348. )
  349. (let* ((middle-angle
  350. (acos (min 1 (max -1
  351. (/ (- (expt target-length 2) (expt superior-length 2) (expt inferior-length 2))
  352. (* 2.0 superior-length inferior-length))))))
  353. )
  354. (rad2deg middle-angle)
  355. ))
  356. (:_inverse-kinematics-analytical-superior
  357. (tc
  358. &key
  359. (target-length (distance (send tc :worldpos) (send root-coords :worldpos)))
  360. (superior-length (send self :superior-link-stretched-length))
  361. (inferior-length (send self :inferior-link-stretched-length))
  362. )
  363. (let* ((tc-local (send root-coords :transformation tc))
  364. (x (elt (send tc-local :pos) 0))
  365. (y (elt (send tc-local :pos) 1))
  366. (z (elt (send tc-local :pos) 2))
  367. (xyz-norm (norm (float-vector x y z)))
  368. (xy-norm (norm (float-vector x y)))
  369. (theta
  370. (if (eps= xyz-norm 0 1e-10) 0.0 (acos (/ z (norm (float-vector x y z))))))
  371. (phi
  372. (if (eps= xy-norm 0 1e-10) 0.0 (* (if (> y 0) 1 -1) (acos (/ x xy-norm)))))
  373. (theta-offset
  374. (acos (min 1 (max -1
  375. (/ (+ (expt target-length 2) (expt superior-length 2) (* -1 (expt inferior-length 2)))
  376. (* 2.0 target-length superior-length))))))
  377. )
  378. (list (rad2deg phi) (rad2deg (- theta theta-offset)))
  379. ))
  380. (:_inverse-kinematics-analytical-inferior
  381. (tc)
  382. (let* ((end2middle-vec
  383. (v- (send middle-coords :worldpos) (send tc :worldpos)))
  384. (end2middle-vec-local
  385. (send tc :inverse-rotate-vector end2middle-vec))
  386. (x (elt end2middle-vec-local 0))
  387. (y (elt end2middle-vec-local 1))
  388. (z (elt end2middle-vec-local 2))
  389. (theta
  390. (acos (/ z (norm (float-vector x y z)))))
  391. (phi
  392. (* (signum y) (acos (/ x (norm (float-vector x y))))))
  393. (joint-middle-axis
  394. (send (send (send joint-middle :child-link) :worldcoords)
  395. :rotate-vector (float-vector 0 1 0)))
  396. (transed-end-axis
  397. (send (send (send (send tc :copy-worldcoords) :rotate phi :z) :rotate theta :y)
  398. :rotate-vector (float-vector 0 1 0)))
  399. (yaw
  400. (vector-angle joint-middle-axis transed-end-axis (scale -1 (normalize-vector end2middle-vec))))
  401. )
  402. (list (rad2deg yaw) (rad2deg theta) (rad2deg phi))
  403. ))
  404. )
  405.  
  406.  
  407. ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
  408. ;; test
  409. ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
  410.  
  411. (defun test-two-stretchable-links-limb-ik
  412. (&key
  413. (wait? nil)
  414. )
  415. (dotimes (i 100)
  416. (let* ((tc (make-coords :pos (scale (random 1000) (random-vector)) :rpy (scale (random pi) (random-vector))))
  417. )
  418. (setq *limb* (instance two-stretchable-links-limb :init
  419. :superior-link-length (+ (random 100.0) 100.0) :inferior-link-length (+ (random 0.0) 100.0)))
  420. (test-two-stretchable-links-limb-ik-one tc)
  421. (when wait? (read-line))
  422. ))
  423.  
  424. (setq *limb* (instance two-stretchable-links-limb :init :superior-link-length 100 :inferior-link-length 100))
  425. (test-two-stretchable-links-limb-ik-one (make-coords))
  426. (when wait? (read-line))
  427.  
  428. (setq *limb* (instance two-stretchable-links-limb :init :superior-link-length 200 :inferior-link-length 100))
  429. (test-two-stretchable-links-limb-ik-one (make-coords))
  430. (when wait? (read-line))
  431.  
  432. (setq *limb* (instance two-stretchable-links-limb :init :superior-link-length 100 :inferior-link-length 200))
  433. (test-two-stretchable-links-limb-ik-one (make-coords))
  434. (when wait? (read-line))
  435.  
  436. (setq *limb* (instance two-stretchable-links-limb :init :superior-link-length 100 :inferior-link-length 100))
  437. (test-two-stretchable-links-limb-ik-one (make-coords :pos (float-vector 0 0 100)))
  438. (when wait? (read-line))
  439.  
  440. (setq *limb* (instance two-stretchable-links-limb :init :superior-link-length 100 :inferior-link-length 100))
  441. (test-two-stretchable-links-limb-ik-one (make-coords :pos (float-vector 0 0 200)))
  442. (when wait? (read-line))
  443.  
  444. (setq *limb* (instance two-stretchable-links-limb :init :superior-link-length 100 :inferior-link-length 100))
  445. (test-two-stretchable-links-limb-ik-one (make-coords :pos (float-vector 0 0 300)))
  446. (when wait? (read-line))
  447.  
  448. (setq *limb* (instance two-stretchable-links-limb :init :superior-link-length 100 :inferior-link-length 100))
  449. (test-two-stretchable-links-limb-ik-one (make-coords :pos (float-vector 100 0 0)))
  450. (when wait? (read-line))
  451.  
  452. (setq *limb* (instance two-stretchable-links-limb :init :superior-link-length 100 :inferior-link-length 100))
  453. (test-two-stretchable-links-limb-ik-one (make-coords :pos (float-vector -100 0 0)))
  454. (when wait? (read-line))
  455.  
  456. (setq *limb* (instance two-stretchable-links-limb :init :superior-link-length 100 :inferior-link-length 100))
  457. (test-two-stretchable-links-limb-ik-one (make-coords :pos (float-vector 0 100 0)))
  458. (when wait? (read-line))
  459.  
  460. (setq *limb* (instance two-stretchable-links-limb :init :superior-link-length 100 :inferior-link-length 100))
  461. (test-two-stretchable-links-limb-ik-one (make-coords :pos (float-vector 0 -100 0)))
  462. (when wait? (read-line))
  463. )
  464.  
  465. (defun test-two-stretchable-links-limb-ik-one
  466. (tc)
  467. (send *limb* :inverse-kinematics-analytical tc)
  468. (objects (list *limb*))
  469. (send tc :draw-on :flush t :size 75 :width 5 :color #f(1 0.5 0.5))
  470. (send (send *limb* :end-coords) :draw-on :flush t :size 50 :width 10 :color #f(0.5 1 0.5))
  471. (assert (eps-coords= tc (send *limb* :end-coords)))
  472. )
  473.  
  474. (warn "(test-two-stretchable-links-limb-ik)~%")
Add Comment
Please, Sign In to add comment