Menu
▾
▴
jskeus-commit — jskeus commit mailing list
You can subscribe to this list here.
| 2011 |
Jan
|
Feb
|
Mar
|
Apr
|
May
|
Jun
(9) |
Jul
(31) |
Aug
|
Sep
(15) |
Oct
(11) |
Nov
(15) |
Dec
(10) |
|---|---|---|---|---|---|---|---|---|---|---|---|---|
| 2012 |
Jan
(11) |
Feb
(15) |
Mar
(36) |
Apr
(8) |
May
(11) |
Jun
(14) |
Jul
(16) |
Aug
(1) |
Sep
(8) |
Oct
(37) |
Nov
(4) |
Dec
(3) |
| 2013 |
Jan
(1) |
Feb
(7) |
Mar
(17) |
Apr
(29) |
May
(23) |
Jun
(45) |
Jul
(8) |
Aug
(13) |
Sep
(7) |
Oct
(11) |
Nov
(25) |
Dec
(40) |
| 2014 |
Jan
(23) |
Feb
(34) |
Mar
(1) |
Apr
(8) |
May
(50) |
Jun
|
Jul
(2) |
Aug
|
Sep
(7) |
Oct
|
Nov
|
Dec
|
| 2015 |
Jan
(6) |
Feb
|
Mar
|
Apr
|
May
|
Jun
|
Jul
|
Aug
|
Sep
|
Oct
|
Nov
|
Dec
|
|
From: <sn...@us...> - 2013-12-15 07:49:34
|
Revision: 1013
http://sourceforge.net/p/jskeus/code/1013
Author: snozawa
Date: 2013-12-15 07:49:30 +0000 (Sun, 15 Dec 2013)
Log Message:
-----------
add calculation of root-angular-velocity and root-spacial-velocity
Modified Paths:
--------------
trunk/irteus/irtdyna.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2013-12-13 16:32:52 UTC (rev 1012)
+++ trunk/irteus/irtdyna.l 2013-12-15 07:49:30 UTC (rev 1013)
@@ -546,9 +546,9 @@
moment (float-vector 0 0 0) ;; [Nm] = [kg m^2/s^2]
ext-force (float-vector 0 0 0) ;; [N] = [kg m/s^2]
ext-moment (float-vector 0 0 0))) ;; [Nm] = [kg m^2/s^2]
- (:angular-velocity () angular-velocity)
+ (:angular-velocity (&optional aa) (if aa (setq angular-velocity aa) angular-velocity))
(:angular-acceleration (&optional aa) (if aa (setq angular-acceleration aa) angular-acceleration))
- (:spacial-velocity () spacial-velocity)
+ (:spacial-velocity (&optional aa) (if aa (setq spacial-velocity aa) spacial-velocity))
(:spacial-acceleration (&optional sa) (if sa (setq spacial-acceleration sa) spacial-acceleration))
(:force () force)
(:moment () moment)
@@ -678,6 +678,8 @@
(&key (debug-view nil)
(jvv (instantiate float-vector (length joint-list))) ;; [rad/s] or [m/s]
(jav (instantiate float-vector (length joint-list))) ;; [rad/s^2] or [m/s^2]
+ (root-spacial-velocity (float-vector 0 0 0))
+ (root-angular-velocity (float-vector 0 0 0))
(root-spacial-acceleration (float-vector 0 0 0))
(root-angular-acceleration (float-vector 0 0 0))
(calc-torque-buffer-args (send self :calc-torque-buffer-args)))
@@ -697,6 +699,10 @@
root-spacial-acceleration)
(send (car (send self :links)) :angular-acceleration
root-angular-acceleration) ;; [rad/s^2]
+ (send (car (send self :links)) :spacial-velocity ;; [m/s]
+ root-spacial-velocity)
+ (send (car (send self :links)) :angular-velocity ;; [rad/s]
+ root-angular-velocity)
;; recursive calculation from root-link
(send* (car (send self :links)) :forward-all-kinematics
@@ -733,7 +739,9 @@
(send self :put :prev-root-coords root-coords)
(send self :put :prev-root-v rv)
(send self :put :prev-root-w rw)
- (list :root-spacial-acceleration ;; [m/s^2]
+ (list :root-spacial-velocity (v- rv (v* rw (scale 1e-3 (send root-coords :worldpos)))) ;; [m/s]
+ :root-angular-velocity rw ;; [rad/s]
+ :root-spacial-acceleration ;; [m/s^2]
(v- rva (v+ (v* rwa (scale 1e-3 (send root-coords :worldpos))) ;; [rad/s^2] * [m]
(v* rw rv))) ;; [rad/s] * [m/s]
:root-angular-acceleration rwa) ;; [rad/s^2]
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-12-13 16:32:55
|
Revision: 1012
http://sourceforge.net/p/jskeus/code/1012
Author: snozawa
Date: 2013-12-13 16:32:52 +0000 (Fri, 13 Dec 2013)
Log Message:
-----------
add root-angular-acceleration and root-spacial-acceleration
Modified Paths:
--------------
trunk/irteus/irtdyna.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2013-12-13 15:56:45 UTC (rev 1011)
+++ trunk/irteus/irtdyna.l 2013-12-13 16:32:52 UTC (rev 1012)
@@ -678,6 +678,8 @@
(&key (debug-view nil)
(jvv (instantiate float-vector (length joint-list))) ;; [rad/s] or [m/s]
(jav (instantiate float-vector (length joint-list))) ;; [rad/s^2] or [m/s^2]
+ (root-spacial-acceleration (float-vector 0 0 0))
+ (root-angular-acceleration (float-vector 0 0 0))
(calc-torque-buffer-args (send self :calc-torque-buffer-args)))
(let ((torque-vector (instantiate float-vector (length joint-list))))
(all-child-links
@@ -691,6 +693,10 @@
(send jnt :joint-velocity (elt jvv i))
(send jnt :joint-acceleration (elt jav i))
))
+ (send (car (send self :links)) :spacial-acceleration ;; [m/s^2]
+ root-spacial-acceleration)
+ (send (car (send self :links)) :angular-acceleration
+ root-angular-acceleration) ;; [rad/s^2]
;; recursive calculation from root-link
(send* (car (send self :links)) :forward-all-kinematics
@@ -727,10 +733,10 @@
(send self :put :prev-root-coords root-coords)
(send self :put :prev-root-v rv)
(send self :put :prev-root-w rw)
- (list :root-coords-spacial-acceleration ;; [m/s^2]
+ (list :root-spacial-acceleration ;; [m/s^2]
(v- rva (v+ (v* rwa (scale 1e-3 (send root-coords :worldpos))) ;; [rad/s^2] * [m]
(v* rw rv))) ;; [rad/s] * [m/s]
- :root-coords-angular-acceleration rwa) ;; [rad/s^2]
+ :root-angular-acceleration rwa) ;; [rad/s^2]
))
(:calc-av-vel-acc-from-pos
(dt av)
@@ -768,17 +774,14 @@
(send l :ext-moment (float-vector 0 0 0))))
;; set spacial-acceleration and angular-acceleration of root-link
- (let* ((ret (send self :calc-root-coords-vel-acc-from-pos dt root-coords)))
- (send (car (send self :links)) :spacial-acceleration ;; [m/s^2]
- (cadr (memq :root-coords-spacial-acceleration ret)))
- (send (car (send self :links)) :angular-acceleration
- (cadr (memq :root-coords-angular-acceleration ret)))) ;; [rad/s^2]
-
;; set joint velocities and joint accelerations
- (let* ((ret (send self :calc-av-vel-acc-from-pos dt av)))
+ (let* ((ret-rc (send self :calc-root-coords-vel-acc-from-pos dt root-coords))
+ (ret-av (send self :calc-av-vel-acc-from-pos dt av)))
(send self :calc-torque
- :jvv (cadr (memq :joint-velocity-vector ret))
- :jav (cadr (memq :joint-acceleration-vector ret))
+ :jvv (cadr (memq :joint-velocity-vector ret-av))
+ :jav (cadr (memq :joint-acceleration-vector ret-av))
+ :root-spacial-acceleration (cadr (memq :root-spacial-acceleration ret-rc))
+ :root-angular-acceleration (cadr (memq :root-angular-acceleration ret-rc))
:debug-view debug-view
:calc-torque-buffer-args calc-torque-buffer-args))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-12-13 15:56:48
|
Revision: 1011
http://sourceforge.net/p/jskeus/code/1011
Author: snozawa
Date: 2013-12-13 15:56:45 +0000 (Fri, 13 Dec 2013)
Log Message:
-----------
separate calc-root-coords-vel-acc-from-pos and calc-av-vel-acc-from-pos
Modified Paths:
--------------
trunk/irteus/irtdyna.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2013-12-11 21:58:26 UTC (rev 1010)
+++ trunk/irteus/irtdyna.l 2013-12-13 15:56:45 UTC (rev 1011)
@@ -706,7 +706,49 @@
(dotimes (i (length torque-vector))
(setf (elt torque-vector i) (send (elt joint-list i) :joint-torque)))
torque-vector))
-
+ (:calc-root-coords-vel-acc-from-pos
+ (dt root-coords)
+ ;; set spacial-acceleration and angular-acceleration of root-link
+ (let* ((dt-1 (/ 1.0 dt))
+ (proot-coords
+ (if (send self :get :prev-root-coords)
+ (send self :get :prev-root-coords)
+ root-coords))
+ (rv (scale (* 1e-3 dt-1) (v- (send root-coords :worldpos) (send proot-coords :worldpos)))) ;; [m/s]
+ (rw (scale dt-1 (send proot-coords :rotate-vector (send proot-coords :difference-rotation root-coords)))) ;; [rad/s]
+ (prv (if (send self :get :prev-root-v)
+ (send self :get :prev-root-v)
+ rv)) ;; [m/s]
+ (prw (if (send self :get :prev-root-w)
+ (send self :get :prev-root-w)
+ rw)) ;; [rad/s]
+ (rva (scale dt-1 (v- rv prv))) ;; [m/s^2]
+ (rwa (scale dt-1 (v- rw prw)))) ;; [rad/s^2]
+ (send self :put :prev-root-coords root-coords)
+ (send self :put :prev-root-v rv)
+ (send self :put :prev-root-w rw)
+ (list :root-coords-spacial-acceleration ;; [m/s^2]
+ (v- rva (v+ (v* rwa (scale 1e-3 (send root-coords :worldpos))) ;; [rad/s^2] * [m]
+ (v* rw rv))) ;; [rad/s] * [m/s]
+ :root-coords-angular-acceleration rwa) ;; [rad/s^2]
+ ))
+ (:calc-av-vel-acc-from-pos
+ (dt av)
+ ;; set joint velocities and joint accelerations
+ (let* ((dt-1 (/ 1.0 dt))
+ (pav (if (send self :get :prev-av)
+ (send self :get :prev-av)
+ av))
+ (jvv (scale dt-1 (map float-vector #'deg2rad (v- av pav)))) ;; [rad/s]
+ (pjvv (if (send self :get :prev-jvv)
+ (send self :get :prev-jvv)
+ jvv)) ;; [rad/s]
+ (jav (scale dt-1 (v- jvv pjvv)))) ;; [rad/s^2]
+ (send self :put :prev-av av)
+ (send self :put :prev-jvv jvv)
+ (list :joint-velocity-vector jvv
+ :joint-acceleration-vector jav)
+ ))
;; calculate Zero Moment Point based on Inverse Dynamics
;; necessary arguments -> av and root-coords
(:calc-zmp
@@ -726,45 +768,19 @@
(send l :ext-moment (float-vector 0 0 0))))
;; set spacial-acceleration and angular-acceleration of root-link
- (let* ((dt-1 (/ 1.0 dt))
- (proot-coords
- (if (send self :get :prev-root-coords)
- (send self :get :prev-root-coords)
- root-coords))
- (rv (scale (* 1e-3 dt-1) (v- (send root-coords :worldpos) (send proot-coords :worldpos)))) ;; [m/s]
- (rw (scale dt-1 (send proot-coords :rotate-vector (send proot-coords :difference-rotation root-coords)))) ;; [rad/s]
- (prv (if (send self :get :prev-root-v)
- (send self :get :prev-root-v)
- rv)) ;; [m/s]
- (prw (if (send self :get :prev-root-w)
- (send self :get :prev-root-w)
- rw)) ;; [rad/s]
- (rva (scale dt-1 (v- rv prv))) ;; [m/s^2]
- (rwa (scale dt-1 (v- rw prw)))) ;; [rad/s^2]
+ (let* ((ret (send self :calc-root-coords-vel-acc-from-pos dt root-coords)))
(send (car (send self :links)) :spacial-acceleration ;; [m/s^2]
- (v- rva (v+ (v* rwa (scale 1e-3 (send root-coords :worldpos))) ;; [rad/s^2] * [m]
- (v* rw rv)))) ;; [rad/s] * [m/s]
- (send (car (send self :links)) :angular-acceleration rwa) ;; [rad/s^2]
- (send self :put :prev-root-coords root-coords)
- (send self :put :prev-root-v rv)
- (send self :put :prev-root-w rw))
+ (cadr (memq :root-coords-spacial-acceleration ret)))
+ (send (car (send self :links)) :angular-acceleration
+ (cadr (memq :root-coords-angular-acceleration ret)))) ;; [rad/s^2]
;; set joint velocities and joint accelerations
- (let* ((dt-1 (/ 1.0 dt))
- (pav (if (send self :get :prev-av)
- (send self :get :prev-av)
- av))
- (jvv (scale dt-1 (map float-vector #'deg2rad (v- av pav)))) ;; [rad/s]
- (pjvv (if (send self :get :prev-jvv)
- (send self :get :prev-jvv)
- jvv)) ;; [rad/s]
- (jav (scale dt-1 (v- jvv pjvv)))) ;; [rad/s^2]
+ (let* ((ret (send self :calc-av-vel-acc-from-pos dt av)))
(send self :calc-torque
- :jvv jvv :jav jav
+ :jvv (cadr (memq :joint-velocity-vector ret))
+ :jav (cadr (memq :joint-acceleration-vector ret))
:debug-view debug-view
- :calc-torque-buffer-args calc-torque-buffer-args)
- (send self :put :prev-av av)
- (send self :put :prev-jvv jvv))
+ :calc-torque-buffer-args calc-torque-buffer-args))
;; calculate ZMP from force and moment of root-link
(let* ((f0 (send (car (send self :links)) :force)) ;; [N]
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-12-11 21:58:29
|
Revision: 1010
http://sourceforge.net/p/jskeus/code/1010
Author: snozawa
Date: 2013-12-11 21:58:26 +0000 (Wed, 11 Dec 2013)
Log Message:
-----------
enable to return ik return value for walking angle-vector solution
Modified Paths:
--------------
trunk/irteus/irtdyna.l
trunk/irteus/irtrobot.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2013-12-11 16:22:23 UTC (rev 1009)
+++ trunk/irteus/irtdyna.l 2013-12-11 21:58:26 UTC (rev 1010)
@@ -1342,17 +1342,18 @@
(support-leg support-leg-coords swing-leg-coords cog
&key (solve-angle-vector :solve-av-by-move-centroid-on-foot)
(solve-angle-vector-args))
- (cond
- ((functionp solve-angle-vector)
- (apply solve-angle-vector
- support-leg support-leg-coords
- swing-leg-coords cog robot solve-angle-vector-args))
- ((and (symbolp solve-angle-vector) (find-method self solve-angle-vector))
- (send* self solve-angle-vector
- support-leg support-leg-coords
- swing-leg-coords cog robot solve-angle-vector-args))
- (t (error ";; in :solve-angle-vector, invalid function or method~%!")))
- (list (send robot :angle-vector) (send robot :copy-worldcoords)))
+ (let ((ik-ret
+ (cond
+ ((functionp solve-angle-vector)
+ (apply solve-angle-vector
+ support-leg support-leg-coords
+ swing-leg-coords cog robot solve-angle-vector-args))
+ ((and (symbolp solve-angle-vector) (find-method self solve-angle-vector))
+ (send* self solve-angle-vector
+ support-leg support-leg-coords
+ swing-leg-coords cog robot solve-angle-vector-args))
+ (t (error ";; in :solve-angle-vector, invalid function or method~%!")))))
+ (list ik-ret (send robot :copy-worldcoords))))
;; solve-angle-vector methods
(:solve-av-by-move-centroid-on-foot
Modified: trunk/irteus/irtrobot.l
===================================================================
--- trunk/irteus/irtrobot.l 2013-12-11 16:22:23 UTC (rev 1009)
+++ trunk/irteus/irtrobot.l 2013-12-11 21:58:26 UTC (rev 1010)
@@ -631,7 +631,7 @@
(elt ret 7) ;; pz
czmp dt))
(push
- (list :angle-vector (send self :angle-vector)
+ (list :angle-vector (car ret);;(send self :angle-vector)
:root-coords (send (car (send self :links)) :copy-worldcoords)
:czmp czmp :refzmp (elt ret 5) :cog (elt ret 6)
:swing-leg-coords (elt ret 4)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-12-11 16:22:27
|
Revision: 1009
http://sourceforge.net/p/jskeus/code/1009
Author: snozawa
Date: 2013-12-11 16:22:23 +0000 (Wed, 11 Dec 2013)
Log Message:
-----------
add go-backward-over mode
Modified Paths:
--------------
trunk/irteus/demo/walk-motion.l
Modified: trunk/irteus/demo/walk-motion.l
===================================================================
--- trunk/irteus/demo/walk-motion.l 2013-12-11 16:21:09 UTC (rev 1008)
+++ trunk/irteus/demo/walk-motion.l 2013-12-11 16:22:23 UTC (rev 1009)
@@ -46,17 +46,26 @@
(walk-motion *robot*))
(warn "(walk-motion-for-sample-robot) for walking motion~%")
-(defun quad-walk-motion-for-sample-robot ()
+(defun quad-walk-motion-for-sample-robot
+ (&key (go-backward-over t))
(unless (boundp '*robot*)
(setq *robot* (instance sample-robot :init)))
;; initial quad pose
(send *robot* :reset-pose)
(send *robot* :fix-leg-to-coords (make-coords) '(:rleg :lleg))
- (let ((tc (list
- (make-coords :pos #f(300 -120 0))
- (make-coords :pos #f(300 120 0))
- (make-coords :pos #f(-200 -120 0) :rpy (list 0 pi/2 0))
- (make-coords :pos #f(-200 120 0) :rpy (list 0 pi/2 0))))
+ (send *robot* :rotate (if go-backward-over -pi/2 pi/2) :y)
+ (let ((tc
+ (if go-backward-over
+ (list
+ (make-coords :pos #f(300 -120 0))
+ (make-coords :pos #f(300 120 0))
+ (make-coords :pos #f(-200 -120 0) :rpy (list 0 pi/2 0))
+ (make-coords :pos #f(-200 120 0) :rpy (list 0 pi/2 0)))
+ (list
+ (make-coords :pos #f(-300 -120 0))
+ (make-coords :pos #f(-300 120 0))
+ (make-coords :pos #f(200 -120 0) :rpy (list 0 pi/2 0))
+ (make-coords :pos #f(200 120 0) :rpy (list 0 pi/2 0)))))
(ik-args
(list :min (float-vector -1000 -1000 -1000 -90 -90 -90)
:max (float-vector 1000 1000 1000 90 90 90)
@@ -67,7 +76,8 @@
#'(lambda () (send *robot* :joint-angle-limit-nspace-for-6dof :limbs '(:rarm :larm :rleg :lleg))))
)
:root-link-virtual-joint-weight #F(0.1 0.1 0.1 0.5 0.5 0.5)
- :cog-gain 5.0 :stop 200
+ :cog-gain 5.0 :stop 200 :centroid-thre 15
+ ;;:debug-view :no-message
:collision-avoidance-link-pair nil)))
(with-move-target-link-list
(mt ll *robot* '(:rleg :lleg :rarm :larm))
@@ -75,7 +85,6 @@
tc
:move-target mt :link-list ll
:target-centroid-pos (vector-mean (send-all tc :worldpos))
- ;;:debug-view :no-message
ik-args))
;; prepare footsteps
(let ((footstep-list
@@ -102,7 +111,6 @@
:init-pose-function
#'(lambda ()
(send* *robot* :move-centroid-on-foot :both '(:rleg :lleg :rarm :larm)
- ;;:debug-view :no-message
:target-centroid-pos
(vector-mean (append (send *robot* :arms :end-coords :worldpos) (send *robot* :legs :end-coords :worldpos)))
ik-args))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-12-11 16:21:12
|
Revision: 1008
http://sourceforge.net/p/jskeus/code/1008
Author: snozawa
Date: 2013-12-11 16:21:09 +0000 (Wed, 11 Dec 2013)
Log Message:
-----------
fix orientation calculation ;; add additional-nspace-list
Modified Paths:
--------------
trunk/irteus/irtdyna.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2013-12-11 11:53:13 UTC (rev 1007)
+++ trunk/irteus/irtdyna.l 2013-12-11 16:21:09 UTC (rev 1008)
@@ -1357,7 +1357,7 @@
;; solve-angle-vector methods
(:solve-av-by-move-centroid-on-foot
(support-leg support-leg-coords swing-leg-coords cog robot
- &rest args &key (cog-gain 3.5) (stop 100) &allow-other-keys)
+ &rest args &key (cog-gain 3.5) (stop 100) (additional-nspace-list) &allow-other-keys)
(let ((legs
(append (send self :get-counter-footstep-limbs support-leg)
support-leg))
@@ -1367,17 +1367,20 @@
:fix-limbs-target-coords fix-coords
:cog-gain cog-gain :stop stop
:additional-nspace-list
- (list (list (car (send robot :links))
- #'(lambda ()
- (let ((xvr (send robot :rotate-vector (case (length legs) (2 #f(1 0 0)) (4 #f(0 0 -1)))))
- (xvf (send (apply #'midcoords 0.5
- (mapcar #'(lambda (x) (elt fix-coords (position x legs))) '(:rleg :lleg)))
- :rotate-vector #f(1 0 0))))
- (dolist (xv (list xvr xvf)) (setf (elt xv 2) 0.0))
- (float-vector 0 0 0 0 0
- (* (if (and (eps= (norm xvf) 0.0) (eps= (norm xvr) 0.0))
- 0.0 (acos (v. (normalize-vector xvf) (normalize-vector xvr))))
- (if (> (elt (v* xvf xvr) 2) 0.0) -1.0 1.0)))))))
+ (append
+ additional-nspace-list
+ (list (list (car (send robot :links))
+ #'(lambda ()
+ (let* ((fcoords (apply #'midcoords 0.5
+ (mapcar #'(lambda (x) (elt fix-coords (position x legs))) '(:rleg :lleg))))
+ (xvr (send robot :rotate-vector (case (length legs) (2 #f(1 0 0)) (4 #f(0 0 1)))))
+ (xvf (send fcoords :rotate-vector #f(1 0 0))))
+ (if (> (elt (send fcoords :inverse-rotate-vector xvr) 0) 0) (setq xvr (scale -1 xvr)))
+ (dolist (xv (list xvr xvf)) (setf (elt xv 2) 0.0))
+ (let ((dth (if (and (eps= (norm xvf) 0.0) (eps= (norm xvr) 0.0))
+ 0.0 (asin (elt (v* (normalize-vector xvf) (normalize-vector xvr)) 2)))))
+ (float-vector 0 0 0 0 0 dth)
+ ))))))
args)))
;; calculate midpoint using cycloig orbit
;; ratio -> midpoint ratio
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-12-11 11:53:17
|
Revision: 1007
http://sourceforge.net/p/jskeus/code/1007
Author: snozawa
Date: 2013-12-11 11:53:13 +0000 (Wed, 11 Dec 2013)
Log Message:
-----------
just fix indent
Modified Paths:
--------------
trunk/irteus/demo/walk-motion.l
Modified: trunk/irteus/demo/walk-motion.l
===================================================================
--- trunk/irteus/demo/walk-motion.l 2013-12-11 11:52:09 UTC (rev 1006)
+++ trunk/irteus/demo/walk-motion.l 2013-12-11 11:53:13 UTC (rev 1007)
@@ -77,41 +77,41 @@
:target-centroid-pos (vector-mean (send-all tc :worldpos))
;;:debug-view :no-message
ik-args))
- ;; prepare footsteps
- (let ((footstep-list
- (list (list (send *robot* :rleg :end-coords :copy-worldcoords)
- (send *robot* :larm :end-coords :copy-worldcoords))
- (list (send (send *robot* :lleg :end-coords :copy-worldcoords) :translate (float-vector 50 0 0) :world)
- (send (send *robot* :rarm :end-coords :copy-worldcoords) :translate (float-vector 50 0 0) :world))
- (list (send (send *robot* :rleg :end-coords :copy-worldcoords) :translate (float-vector 100 0 0) :world)
- (send (send *robot* :larm :end-coords :copy-worldcoords) :translate (float-vector 100 0 0) :world))
- (list (send (send *robot* :lleg :end-coords :copy-worldcoords) :translate (float-vector 150 50 0) :world)
- (send (send *robot* :rarm :end-coords :copy-worldcoords) :translate (float-vector 150 50 0) :world))
- (list (send (send *robot* :rleg :end-coords :copy-worldcoords) :translate (float-vector 200 100 0) :world)
- (send (send *robot* :larm :end-coords :copy-worldcoords) :translate (float-vector 200 100 0) :world))
- (list (send (send *robot* :lleg :end-coords :copy-worldcoords) :translate (float-vector 200 100 0) :world)
- (send (send *robot* :rarm :end-coords :copy-worldcoords) :translate (float-vector 200 100 0) :world))
- )))
- (dolist (x footstep-list)
- (send (car x) :put :l/r (if (evenp (position x footstep-list)) :rleg :lleg))
- (send (cadr x) :put :l/r (if (evenp (position x footstep-list)) :larm :rarm)))
- (objects (list *robot*))
- ;; solve walk motion
- (send *robot* :calc-walk-pattern-from-footstep-list
- footstep-list :debug-view :no-message
- :init-pose-function
- #'(lambda ()
- (send* *robot* :move-centroid-on-foot :both '(:rleg :lleg :rarm :larm)
- ;;:debug-view :no-message
- :target-centroid-pos
- (vector-mean (append (send *robot* :arms :end-coords :worldpos) (send *robot* :legs :end-coords :worldpos)))
- ik-args))
- :solve-angle-vector-args ik-args
- :default-zmp-offsets
- (list :rleg (float-vector 0 0 0) :lleg (float-vector 0 0 0)
- :rarm (float-vector 0 0 0) :larm (float-vector 0 0 0))
- :default-step-height 70)
- )))
+ ;; prepare footsteps
+ (let ((footstep-list
+ (list (list (send *robot* :rleg :end-coords :copy-worldcoords)
+ (send *robot* :larm :end-coords :copy-worldcoords))
+ (list (send (send *robot* :lleg :end-coords :copy-worldcoords) :translate (float-vector 50 0 0) :world)
+ (send (send *robot* :rarm :end-coords :copy-worldcoords) :translate (float-vector 50 0 0) :world))
+ (list (send (send *robot* :rleg :end-coords :copy-worldcoords) :translate (float-vector 100 0 0) :world)
+ (send (send *robot* :larm :end-coords :copy-worldcoords) :translate (float-vector 100 0 0) :world))
+ (list (send (send *robot* :lleg :end-coords :copy-worldcoords) :translate (float-vector 150 50 0) :world)
+ (send (send *robot* :rarm :end-coords :copy-worldcoords) :translate (float-vector 150 50 0) :world))
+ (list (send (send *robot* :rleg :end-coords :copy-worldcoords) :translate (float-vector 200 100 0) :world)
+ (send (send *robot* :larm :end-coords :copy-worldcoords) :translate (float-vector 200 100 0) :world))
+ (list (send (send *robot* :lleg :end-coords :copy-worldcoords) :translate (float-vector 200 100 0) :world)
+ (send (send *robot* :rarm :end-coords :copy-worldcoords) :translate (float-vector 200 100 0) :world))
+ )))
+ (dolist (x footstep-list)
+ (send (car x) :put :l/r (if (evenp (position x footstep-list)) :rleg :lleg))
+ (send (cadr x) :put :l/r (if (evenp (position x footstep-list)) :larm :rarm)))
+ (objects (list *robot*))
+ ;; solve walk motion
+ (send *robot* :calc-walk-pattern-from-footstep-list
+ footstep-list :debug-view :no-message
+ :init-pose-function
+ #'(lambda ()
+ (send* *robot* :move-centroid-on-foot :both '(:rleg :lleg :rarm :larm)
+ ;;:debug-view :no-message
+ :target-centroid-pos
+ (vector-mean (append (send *robot* :arms :end-coords :worldpos) (send *robot* :legs :end-coords :worldpos)))
+ ik-args))
+ :solve-angle-vector-args ik-args
+ :default-zmp-offsets
+ (list :rleg (float-vector 0 0 0) :lleg (float-vector 0 0 0)
+ :rarm (float-vector 0 0 0) :larm (float-vector 0 0 0))
+ :default-step-height 70)
+ )))
(warn "(quad-walk-motion-for-sample-robot) for walking motion~%")
(defun walk-motion-for-robots ()
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-12-11 11:52:14
|
Revision: 1006
http://sourceforge.net/p/jskeus/code/1006
Author: snozawa
Date: 2013-12-11 11:52:09 +0000 (Wed, 11 Dec 2013)
Log Message:
-----------
use absolute-p version 6dof-joint
Modified Paths:
--------------
trunk/irteus/demo/walk-motion.l
Modified: trunk/irteus/demo/walk-motion.l
===================================================================
--- trunk/irteus/demo/walk-motion.l 2013-12-11 11:47:09 UTC (rev 1005)
+++ trunk/irteus/demo/walk-motion.l 2013-12-11 11:52:09 UTC (rev 1006)
@@ -51,18 +51,32 @@
(setq *robot* (instance sample-robot :init)))
;; initial quad pose
(send *robot* :reset-pose)
- (send *robot* :newcoords (make-coords :pos #f(0 0 400) :rpy (list 0 (deg2rad -90) 0)))
- (with-move-target-link-list
- (mt ll *robot* '(:rleg :lleg :rarm :larm))
- (send *robot* :inverse-kinematics
- (list
- (make-coords :pos #f(300 -120 0))
- (make-coords :pos #f(300 120 0))
- (make-coords :pos #f(-200 -120 0) :rpy (list 0 pi/2 0))
- (make-coords :pos #f(-200 120 0) :rpy (list 0 pi/2 0)))
- :stop 200 :move-target mt :link-list ll
- ;;:debug-view :no-message
- ))
+ (send *robot* :fix-leg-to-coords (make-coords) '(:rleg :lleg))
+ (let ((tc (list
+ (make-coords :pos #f(300 -120 0))
+ (make-coords :pos #f(300 120 0))
+ (make-coords :pos #f(-200 -120 0) :rpy (list 0 pi/2 0))
+ (make-coords :pos #f(-200 120 0) :rpy (list 0 pi/2 0))))
+ (ik-args
+ (list :min (float-vector -1000 -1000 -1000 -90 -90 -90)
+ :max (float-vector 1000 1000 1000 90 90 90)
+ :joint-args '(:absolute-p t)
+ :additional-nspace-list
+ (list
+ (list (car (send *robot* :links))
+ #'(lambda () (send *robot* :joint-angle-limit-nspace-for-6dof :limbs '(:rarm :larm :rleg :lleg))))
+ )
+ :root-link-virtual-joint-weight #F(0.1 0.1 0.1 0.5 0.5 0.5)
+ :cog-gain 5.0 :stop 200
+ :collision-avoidance-link-pair nil)))
+ (with-move-target-link-list
+ (mt ll *robot* '(:rleg :lleg :rarm :larm))
+ (send* *robot* :fullbody-inverse-kinematics
+ tc
+ :move-target mt :link-list ll
+ :target-centroid-pos (vector-mean (send-all tc :worldpos))
+ ;;:debug-view :no-message
+ ik-args))
;; prepare footsteps
(let ((footstep-list
(list (list (send *robot* :rleg :end-coords :copy-worldcoords)
@@ -87,16 +101,17 @@
footstep-list :debug-view :no-message
:init-pose-function
#'(lambda ()
- (send *robot* :move-centroid-on-foot :both '(:rleg :lleg :rarm :larm)
- ;;:debug-view :no-message
- :stop 200 :root-link-virtual-joint-weight #f(0.1 0.1 0.1 0 0 0)
- :target-centroid-pos (vector-mean (append (send *robot* :arms :end-coords :worldpos) (send *robot* :legs :end-coords :worldpos)))))
- :solve-angle-vector-args (list :root-link-virtual-joint-weight #f(0.1 0.1 0.1 0 0 0))
+ (send* *robot* :move-centroid-on-foot :both '(:rleg :lleg :rarm :larm)
+ ;;:debug-view :no-message
+ :target-centroid-pos
+ (vector-mean (append (send *robot* :arms :end-coords :worldpos) (send *robot* :legs :end-coords :worldpos)))
+ ik-args))
+ :solve-angle-vector-args ik-args
:default-zmp-offsets
(list :rleg (float-vector 0 0 0) :lleg (float-vector 0 0 0)
:rarm (float-vector 0 0 0) :larm (float-vector 0 0 0))
:default-step-height 70)
- ))
+ )))
(warn "(quad-walk-motion-for-sample-robot) for walking motion~%")
(defun walk-motion-for-robots ()
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-12-11 11:47:14
|
Revision: 1005
http://sourceforge.net/p/jskeus/code/1005
Author: snozawa
Date: 2013-12-11 11:47:09 +0000 (Wed, 11 Dec 2013)
Log Message:
-----------
add absolute-p for 6dof-joint
Modified Paths:
--------------
trunk/irteus/irtmodel.l
trunk/irteus/irtrobot.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2013-12-11 07:16:50 UTC (rev 1004)
+++ trunk/irteus/irtmodel.l 2013-12-11 11:47:09 UTC (rev 1005)
@@ -520,13 +520,23 @@
(/ pi 4) (/ pi 4) (/ pi 4))) ;; [rad/s]
((:max-joint-mjt mjt)
(float-vector 100 100 100 100 100 100)) ;; [N] [N] [N] [Nm] [Nm] [Nm]
+ (absolute-p nil)
&allow-other-keys)
(setq axis (list #f(1 0 0) #f(0 1 0) #f(0 0 1) :x :y :z))
(send-super* :init :min min :max max
:max-joint-velocity mjv
:max-joint-torque mjt
args)
- (setq joint-angle (float-vector 0 0 0 0 0 0)) ;; [mm] [deg]
+ (if absolute-p
+ (let* ((c (send parent-link :transformation child-link))
+ (b (make-coords :pos (send default-coords :worldpos)))
+ (p (send b :inverse-transform-vector (send c :worldpos)))
+ (r (matrix-log (m* (transpose (send b :worldrot)) (send c :worldrot)))))
+ (setq joint-angle
+ (float-vector (elt p 0) (elt p 1) (elt p 2) ;; [mm]
+ (rad2deg (elt r 0)) (rad2deg (elt r 1)) (rad2deg (elt r 2)))) ;; [deg]
+ (setq default-coords b))
+ (setq joint-angle (float-vector 0 0 0 0 0 0))) ;; [mm] [deg]
self)
(:joint-angle
(&optional v &key relative &allow-other-keys) ;; v [mm] [deg]
Modified: trunk/irteus/irtrobot.l
===================================================================
--- trunk/irteus/irtrobot.l 2013-12-11 07:16:50 UTC (rev 1004)
+++ trunk/irteus/irtrobot.l 2013-12-11 11:47:09 UTC (rev 1005)
@@ -394,11 +394,12 @@
(cog-gain 1.0) ;; <- cog gain for null-space calculation. cog-gain should be over zero.
(centroid-thre 5.0) ;; cog convergence threshould [mm]
(additional-weight-list)
+ (joint-args nil)
&allow-other-keys)
(with-append-root-joint ;; inverse-kinematics with base-link
(link-list-with-robot-6dof self link-list
:joint-class 6dof-joint
- :joint-args (list :min min :max max))
+ :joint-args (append (list :min min :max max) joint-args))
(send* self :inverse-kinematics target-coords
:move-target move-target :link-list link-list-with-robot-6dof
:cog-gain cog-gain :centroid-thre centroid-thre :target-centroid-pos target-centroid-pos
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-12-11 07:16:54
|
Revision: 1004
http://sourceforge.net/p/jskeus/code/1004
Author: snozawa
Date: 2013-12-11 07:16:50 +0000 (Wed, 11 Dec 2013)
Log Message:
-----------
enable to set limbs
Modified Paths:
--------------
trunk/irteus/irtrobot.l
Modified: trunk/irteus/irtrobot.l
===================================================================
--- trunk/irteus/irtrobot.l 2013-12-11 04:38:52 UTC (rev 1003)
+++ trunk/irteus/irtrobot.l 2013-12-11 07:16:50 UTC (rev 1004)
@@ -409,15 +409,13 @@
args)
))
(:joint-angle-limit-nspace-for-6dof
- (&key (avoid-nspace-gain 0.01))
- (let* ((ll (mapcar #'(lambda (l) (send self l :links))
- '(:rleg :lleg)))
+ (&key (avoid-nspace-gain 0.01) (limbs '(:rleg :lleg)))
+ (let* ((ll (mapcar #'(lambda (l) (send self l :links)) limbs))
(J (send self :calc-jacobian-from-link-list
(mapcar #'(lambda (l) (append (list (car (send self :links))) l)) ll)
- :move-target (mapcar #'(lambda (l) (send self l :end-coords))
- '(:rleg :lleg))
- :translation-axis '(t t)
- :rotation-axis '(t t)))
+ :move-target (mapcar #'(lambda (l) (send self l :end-coords)) limbs)
+ :translation-axis (make-list (length limbs) :initial-element t)
+ :rotation-axis (make-list (length limbs) :initial-element t)))
(Jb (make-matrix (array-dimension J 0) 6))
(Jj (make-matrix (array-dimension J 0) (send self :calc-target-joint-dimension ll))))
(dotimes (i (array-dimension Jb 0))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-12-11 04:38:56
|
Revision: 1003
http://sourceforge.net/p/jskeus/code/1003
Author: snozawa
Date: 2013-12-11 04:38:52 +0000 (Wed, 11 Dec 2013)
Log Message:
-----------
fix calling of function for additional-weight-list and additional-nspace-list
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2013-12-11 04:06:46 UTC (rev 1002)
+++ trunk/irteus/irtmodel.l 2013-12-11 04:38:52 UTC (rev 1003)
@@ -1372,12 +1372,12 @@
(let ((ll-pos (position (car ls) union-link-list :test #'equal)))
(when ll-pos
(let ((idx (reduce #'+ (send-all (send-all (subseq union-link-list 0 ll-pos) :joint) :joint-dof)
- :initial-value 0)))
+ :initial-value 0))
+ (var (if (functionp (cadr ls)) (funcall (cadr ls)) (cadr ls))))
(dotimes (ii (send (send (car ls) :joint) :joint-dof))
- (let ((var (if (functionp (cadr ls)) (funcall (cadr ls)) (cadr ls))))
- (setf (elt weight (+ ii idx)) (* (elt weight (+ ii idx))
- (if (float-vector-p var) (elt var ii) var)))))
- ))))
+ (setf (elt weight (+ ii idx)) (* (elt weight (+ ii idx))
+ (if (float-vector-p var) (elt var ii) var)))))
+ )))
(when (and debug-view (not (memq :no-message debug-view)) additional-weight-list)
(format-array weight "addwei:"))
;; calc weight from joint limit
@@ -1447,14 +1447,14 @@
(let ((ll-pos (position (car ls) union-link-list :test #'equal)))
(when ll-pos
(let ((idx (reduce #'+ (send-all (send-all (subseq union-link-list 0 ll-pos) :joint) :joint-dof)
- :initial-value 0)))
+ :initial-value 0))
+ (var (if (functionp (cadr ls)) (funcall (cadr ls)) (cadr ls))))
(dotimes (ii (send (send (car ls) :joint) :joint-dof))
- (let ((var (if (functionp (cadr ls)) (funcall (cadr ls)) (cadr ls))))
- (setf (elt tmp-nspace (+ ii idx))
- (+ (elt tmp-nspace (+ ii idx))
- (* (if (float-vector-p var) (elt var ii) var)
- (elt weight (+ ii idx)))))))
- ))))
+ (setf (elt tmp-nspace (+ ii idx))
+ (+ (elt tmp-nspace (+ ii idx))
+ (* (if (float-vector-p var) (elt var ii) var)
+ (elt weight (+ ii idx)))))))
+ )))
(cond
((functionp null-space) (setq null-space (funcall null-space)))
((listp null-space) (setq null-space (eval null-space))))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-12-11 04:06:48
|
Revision: 1002
http://sourceforge.net/p/jskeus/code/1002
Author: snozawa
Date: 2013-12-11 04:06:46 +0000 (Wed, 11 Dec 2013)
Log Message:
-----------
add joint-angle-limit-nspace-for-6dof which is experimental currently
Modified Paths:
--------------
trunk/irteus/irtrobot.l
Modified: trunk/irteus/irtrobot.l
===================================================================
--- trunk/irteus/irtrobot.l 2013-12-10 16:02:41 UTC (rev 1001)
+++ trunk/irteus/irtrobot.l 2013-12-11 04:06:46 UTC (rev 1002)
@@ -408,6 +408,33 @@
(list (list (car (send self :links)) root-link-virtual-joint-weight)))
args)
))
+ (:joint-angle-limit-nspace-for-6dof
+ (&key (avoid-nspace-gain 0.01))
+ (let* ((ll (mapcar #'(lambda (l) (send self l :links))
+ '(:rleg :lleg)))
+ (J (send self :calc-jacobian-from-link-list
+ (mapcar #'(lambda (l) (append (list (car (send self :links))) l)) ll)
+ :move-target (mapcar #'(lambda (l) (send self l :end-coords))
+ '(:rleg :lleg))
+ :translation-axis '(t t)
+ :rotation-axis '(t t)))
+ (Jb (make-matrix (array-dimension J 0) 6))
+ (Jj (make-matrix (array-dimension J 0) (send self :calc-target-joint-dimension ll))))
+ (dotimes (i (array-dimension Jb 0))
+ (dotimes (ii (array-dimension Jb 1))
+ (setf (aref Jb i ii) (aref J i ii))))
+ (dotimes (i (array-dimension Jj 0))
+ (dotimes (ii (array-dimension Jj 1))
+ (setf (aref Jj i ii) (aref J i (+ 6 ii)))))
+ (let ((dthb
+ (transform (m* (send self :calc-inverse-jacobian Jb) Jj)
+ (scale (* -1 avoid-nspace-gain)
+ (joint-angle-limit-nspace
+ (send-all (send self :calc-union-link-list ll) :joint))))))
+ (setf (elt dthb 0) 0)
+ (setf (elt dthb 1) 0)
+ dthb
+ )))
(:joint-order
(limb &optional jname-list)
(let ((joint-list (mapcar #'(lambda (x) (cons x (find-if #'(lambda (y) (eq y (send self x))) (send self limb :joint-list)))) (mapcan #'(lambda (x) (if (substringp (format nil "~A-" (symbol-name limb)) (string x)) (list x)))(send self :methods))))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-12-10 16:02:45
|
Revision: 1001
http://sourceforge.net/p/jskeus/code/1001
Author: snozawa
Date: 2013-12-10 16:02:41 +0000 (Tue, 10 Dec 2013)
Log Message:
-----------
multiply weight for additional nspace
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2013-12-10 15:45:45 UTC (rev 1000)
+++ trunk/irteus/irtmodel.l 2013-12-10 16:02:41 UTC (rev 1001)
@@ -1452,7 +1452,8 @@
(let ((var (if (functionp (cadr ls)) (funcall (cadr ls)) (cadr ls))))
(setf (elt tmp-nspace (+ ii idx))
(+ (elt tmp-nspace (+ ii idx))
- (if (float-vector-p var) (elt var ii) var)))))
+ (* (if (float-vector-p var) (elt var ii) var)
+ (elt weight (+ ii idx)))))))
))))
(cond
((functionp null-space) (setq null-space (funcall null-space)))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-12-10 15:45:50
|
Revision: 1000
http://sourceforge.net/p/jskeus/code/1000
Author: snozawa
Date: 2013-12-10 15:45:45 +0000 (Tue, 10 Dec 2013)
Log Message:
-----------
fix order of null-space setting
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2013-12-06 04:56:04 UTC (rev 999)
+++ trunk/irteus/irtmodel.l 2013-12-10 15:45:45 UTC (rev 1000)
@@ -1443,9 +1443,6 @@
:target-centroid-pos target-centroid-pos :cog-gain cog-gain)
tmp-nspace tmp-nspace)))
;; add null-space from arguments
- (cond
- ((functionp null-space) (setq null-space (funcall null-space)))
- ((listp null-space) (setq null-space (eval null-space))))
(dolist (ls additional-nspace-list)
(let ((ll-pos (position (car ls) union-link-list :test #'equal)))
(when ll-pos
@@ -1457,6 +1454,9 @@
(+ (elt tmp-nspace (+ ii idx))
(if (float-vector-p var) (elt var ii) var)))))
))))
+ (cond
+ ((functionp null-space) (setq null-space (funcall null-space)))
+ ((listp null-space) (setq null-space (eval null-space))))
(if null-space
(setq tmp-nspace
(v+ null-space tmp-nspace tmp-nspace)))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-12-06 04:56:13
|
Revision: 999
http://sourceforge.net/p/jskeus/code/999
Author: snozawa
Date: 2013-12-06 04:56:04 +0000 (Fri, 06 Dec 2013)
Log Message:
-----------
flush starndard-input in do-until-key to use several do-until-key in one function
Modified Paths:
--------------
trunk/irteus/irtutil.l
Modified: trunk/irteus/irtutil.l
===================================================================
--- trunk/irteus/irtutil.l 2013-12-03 11:34:55 UTC (rev 998)
+++ trunk/irteus/irtutil.l 2013-12-06 04:56:04 UTC (rev 999)
@@ -42,9 +42,12 @@
(defmacro do-until-key-with-check
(check &rest forms)
- `(while (and (null (select-stream (list *standard-input*) 0.0000001))
- (eval ,check))
- ,@forms
+ `(prog1
+ (while (and (null (select-stream (list *standard-input*) 0.0000001))
+ (eval ,check))
+ ,@forms
+ )
+ (let ((strm (car (select-stream (list *standard-input*) 0.1)))) (if strm (read-line strm nil nil)))
))
(defmacro do-until-key (&rest forms)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <k-...@us...> - 2013-12-03 11:34:58
|
Revision: 998
http://sourceforge.net/p/jskeus/code/998
Author: k-okada
Date: 2013-12-03 11:34:55 +0000 (Tue, 03 Dec 2013)
Log Message:
-----------
add dif-pos-ratio and dif-rot-ratio only for velocity , see issue #261
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2013-12-03 11:27:28 UTC (rev 997)
+++ trunk/irteus/irtmodel.l 2013-12-03 11:34:55 UTC (rev 998)
@@ -1738,6 +1738,8 @@
(rthre (cond
((atom move-target) (deg2rad 1))
(t (make-list (length move-target) :initial-element (deg2rad 1)))))
+ (dif-pos-ratio 1.0)
+ (dif-rot-ratio 1.0)
union-link-list
target-coords ;required for debug-view
(jacobi)
@@ -1825,8 +1827,8 @@
(format-array vel-r "vel-rot :")
(warn "vel-pos-norm : ~7,3f/~7,3f~%vel-rot-norm : ~7,3f/~7,3f~%"
(* 1000.0 (norm vel-p)) (elt thre i) (norm vel-r) (elt rthre i)))
- (dotimes (j (length vel-p)) (setf (elt tmp-dim j) (elt vel-p j)))
- (dotimes (j (length vel-r)) (setf (elt tmp-dim (+ j (length vel-p))) (elt vel-r j)))
+ (dotimes (j (length vel-p)) (setf (elt tmp-dim j) (* dif-pos-ratio (elt vel-p j))))
+ (dotimes (j (length vel-r)) (setf (elt tmp-dim (+ j (length vel-p))) (* dif-rot-ratio (elt vel-r j))))
))
(dotimes (i (length tmp-dims))
(dotimes (j (length (elt tmp-dims i)))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <k-...@us...> - 2013-12-03 11:27:32
|
Revision: 997
http://sourceforge.net/p/jskeus/code/997
Author: k-okada
Date: 2013-12-03 11:27:28 +0000 (Tue, 03 Dec 2013)
Log Message:
-----------
revert wrong commit (r996): add dif-pos-ratio, and dif-rot-ratio option, see Issue #261
Revision Links:
--------------
http://sourceforge.net/p/jskeus/code/996
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2013-12-02 16:05:26 UTC (rev 996)
+++ trunk/irteus/irtmodel.l 2013-12-03 11:27:28 UTC (rev 997)
@@ -1881,8 +1881,6 @@
(rthre (cond
((atom move-target) (deg2rad 1))
(t (make-list (length move-target) :initial-element (deg2rad 1)))))
- (dif-pos-ratio 1.0)
- (dif-rot-ratio 1.0)
(union-link-list)
(centroid-thre 1.0) (target-centroid-pos) (centroid-offset-func)
(dump-command t)
@@ -1964,13 +1962,13 @@
target-coords))
(dif-pos
(mapcar #'(lambda (mv tc trans-axis)
- (scale dif-pos-ratio (send mv :difference-position tc
- :translation-axis trans-axis)))
+ (send mv :difference-position tc
+ :translation-axis trans-axis))
move-target target-coords translation-axis))
(dif-rot
(mapcar #'(lambda (mv tc rot-axis)
- (scale dif-rot-ratio (send mv :difference-rotation tc
- :rotation-axis rot-axis)))
+ (send mv :difference-rotation tc
+ :rotation-axis rot-axis))
move-target target-coords rotation-axis)))
(setq success (send* self :inverse-kinematics-loop
dif-pos dif-rot
@@ -2016,8 +2014,8 @@
(when warnp
(warn ";; inverse-kinematics failed.~%")
(dotimes (i (length move-target))
- (warn ";; dif-pos : ~a/(~a/~a) (~a)~%" (elt dif-pos i) (norm (elt dif-pos i)) (elt thre i) dif-pos-ratio)
- (warn ";; dif-rot : ~a/(~a/~a) (~a)~%" (elt dif-rot i) (norm (elt dif-rot i)) (elt rthre i) dif-rot-ratio))
+ (warn ";; dif-pos : ~a/(~a/~a)~%" (elt dif-pos i) (norm (elt dif-pos i)) (elt thre i))
+ (warn ";; dif-rot : ~a/(~a/~a)~%" (elt dif-rot i) (norm (elt dif-rot i)) (elt rthre i)))
(if target-centroid-pos
(let ((dif-cog (send self :difference-cog-position target-centroid-pos centroid-offset-func)))
(warn ";; cog-dif : ~a/(~a/~a)~%" dif-cog (norm dif-cog) centroid-thre)))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <k-...@us...> - 2013-12-02 16:05:29
|
Revision: 996
http://sourceforge.net/p/jskeus/code/996
Author: k-okada
Date: 2013-12-02 16:05:26 +0000 (Mon, 02 Dec 2013)
Log Message:
-----------
add dif-pos-ratio, and dif-rot-ratio option, see Issue #261
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2013-12-02 09:48:50 UTC (rev 995)
+++ trunk/irteus/irtmodel.l 2013-12-02 16:05:26 UTC (rev 996)
@@ -1881,6 +1881,8 @@
(rthre (cond
((atom move-target) (deg2rad 1))
(t (make-list (length move-target) :initial-element (deg2rad 1)))))
+ (dif-pos-ratio 1.0)
+ (dif-rot-ratio 1.0)
(union-link-list)
(centroid-thre 1.0) (target-centroid-pos) (centroid-offset-func)
(dump-command t)
@@ -1962,13 +1964,13 @@
target-coords))
(dif-pos
(mapcar #'(lambda (mv tc trans-axis)
- (send mv :difference-position tc
- :translation-axis trans-axis))
+ (scale dif-pos-ratio (send mv :difference-position tc
+ :translation-axis trans-axis)))
move-target target-coords translation-axis))
(dif-rot
(mapcar #'(lambda (mv tc rot-axis)
- (send mv :difference-rotation tc
- :rotation-axis rot-axis))
+ (scale dif-rot-ratio (send mv :difference-rotation tc
+ :rotation-axis rot-axis)))
move-target target-coords rotation-axis)))
(setq success (send* self :inverse-kinematics-loop
dif-pos dif-rot
@@ -2014,8 +2016,8 @@
(when warnp
(warn ";; inverse-kinematics failed.~%")
(dotimes (i (length move-target))
- (warn ";; dif-pos : ~a/(~a/~a)~%" (elt dif-pos i) (norm (elt dif-pos i)) (elt thre i))
- (warn ";; dif-rot : ~a/(~a/~a)~%" (elt dif-rot i) (norm (elt dif-rot i)) (elt rthre i)))
+ (warn ";; dif-pos : ~a/(~a/~a) (~a)~%" (elt dif-pos i) (norm (elt dif-pos i)) (elt thre i) dif-pos-ratio)
+ (warn ";; dif-rot : ~a/(~a/~a) (~a)~%" (elt dif-rot i) (norm (elt dif-rot i)) (elt rthre i) dif-rot-ratio))
(if target-centroid-pos
(let ((dif-cog (send self :difference-cog-position target-centroid-pos centroid-offset-func)))
(warn ";; cog-dif : ~a/(~a/~a)~%" dif-cog (norm dif-cog) centroid-thre)))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-12-02 09:48:53
|
Revision: 995
http://sourceforge.net/p/jskeus/code/995
Author: snozawa
Date: 2013-12-02 09:48:50 +0000 (Mon, 02 Dec 2013)
Log Message:
-----------
return float-vector because angle-vector should be represented as float-vector
Modified Paths:
--------------
trunk/irteus/irtrobot.l
Modified: trunk/irteus/irtrobot.l
===================================================================
--- trunk/irteus/irtrobot.l 2013-12-02 02:56:00 UTC (rev 994)
+++ trunk/irteus/irtrobot.l 2013-12-02 09:48:50 UTC (rev 995)
@@ -212,7 +212,9 @@
(send-all (send self :gripper limb :links) :joint))
((memq :angle-vector args)
(if (null (cdr args))
- (send-all (send self :gripper limb :joint-list) :joint-angle)
+ (concatenate
+ float-vector
+ (send-all (send self :gripper limb :joint-list) :joint-angle))
(map float-vector #'(lambda (x y) (send x :joint-angle y))
(send self :gripper limb :joint-list) (cadr args))))))
;;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <k-...@us...> - 2013-12-02 02:56:03
|
Revision: 994
http://sourceforge.net/p/jskeus/code/994
Author: k-okada
Date: 2013-12-02 02:56:00 +0000 (Mon, 02 Dec 2013)
Log Message:
-----------
fix dispay det(JJt) not det(J), for non square matrix
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2013-12-01 22:19:59 UTC (rev 993)
+++ trunk/irteus/irtmodel.l 2013-12-02 02:56:00 UTC (rev 994)
@@ -971,7 +971,7 @@
(let ((J#x (transform jacobi# union-vel tmp-len)))
(when (and debug-view (not (memq :no-message debug-view)))
(format-array jacobi "J :")
- (warn "det(J):~7,3f~%" (matrix-determinant jacobi) )
+ (warn "d(JJt):~7,3f~%" (matrix-determinant (m* jacobi (transpose jacobi))) )
(format-array (transpose jacobi#) "J#t :")
(format-array union-vel "x :")
(format-array (map float-vector #'rad2deg J#x) "J#x :"))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <k-...@us...> - 2013-12-01 22:20:04
|
Revision: 993
http://sourceforge.net/p/jskeus/code/993
Author: k-okada
Date: 2013-12-01 22:19:59 +0000 (Sun, 01 Dec 2013)
Log Message:
-----------
add min/max, matrix-determinant for debug-view
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2013-11-30 14:14:13 UTC (rev 992)
+++ trunk/irteus/irtmodel.l 2013-12-01 22:19:59 UTC (rev 993)
@@ -971,6 +971,7 @@
(let ((J#x (transform jacobi# union-vel tmp-len)))
(when (and debug-view (not (memq :no-message debug-view)))
(format-array jacobi "J :")
+ (warn "det(J):~7,3f~%" (matrix-determinant jacobi) )
(format-array (transpose jacobi#) "J#t :")
(format-array union-vel "x :")
(format-array (map float-vector #'rad2deg J#x) "J#x :"))
@@ -1377,7 +1378,7 @@
(setf (elt weight (+ ii idx)) (* (elt weight (+ ii idx))
(if (float-vector-p var) (elt var ii) var)))))
))))
- (when (and debug-view (not (memq :no-message debug-view)))
+ (when (and debug-view (not (memq :no-message debug-view)) additional-weight-list)
(format-array weight "addwei:"))
;; calc weight from joint limit
(setq tmp-weight
@@ -1501,7 +1502,22 @@
(if (vectorp a)
(dotimes (i (length a)) (warn "~7,1f " (elt a i)))
(warn "~7,1f " a))))
- (warn "~%"))
+ (warn "~%")
+ (warn " min :")
+ (dolist (j (send-all union-link-list :joint))
+ (let ((a (send j :min-angle)))
+ (if (vectorp a)
+ (dotimes (i (length a)) (warn "~7,1f " (elt a i)))
+ (warn "~7,1f " a))))
+ (warn "~%")
+ (warn " max :")
+ (dolist (j (send-all union-link-list :joint))
+ (let ((a (send j :max-angle)))
+ (if (vectorp a)
+ (dotimes (i (length a)) (warn "~7,1f " (elt a i)))
+ (warn "~7,1f " a))))
+ (warn "~%")
+ )
(setq weight ;; copied to tmp-weight
(send self :calc-inverse-kinematics-weight-from-link-list
link-list :weight weight :fik-len fik-len
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <k-...@us...> - 2013-11-30 14:14:18
|
Revision: 992
http://sourceforge.net/p/jskeus/code/992
Author: k-okada
Date: 2013-11-30 14:14:13 +0000 (Sat, 30 Nov 2013)
Log Message:
-----------
do nothing when x::window-main-one is called in headless system, Issue #46
Modified Paths:
--------------
trunk/irteus/irtx.l
Modified: trunk/irteus/irtx.l
===================================================================
--- trunk/irteus/irtx.l 2013-11-30 14:12:27 UTC (rev 991)
+++ trunk/irteus/irtx.l 2013-11-30 14:14:13 UTC (rev 992)
@@ -27,6 +27,9 @@
(setq *skip-motion-event* (list :motionNotify :configureNotify :expose))
(defun window-main-one (&optional fd)
+ (when (or (null *display*) (= *display* 0))
+ (warning-message 3 ";; (window-main-one ~A) was called, but no display is found.~%" fd)
+ (return-from window-main-one nil))
(catch :window-main-loop
(while (> (EventsQueued *display* 1) 0)
(NextEvent *display* event)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <k-...@us...> - 2013-11-30 14:12:33
|
Revision: 991
http://sourceforge.net/p/jskeus/code/991
Author: k-okada
Date: 2013-11-30 14:12:27 +0000 (Sat, 30 Nov 2013)
Log Message:
-----------
create irtviewer-dummy for headless system, Issue #33
Modified Paths:
--------------
trunk/irteus/irtviewer.l
Modified: trunk/irteus/irtviewer.l
===================================================================
--- trunk/irteus/irtviewer.l 2013-11-27 11:51:55 UTC (rev 990)
+++ trunk/irteus/irtviewer.l 2013-11-30 14:12:27 UTC (rev 991)
@@ -324,13 +324,7 @@
(in-package "USER")
-(setq *irtviewer-objects* nil)
(defun objects (&optional (objs t) vw)
- (when (or (null x::*display*) (= x::*display* 0))
- (if (not (eq objs t))
- (setq *irtviewer-objects* objs))
- (warning-message 1 ";; (objects ~A) was called, but no display is found.~%" (if (eq objs t) "" objs))
- (return-from objects *irtviewer-objects*))
(when
(and objs (not (boundp '*irtviewer*)) (null vw))
(warn ";; (make-irtviewer) executed~%")
@@ -346,11 +340,49 @@
;;
(proclaim '(special *irtviewer*))
(defun make-irtviewer (&rest args)
- (when (or (null x::*display*) (= x::*display* 0))
+ (cond
+ ((or (null x::*display*) (= x::*display* 0))
(warning-message 1 ";; no display is found. exit from (make-irtviewer)~%")
- (return-from make-irtviewer nil))
- (setq *irtviewer* (send-lexpr (instantiate x::irtviewer) :create args)))
+ (setq *irtviewer* (instance irtviewer-dummy))
+ (setq *viewer* (instance viewer-dummy))
+ (setf (get *viewer* :pickviewer) *irtviewer*))
+ (t
+ (setq *irtviewer* (send-lexpr (instantiate x::irtviewer) :create args))))
+ *irtviewer*)
+(defclass viewer-dummy
+ :super propertied-object
+ :slots ())
+(defmethod viewer-dummy
+ (:nomethod (&rest args) t)
+ )
+
+(defclass irtviewer-dummy
+ :super propertied-object
+ :slots (objects draw-things))
+(defmethod irtviewer-dummy
+ (:objects (&rest args)
+ (when
+ args
+ (cond
+ ((null (car args))
+ (setq objects nil))
+ ((consp (car args))
+ (setq objects (car args)))
+ ((atom (car args))
+ (setq objects (append args objects)))
+ ))
+ (send-all objects :worldcoords)
+ (setq draw-things (x::draw-things objects))
+ objects)
+ (:nomethod (&rest args) t)
+ ) ;;
+
+(defun make-irtviewer-dummy (&rest args)
+ (warning-message 1 ";; no display is found. exit from (make-irtviewer)~%")
+ (setq *irtviewer* (instance x::irtviewer-dummy)))
+
+
(in-package "GL")
(provide :irtviewer "$Id$")
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-11-27 11:51:58
|
Revision: 990
http://sourceforge.net/p/jskeus/code/990
Author: snozawa
Date: 2013-11-27 11:51:55 +0000 (Wed, 27 Nov 2013)
Log Message:
-----------
check existence of objs
Modified Paths:
--------------
trunk/irteus/irtsensor.l
Modified: trunk/irteus/irtsensor.l
===================================================================
--- trunk/irteus/irtsensor.l 2013-11-24 05:14:21 UTC (rev 989)
+++ trunk/irteus/irtsensor.l 2013-11-27 11:51:55 UTC (rev 990)
@@ -51,6 +51,7 @@
(:simulate
(objs)
(let (r)
+ (unless objs (return-from :simulate nil))
(if (atom objs) (setq objs (list objs)))
(setq
data
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-11-24 05:14:24
|
Revision: 989
http://sourceforge.net/p/jskeus/code/989
Author: snozawa
Date: 2013-11-24 05:14:21 +0000 (Sun, 24 Nov 2013)
Log Message:
-----------
add interpolation of joint-angle from min-max-table according to [#43]
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2013-11-23 04:04:54 UTC (rev 988)
+++ trunk/irteus/irtmodel.l 2013-11-24 05:14:21 UTC (rev 989)
@@ -76,10 +76,26 @@
(:max-joint-torque (&optional mjt) (if mjt (setq max-joint-torque mjt) max-joint-torque))
(:joint-min-max-table (&optional mm-table) (if mm-table (setq joint-min-max-table mm-table) joint-min-max-table))
(:joint-min-max-target (&optional mm-target) (if mm-target (setq joint-min-max-target mm-target) joint-min-max-target))
- (:joint-min-max-table-min-angle (&optional (target-angle (send joint-min-max-target :joint-angle)))
- (car (gethash (floor target-angle) joint-min-max-table)))
- (:joint-min-max-table-max-angle (&optional (target-angle (send joint-min-max-target :joint-angle)))
- (cdr (gethash (floor target-angle) joint-min-max-table)))
+ (:joint-min-max-table-angle-interpolate
+ (target-angle car-or-cadr)
+ (let* ((int-target-angle (floor target-angle)) ;; integer value convered as a hash tablle key
+ (target-range (list int-target-angle (1+ int-target-angle))) ;; range of target-joint's angle, e.g., (car target-range) <= target-ang <= (cadr target-range)
+ (joint-range (mapcar #'(lambda (x) (funcall car-or-cadr (gethash x joint-min-max-table)))
+ target-range)) ;; range of joint-angle based on target-range
+ (tmp-ratio (- target-angle (float int-target-angle))))
+ ;; overrange check
+ (unless (elt joint-range 0) (setf (elt joint-range 0) (elt joint-range 1)))
+ (unless (elt joint-range 1) (setf (elt joint-range 1) (elt joint-range 0)))
+ ;; calc min max
+ ;; interpolated joint-angle based on joint-range
+ ;; e.g., (car joint-range) <= min[max]-angle <= (cadr joint-range) or (cadr joint-range) <= min[max]-angle <= (car joint-range)
+ (+ (* (car joint-range) (- 1 tmp-ratio)) (* (cadr joint-range) tmp-ratio))))
+ (:joint-min-max-table-min-angle
+ (&optional (target-angle (send joint-min-max-target :joint-angle)))
+ (send self :joint-min-max-table-angle-interpolate target-angle #'car))
+ (:joint-min-max-table-max-angle
+ (&optional (target-angle (send joint-min-max-target :joint-angle)))
+ (send self :joint-min-max-table-angle-interpolate target-angle #'cdr))
)
(defun calc-jacobian-default-rotate-vector
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
