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-11-23 04:04:56
|
Revision: 988
http://sourceforge.net/p/jskeus/code/988
Author: snozawa
Date: 2013-11-23 04:04:54 +0000 (Sat, 23 Nov 2013)
Log Message:
-----------
fix bug of defun ) shortage
Modified Paths:
--------------
trunk/irteus/demo/full-body-ik.l
Modified: trunk/irteus/demo/full-body-ik.l
===================================================================
--- trunk/irteus/demo/full-body-ik.l 2013-11-22 23:22:56 UTC (rev 987)
+++ trunk/irteus/demo/full-body-ik.l 2013-11-23 04:04:54 UTC (rev 988)
@@ -50,6 +50,7 @@
(send *irtviewer* :flush)
)
))
+ )
;;
(unless (boundp '*irtviewer*) (make-irtviewer))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <k-...@us...> - 2013-11-22 23:22:59
|
Revision: 987
http://sourceforge.net/p/jskeus/code/987
Author: k-okada
Date: 2013-11-22 23:22:56 +0000 (Fri, 22 Nov 2013)
Log Message:
-----------
output ik-failed, see issue #42 (https://sourceforge.net/p/jskeus/tickets/42/)
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2013-11-22 23:14:48 UTC (rev 986)
+++ trunk/irteus/irtmodel.l 2013-11-22 23:22:56 UTC (rev 987)
@@ -2027,6 +2027,10 @@
(format f "(defun ~A-check () (let ((r *robot*)) (send* r :inverse-kinematics ~A)))~%" command-id command-args)
(format f "(defun ik-setup () (~A-setup))~%" command-id)
(format f "(defun ik-check () (~A-check))~%" command-id)
+ (format f "(setq ik-failed '(")
+ (format f "(:dif-pos . ~A) (:dif-rot . ~A~%)" dif-pos dif-rot)
+ (if target-centroid-pos (format f "(:dif-cog . ~A)" (send self :difference-cog-position target-centroid-pos centroid-offset-func)))
+ (format f "))~%")
)
) ;;let
) ;; dump
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <k-...@us...> - 2013-11-22 23:14:54
|
Revision: 986
http://sourceforge.net/p/jskeus/code/986
Author: k-okada
Date: 2013-11-22 23:14:48 +0000 (Fri, 22 Nov 2013)
Log Message:
-----------
find joint value with in joint limit, in case of joint is out of limit, http://sourceforge.net/p/jskeus/tickets/43/
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2013-11-22 13:59:16 UTC (rev 985)
+++ trunk/irteus/irtmodel.l 2013-11-22 23:14:48 UTC (rev 986)
@@ -703,7 +703,7 @@
(let ((i 0))
(dolist (j joint-list)
(when vec
- ;; for joint w/ min-max-table
+ ;; for joint w/ min-max-table, see http://sourceforge.net/p/jskeus/tickets/43/
(when (and (send j :joint-min-max-table) (send j :joint-min-max-target))
;; currently only 1dof joint is supported
(when (and (= (send j :joint-dof) 1) (= (send (send j :joint-min-max-target) :joint-dof) 1))
@@ -723,9 +723,31 @@
(setq (j . joint-angle) tmp-joint-angle
(jj . joint-angle) tmp-target-joint-angle))
(t
- ))
- );; let*
- )) ;; when
+ ;;
+ (do ((i 0 (incf i 0.1)))
+ ((> i 1))
+ (setq tmp-joint-min-angle (send j :joint-min-max-table-min-angle tmp-target-joint-angle)
+ tmp-joint-max-angle (send j :joint-min-max-table-max-angle tmp-target-joint-angle))
+ (setq tmp-target-joint-min-angle (send j :joint-min-max-table-min-angle tmp-joint-angle)
+ tmp-target-joint-max-angle (send j :joint-min-max-table-max-angle tmp-joint-angle))
+
+ (if (< tmp-joint-angle tmp-joint-min-angle)
+ (incf tmp-joint-angle (* (- tmp-joint-min-angle tmp-joint-angle) i)))
+ (if (> tmp-joint-angle tmp-joint-max-angle)
+ (incf tmp-joint-angle (* (- tmp-joint-max-angle tmp-joint-angle) i)))
+ (if (< tmp-target-joint-angle tmp-target-joint-min-angle)
+ (incf tmp-target-joint-angle (* (- tmp-target-joint-min-angle tmp-target-joint-angle) i)))
+ (if (> tmp-target-joint-angle tmp-target-joint-max-angle)
+ (incf tmp-target-joint-angle (* (- tmp-target-joint-max-angle tmp-target-joint-angle) i)))
+ )
+ (setq (j . joint-angle) tmp-joint-angle
+ (jj . joint-angle) tmp-target-joint-angle)
+ (setf (elt vec i) tmp-joint-angle)
+ (setf (elt vec ii) tmp-target-joint-angle)
+ )
+ );; cond
+ );; let*
+ )) ;; when
(case (send j :joint-dof)
(1 (send j :joint-angle (elt vec i)))
(t (send j :joint-angle (subseq vec i (+ i (send j :joint-dof)))))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <k-...@us...> - 2013-11-22 13:59:20
|
Revision: 985
http://sourceforge.net/p/jskeus/code/985
Author: k-okada
Date: 2013-11-22 13:59:16 +0000 (Fri, 22 Nov 2013)
Log Message:
-----------
remove debug codefix :angle-vector consider mutual effect ,add :joint-min-max-table-{min/max}-angle, add min-max-table for rotationa joint, see #43
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2013-11-22 13:54:25 UTC (rev 984)
+++ trunk/irteus/irtmodel.l 2013-11-22 13:59:16 UTC (rev 985)
@@ -720,7 +720,6 @@
tmp-target-joint-max-angle (send j :joint-min-max-table-max-angle tmp-joint-angle))
(cond ((<= tmp-joint-min-angle tmp-joint-angle tmp-joint-max-angle) ;; ok
;; force change joint-angle to avoid problem
- (print (list tmp-joint-angle tmp-target-joint-angle))
(setq (j . joint-angle) tmp-joint-angle
(jj . joint-angle) tmp-target-joint-angle))
(t
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <k-...@us...> - 2013-11-22 13:54:29
|
Revision: 984
http://sourceforge.net/p/jskeus/code/984
Author: k-okada
Date: 2013-11-22 13:54:25 +0000 (Fri, 22 Nov 2013)
Log Message:
-----------
fix :angle-vector consider mutual effect ,add :joint-min-max-table-{min/max}-angle, add min-max-table for rotationa joint, see #43
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2013-11-22 11:16:45 UTC (rev 983)
+++ trunk/irteus/irtmodel.l 2013-11-22 13:54:25 UTC (rev 984)
@@ -76,14 +76,10 @@
(: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 ()
- (let* ((target-angle (send joint-min-max-target :joint-angle)) ;; integer
- (min-max-angle (gethash (floor target-angle) joint-min-max-table)))
- (car min-max-angle)))
- (:joint-min-max-table-max-angle ()
- (let* ((target-angle (send joint-min-max-target :joint-angle)) ;; integer
- (min-max-angle (gethash (floor target-angle) joint-min-max-table)))
- (cdr min-max-angle)))
+ (: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)))
)
(defun calc-jacobian-default-rotate-vector
@@ -706,7 +702,31 @@
(angle-vector (instantiate float-vector (calc-target-joint-dimension joint-list))))
(let ((i 0))
(dolist (j joint-list)
- (if vec
+ (when vec
+ ;; for joint w/ min-max-table
+ (when (and (send j :joint-min-max-table) (send j :joint-min-max-target))
+ ;; currently only 1dof joint is supported
+ (when (and (= (send j :joint-dof) 1) (= (send (send j :joint-min-max-target) :joint-dof) 1))
+ ;; find index of joint-min-max-target
+ (let* ((ii 0) (jj (elt joint-list ii))
+ tmp-joint-angle tmp-joint-min-angle tmp-joint-max-angle
+ tmp-target-joint-angle tmp-target-joint-min-angle tmp-target-joint-max-angle)
+ (while (not (eq jj (send j :joint-min-max-target)))
+ (incf ii) (setq jj (elt joint-list ii)))
+ (setq tmp-joint-angle (elt vec i) tmp-target-joint-angle (elt vec ii))
+ (setq tmp-joint-min-angle (send j :joint-min-max-table-min-angle tmp-target-joint-angle)
+ tmp-joint-max-angle (send j :joint-min-max-table-max-angle tmp-target-joint-angle))
+ (setq tmp-target-joint-min-angle (send j :joint-min-max-table-min-angle tmp-joint-angle)
+ tmp-target-joint-max-angle (send j :joint-min-max-table-max-angle tmp-joint-angle))
+ (cond ((<= tmp-joint-min-angle tmp-joint-angle tmp-joint-max-angle) ;; ok
+ ;; force change joint-angle to avoid problem
+ (print (list tmp-joint-angle tmp-target-joint-angle))
+ (setq (j . joint-angle) tmp-joint-angle
+ (jj . joint-angle) tmp-target-joint-angle))
+ (t
+ ))
+ );; let*
+ )) ;; when
(case (send j :joint-dof)
(1 (send j :joint-angle (elt vec i)))
(t (send j :joint-angle (subseq vec i (+ i (send j :joint-dof)))))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <k-...@us...> - 2013-11-22 11:16:53
|
Revision: 983
http://sourceforge.net/p/jskeus/code/983
Author: k-okada
Date: 2013-11-22 11:16:45 +0000 (Fri, 22 Nov 2013)
Log Message:
-----------
clean up code, add :joint-min-max-table-{min/max}-angle, add min-max-table for rotationa joint, see #255
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2013-11-22 11:07:00 UTC (rev 982)
+++ trunk/irteus/irtmodel.l 2013-11-22 11:16:45 UTC (rev 983)
@@ -76,6 +76,14 @@
(: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 ()
+ (let* ((target-angle (send joint-min-max-target :joint-angle)) ;; integer
+ (min-max-angle (gethash (floor target-angle) joint-min-max-table)))
+ (car min-max-angle)))
+ (:joint-min-max-table-max-angle ()
+ (let* ((target-angle (send joint-min-max-target :joint-angle)) ;; integer
+ (min-max-angle (gethash (floor target-angle) joint-min-max-table)))
+ (cdr min-max-angle)))
)
(defun calc-jacobian-default-rotate-vector
@@ -163,11 +171,8 @@
(let ()
(when v
(when (and joint-min-max-table joint-min-max-target)
- (let* ((target-angle (send joint-min-max-target :joint-angle)) ;; integer
- (min-max-angle (gethash (floor target-angle) joint-min-max-table)))
- (setq min-angle (car min-max-angle)
- max-angle (cdr min-max-angle))
- ))
+ (setq min-angle (send self :joint-min-max-table-min-angle)
+ max-angle (send self :joint-min-max-table-max-angle)))
(if relative (setq v (+ v joint-angle)))
(cond ((> v max-angle)
(unless relative (warning-message 3 ";; ~A :joint-angle(~A) violate max-angle(~A)~%" self v max-angle))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <k-...@us...> - 2013-11-22 11:07:03
|
Revision: 982
http://sourceforge.net/p/jskeus/code/982
Author: k-okada
Date: 2013-11-22 11:07:00 +0000 (Fri, 22 Nov 2013)
Log Message:
-----------
add min-max-table for rotationa joint, see #255
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2013-11-19 13:14:20 UTC (rev 981)
+++ trunk/irteus/irtmodel.l 2013-11-22 11:07:00 UTC (rev 982)
@@ -31,7 +31,8 @@
:super propertied-object
:slots (parent-link child-link joint-angle min-angle max-angle default-coords
joint-velocity joint-acceleration joint-torque
- max-joint-velocity max-joint-torque))
+ max-joint-velocity max-joint-torque
+ joint-min-max-table joint-min-max-target))
(defmethod joint
(:init (&key (name (intern (format nil "joint~A" (sys::address self)) "KEYWORD"))
@@ -39,6 +40,8 @@
(min -90) (max 90)
((:max-joint-velocity mjv))
((:max-joint-torque mjt))
+ ((:joint-min-max-table mm-table))
+ ((:joint-min-max-target mm-target))
&allow-other-keys)
(send self :name name)
(setq parent-link plink child-link clink
@@ -53,6 +56,8 @@
(error "[joint ~A] warning negative max-joint-torque value ~A~%" name mjt))
(send self :max-joint-velocity mjv)
(send self :max-joint-torque mjt)
+ (send self :joint-min-max-table mm-table)
+ (send self :joint-min-max-target mm-target)
(setq default-coords (send child-link :copy-coords))
self)
(:min-angle (&optional v) (if v (setq min-angle v)) min-angle)
@@ -69,6 +74,8 @@
(:joint-torque (&optional jt) (if jt (setq joint-torque jt) joint-torque))
(:max-joint-velocity (&optional mjv) (if mjv (setq max-joint-velocity mjv) max-joint-velocity))
(: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))
)
(defun calc-jacobian-default-rotate-vector
@@ -155,6 +162,12 @@
"(self class &optional v &key relative &allow-other-keys) ;; v and return value are joint-angle scalar ;; scalar is rotational value[deg]"
(let ()
(when v
+ (when (and joint-min-max-table joint-min-max-target)
+ (let* ((target-angle (send joint-min-max-target :joint-angle)) ;; integer
+ (min-max-angle (gethash (floor target-angle) joint-min-max-table)))
+ (setq min-angle (car min-max-angle)
+ max-angle (cdr min-max-angle))
+ ))
(if relative (setq v (+ v joint-angle)))
(cond ((> v max-angle)
(unless relative (warning-message 3 ";; ~A :joint-angle(~A) violate max-angle(~A)~%" self v max-angle))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-11-19 13:14:25
|
Revision: 981
http://sourceforge.net/p/jskeus/code/981
Author: snozawa
Date: 2013-11-19 13:14:20 +0000 (Tue, 19 Nov 2013)
Log Message:
-----------
extract root-link coordinates as :root-coords ;; (send robot :worldcoords) != root-coords for some euscollada robots
Modified Paths:
--------------
trunk/irteus/irtrobot.l
Modified: trunk/irteus/irtrobot.l
===================================================================
--- trunk/irteus/irtrobot.l 2013-11-19 12:57:38 UTC (rev 980)
+++ trunk/irteus/irtrobot.l 2013-11-19 13:14:20 UTC (rev 981)
@@ -604,7 +604,7 @@
czmp dt))
(push
(list :angle-vector (send self :angle-vector)
- :root-coords (send self :copy-worldcoords)
+ :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)
:support-leg-coords (elt ret 3)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-11-19 12:57:41
|
Revision: 980
http://sourceforge.net/p/jskeus/code/980
Author: snozawa
Date: 2013-11-19 12:57:38 +0000 (Tue, 19 Nov 2013)
Log Message:
-----------
rename :root-link -> :root-coords because of mis naming (typo)
Modified Paths:
--------------
trunk/irteus/irtrobot.l
Modified: trunk/irteus/irtrobot.l
===================================================================
--- trunk/irteus/irtrobot.l 2013-11-19 01:42:38 UTC (rev 979)
+++ trunk/irteus/irtrobot.l 2013-11-19 12:57:38 UTC (rev 980)
@@ -604,7 +604,7 @@
czmp dt))
(push
(list :angle-vector (send self :angle-vector)
- :root-link (send self :copy-worldcoords)
+ :root-coords (send self :copy-worldcoords)
:czmp czmp :refzmp (elt ret 5) :cog (elt ret 6)
:swing-leg-coords (elt ret 4)
:support-leg-coords (elt ret 3)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <k-...@us...> - 2013-11-19 01:42:41
|
Revision: 979
http://sourceforge.net/p/jskeus/code/979
Author: k-okada
Date: 2013-11-19 01:42:38 +0000 (Tue, 19 Nov 2013)
Log Message:
-----------
add :draw-floor and :draw-origin option in make-irtviewer
Modified Paths:
--------------
trunk/irteus/irtgl.l
trunk/irteus/irtviewer.l
Modified: trunk/irteus/irtgl.l
===================================================================
--- trunk/irteus/irtgl.l 2013-11-18 01:27:38 UTC (rev 978)
+++ trunk/irteus/irtgl.l 2013-11-19 01:42:38 UTC (rev 979)
@@ -195,20 +195,40 @@
))
)
-(defun draw-globjects (vwr draw-things &key (clear t) (flush t))
+(defun draw-globjects (vwr draw-things &key (clear t) (flush t) (draw-origin 150) (draw-floor nil))
(let (pcolor)
(resetperspective (send vwr :viewing) (send vwr :viewsurface))
(if clear (send vwr :viewsurface :clear))
;;(apply #'geo::draw things)
(setq pcolor (send vwr :viewsurface :color))
;; draw origin
- (glDisable GL_LIGHTING)
- (glBegin GL_LINES)
- (glColor3fv #f(1 0 0)) (glVertex3fv #f(0 0 0)) (glVertex3fv #f(100 0 0))
- (glColor3fv #f(0 1 0)) (glVertex3fv #f(0 0 0)) (glVertex3fv #f(0 100 0))
- (glColor3fv #f(0 0 1)) (glVertex3fv #f(0 0 0)) (glVertex3fv #f(0 0 100))
- (glEnd GL_LINES)
- (glEnable GL_LIGHTING)
+ (when draw-origin
+ (let ((l (if (numberp draw-origin) draw-origin 150)))
+ (glDisable GL_LIGHTING)
+ (glBegin GL_LINES)
+ (glColor3fv #f(1 0 0)) (glVertex3fv #f(0 0 0)) (glVertex3fv (float-vector l 0 0))
+ (glColor3fv #f(0 1 0)) (glVertex3fv #f(0 0 0)) (glVertex3fv (float-vector 0 l 0))
+ (glColor3fv #f(0 0 1)) (glVertex3fv #f(0 0 0)) (glVertex3fv (float-vector 0 0 l))
+ (glEnd GL_LINES)
+ (glEnable GL_LIGHTING)))
+ ;; draw floor
+ (when draw-floor
+ (let* ((l (if (numberp draw-floor) draw-floor 1000))
+ (s 5000) (-s (- s)))
+ (glDisable GL_LIGHTING)
+ (glBegin GL_LINES)
+ (glColor3fv #f(1 1 1))
+ (do ((y -s (+ y l)))
+ ((> y s))
+ (do ((x -s (+ x l)))
+ ((> x s))
+ (glVertex3fv (float-vector x -s 0))
+ (glVertex3fv (float-vector x s 0))
+ (glVertex3fv (float-vector -s y 0))
+ (glVertex3fv (float-vector s y 0))))
+ (glEnd GL_LINES)
+ (glEnable GL_LIGHTING)))
+ ;;
(glDisable GL_BLEND)
(send vwr :viewsurface :color pcolor)
Modified: trunk/irteus/irtviewer.l
===================================================================
--- trunk/irteus/irtviewer.l 2013-11-18 01:27:38 UTC (rev 978)
+++ trunk/irteus/irtviewer.l 2013-11-19 01:42:38 UTC (rev 979)
@@ -61,7 +61,10 @@
up-down-angle
viewpoint
viewtarget
- drawmode))
+ drawmode
+ draw-origin
+ draw-floor
+ ))
(defmethod irtviewer
(:create
(&rest args
@@ -69,12 +72,14 @@
(view-name (gensym "title"))
(hither 200.0) (yon 50000.0)
(width 500) (height 500)
+ ((:draw-origin do) 150) ((:draw-floor df) nil)
&allow-other-keys)
(let ()
(setq left-right-angle 60
up-down-angle 20
viewpoint (float-vector 700 400 250)
- viewtarget (float-vector 0 0 0))
+ viewtarget (float-vector 0 0 0)
+ draw-origin do draw-floor df)
(send-super* :create :width width :height height :title title
:event-mask '(:configure) args)
(setq gl::*perspective-far* yon)
@@ -243,7 +248,7 @@
(:draw-objects
(&rest args)
(send viewer :viewsurface :makecurrent)
- (apply #'gl::draw-globjects viewer draw-things args))
+ (apply #'gl::draw-globjects viewer draw-things :draw-origin draw-origin :draw-floor draw-floor args))
(:objects
(&rest args)
(when
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <k-...@us...> - 2013-11-18 01:27:41
|
Revision: 978
http://sourceforge.net/p/jskeus/code/978
Author: k-okada
Date: 2013-11-18 01:27:38 +0000 (Mon, 18 Nov 2013)
Log Message:
-----------
update :cog-convergence-check to support numberp, functionp, vectorp for centroid-thre argument
Modified Paths:
--------------
trunk/irteus/irtdyna.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2013-11-16 06:12:30 UTC (rev 977)
+++ trunk/irteus/irtdyna.l 2013-11-18 01:27:38 UTC (rev 978)
@@ -469,8 +469,12 @@
translation-axis))
(:cog-convergence-check
(centroid-thre target-centroid-pos &optional centroid-offset-func)
- (> centroid-thre
- (norm (send self :difference-cog-position target-centroid-pos centroid-offset-func))))
+ (let ((cdiff
+ (send self :difference-cog-position target-centroid-pos centroid-offset-func)))
+ (cond
+ ((numberp centroid-thre) (> centroid-thre (norm cdiff)))
+ ((functionp centroid-thre) (funcall cdiff))
+ ((vectorp centroid-thre) (v< (map float-vector #'abs cdiff) centroid-thre)))))
)
;; recursive computation for inverse-dynamics
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-11-16 06:12:35
|
Revision: 977
http://sourceforge.net/p/jskeus/code/977
Author: snozawa
Date: 2013-11-16 06:12:30 +0000 (Sat, 16 Nov 2013)
Log Message:
-----------
fix null-space calculation
Modified Paths:
--------------
trunk/irteus/demo/walk-motion.l
trunk/irteus/irtdyna.l
Modified: trunk/irteus/demo/walk-motion.l
===================================================================
--- trunk/irteus/demo/walk-motion.l 2013-11-16 05:19:40 UTC (rev 976)
+++ trunk/irteus/demo/walk-motion.l 2013-11-16 06:12:30 UTC (rev 977)
@@ -82,15 +82,16 @@
(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*))
- ;; test move-centroid-on-foot
- ;; (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 walk motion
(send *robot* :calc-walk-pattern-from-footstep-list
footstep-list :debug-view :no-message
- :init-pose-function #'(lambda ())
+ :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))
: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))
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2013-11-16 05:19:40 UTC (rev 976)
+++ trunk/irteus/irtdyna.l 2013-11-16 06:12:30 UTC (rev 977)
@@ -1356,16 +1356,18 @@
&rest args &key (cog-gain 3.5) (stop 100) &allow-other-keys)
(let ((legs
(append (send self :get-counter-footstep-limbs support-leg)
- support-leg)))
+ support-leg))
+ (fix-coords (append swing-leg-coords support-leg-coords)))
(send* robot :move-centroid-on-foot
:both legs :target-centroid-pos cog
- :fix-limbs-target-coords (append swing-leg-coords support-leg-coords)
+ :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 #f(1 0 0)))
- (xvf (send (midcoords 0.5 (car swing-leg-coords) (car support-leg-coords))
+ (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
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-11-16 05:19:42
|
Revision: 976
http://sourceforge.net/p/jskeus/code/976
Author: snozawa
Date: 2013-11-16 05:19:40 +0000 (Sat, 16 Nov 2013)
Log Message:
-----------
use additional-nspace-list instead of null-space
Modified Paths:
--------------
trunk/irteus/irtdyna.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2013-11-16 05:13:13 UTC (rev 975)
+++ trunk/irteus/irtdyna.l 2013-11-16 05:19:40 UTC (rev 976)
@@ -1361,17 +1361,17 @@
:both legs :target-centroid-pos cog
:fix-limbs-target-coords (append swing-leg-coords support-leg-coords)
:cog-gain cog-gain :stop stop
- :null-space
- #'(lambda ()
- (let ((xvr (send robot :rotate-vector #f(1 0 0)))
- (xvf (send (midcoords 0.5 (car swing-leg-coords) (car support-leg-coords))
- :rotate-vector #f(1 0 0)))
- (nv (instantiate float-vector (send robot :calc-target-joint-dimension (mapcar #'(lambda (l) (send robot :link-list (send (send robot l :end-coords) :parent))) legs)))))
- (dolist (xv (list xvr xvf)) (setf (elt xv 2) 0.0))
- (setf (elt nv 5) (* (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)))
- nv))
+ :additional-nspace-list
+ (list (list (car (send robot :links))
+ #'(lambda ()
+ (let ((xvr (send robot :rotate-vector #f(1 0 0)))
+ (xvf (send (midcoords 0.5 (car swing-leg-coords) (car support-leg-coords))
+ :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)))))))
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-11-16 05:13:16
|
Revision: 975
http://sourceforge.net/p/jskeus/code/975
Author: snozawa
Date: 2013-11-16 05:13:13 +0000 (Sat, 16 Nov 2013)
Log Message:
-----------
add quad walking sample
Modified Paths:
--------------
trunk/irteus/demo/walk-motion.l
Modified: trunk/irteus/demo/walk-motion.l
===================================================================
--- trunk/irteus/demo/walk-motion.l 2013-11-16 05:12:52 UTC (rev 974)
+++ trunk/irteus/demo/walk-motion.l 2013-11-16 05:13:13 UTC (rev 975)
@@ -44,9 +44,60 @@
(unless (boundp '*robot*)
(setq *robot* (instance sample-robot :init)))
(walk-motion *robot*))
-
(warn "(walk-motion-for-sample-robot) for walking motion~%")
+(defun quad-walk-motion-for-sample-robot ()
+ (unless (boundp '*robot*)
+ (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
+ ))
+ ;; 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*))
+ ;; test move-centroid-on-foot
+ ;; (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 walk motion
+ (send *robot* :calc-walk-pattern-from-footstep-list
+ footstep-list :debug-view :no-message
+ :init-pose-function #'(lambda ())
+ :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 ()
(unless (boundp '*robots*)
(setq *robots*
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-11-16 05:12:56
|
Revision: 974
http://sourceforge.net/p/jskeus/code/974
Author: snozawa
Date: 2013-11-16 05:12:52 +0000 (Sat, 16 Nov 2013)
Log Message:
-----------
support quad walkint
Modified Paths:
--------------
trunk/irteus/irtdyna.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2013-11-16 05:07:22 UTC (rev 973)
+++ trunk/irteus/irtdyna.l 2013-11-16 05:12:52 UTC (rev 974)
@@ -1140,21 +1140,27 @@
(fs)
(if (symbolp (car fs))
(mapcar #'(lambda (f)
- (case f (:rleg :lleg) (:lleg :rleg)))
+ (case f
+ (:rleg :lleg)
+ (:lleg :rleg)
+ (:rarm :larm)
+ (:larm :rarm)
+ ))
fs)
(send self :get-counter-footstep-limbs (send self :get-footstep-limbs fs)))
)
(:get-limbs-zmp
(limb-coords limb-names)
- (reduce
- #'v+
- (apply
- #'append
- (mapcar #'(lambda (lc ln)
- (list (send lc :worldpos)
- (send lc :rotate-vector
- (cadr (memq ln default-zmp-offsets)))))
- limb-coords limb-names)))
+ (scale (/ 1.0 (length limb-names))
+ (reduce
+ #'v+
+ (mapcar #'(lambda (lc ln)
+ (v+ (send lc :worldpos)
+ (send lc :rotate-vector
+ (cadr (memq ln default-zmp-offsets)))))
+ limb-coords limb-names)
+ :initial-value (float-vector 0 0 0)
+ ))
)
;; initialize parameter and generate first gait parameter
(:initialize-gait-parameter
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-11-16 05:07:24
|
Revision: 973
http://sourceforge.net/p/jskeus/code/973
Author: snozawa
Date: 2013-11-16 05:07:22 +0000 (Sat, 16 Nov 2013)
Log Message:
-----------
add root-link-virtual-joint-weight arguments and init-pose-function
Modified Paths:
--------------
trunk/irteus/irtrobot.l
Modified: trunk/irteus/irtrobot.l
===================================================================
--- trunk/irteus/irtrobot.l 2013-11-16 04:21:00 UTC (rev 972)
+++ trunk/irteus/irtrobot.l 2013-11-16 05:07:22 UTC (rev 973)
@@ -536,6 +536,7 @@
(send self leg :end-coords :worldpos)))
(fix-limbs-target-coords
(mapcar #'(lambda (x) (send self x :end-coords :copy-worldcoords)) fix-limbs))
+ (root-link-virtual-joint-weight #f(0.1 0.1 0.0 0.0 0.0 0.5)) ;; use only translation x, y and rotation z
&allow-other-keys)
"(leg fix-limbs
&rest args
@@ -558,7 +559,7 @@
fix-limbs -> limb names which are fixed in this IK."
(with-move-target-link-list
(mt ll self fix-limbs)
- (let* ((root-link-virtual-joint-weight #f(0.1 0.1 0.0 0.0 0.0 0.5))) ;; use only translation x, y and rotation z
+ (let* ()
(send* self :fullbody-inverse-kinematics
fix-limbs-target-coords
:move-target mt :link-list ll
@@ -572,11 +573,12 @@
(footstep-list
&key (default-step-height 50) (dt 0.1) (default-step-time 1.0)
(default-zmp-offsets (list :rleg (float-vector 0 0 0) :lleg (float-vector 0 0 0)))
- (solve-angle-vector-args) (debug-view nil))
+ (solve-angle-vector-args) (debug-view nil)
+ (init-pose-function #'(lambda () (send self :move-centroid-on-foot :both '(:rleg :lleg)))))
(let* ((res) (ret) (tm 0)
(gg (instance gait-generator :init self dt)))
+ (funcall init-pose-function)
;; initial move centroid on foot
- (send self :move-centroid-on-foot :both '(:rleg :lleg))
(send gg :initialize-gait-parameter
footstep-list default-step-time (send (car (send self :links)) :get :c-til)
:default-step-height default-step-height :default-double-support-ratio 0.2
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-11-16 04:21:04
|
Revision: 972
http://sourceforge.net/p/jskeus/code/972
Author: snozawa
Date: 2013-11-16 04:21:00 +0000 (Sat, 16 Nov 2013)
Log Message:
-----------
support :get-limbs-zmp, :get-counter-footstep-limbs
Modified Paths:
--------------
trunk/irteus/irtdyna.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2013-11-16 03:44:18 UTC (rev 971)
+++ trunk/irteus/irtdyna.l 2013-11-16 04:21:00 UTC (rev 972)
@@ -1129,6 +1129,33 @@
(_args)
(setq ,list-name (append ,list-name (list _args)))))))
self)
+ (:get-footstep-limbs
+ (fs)
+ (mapcar #'(lambda (tmpfs)
+ (if (find-method tmpfs :l/r)
+ (send tmpfs :l/r)
+ (send tmpfs :get :l/r)))
+ fs))
+ (:get-counter-footstep-limbs
+ (fs)
+ (if (symbolp (car fs))
+ (mapcar #'(lambda (f)
+ (case f (:rleg :lleg) (:lleg :rleg)))
+ fs)
+ (send self :get-counter-footstep-limbs (send self :get-footstep-limbs fs)))
+ )
+ (:get-limbs-zmp
+ (limb-coords limb-names)
+ (reduce
+ #'v+
+ (apply
+ #'append
+ (mapcar #'(lambda (lc ln)
+ (list (send lc :worldpos)
+ (send lc :rotate-vector
+ (cadr (memq ln default-zmp-offsets)))))
+ limb-coords limb-names)))
+ )
;; initialize parameter and generate first gait parameter
(:initialize-gait-parameter
(fsl time cog ;; time is [s], cog is [mm]
@@ -1137,7 +1164,7 @@
((:default-zmp-offsets dzo) (list :rleg (float-vector 0 0 0) :lleg (float-vector 0 0 0))) ;; [mm]
(delay 1.6) ;; delay [s]
(q 1.0) (r 1e-6))
- (setq footstep-node-list (mapcar #'(lambda (l) (list l)) fsl)
+ (setq footstep-node-list (mapcar #'(lambda (l) (if (consp l) l (list l))) fsl)
one-step-len (round (/ time dt))
support-leg-coords-list nil
support-leg-list nil
@@ -1149,11 +1176,10 @@
default-zmp-offsets dzo
index-count one-step-len
finalize-p nil)
- (let ((current-swing-limbs (list (if (find-method (car (car footstep-node-list)) :l/r)
- (send (car (car footstep-node-list)) :l/r)
- (send (car (car footstep-node-list)) :get :l/r)))))
+ (let ((current-swing-limbs
+ (send self :get-footstep-limbs (car footstep-node-list))))
(send self :append-support-leg-list
- (list (case (car current-swing-limbs) (:rleg :lleg) (:lleg :rleg))))
+ (send self :get-counter-footstep-limbs (car footstep-node-list)))
(let ((current-support-limbs (car support-leg-list)))
(send self :append-support-leg-coords-list
(mapcar #'(lambda (l) (send robot l :end-coords :copy-worldcoords)) current-support-limbs))
@@ -1163,12 +1189,8 @@
(current-swing-leg-dst-coords (car swing-leg-dst-coords-list)))
(send self :append-refzmp-cur-list
(midpoint 0.5
- (v+ (send (car current-support-leg-coords) :worldpos)
- (send (car current-support-leg-coords) :rotate-vector
- (cadr (memq (car current-swing-limbs) default-zmp-offsets))))
- (v+ (send (car current-swing-leg-dst-coords) :worldpos)
- (send (car current-swing-leg-dst-coords) :rotate-vector
- (cadr (memq (car current-support-limbs) default-zmp-offsets))))))
+ (send self :get-limbs-zmp current-support-leg-coords current-swing-limbs)
+ (send self :get-limbs-zmp current-swing-leg-dst-coords current-support-limbs)))
(send self :append-step-height-list 0.0)
(pop footstep-node-list)
;; currently, not error system
@@ -1180,9 +1202,7 @@
:init-xk cog
)
swing-leg-src-coords current-swing-leg-dst-coords
- refzmp-next (v+ (send (car (car swing-leg-dst-coords-list)) :worldpos)
- (send (car (car swing-leg-dst-coords-list)) :rotate-vector
- (cadr (memq (car current-support-limbs) default-zmp-offsets))))
+ refzmp-next (send self :get-limbs-zmp (car swing-leg-dst-coords-list) current-support-limbs)
refzmp-prev (car refzmp-cur-list))
(send self :make-gait-parameter)
t))))
@@ -1190,17 +1210,17 @@
(:finalize-gait-parameter
()
(let ((dst-swing-coords (car (last support-leg-coords-list))))
- (send self :append-support-leg-list (list (case (car (car (last support-leg-list))) (:rleg :lleg) (:lleg :rleg))))
+ (send self :append-support-leg-list
+ (send self :get-counter-footstep-limbs (car (last support-leg-list))))
(send self :append-support-leg-coords-list (car (last swing-leg-dst-coords-list)))
(send self :append-swing-leg-dst-coords-list dst-swing-coords)
(send self :append-refzmp-cur-list
(midpoint 0.5
- (v+ (send (car (car (last support-leg-coords-list))) :worldpos)
- (send (car (car (last support-leg-coords-list))) :rotate-vector
- (cadr (memq (car (car (last support-leg-list))) default-zmp-offsets))))
- (v+ (send (car (car (last swing-leg-dst-coords-list))) :worldpos)
- (send (car (car (last swing-leg-dst-coords-list))) :rotate-vector
- (cadr (memq (case (car (car (last support-leg-list))) (:rleg :lleg) (:lleg :rleg)) default-zmp-offsets))))))
+ (send self :get-limbs-zmp
+ (car (last support-leg-coords-list)) (car (last support-leg-list)))
+ (send self :get-limbs-zmp
+ (car (last swing-leg-dst-coords-list))
+ (send self :get-counter-footstep-limbs (car (last support-leg-list))))))
(send self :append-step-height-list 0.0)
(setq refzmp-next (car (last refzmp-cur-list)))
t))
@@ -1209,17 +1229,11 @@
()
(let ((fs (pop footstep-node-list)))
(send self :append-support-leg-list
- (list
- (case (if (find-method (car fs) :l/r)
- (send (car fs) :l/r)
- (send (car fs) :get :l/r))
- (:rleg :lleg) (:lleg :rleg))))
+ (send self :get-counter-footstep-limbs fs))
(send self :append-support-leg-coords-list (car (last swing-leg-dst-coords-list)))
(send self :append-swing-leg-dst-coords-list (send-all fs :worldcoords))
(send self :append-refzmp-cur-list
- (v+ (send (car (car (last support-leg-coords-list))) :worldpos)
- (send (car (car (last support-leg-coords-list))) :rotate-vector
- (cadr (memq (car (car (last support-leg-list))) default-zmp-offsets)))))
+ (send self :get-limbs-zmp (car (last support-leg-coords-list)) (car (last support-leg-list))))
(send self :append-step-height-list default-step-height)
t))
;; calculate swing leg coords
@@ -1334,7 +1348,9 @@
(: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)
- (let ((legs (list (case (car support-leg) (:lleg :rleg) (:rleg :lleg)) (car support-leg))))
+ (let ((legs
+ (append (send self :get-counter-footstep-limbs support-leg)
+ support-leg)))
(send* robot :move-centroid-on-foot
:both legs :target-centroid-pos cog
:fix-limbs-target-coords (append swing-leg-coords support-leg-coords)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-11-16 03:44:23
|
Revision: 971
http://sourceforge.net/p/jskeus/code/971
Author: snozawa
Date: 2013-11-16 03:44:18 +0000 (Sat, 16 Nov 2013)
Log Message:
-----------
enable to set list footsteps
Modified Paths:
--------------
trunk/irteus/irtdyna.l
trunk/irteus/irtrobot.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2013-11-12 06:50:43 UTC (rev 970)
+++ trunk/irteus/irtdyna.l 2013-11-16 03:44:18 UTC (rev 971)
@@ -1134,10 +1134,10 @@
(fsl time cog ;; time is [s], cog is [mm]
&key ((:default-step-height dsh) 50) ;; height [mm]
((:default-double-support-ratio ddsr) 0.2)
- ((:default-zmp-offsets dzo) (list (float-vector 0 0 0) (float-vector 0 0 0))) ;; (list rleg lleg), [mm]
+ ((:default-zmp-offsets dzo) (list :rleg (float-vector 0 0 0) :lleg (float-vector 0 0 0))) ;; [mm]
(delay 1.6) ;; delay [s]
(q 1.0) (r 1e-6))
- (setq footstep-node-list fsl
+ (setq footstep-node-list (mapcar #'(lambda (l) (list l)) fsl)
one-step-len (round (/ time dt))
support-leg-coords-list nil
support-leg-list nil
@@ -1149,26 +1149,26 @@
default-zmp-offsets dzo
index-count one-step-len
finalize-p nil)
- (send self :append-support-leg-list
- (case (if (find-method (car footstep-node-list) :l/r)
- (send (car footstep-node-list) :l/r)
- (send (car footstep-node-list) :get :l/r))
- (:rleg :lleg) (:lleg :rleg)))
- (send self :append-support-leg-coords-list
- (send robot (car support-leg-list) :end-coords :copy-worldcoords))
- (send self :append-swing-leg-dst-coords-list
- (send robot (if (find-method (car footstep-node-list) :l/r)
- (send (car footstep-node-list) :l/r)
- (send (car footstep-node-list) :get :l/r))
- :end-coords :copy-worldcoords))
- (send self :append-refzmp-cur-list
- (midpoint 0.5
- (v+ (send (car support-leg-coords-list) :worldpos)
- (send (car support-leg-coords-list) :rotate-vector
- (case (car support-leg-list) (:rleg (car default-zmp-offsets)) (:lleg (cadr default-zmp-offsets)))))
- (v+ (send (car swing-leg-dst-coords-list) :worldpos)
- (send (car swing-leg-dst-coords-list) :rotate-vector
- (case (car support-leg-list) (:lleg (car default-zmp-offsets)) (:rleg (cadr default-zmp-offsets)))))))
+ (let ((current-swing-limbs (list (if (find-method (car (car footstep-node-list)) :l/r)
+ (send (car (car footstep-node-list)) :l/r)
+ (send (car (car footstep-node-list)) :get :l/r)))))
+ (send self :append-support-leg-list
+ (list (case (car current-swing-limbs) (:rleg :lleg) (:lleg :rleg))))
+ (let ((current-support-limbs (car support-leg-list)))
+ (send self :append-support-leg-coords-list
+ (mapcar #'(lambda (l) (send robot l :end-coords :copy-worldcoords)) current-support-limbs))
+ (send self :append-swing-leg-dst-coords-list
+ (mapcar #'(lambda (l) (send robot l :end-coords :copy-worldcoords)) current-swing-limbs))
+ (let ((current-support-leg-coords (car support-leg-coords-list))
+ (current-swing-leg-dst-coords (car swing-leg-dst-coords-list)))
+ (send self :append-refzmp-cur-list
+ (midpoint 0.5
+ (v+ (send (car current-support-leg-coords) :worldpos)
+ (send (car current-support-leg-coords) :rotate-vector
+ (cadr (memq (car current-swing-limbs) default-zmp-offsets))))
+ (v+ (send (car current-swing-leg-dst-coords) :worldpos)
+ (send (car current-swing-leg-dst-coords) :rotate-vector
+ (cadr (memq (car current-support-limbs) default-zmp-offsets))))))
(send self :append-step-height-list 0.0)
(pop footstep-node-list)
;; currently, not error system
@@ -1179,28 +1179,28 @@
:q q :r r
:init-xk cog
)
- swing-leg-src-coords (car swing-leg-dst-coords-list)
- refzmp-next (v+ (send (car swing-leg-dst-coords-list) :worldpos)
- (send (car swing-leg-dst-coords-list) :rotate-vector
- (case (car support-leg-list) (:lleg (car default-zmp-offsets)) (:rleg (cadr default-zmp-offsets)))))
+ swing-leg-src-coords current-swing-leg-dst-coords
+ refzmp-next (v+ (send (car (car swing-leg-dst-coords-list)) :worldpos)
+ (send (car (car swing-leg-dst-coords-list)) :rotate-vector
+ (cadr (memq (car current-support-limbs) default-zmp-offsets))))
refzmp-prev (car refzmp-cur-list))
(send self :make-gait-parameter)
- t)
+ t))))
;; append final gait parameters
(:finalize-gait-parameter
()
(let ((dst-swing-coords (car (last support-leg-coords-list))))
- (send self :append-support-leg-list (case (car (last support-leg-list)) (:rleg :lleg) (:lleg :rleg)))
+ (send self :append-support-leg-list (list (case (car (car (last support-leg-list))) (:rleg :lleg) (:lleg :rleg))))
(send self :append-support-leg-coords-list (car (last swing-leg-dst-coords-list)))
(send self :append-swing-leg-dst-coords-list dst-swing-coords)
(send self :append-refzmp-cur-list
(midpoint 0.5
- (v+ (send (car (last support-leg-coords-list)) :worldpos)
- (send (car (last support-leg-coords-list)) :rotate-vector
- (case (car (last support-leg-list)) (:rleg (car default-zmp-offsets)) (:lleg (cadr default-zmp-offsets)))))
- (v+ (send (car (last swing-leg-dst-coords-list)) :worldpos)
- (send (car (last swing-leg-dst-coords-list)) :rotate-vector
- (case (car (last support-leg-list)) (:lleg (car default-zmp-offsets)) (:rleg (cadr default-zmp-offsets)))))))
+ (v+ (send (car (car (last support-leg-coords-list))) :worldpos)
+ (send (car (car (last support-leg-coords-list))) :rotate-vector
+ (cadr (memq (car (car (last support-leg-list))) default-zmp-offsets))))
+ (v+ (send (car (car (last swing-leg-dst-coords-list))) :worldpos)
+ (send (car (car (last swing-leg-dst-coords-list))) :rotate-vector
+ (cadr (memq (case (car (car (last support-leg-list))) (:rleg :lleg) (:lleg :rleg)) default-zmp-offsets))))))
(send self :append-step-height-list 0.0)
(setq refzmp-next (car (last refzmp-cur-list)))
t))
@@ -1209,16 +1209,17 @@
()
(let ((fs (pop footstep-node-list)))
(send self :append-support-leg-list
- (case (if (find-method fs :l/r)
- (send fs :l/r)
- (send fs :get :l/r))
- (:rleg :lleg) (:lleg :rleg)))
+ (list
+ (case (if (find-method (car fs) :l/r)
+ (send (car fs) :l/r)
+ (send (car fs) :get :l/r))
+ (:rleg :lleg) (:lleg :rleg))))
(send self :append-support-leg-coords-list (car (last swing-leg-dst-coords-list)))
- (send self :append-swing-leg-dst-coords-list (send fs :worldcoords))
+ (send self :append-swing-leg-dst-coords-list (send-all fs :worldcoords))
(send self :append-refzmp-cur-list
- (v+ (send (car (last support-leg-coords-list)) :worldpos)
- (send (car (last support-leg-coords-list)) :rotate-vector
- (case (car (last support-leg-list)) (:rleg (car default-zmp-offsets)) (:lleg (cadr default-zmp-offsets))))))
+ (v+ (send (car (car (last support-leg-coords-list))) :worldpos)
+ (send (car (car (last support-leg-coords-list))) :rotate-vector
+ (cadr (memq (car (car (last support-leg-list))) default-zmp-offsets)))))
(send self :append-step-height-list default-step-height)
t))
;; calculate swing leg coords
@@ -1260,10 +1261,12 @@
(type)
(list (car support-leg-list)
(car support-leg-coords-list)
- (send self :calc-current-swing-leg-coords
- (send self :calc-ratio-from-double-support-ratio)
- swing-leg-src-coords (car swing-leg-dst-coords-list)
- :type type :step-height (car step-height-list))
+ (mapcar #'(lambda (sw-sc sw-dc)
+ (send self :calc-current-swing-leg-coords
+ (send self :calc-ratio-from-double-support-ratio)
+ sw-sc sw-dc
+ :type type :step-height (car step-height-list)))
+ swing-leg-src-coords (car swing-leg-dst-coords-list))
(send self :calc-current-refzmp
refzmp-prev (car refzmp-cur-list) refzmp-next)
)
@@ -1331,15 +1334,15 @@
(: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)
- (let ((legs (list (case support-leg (:lleg :rleg) (:rleg :lleg)) support-leg)))
+ (let ((legs (list (case (car support-leg) (:lleg :rleg) (:rleg :lleg)) (car support-leg))))
(send* robot :move-centroid-on-foot
:both legs :target-centroid-pos cog
- :fix-limbs-target-coords (list swing-leg-coords support-leg-coords)
+ :fix-limbs-target-coords (append swing-leg-coords support-leg-coords)
:cog-gain cog-gain :stop stop
:null-space
#'(lambda ()
(let ((xvr (send robot :rotate-vector #f(1 0 0)))
- (xvf (send (midcoords 0.5 swing-leg-coords support-leg-coords)
+ (xvf (send (midcoords 0.5 (car swing-leg-coords) (car support-leg-coords))
:rotate-vector #f(1 0 0)))
(nv (instantiate float-vector (send robot :calc-target-joint-dimension (mapcar #'(lambda (l) (send robot :link-list (send (send robot l :end-coords) :parent))) legs)))))
(dolist (xv (list xvr xvf)) (setf (elt xv 2) 0.0))
Modified: trunk/irteus/irtrobot.l
===================================================================
--- trunk/irteus/irtrobot.l 2013-11-12 06:50:43 UTC (rev 970)
+++ trunk/irteus/irtrobot.l 2013-11-16 03:44:18 UTC (rev 971)
@@ -571,7 +571,7 @@
(:calc-walk-pattern-from-footstep-list
(footstep-list
&key (default-step-height 50) (dt 0.1) (default-step-time 1.0)
- (default-zmp-offsets (list (float-vector 0 0 0) (float-vector 0 0 0)))
+ (default-zmp-offsets (list :rleg (float-vector 0 0 0) :lleg (float-vector 0 0 0)))
(solve-angle-vector-args) (debug-view nil))
(let* ((res) (ret) (tm 0)
(gg (instance gait-generator :init self dt)))
@@ -623,8 +623,8 @@
(send *viewer* :viewsurface :color col)
(funcall func)
(send *viewer* :viewsurface :color pc))))
- (send spc :draw-on :flush nil :size 300 :color #f(1 0 0))
- (send swc :draw-on :flush nil :size 300 :color #f(0 1 0))
+ (send-all spc :draw-on :flush nil :size 300 :color #f(1 0 0))
+ (send-all swc :draw-on :flush nil :size 300 :color #f(0 1 0))
(send rz :draw-on :flush nil :size 300 :color #f(0 0 1))
(send czmp :draw-on :flush nil :size 200 :width 5)
(with-modify-color
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-11-12 06:50:47
|
Revision: 970
http://sourceforge.net/p/jskeus/code/970
Author: snozawa
Date: 2013-11-12 06:50:43 +0000 (Tue, 12 Nov 2013)
Log Message:
-----------
add read-char-case which can impelement y-or-n-p
Modified Paths:
--------------
trunk/irteus/irtutil.l
Modified: trunk/irteus/irtutil.l
===================================================================
--- trunk/irteus/irtutil.l 2013-11-12 04:43:16 UTC (rev 969)
+++ trunk/irteus/irtutil.l 2013-11-12 06:50:43 UTC (rev 970)
@@ -327,6 +327,30 @@
(setq *error-output* ,tmp-stdout)
,ret))))))
+;; usage:
+;; (read-char-case "y or n or r: "
+;; (#\y (format t "yes~%") t)
+;; (#\n (format t "no~%") t)
+;; (#\r (format t "retry~%") t)
+;; )
+(defmacro read-char-case (str &rest clauses)
+ (let ((ch (gensym))
+ (missed-return (gensym))
+ (flag (gensym)))
+ `(progn
+ (let ((,flag ',missed-return))
+ (while (eq ,flag ',missed-return)
+ (format t ,str)
+ (finish-output *standard-output*)
+ (setq ,flag
+ (let ((,ch (read-char))) ;read-block
+ (case ,ch
+ ,@clauses
+ (t ;if does not match to anything
+ ',missed-return))))
+ )
+ ,flag))))
+
(provide :irtutil "$Id$")
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-11-12 04:43:18
|
Revision: 969
http://sourceforge.net/p/jskeus/code/969
Author: snozawa
Date: 2013-11-12 04:43:16 +0000 (Tue, 12 Nov 2013)
Log Message:
-----------
add name arguments to bumpser-sensor
Modified Paths:
--------------
trunk/irteus/irtsensor.l
Modified: trunk/irteus/irtsensor.l
===================================================================
--- trunk/irteus/irtsensor.l 2013-11-01 13:31:14 UTC (rev 968)
+++ trunk/irteus/irtsensor.l 2013-11-12 04:43:16 UTC (rev 969)
@@ -44,10 +44,10 @@
:slots (bumper-threshold))
(defmethod bumper-model
(:init
- (b &rest args &key ((:bumper-threshold bt) 20))
+ (b &rest args &key ((:bumper-threshold bt) 20) name)
(setq data 0)
(setq bumper-threshold bt)
- (send-super* :init b args))
+ (send-super* :init b :name name args))
(:simulate
(objs)
(let (r)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-11-01 13:31:19
|
Revision: 968
http://sourceforge.net/p/jskeus/code/968
Author: snozawa
Date: 2013-11-01 13:31:14 +0000 (Fri, 01 Nov 2013)
Log Message:
-----------
support specifying wrt in convert-to-faces
Modified Paths:
--------------
trunk/irteus/irtgl.l
Modified: trunk/irteus/irtgl.l
===================================================================
--- trunk/irteus/irtgl.l 2013-10-25 03:49:00 UTC (rev 967)
+++ trunk/irteus/irtgl.l 2013-11-01 13:31:14 UTC (rev 968)
@@ -706,7 +706,7 @@
(nconc minfo (list (list :normals nmat)))
))
)))
- (:convert-to-faces (&rest args) ;; check-normal <-> check-vertices-order, add face-color
+ (:convert-to-faces (&rest args &key (wrt :local) &allow-other-keys) ;; check-normal <-> check-vertices-order, add face-color
(let (ret)
(dolist (mesh mesh-list)
(let ((vlst (cadr (assoc :vertices mesh)))
@@ -729,11 +729,15 @@
(dotimes (j mesh-size)
(push (user::c-matrix-row vlst (elt idces (+ v j))) vs)
(if nlst (push (user::c-matrix-row nlst (elt idces (+ v j))) ns)))
+ (setq vs (case wrt
+ (:world (mapcar #'(lambda (x) (send self :transform-vector x)) (nreverse vs)))
+ (:local (nreverse vs))
+ (t )))
(push
(cond
;;((and ns check-vertices-order) )
- ;;(ns (instance face :init :vertices (nreverse vs) :normal ns))
- (t (instance face :init :vertices (nreverse vs))))
+ ;;(ns (instance face :init :vertices vs :normal ns))
+ (t (instance face :init :vertices vs)))
lst))))
(t
(do ((i 0 (+ i 1))
@@ -743,10 +747,14 @@
(dotimes (j mesh-size)
(push (user::c-matrix-row vlst (+ v j)) vs)
(if nlst (push (user::c-matrix-row nlst (+ v j)) ns)))
+ (setq vs (case wrt
+ (:world (mapcar #'(lambda (x) (send self :transform-vector x)) (nreverse vs)))
+ (:local (nreverse vs))
+ (t )))
(push
(if ns
- (instance face :init :vertices (nreverse vs) :normal ns)
- (instance face :init :vertices (nreverse vs)))
+ (instance face :init :vertices vs :normal ns)
+ (instance face :init :vertices vs))
lst))))
) ;; /cond
(push (nreverse lst) ret)))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ky...@us...> - 2013-10-25 03:49:02
|
Revision: 967
http://sourceforge.net/p/jskeus/code/967
Author: kyouhei
Date: 2013-10-25 03:49:00 +0000 (Fri, 25 Oct 2013)
Log Message:
-----------
impliment :glvertices for getting single mesh
Modified Paths:
--------------
trunk/irteus/irtgl.l
Modified: trunk/irteus/irtgl.l
===================================================================
--- trunk/irteus/irtgl.l 2013-10-09 02:07:51 UTC (rev 966)
+++ trunk/irteus/irtgl.l 2013-10-25 03:49:00 UTC (rev 967)
@@ -771,7 +771,18 @@
(user::c-coords-transform-vector
zero wrot nmat nmat))))
self))
- (:devide-glvertices ()) ;; not implemented yet
+ (:glvertices (&optional (name) (test #'string=))
+ (let (ret)
+ (cond
+ (name
+ (let ((m (find-if #'(lambda (m) (funcall test name (cadr (assoc :name m)))) mesh-list)))
+ (if m (setq ret (instance glvertices :init (list m))))))
+ (t
+ (dolist (minfo mesh-list)
+ (push (instance glvertices :init (list minfo)) ret))
+ (setq ret (nreverse ret))
+ ))
+ ret))
(:append-glvertices (glv-lst) ;; not checked
(if (atom glv-lst) (setq glv-lst (list glv-lst)))
(let (ret (sw (send self :worldcoords)))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-10-09 02:07:53
|
Revision: 966
http://sourceforge.net/p/jskeus/code/966
Author: snozawa
Date: 2013-10-09 02:07:51 +0000 (Wed, 09 Oct 2013)
Log Message:
-----------
update to use additional-weight-list
Modified Paths:
--------------
trunk/irteus/irtrobot.l
Modified: trunk/irteus/irtrobot.l
===================================================================
--- trunk/irteus/irtrobot.l 2013-10-06 21:55:17 UTC (rev 965)
+++ trunk/irteus/irtrobot.l 2013-10-09 02:07:51 UTC (rev 966)
@@ -391,6 +391,7 @@
(target-centroid-pos (apply #'midpoint 0.5 (send self :legs :end-coords :worldpos))) ;; <- target centroid position.
(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)
&allow-other-keys)
(with-append-root-joint ;; inverse-kinematics with base-link
(link-list-with-robot-6dof self link-list
@@ -399,13 +400,10 @@
(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
- :weight #'(lambda (union-link-list)
- (let ((tmp-weight (fill (instantiate float-vector (send self :calc-target-joint-dimension union-link-list)) 1.0)))
- (dotimes (i 6) (setf (elt tmp-weight i) (elt root-link-virtual-joint-weight i)))
- (if (memq :weight args)
- (let ((tmp-weight2 (funcall (cadr (memq :weight args)) union-link-list)))
- (dotimes (i (length tmp-weight2)) (setf (elt tmp-weight i) (* (elt tmp-weight i) (elt tmp-weight2 i))))))
- tmp-weight))
+ :additional-weight-list
+ (append
+ additional-weight-list
+ (list (list (car (send self :links)) root-link-virtual-joint-weight)))
args)
))
(:joint-order
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-10-06 21:55:21
|
Revision: 965
http://sourceforge.net/p/jskeus/code/965
Author: snozawa
Date: 2013-10-06 21:55:17 +0000 (Sun, 06 Oct 2013)
Log Message:
-----------
add init-pose to robot-model
Modified Paths:
--------------
trunk/irteus/irtrobot.l
Modified: trunk/irteus/irtrobot.l
===================================================================
--- trunk/irteus/irtrobot.l 2013-10-06 21:55:07 UTC (rev 964)
+++ trunk/irteus/irtrobot.l 2013-10-06 21:55:17 UTC (rev 965)
@@ -314,6 +314,8 @@
((null look-at-target))
(t (send self :head :look-at (send (car target-coords) :worldpos)))
))
+ ;; init-pose
+ (:init-pose () (send self :angle-vector (instantiate float-vector (send self :calc-target-joint-dimension (cdr (send self :links))))))
(:torque-vector
(&key (force-list) (moment-list) (target-coords)
(debug-view nil)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-10-06 21:55:10
|
Revision: 964
http://sourceforge.net/p/jskeus/code/964
Author: snozawa
Date: 2013-10-06 21:55:07 +0000 (Sun, 06 Oct 2013)
Log Message:
-----------
add torque-ratio-vector to cascaded-link
Modified Paths:
--------------
trunk/irteus/irtdyna.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2013-10-04 11:35:02 UTC (rev 963)
+++ trunk/irteus/irtdyna.l 2013-10-06 21:55:07 UTC (rev 964)
@@ -652,6 +652,13 @@
(t (elt (send j :max-joint-torque) k))))
(incf i)))
ret))
+ (:torque-ratio-vector
+ (&rest args &key (torque (send* self :torque-vector args)))
+ (let* ((mtq (send self :max-torque-vector))
+ (ret (instantiate float-vector (length mtq))))
+ (dotimes (i (length mtq))
+ (setf (elt ret i) (/ (elt torque i) (elt mtq i))))
+ ret))
(:calc-torque-buffer-args
()
;; buffers for computation
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
