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...> - 2014-01-08 17:10:16
|
Revision: 1038
http://sourceforge.net/p/jskeus/code/1038
Author: snozawa
Date: 2014-01-08 17:10:11 +0000 (Wed, 08 Jan 2014)
Log Message:
-----------
add method to calc worldcoords cop
Modified Paths:
--------------
trunk/irteus/irtdyna.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2014-01-07 17:43:45 UTC (rev 1037)
+++ trunk/irteus/irtdyna.l 2014-01-08 17:10:11 UTC (rev 1038)
@@ -875,6 +875,30 @@
(* (* 1e-3 (elt (send self :get :zmp) 0)) f0y -1.0)
(* (* 1e-3 (elt (send self :get :zmp) 1)) f0x)))))
(send self :get :zmp))
+ (:calc-cop-from-force-moment ;; calc cop at ee-coords
+ (force moment ;; [N] [Nm], global force & moment
+ sensor-coords cop-coords
+ &key (fz-thre 1))
+ ;; cop-coords : reference coords for COP, pos,z=COP pos z, c^p_{c,z}=0, rot, z=normal vector
+ ;; c^p_{c,x}=(c^f_{s,z} c^p_{s,x} - c^p_{s,z} c^f_{s,x} - c^n_{s,y})/c^f_{s,z}
+ ;; c^p_{c,y}=(c^f_{s,z} c^p_{s,y} - c^p_{s,z} c^f_{s,y} + c^n_{s,x})/c^f_{s,z}
+ ;; cop-coords relative values
+ (let* ((cfs (send cop-coords :inverse-rotate-vector force))
+ (cms (send cop-coords :inverse-rotate-vector moment))
+ (cps (scale 1e-3 (send cop-coords :inverse-transform-vector
+ (send sensor-coords :worldpos)))))
+ (setq tmpcopx (+ (* (elt cfs 2) (elt cps 0))
+ (- (* (elt cps 2) (elt cfs 0)))
+ (- (elt cms 1)))
+ tmpcopy (+ (* (elt cfs 2) (elt cps 1))
+ (- (* (elt cps 2) (elt cfs 1)))
+ (elt cms 0))
+ tmpfz (elt cfs 2))
+ (if (< tmpfz fz-thre)
+ nil
+ (send cop-coords :transform-vector
+ (scale 1e3 (float-vector (/ tmpcopx tmpfz) (/ tmpcopy tmpfz) 0))))
+ ))
;; calculate contact force from total wrench
;; total wrench = total force[N] + total moment[Nm] around the origin
;; return = contact wrenches at each contact points
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2014-01-07 17:43:49
|
Revision: 1037
http://sourceforge.net/p/jskeus/code/1037
Author: snozawa
Date: 2014-01-07 17:43:45 +0000 (Tue, 07 Jan 2014)
Log Message:
-----------
add weight for calc contact force
Modified Paths:
--------------
trunk/irteus/irtdyna.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2014-01-06 13:26:50 UTC (rev 1036)
+++ trunk/irteus/irtdyna.l 2014-01-07 17:43:45 UTC (rev 1037)
@@ -880,7 +880,8 @@
;; return = contact wrenches at each contact points
;; contact force [N] and contact moment [Nm] at contact points
(:calc-contact-wrenches-from-total-wrench
- (target-pos-list &optional (total-wrench)) ;; target-pos-list [mm]
+ (target-pos-list ;; target-pos-list [mm]
+ &key (total-wrench) (weight (fill (instantiate float-vector (* 6 (length target-pos-list))) 1)))
(unless total-wrench ;; if total-wrench is not set, set default value
(setq total-wrench ;; default wrench = wrench derived from weight-force
(let* ((weight-force (* (send self :weight) 1e-6 (elt *g-vec* 2)))) ;; weight[g] * 1e-6 * gvec[mm/s^2] = force[N]
@@ -890,7 +891,9 @@
;; set default force & moment by solving mimimum internal forces
(let ((ret-f
(transform
- (pseudo-inverse (send self :calc-grasp-matrix target-pos-list))
+ (send self :calc-inverse-jacobian
+ (send self :calc-grasp-matrix target-pos-list)
+ :weight weight)
total-wrench))
(force-list) (moment-list))
(dotimes (i (length target-pos-list))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2014-01-06 13:26:54
|
Revision: 1036
http://sourceforge.net/p/jskeus/code/1036
Author: snozawa
Date: 2014-01-06 13:26:50 +0000 (Mon, 06 Jan 2014)
Log Message:
-----------
set default-zmp-offsets based on all-limbs
Modified Paths:
--------------
trunk/irteus/demo/walk-motion.l
trunk/irteus/irtdyna.l
trunk/irteus/irtrobot.l
Modified: trunk/irteus/demo/walk-motion.l
===================================================================
--- trunk/irteus/demo/walk-motion.l 2014-01-06 12:01:19 UTC (rev 1035)
+++ trunk/irteus/demo/walk-motion.l 2014-01-06 13:26:50 UTC (rev 1036)
@@ -103,9 +103,6 @@
(append (list ;;:debug-view :no-message
:centroid-thre 100
:thre '(10 10 10 10)) 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)
)))
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2014-01-06 12:01:19 UTC (rev 1035)
+++ trunk/irteus/irtdyna.l 2014-01-06 13:26:50 UTC (rev 1036)
@@ -1291,9 +1291,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 :rleg (float-vector 0 0 0) :lleg (float-vector 0 0 0))) ;; [mm]
(delay 1.6) ;; delay [s]
((:all-limbs al) '(:rleg :lleg))
+ ((:default-zmp-offsets dzo)
+ (mapcan #'(lambda (x) (list x (float-vector 0 0 0))) al)) ;; [mm]
(q 1.0) (r 1e-6))
(setq footstep-node-list (mapcar #'(lambda (l) (if (consp l) l (list l))) fsl)
one-step-len (round (/ time dt))
Modified: trunk/irteus/irtrobot.l
===================================================================
--- trunk/irteus/irtrobot.l 2014-01-06 12:01:19 UTC (rev 1035)
+++ trunk/irteus/irtrobot.l 2014-01-06 13:26:50 UTC (rev 1036)
@@ -582,8 +582,10 @@
(: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 :rleg (float-vector 0 0 0) :lleg (float-vector 0 0 0)))
- (solve-angle-vector-args) (debug-view nil) ((:all-limbs al) '(:rleg :lleg))
+ (solve-angle-vector-args) (debug-view nil)
+ ((:all-limbs al) '(:rleg :lleg))
+ ((:default-zmp-offsets dzo)
+ (mapcan #'(lambda (x) (list x (float-vector 0 0 0))) al)) ;; [mm]
(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)))
@@ -592,7 +594,7 @@
(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
- :default-zmp-offsets default-zmp-offsets :all-limbs al)
+ :default-zmp-offsets dzo :all-limbs al)
(while (null (setq ret (send gg :proc-one-tick :type :cycloid :debug t :solve-angle-vector-args solve-angle-vector-args))))
(dotimes (i 2) (send self :calc-zmp)) ;; for zmp initialization
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2014-01-06 12:01:22
|
Revision: 1035
http://sourceforge.net/p/jskeus/code/1035
Author: snozawa
Date: 2014-01-06 12:01:19 +0000 (Mon, 06 Jan 2014)
Log Message:
-----------
use get-limbs-zmp instead of get-limbs-zmp-list
Modified Paths:
--------------
trunk/irteus/irtdyna.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2014-01-06 11:52:52 UTC (rev 1034)
+++ trunk/irteus/irtdyna.l 2014-01-06 12:01:19 UTC (rev 1035)
@@ -1320,13 +1320,9 @@
(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
- (scale (/ 1.0 (length (append current-swing-limbs current-support-leg-coords)))
- (reduce
- #'v+
- (append
- (send self :get-limbs-zmp-list current-swing-leg-dst-coords current-swing-limbs)
- (send self :get-limbs-zmp-list current-support-leg-coords current-support-limbs))
- :initial-value (float-vector 0 0 0))))
+ (send self :get-limbs-zmp
+ (append current-swing-leg-dst-coords current-support-leg-coords)
+ (append current-swing-limbs current-support-limbs)))
(send self :append-step-height-list 0.0)
(pop footstep-node-list)
;; currently, not error system
@@ -1362,16 +1358,9 @@
(elt dst-swing-coords (position x prev-support-limbs)))
(send self :get-swing-limbs (car (last support-leg-list)))))
(send self :append-refzmp-cur-list
- (scale (/ 1.0 (length (append (car (last support-leg-list)) (send self :get-counter-footstep-limbs (car (last support-leg-list))))))
- (reduce
- #'v+
- (append
- (send self :get-limbs-zmp-list
- (car (last support-leg-coords-list)) (car (last support-leg-list)))
- (send self :get-limbs-zmp-list
- (car (last swing-leg-dst-coords-list))
- (send self :get-counter-footstep-limbs (car (last support-leg-list)))))
- :initial-value (float-vector 0 0 0))))
+ (send self :get-limbs-zmp
+ (append (car (last support-leg-coords-list)) (car (last swing-leg-dst-coords-list)))
+ (append (car (last support-leg-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))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2014-01-06 11:52:57
|
Revision: 1034
http://sourceforge.net/p/jskeus/code/1034
Author: snozawa
Date: 2014-01-06 11:52:52 +0000 (Mon, 06 Jan 2014)
Log Message:
-----------
fix initialize of refzmp-next
Modified Paths:
--------------
trunk/irteus/irtdyna.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2014-01-06 03:55:57 UTC (rev 1033)
+++ trunk/irteus/irtdyna.l 2014-01-06 11:52:52 UTC (rev 1034)
@@ -1338,7 +1338,7 @@
:init-xk cog
)
swing-leg-src-coords current-swing-leg-dst-coords
- refzmp-next (send self :get-limbs-zmp (car swing-leg-dst-coords-list) current-support-limbs)
+ refzmp-next (send self :get-limbs-zmp current-support-leg-coords current-support-limbs)
refzmp-prev (car refzmp-cur-list))
(send self :make-gait-parameter)
t))))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2014-01-06 03:56:02
|
Revision: 1033
http://sourceforge.net/p/jskeus/code/1033
Author: snozawa
Date: 2014-01-06 03:55:57 +0000 (Mon, 06 Jan 2014)
Log Message:
-----------
increase loop count for solving riccati equation
Modified Paths:
--------------
trunk/irteus/irtdyna.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2013-12-28 20:19:27 UTC (rev 1032)
+++ trunk/irteus/irtdyna.l 2014-01-06 03:55:57 UTC (rev 1033)
@@ -970,7 +970,7 @@
(ct (transpose c))
(zero-matrix (make-matrix (array-dimension c 1) (array-dimension c 1))))
(do ((loop 0 (1+ loop)))
- ((>= loop 2000))
+ ((>= loop 10000))
(let* ((tmp-PA (m* P A)))
(setq R+btPb-1 (/ 1.0 (+ R (aref (m* bt (m* P b)) 0 0)))
K (scale-matrix R+btPb-1 (m* bt tmp-PA)))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ky...@us...> - 2013-12-28 20:19:30
|
Revision: 1032
http://sourceforge.net/p/jskeus/code/1032
Author: kyouhei
Date: 2013-12-28 20:19:27 +0000 (Sat, 28 Dec 2013)
Log Message:
-----------
remove temporary debug message
Modified Paths:
--------------
trunk/irteus/irtsensor.l
Modified: trunk/irteus/irtsensor.l
===================================================================
--- trunk/irteus/irtsensor.l 2013-12-28 08:05:41 UTC (rev 1031)
+++ trunk/irteus/irtsensor.l 2013-12-28 20:19:27 UTC (rev 1032)
@@ -265,7 +265,6 @@
(dotimes (x width)
(if (< (elt fv vptr) 1.0)
(let ((zpos (/ (* fp np) (- (* (elt fv vptr) (- fp np)) fp))))
- (pprint (list x y))
(setf (elt pos 0) (* (- cx x) (/ zpos focus)))
(setf (elt pos 1) (* (- y cy) (/ zpos focus)))
(setf (elt pos 2) (- zpos))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-12-28 08:05:45
|
Revision: 1031
http://sourceforge.net/p/jskeus/code/1031
Author: snozawa
Date: 2013-12-28 08:05:41 +0000 (Sat, 28 Dec 2013)
Log Message:
-----------
fix selecting of start leg
Modified Paths:
--------------
trunk/irteus/irtrobot.l
Modified: trunk/irteus/irtrobot.l
===================================================================
--- trunk/irteus/irtrobot.l 2013-12-27 04:03:25 UTC (rev 1030)
+++ trunk/irteus/irtrobot.l 2013-12-28 08:05:41 UTC (rev 1031)
@@ -716,7 +716,9 @@
(y-sign (if (> yy 0.0) 1.0 -1.0))
(th-sign (if (> th 0.0) 1.0 -1.0))
(dx 0.0) (dy 0.0) (dth 0.0) (cnt 0)
- (leg (if (> yy 0.0) :lleg :rleg))
+ (leg (if (eps= (float yy) 0.0)
+ (if (> th 0.0) :lleg :rleg)
+ (if (> yy 0.0) :lleg :rleg)))
(mc (apply #'midcoords 0.5 (send self :legs :end-coords :copy-worldcoords)))
;;(mc (make-coords))
(leg-translate-pos
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-12-27 04:03:29
|
Revision: 1030
http://sourceforge.net/p/jskeus/code/1030
Author: snozawa
Date: 2013-12-27 04:03:25 +0000 (Fri, 27 Dec 2013)
Log Message:
-----------
return pattern list
Modified Paths:
--------------
trunk/irteus/demo/walk-motion.l
Modified: trunk/irteus/demo/walk-motion.l
===================================================================
--- trunk/irteus/demo/walk-motion.l 2013-12-19 18:06:25 UTC (rev 1029)
+++ trunk/irteus/demo/walk-motion.l 2013-12-27 04:03:25 UTC (rev 1030)
@@ -161,8 +161,10 @@
(load (format nil "models/~A-robot.l" form))
(funcall form))
'(h7 h6 kaz3))))
- (dolist (rb *robots*)
- (format t ";; ~A walking~%" (send rb :name))
- (walk-motion rb)
- ))
+ (mapcar #'(lambda
+ (rb)
+ (format t ";; ~A walking~%" (send rb :name))
+ (walk-motion rb))
+ *robots*)
+ )
(warn "(walk-motion-for-robots) for walking motion for several robot models~%")
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-12-19 18:06:28
|
Revision: 1029
http://sourceforge.net/p/jskeus/code/1029
Author: snozawa
Date: 2013-12-19 18:06:25 +0000 (Thu, 19 Dec 2013)
Log Message:
-----------
add crawl example
Modified Paths:
--------------
trunk/irteus/demo/walk-motion.l
Modified: trunk/irteus/demo/walk-motion.l
===================================================================
--- trunk/irteus/demo/walk-motion.l 2013-12-19 17:59:55 UTC (rev 1028)
+++ trunk/irteus/demo/walk-motion.l 2013-12-19 18:06:25 UTC (rev 1029)
@@ -47,7 +47,7 @@
(warn "(walk-motion-for-sample-robot) for walking motion~%")
(defun quad-walk-motion-for-sample-robot
- (&key (go-backward-over t))
+ (gen-footstep-func &key (go-backward-over t))
(unless (boundp '*robot*)
(setq *robot* (instance sample-robot :init)))
;; initial quad pose
@@ -87,41 +87,73 @@
:target-centroid-pos (vector-mean (send-all tc :worldpos))
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)))
+ (let ((footstep-list (funcall gen-footstep-func)))
(objects (list *robot*))
;; solve walk motion
(send *robot* :calc-walk-pattern-from-footstep-list
footstep-list :debug-view :no-message
+ :all-limbs '(:rleg :lleg :rarm :larm)
:init-pose-function
#'(lambda ()
(send* *robot* :move-centroid-on-foot :both '(:rleg :lleg :rarm :larm)
: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
+ :solve-angle-vector-args
+ (append (list ;;:debug-view :no-message
+ :centroid-thre 100
+ :thre '(10 10 10 10)) 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 trot-walk-motion-for-sample-robot
+ (&key (go-backward-over t))
+ (quad-walk-motion-for-sample-robot
+ #'(lambda ()
+ (let ((fs
+ (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 fs)
+ (send (car x) :put :l/r (if (evenp (position x fs)) :rleg :lleg))
+ (send (cadr x) :put :l/r (if (evenp (position x fs)) :larm :rarm)))
+ fs))
+ :go-backward-over go-backward-over))
+(warn "(trot-walk-motion-for-sample-robot) for walking motion~%")
+
+(defun crawl-walk-motion-for-sample-robot
+ (&key (go-backward-over t))
+ (quad-walk-motion-for-sample-robot
+ #'(lambda ()
+ (let ((fs
+ (list (list (send *robot* :lleg :end-coords :copy-worldcoords))
+ (list (send (send *robot* :larm :end-coords :copy-worldcoords) :translate (float-vector 50 0 0) :world))
+ (list (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 50 0 0) :world))
+ (list (send (send *robot* :lleg :end-coords :copy-worldcoords) :translate (float-vector 50 0 0) :world))
+ )))
+ (send (car (elt fs 0)) :put :l/r :lleg)
+ (send (car (elt fs 1)) :put :l/r :larm)
+ (send (car (elt fs 2)) :put :l/r :rarm)
+ (send (car (elt fs 3)) :put :l/r :rleg)
+ (send (car (elt fs 4)) :put :l/r :lleg)
+ fs))
+ :go-backward-over go-backward-over))
+(warn "(crawl-walk-motion-for-sample-robot) for walking motion~%")
+
(defun walk-motion-for-robots ()
(unless (boundp '*robots*)
(setq *robots*
@@ -133,4 +165,4 @@
(format t ";; ~A walking~%" (send rb :name))
(walk-motion rb)
))
-(warn "(walk-motion-for-robots) for walking motion for several robot models~%")
\ No newline at end of file
+(warn "(walk-motion-for-robots) for walking motion for several robot models~%")
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-12-19 17:59:57
|
Revision: 1028
http://sourceforge.net/p/jskeus/code/1028
Author: snozawa
Date: 2013-12-19 17:59:55 +0000 (Thu, 19 Dec 2013)
Log Message:
-----------
add all-limbs arguments and support crawl walk
Modified Paths:
--------------
trunk/irteus/irtdyna.l
trunk/irteus/irtrobot.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2013-12-19 17:59:11 UTC (rev 1027)
+++ trunk/irteus/irtdyna.l 2013-12-19 17:59:55 UTC (rev 1028)
@@ -1226,6 +1226,7 @@
finalize-p ;; Are all footstep-node-list executed? finalize flag for make-gait-parameter. Is this necessary?
;; preview controller parameters
preview-controller ;; preview controller for ZMP -> COG
+ all-limbs
)
)
@@ -1344,11 +1345,22 @@
;; append final gait parameters
(:finalize-gait-parameter
()
- (let ((dst-swing-coords (car (last support-leg-coords-list))))
+ (let ((dst-swing-coords (car (last support-leg-coords-list)))
+ (prev-support-limbs (car (last support-leg-list))))
(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 :get-counter-footstep-limbs
+ (send self :get-swing-limbs (cadr (reverse support-leg-list)))))
+ (send self :append-support-leg-coords-list
+ (mapcar #'(lambda (x)
+ (if (memq x prev-support-limbs)
+ (elt (car (last support-leg-coords-list)) (position x prev-support-limbs))
+ (elt (car (last swing-leg-dst-coords-list))
+ (position x (send self :get-swing-limbs prev-support-limbs)))))
+ (car (last support-leg-list))))
+ (send self :append-swing-leg-dst-coords-list
+ (mapcar #'(lambda (x)
+ (elt dst-swing-coords (position x prev-support-limbs)))
+ (send self :get-swing-limbs (car (last support-leg-list)))))
(send self :append-refzmp-cur-list
(scale (/ 1.0 (length (append (car (last support-leg-list)) (send self :get-counter-footstep-limbs (car (last support-leg-list))))))
(reduce
@@ -1366,10 +1378,17 @@
;; generate gait parameters
(:make-gait-parameter
()
- (let ((fs (pop footstep-node-list)))
+ (let ((fs (pop footstep-node-list))
+ (prev-support-limbs (car (last support-leg-list))))
(send self :append-support-leg-list
(send self :get-counter-footstep-limbs fs))
- (send self :append-support-leg-coords-list (car (last swing-leg-dst-coords-list)))
+ (send self :append-support-leg-coords-list
+ (mapcar #'(lambda (x)
+ (if (memq x prev-support-limbs)
+ (elt (car (last support-leg-coords-list)) (position x prev-support-limbs))
+ (elt (car (last swing-leg-dst-coords-list))
+ (position x (send self :get-swing-limbs prev-support-limbs)))))
+ (car (last support-leg-list))))
(send self :append-swing-leg-dst-coords-list (send-all fs :worldcoords))
(send self :append-refzmp-cur-list
(send self :get-limbs-zmp (car (last support-leg-coords-list)) (car (last support-leg-list))))
@@ -1449,6 +1468,7 @@
(:update-current-gait-parameter
()
(setq refzmp-prev (car refzmp-cur-list))
+ (let ((prev-support-limbs2 (car support-leg-list)))
(cond
(footstep-node-list
(send self :make-gait-parameter)
@@ -1459,7 +1479,13 @@
))
(setq index-count one-step-len)
(pop support-leg-list)
- (setq swing-leg-src-coords (pop support-leg-coords-list))
+ (setq swing-leg-src-coords
+ (if support-leg-list
+ (mapcar #'(lambda (x)
+ (elt (car support-leg-coords-list) (position x prev-support-limbs2)))
+ (send self :get-swing-limbs (car support-leg-list)))))
+ (pop support-leg-coords-list)
+ )
(pop swing-leg-dst-coords-list)
(pop refzmp-cur-list)
(pop step-height-list)
Modified: trunk/irteus/irtrobot.l
===================================================================
--- trunk/irteus/irtrobot.l 2013-12-19 17:59:11 UTC (rev 1027)
+++ trunk/irteus/irtrobot.l 2013-12-19 17:59:55 UTC (rev 1028)
@@ -583,7 +583,7 @@
(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) ((:all-limbs al) '(:rleg :lleg))
(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)))
@@ -592,7 +592,7 @@
(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
- :default-zmp-offsets default-zmp-offsets)
+ :default-zmp-offsets default-zmp-offsets :all-limbs al)
(while (null (setq ret (send gg :proc-one-tick :type :cycloid :debug t :solve-angle-vector-args solve-angle-vector-args))))
(dotimes (i 2) (send self :calc-zmp)) ;; for zmp initialization
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-12-19 17:59:14
|
Revision: 1027
http://sourceforge.net/p/jskeus/code/1027
Author: snozawa
Date: 2013-12-19 17:59:11 +0000 (Thu, 19 Dec 2013)
Log Message:
-----------
use get-limbs-zmp-list
Modified Paths:
--------------
trunk/irteus/irtdyna.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2013-12-19 17:58:39 UTC (rev 1026)
+++ trunk/irteus/irtdyna.l 2013-12-19 17:59:11 UTC (rev 1027)
@@ -1261,23 +1261,30 @@
(if (symbolp (car fs))
(remove-if
#'(lambda (l)
- (memq l (remove-if-not #'(lambda (x) (memq x fs)) '(:rleg :lleg :rarm :larm))))
- '(:rleg :lleg :rarm :larm))
+ (memq l (remove-if-not #'(lambda (x) (memq x fs)) all-limbs)))
+ all-limbs)
(send self :get-counter-footstep-limbs (send self :get-footstep-limbs fs)))
)
+ (:get-limbs-zmp-list
+ (limb-coords limb-names)
+ (mapcar #'(lambda (lc ln)
+ (v+ (send lc :worldpos)
+ (send lc :rotate-vector
+ (cadr (memq ln default-zmp-offsets)))))
+ limb-coords limb-names))
(:get-limbs-zmp
(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)
+ (send self :get-limbs-zmp-list limb-coords limb-names)
:initial-value (float-vector 0 0 0)
))
)
+ (:get-swing-limbs
+ (limbs)
+ (remove-if #'(lambda (x) (memq x limbs)) all-limbs)
+ )
;; initialize parameter and generate first gait parameter
(:initialize-gait-parameter
(fsl time cog ;; time is [s], cog is [mm]
@@ -1285,6 +1292,7 @@
((:default-double-support-ratio ddsr) 0.2)
((:default-zmp-offsets dzo) (list :rleg (float-vector 0 0 0) :lleg (float-vector 0 0 0))) ;; [mm]
(delay 1.6) ;; delay [s]
+ ((:all-limbs al) '(:rleg :lleg))
(q 1.0) (r 1e-6))
(setq footstep-node-list (mapcar #'(lambda (l) (if (consp l) l (list l))) fsl)
one-step-len (round (/ time dt))
@@ -1297,7 +1305,8 @@
default-double-support-ratio ddsr
default-zmp-offsets dzo
index-count one-step-len
- finalize-p nil)
+ finalize-p nil
+ all-limbs al)
(let ((current-swing-limbs
(send self :get-footstep-limbs (car footstep-node-list))))
(send self :append-support-leg-list
@@ -1310,9 +1319,13 @@
(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
- (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)))
+ (scale (/ 1.0 (length (append current-swing-limbs current-support-leg-coords)))
+ (reduce
+ #'v+
+ (append
+ (send self :get-limbs-zmp-list current-swing-leg-dst-coords current-swing-limbs)
+ (send self :get-limbs-zmp-list current-support-leg-coords current-support-limbs))
+ :initial-value (float-vector 0 0 0))))
(send self :append-step-height-list 0.0)
(pop footstep-node-list)
;; currently, not error system
@@ -1337,12 +1350,16 @@
(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
- (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))))))
+ (scale (/ 1.0 (length (append (car (last support-leg-list)) (send self :get-counter-footstep-limbs (car (last support-leg-list))))))
+ (reduce
+ #'v+
+ (append
+ (send self :get-limbs-zmp-list
+ (car (last support-leg-coords-list)) (car (last support-leg-list)))
+ (send self :get-limbs-zmp-list
+ (car (last swing-leg-dst-coords-list))
+ (send self :get-counter-footstep-limbs (car (last support-leg-list)))))
+ :initial-value (float-vector 0 0 0))))
(send self :append-step-height-list 0.0)
(setq refzmp-next (car (last refzmp-cur-list)))
t))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-12-19 17:58:40
|
Revision: 1026
http://sourceforge.net/p/jskeus/code/1026
Author: snozawa
Date: 2013-12-19 17:58:39 +0000 (Thu, 19 Dec 2013)
Log Message:
-----------
use rest of limbs
Modified Paths:
--------------
trunk/irteus/irtdyna.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2013-12-15 14:22:45 UTC (rev 1025)
+++ trunk/irteus/irtdyna.l 2013-12-19 17:58:39 UTC (rev 1026)
@@ -1259,14 +1259,10 @@
(:get-counter-footstep-limbs
(fs)
(if (symbolp (car fs))
- (mapcar #'(lambda (f)
- (case f
- (:rleg :lleg)
- (:lleg :rleg)
- (:rarm :larm)
- (:larm :rarm)
- ))
- fs)
+ (remove-if
+ #'(lambda (l)
+ (memq l (remove-if-not #'(lambda (x) (memq x fs)) '(:rleg :lleg :rarm :larm))))
+ '(:rleg :lleg :rarm :larm))
(send self :get-counter-footstep-limbs (send self :get-footstep-limbs fs)))
)
(:get-limbs-zmp
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-12-15 14:22:50
|
Revision: 1025
http://sourceforge.net/p/jskeus/code/1025
Author: snozawa
Date: 2013-12-15 14:22:45 +0000 (Sun, 15 Dec 2013)
Log Message:
-----------
fix order of joints and set list of transform-coords
Modified Paths:
--------------
trunk/irteus/irtdyna.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2013-12-15 12:35:51 UTC (rev 1024)
+++ trunk/irteus/irtdyna.l 2013-12-15 14:22:45 UTC (rev 1025)
@@ -815,18 +815,25 @@
))
(:calc-torque-from-ext-wrenches
(&key (force-list) (moment-list) (target-coords))
- (let ((jacobi
- (send self :calc-jacobian-from-link-list
- (mapcar #'(lambda (tc) (send self :link-list (send tc :parent))) target-coords)
- :move-target target-coords
- :transform-coords (make-coords)
- :rotation-axis (make-list (length target-coords) :initial-element t)
- :translation-axis (make-list (length target-coords) :initial-element t)))
- (wrenches
- (mapcar #'(lambda (f m) (concatenate float-vector f m)) force-list moment-list)))
- (transform (transpose jacobi)
- (apply #'concatenate float-vector wrenches))
- ))
+ (let* ((link-list (mapcar #'(lambda (tc) (send self :link-list (send tc :parent))) target-coords))
+ (jacobi
+ (send self :calc-jacobian-from-link-list
+ link-list
+ :move-target target-coords
+ :transform-coords (mapcar #'(lambda (x) (make-coords)) target-coords)
+ :rotation-axis (make-list (length target-coords) :initial-element t)
+ :translation-axis (make-list (length target-coords) :initial-element t)))
+ (wrenches
+ (mapcar #'(lambda (f m) (concatenate float-vector f m)) force-list moment-list))
+ (tq (transform (transpose jacobi)
+ (apply #'concatenate float-vector wrenches)))
+ (ret-tq (instantiate float-vector (length joint-list)))
+ (ul (send self :calc-union-link-list link-list)))
+ ;; tq = torque vector included in link-list
+ ;; ret-tq = all torque vector in joint-list
+ (dotimes (i (length ul))
+ (setf (elt ret-tq (position (send (elt ul i) :joint) joint-list)) (elt tq i)))
+ ret-tq))
;; calculate Zero Moment Point based on Inverse Dynamics
;; necessary arguments -> av and root-coords
(:calc-zmp
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-12-15 12:35:57
|
Revision: 1024
http://sourceforge.net/p/jskeus/code/1024
Author: snozawa
Date: 2013-12-15 12:35:51 +0000 (Sun, 15 Dec 2013)
Log Message:
-----------
add :calc-torque-from-ext-wrenches
Modified Paths:
--------------
trunk/irteus/irtdyna.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2013-12-15 10:36:03 UTC (rev 1023)
+++ trunk/irteus/irtdyna.l 2013-12-15 12:35:51 UTC (rev 1024)
@@ -813,6 +813,20 @@
(list :joint-velocity-vector jvv
:joint-acceleration-vector jav)
))
+ (:calc-torque-from-ext-wrenches
+ (&key (force-list) (moment-list) (target-coords))
+ (let ((jacobi
+ (send self :calc-jacobian-from-link-list
+ (mapcar #'(lambda (tc) (send self :link-list (send tc :parent))) target-coords)
+ :move-target target-coords
+ :transform-coords (make-coords)
+ :rotation-axis (make-list (length target-coords) :initial-element t)
+ :translation-axis (make-list (length target-coords) :initial-element t)))
+ (wrenches
+ (mapcar #'(lambda (f m) (concatenate float-vector f m)) force-list moment-list)))
+ (transform (transpose jacobi)
+ (apply #'concatenate float-vector wrenches))
+ ))
;; calculate Zero Moment Point based on Inverse Dynamics
;; necessary arguments -> av and root-coords
(:calc-zmp
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-12-15 10:36:05
|
Revision: 1023
http://sourceforge.net/p/jskeus/code/1023
Author: snozawa
Date: 2013-12-15 10:36:03 +0000 (Sun, 15 Dec 2013)
Log Message:
-----------
enable to set external force and moment in :calc-torque method
Modified Paths:
--------------
trunk/irteus/irtdyna.l
trunk/irteus/irtrobot.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2013-12-15 10:26:05 UTC (rev 1022)
+++ trunk/irteus/irtdyna.l 2013-12-15 10:36:03 UTC (rev 1023)
@@ -683,7 +683,33 @@
(dt 0.005) ;; [sec]
(av (send self :angle-vector))
(root-coords (send (car (send self :links)) :copy-worldcoords))
+ (force-list) (moment-list) (target-coords)
(calc-torque-buffer-args (send self :calc-torque-buffer-args)))
+ ;; check length
+ (unless (= (length force-list) (length moment-list) (length target-coords))
+ (error ";; ERROR : list length differ in :calc-torque : force-list ~A moment-list ~A target-coords ~A~%"
+ (length force-list) (length moment-list) (length target-coords)))
+ ;; set ext wrench
+ (mapcar #'(lambda (fv mv tc)
+ (send (send tc :parent) :ext-force fv)
+ ;; calc moment-offset caused by fv ;
+ ;; current irtdyna.l's requires moment around the origin
+ (let* ((moment-offset (v* (scale 1e-3 (send tc :worldpos)) fv)))
+ (send (send tc :parent) :ext-moment (v+ mv moment-offset))))
+ force-list moment-list target-coords)
+ (send self :calc-torque-without-ext-wrench
+ :debug-view debug-view
+ :calc-statics-p calc-statics-p
+ :av av :root-coords root-coords :dt dt
+ :calc-torque-buffer-args calc-torque-buffer-args)
+ )
+ (:calc-torque-without-ext-wrench
+ (&key (debug-view nil)
+ (calc-statics-p t)
+ (dt 0.005) ;; [sec]
+ (av (send self :angle-vector))
+ (root-coords (send (car (send self :links)) :copy-worldcoords))
+ (calc-torque-buffer-args (send self :calc-torque-buffer-args)))
(send* self :calc-torque-from-vel-acc
:debug-view debug-view
:calc-torque-buffer-args calc-torque-buffer-args
Modified: trunk/irteus/irtrobot.l
===================================================================
--- trunk/irteus/irtrobot.l 2013-12-15 10:26:05 UTC (rev 1022)
+++ trunk/irteus/irtrobot.l 2013-12-15 10:36:03 UTC (rev 1023)
@@ -335,22 +335,11 @@
(let* ((ret-fm (send self :calc-contact-wrenches-from-total-wrench (send-all target-coords :worldpos))))
(setq force-list (car ret-fm) moment-list (cadr ret-fm))
)))
- (unless (= (length force-list) (length moment-list)
- (length target-coords))
- (warn ";; ERROR : list length differ : force-list ~A moment-list ~A target-coords ~A~%"
- (length force-list) (length moment-list) (length target-coords))
- (return-from :torque-vector (instantiate float-vector (length joint-list))))
- (mapcar #'(lambda (fv mv tc)
- (send (send tc :parent) :ext-force fv)
- ;; calc moment-offset caused by fv ;
- ;; current irtdyna.l's requires moment around the origin
- (let* ((moment-offset (v* (scale 1e-3 (send tc :worldpos)) fv)))
- (send (send tc :parent) :ext-moment (v+ mv moment-offset))))
- force-list moment-list target-coords)
(send self :calc-torque
:debug-view debug-view
:calc-statics-p calc-statics-p
:av av :root-coords root-coords :dt dt
+ :force-list force-list :moment-list moment-list :target-coords target-coords
:calc-torque-buffer-args calc-torque-buffer-args)
)
(:calc-force-from-joint-torque
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-12-15 10:26:10
|
Revision: 1022
http://sourceforge.net/p/jskeus/code/1022
Author: snozawa
Date: 2013-12-15 10:26:05 +0000 (Sun, 15 Dec 2013)
Log Message:
-----------
set default total wrench
Modified Paths:
--------------
trunk/irteus/irtdyna.l
trunk/irteus/irtrobot.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2013-12-15 10:16:36 UTC (rev 1021)
+++ trunk/irteus/irtdyna.l 2013-12-15 10:26:05 UTC (rev 1022)
@@ -833,7 +833,13 @@
;; return = contact wrenches at each contact points
;; contact force [N] and contact moment [Nm] at contact points
(:calc-contact-wrenches-from-total-wrench
- (target-pos-list total-wrench) ;; target-pos-list [mm]
+ (target-pos-list &optional (total-wrench)) ;; target-pos-list [mm]
+ (unless total-wrench ;; if total-wrench is not set, set default value
+ (setq total-wrench ;; default wrench = wrench derived from weight-force
+ (let* ((weight-force (* (send self :weight) 1e-6 (elt *g-vec* 2)))) ;; weight[g] * 1e-6 * gvec[mm/s^2] = force[N]
+ (float-vector 0 0 weight-force
+ (* 1e-3 (elt (send self :centroid nil) 1) weight-force)
+ (* -1e-3 (elt (send self :centroid nil) 0) weight-force) 0))))
;; set default force & moment by solving mimimum internal forces
(let ((ret-f
(transform
Modified: trunk/irteus/irtrobot.l
===================================================================
--- trunk/irteus/irtrobot.l 2013-12-15 10:16:36 UTC (rev 1021)
+++ trunk/irteus/irtrobot.l 2013-12-15 10:26:05 UTC (rev 1022)
@@ -332,12 +332,7 @@
(push (send self limb :end-coords) target-coords)))
(unless (or force-list moment-list) ;; force [N], moment [Nm]
;; set default force & moment by solving mimimum internal forces
- (let* ((weight-force (* (send self :weight) 1e-6 (elt *g-vec* 2))) ;; weight[g] * 1e-6 * gvec[mm/s^2] = force[N]
- (ret-fm (send self :calc-contact-wrenches-from-total-wrench
- (send-all target-coords :worldpos)
- (float-vector 0 0 weight-force
- (* 1e-3 (elt (send self :centroid nil) 1) weight-force)
- (* -1e-3 (elt (send self :centroid nil) 0) weight-force) 0))))
+ (let* ((ret-fm (send self :calc-contact-wrenches-from-total-wrench (send-all target-coords :worldpos))))
(setq force-list (car ret-fm) moment-list (cadr ret-fm))
)))
(unless (= (length force-list) (length moment-list)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-12-15 10:16:41
|
Revision: 1021
http://sourceforge.net/p/jskeus/code/1021
Author: snozawa
Date: 2013-12-15 10:16:36 +0000 (Sun, 15 Dec 2013)
Log Message:
-----------
add calc-contact-wrenches-from-total-wrench and use it in :torque-vector
Modified Paths:
--------------
trunk/irteus/irtdyna.l
trunk/irteus/irtrobot.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2013-12-15 10:16:15 UTC (rev 1020)
+++ trunk/irteus/irtdyna.l 2013-12-15 10:16:36 UTC (rev 1021)
@@ -828,6 +828,24 @@
(* (* 1e-3 (elt (send self :get :zmp) 0)) f0y -1.0)
(* (* 1e-3 (elt (send self :get :zmp) 1)) f0x)))))
(send self :get :zmp))
+ ;; calculate contact force from total wrench
+ ;; total wrench = total force[N] + total moment[Nm] around the origin
+ ;; return = contact wrenches at each contact points
+ ;; contact force [N] and contact moment [Nm] at contact points
+ (:calc-contact-wrenches-from-total-wrench
+ (target-pos-list total-wrench) ;; target-pos-list [mm]
+ ;; set default force & moment by solving mimimum internal forces
+ (let ((ret-f
+ (transform
+ (pseudo-inverse (send self :calc-grasp-matrix target-pos-list))
+ total-wrench))
+ (force-list) (moment-list))
+ (dotimes (i (length target-pos-list))
+ (push (subseq ret-f (* i 6) (+ (* i 6) 3)) force-list)
+ (push (subseq ret-f (+ (* i 6) 3) (+ (* i 6) 6)) moment-list)
+ )
+ (list (reverse force-list) (reverse moment-list))
+ ))
(:draw-torque
(vwer &key flush (width 2) (size 100) (color (float-vector 1 0.3 0)) (warning-color (float-vector 1 0 0)) (torque-threshold nil)
Modified: trunk/irteus/irtrobot.l
===================================================================
--- trunk/irteus/irtrobot.l 2013-12-15 10:16:15 UTC (rev 1020)
+++ trunk/irteus/irtrobot.l 2013-12-15 10:16:36 UTC (rev 1021)
@@ -333,19 +333,13 @@
(unless (or force-list moment-list) ;; force [N], moment [Nm]
;; set default force & moment by solving mimimum internal forces
(let* ((weight-force (* (send self :weight) 1e-6 (elt *g-vec* 2))) ;; weight[g] * 1e-6 * gvec[mm/s^2] = force[N]
- (ret-f
- (transform
- (pseudo-inverse (send self :calc-grasp-matrix (send-all target-coords :worldpos)))
- (float-vector 0 0 weight-force
- (* 1e-3 (elt (send self :centroid nil) 1) weight-force)
- (* -1e-3 (elt (send self :centroid nil) 0) weight-force) 0))))
- (dotimes (i (length target-coords))
- (push (subseq ret-f (* i 6) (+ (* i 6) 3)) force-list)
- (push (subseq ret-f (+ (* i 6) 3) (+ (* i 6) 6)) moment-list)
- ))
- (setq force-list (reverse force-list))
- (setq moment-list (reverse moment-list))
- ))
+ (ret-fm (send self :calc-contact-wrenches-from-total-wrench
+ (send-all target-coords :worldpos)
+ (float-vector 0 0 weight-force
+ (* 1e-3 (elt (send self :centroid nil) 1) weight-force)
+ (* -1e-3 (elt (send self :centroid nil) 0) weight-force) 0))))
+ (setq force-list (car ret-fm) moment-list (cadr ret-fm))
+ )))
(unless (= (length force-list) (length moment-list)
(length target-coords))
(warn ";; ERROR : list length differ : force-list ~A moment-list ~A target-coords ~A~%"
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-12-15 10:16:18
|
Revision: 1020
http://sourceforge.net/p/jskeus/code/1020
Author: snozawa
Date: 2013-12-15 10:16:15 +0000 (Sun, 15 Dec 2013)
Log Message:
-----------
add comment for unit system
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2013-12-15 10:11:25 UTC (rev 1019)
+++ trunk/irteus/irtmodel.l 2013-12-15 10:16:15 UTC (rev 1020)
@@ -2165,7 +2165,7 @@
;; | E_3 0 E_3 0 ... |
;; | p1_hat E_3 p2_hat E_3 ... |
(:calc-grasp-matrix
- (contact-points &optional (ret (make-matrix 6 (* 6 (length contact-points)))))
+ (contact-points &optional (ret (make-matrix 6 (* 6 (length contact-points))))) ;; contact-points [mm]
(let ((contact-matrices (mapcar #'(lambda (x) (outer-product-matrix (scale 1e-3 x))) contact-points)))
(dotimes (c (length contact-points))
(dotimes (i 3)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-12-15 10:11:27
|
Revision: 1019
http://sourceforge.net/p/jskeus/code/1019
Author: snozawa
Date: 2013-12-15 10:11:25 +0000 (Sun, 15 Dec 2013)
Log Message:
-----------
fix position of calculation of :weight and fix indent
Modified Paths:
--------------
trunk/irteus/irtrobot.l
Modified: trunk/irteus/irtrobot.l
===================================================================
--- trunk/irteus/irtrobot.l 2013-12-15 09:20:59 UTC (rev 1018)
+++ trunk/irteus/irtrobot.l 2013-12-15 10:11:25 UTC (rev 1019)
@@ -327,25 +327,25 @@
(root-coords (send (car (send self :links)) :copy-worldcoords))
(calc-torque-buffer-args (send self :calc-torque-buffer-args)))
(unless (every #'null (send self :legs)) ;; for legged robot, set default external force and moment at the end-effectors
- (let ((weight-force (* (send self :weight) 1e-6 (elt *g-vec* 2)))) ;; weight[g] * 1e-6 * gvec[mm/s^2] = force[N]
- (unless target-coords
- (dolist (limb '(:rleg :lleg))
- (push (send self limb :end-coords) target-coords)))
- (unless (or force-list moment-list) ;; force [N], moment [Nm]
- ;; set default force & moment by solving mimimum internal forces
- (let ((ret-f
- (transform
- (pseudo-inverse (send self :calc-grasp-matrix (send-all target-coords :worldpos)))
- (float-vector 0 0 weight-force
- (* 1e-3 (elt (send self :centroid nil) 1) weight-force)
- (* -1e-3 (elt (send self :centroid nil) 0) weight-force) 0))))
- (dotimes (i (length target-coords))
- (push (subseq ret-f (* i 6) (+ (* i 6) 3)) force-list)
- (push (subseq ret-f (+ (* i 6) 3) (+ (* i 6) 6)) moment-list)
- ))
- (setq force-list (reverse force-list))
- (setq moment-list (reverse moment-list))
- )))
+ (unless target-coords
+ (dolist (limb '(:rleg :lleg))
+ (push (send self limb :end-coords) target-coords)))
+ (unless (or force-list moment-list) ;; force [N], moment [Nm]
+ ;; set default force & moment by solving mimimum internal forces
+ (let* ((weight-force (* (send self :weight) 1e-6 (elt *g-vec* 2))) ;; weight[g] * 1e-6 * gvec[mm/s^2] = force[N]
+ (ret-f
+ (transform
+ (pseudo-inverse (send self :calc-grasp-matrix (send-all target-coords :worldpos)))
+ (float-vector 0 0 weight-force
+ (* 1e-3 (elt (send self :centroid nil) 1) weight-force)
+ (* -1e-3 (elt (send self :centroid nil) 0) weight-force) 0))))
+ (dotimes (i (length target-coords))
+ (push (subseq ret-f (* i 6) (+ (* i 6) 3)) force-list)
+ (push (subseq ret-f (+ (* i 6) 3) (+ (* i 6) 6)) moment-list)
+ ))
+ (setq force-list (reverse force-list))
+ (setq moment-list (reverse moment-list))
+ ))
(unless (= (length force-list) (length moment-list)
(length target-coords))
(warn ";; ERROR : list length differ : force-list ~A moment-list ~A target-coords ~A~%"
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-12-15 09:21:02
|
Revision: 1018
http://sourceforge.net/p/jskeus/code/1018
Author: snozawa
Date: 2013-12-15 09:20:59 +0000 (Sun, 15 Dec 2013)
Log Message:
-----------
use dynamics in calculating zmp
Modified Paths:
--------------
trunk/irteus/irtdyna.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2013-12-15 08:34:12 UTC (rev 1017)
+++ trunk/irteus/irtdyna.l 2013-12-15 09:20:59 UTC (rev 1018)
@@ -808,6 +808,7 @@
(send self :calc-torque
:dt dt :av av :root-coords root-coords
:debug-view debug-view
+ :calc-statics-p nil
:calc-torque-buffer-args calc-torque-buffer-args)
;; calculate ZMP from force and moment of root-link
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-12-15 08:34:15
|
Revision: 1017
http://sourceforge.net/p/jskeus/code/1017
Author: snozawa
Date: 2013-12-15 08:34:12 +0000 (Sun, 15 Dec 2013)
Log Message:
-----------
enable to select statics or dynamics ;; currently default = statics according to previous specification
Modified Paths:
--------------
trunk/irteus/demo/crank-motion.l
trunk/irteus/irtdyna.l
trunk/irteus/irtrobot.l
Modified: trunk/irteus/demo/crank-motion.l
===================================================================
--- trunk/irteus/demo/crank-motion.l 2013-12-15 08:18:03 UTC (rev 1016)
+++ trunk/irteus/demo/crank-motion.l 2013-12-15 08:34:12 UTC (rev 1017)
@@ -119,8 +119,7 @@
(setf (elt ac 2) 0) ac))
(send-all move-target :worldpos))
(append (list cog-target-pos) target-coords))
- (send *robot* :draw-torque *viewer*
- :torque-vector (send *robot* :torque-vector :dt 0.1))
+ (send *robot* :draw-torque *viewer*)
(send *irtviewer* :flush)
))
)))
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2013-12-15 08:18:03 UTC (rev 1016)
+++ trunk/irteus/irtdyna.l 2013-12-15 08:34:12 UTC (rev 1017)
@@ -676,21 +676,28 @@
)
(:calc-torque
(&key (debug-view nil)
- (dt 0.005)
+ ;; statics or dynamics
+ ;; if t -> Calc statics. Do not update vel and acc for av and root-coords.
+ ;; otherwise -> Calc dynamics. Update vel and acc from current av and root-coords.
+ (calc-statics-p t)
+ (dt 0.005) ;; [sec]
(av (send self :angle-vector))
(root-coords (send (car (send self :links)) :copy-worldcoords))
(calc-torque-buffer-args (send self :calc-torque-buffer-args)))
- (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-from-vel-acc
- :jvv (cadr (memq :joint-velocity-vector ret-av))
- :jav (cadr (memq :joint-acceleration-vector ret-av))
- :root-spacial-velocity (cadr (memq :root-spacial-velocity ret-rc))
- :root-angular-velocity (cadr (memq :root-angular-velocity ret-rc))
- :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)))
+ (send* self :calc-torque-from-vel-acc
+ :debug-view debug-view
+ :calc-torque-buffer-args calc-torque-buffer-args
+ (unless calc-statics-p
+ (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)))
+ (list
+ :jvv (cadr (memq :joint-velocity-vector ret-av))
+ :jav (cadr (memq :joint-acceleration-vector ret-av))
+ :root-spacial-velocity (cadr (memq :root-spacial-velocity ret-rc))
+ :root-angular-velocity (cadr (memq :root-angular-velocity ret-rc))
+ :root-spacial-acceleration (cadr (memq :root-spacial-acceleration ret-rc))
+ :root-angular-acceleration (cadr (memq :root-angular-acceleration ret-rc))
+ )))))
(:calc-torque-from-vel-acc
(&key (debug-view nil)
(jvv (instantiate float-vector (length joint-list))) ;; [rad/s] or [m/s]
Modified: trunk/irteus/irtrobot.l
===================================================================
--- trunk/irteus/irtrobot.l 2013-12-15 08:18:03 UTC (rev 1016)
+++ trunk/irteus/irtrobot.l 2013-12-15 08:34:12 UTC (rev 1017)
@@ -321,6 +321,7 @@
(:torque-vector
(&key (force-list) (moment-list) (target-coords)
(debug-view nil)
+ (calc-statics-p t)
(dt 0.005)
(av (send self :angle-vector))
(root-coords (send (car (send self :links)) :copy-worldcoords))
@@ -359,6 +360,7 @@
force-list moment-list target-coords)
(send self :calc-torque
:debug-view debug-view
+ :calc-statics-p calc-statics-p
:av av :root-coords root-coords :dt dt
: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-15 08:18:09
|
Revision: 1016
http://sourceforge.net/p/jskeus/code/1016
Author: snozawa
Date: 2013-12-15 08:18:03 +0000 (Sun, 15 Dec 2013)
Log Message:
-----------
remove print in :calc-torque
Modified Paths:
--------------
trunk/irteus/irtdyna.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2013-12-15 08:16:36 UTC (rev 1015)
+++ trunk/irteus/irtdyna.l 2013-12-15 08:18:03 UTC (rev 1016)
@@ -682,7 +682,6 @@
(calc-torque-buffer-args (send self :calc-torque-buffer-args)))
(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)))
- (print (append ret-rc ret-av))
(send self :calc-torque-from-vel-acc
:jvv (cadr (memq :joint-velocity-vector ret-av))
:jav (cadr (memq :joint-acceleration-vector ret-av))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-12-15 08:16:41
|
Revision: 1015
http://sourceforge.net/p/jskeus/code/1015
Author: snozawa
Date: 2013-12-15 08:16:36 +0000 (Sun, 15 Dec 2013)
Log Message:
-----------
set vel and acc from av and root-coords in calc-torque fix calc-torque in crank-motion sample
Modified Paths:
--------------
trunk/irteus/demo/crank-motion.l
trunk/irteus/irtdyna.l
trunk/irteus/irtrobot.l
Modified: trunk/irteus/demo/crank-motion.l
===================================================================
--- trunk/irteus/demo/crank-motion.l 2013-12-15 07:54:54 UTC (rev 1014)
+++ trunk/irteus/demo/crank-motion.l 2013-12-15 08:16:36 UTC (rev 1015)
@@ -119,7 +119,8 @@
(setf (elt ac 2) 0) ac))
(send-all move-target :worldpos))
(append (list cog-target-pos) target-coords))
- (send *robot* :draw-torque *viewer*)
+ (send *robot* :draw-torque *viewer*
+ :torque-vector (send *robot* :torque-vector :dt 0.1))
(send *irtviewer* :flush)
))
)))
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2013-12-15 07:54:54 UTC (rev 1014)
+++ trunk/irteus/irtdyna.l 2013-12-15 08:16:36 UTC (rev 1015)
@@ -676,6 +676,24 @@
)
(:calc-torque
(&key (debug-view nil)
+ (dt 0.005)
+ (av (send self :angle-vector))
+ (root-coords (send (car (send self :links)) :copy-worldcoords))
+ (calc-torque-buffer-args (send self :calc-torque-buffer-args)))
+ (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)))
+ (print (append ret-rc ret-av))
+ (send self :calc-torque-from-vel-acc
+ :jvv (cadr (memq :joint-velocity-vector ret-av))
+ :jav (cadr (memq :joint-acceleration-vector ret-av))
+ :root-spacial-velocity (cadr (memq :root-spacial-velocity ret-rc))
+ :root-angular-velocity (cadr (memq :root-angular-velocity ret-rc))
+ :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)))
+ (:calc-torque-from-vel-acc
+ (&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))
@@ -781,19 +799,10 @@
(send l :ext-force (float-vector 0 0 0))
(send l :ext-moment (float-vector 0 0 0))))
- ;; set spacial-acceleration and angular-acceleration of root-link
- ;; set joint velocities and joint accelerations
- (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-av))
- :jav (cadr (memq :joint-acceleration-vector ret-av))
- :root-spacial-velocity (cadr (memq :root-spacial-velocity ret-rc))
- :root-angular-velocity (cadr (memq :root-angular-velocity ret-rc))
- :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))
+ (send self :calc-torque
+ :dt dt :av av :root-coords root-coords
+ :debug-view debug-view
+ :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]
Modified: trunk/irteus/irtrobot.l
===================================================================
--- trunk/irteus/irtrobot.l 2013-12-15 07:54:54 UTC (rev 1014)
+++ trunk/irteus/irtrobot.l 2013-12-15 08:16:36 UTC (rev 1015)
@@ -321,8 +321,9 @@
(:torque-vector
(&key (force-list) (moment-list) (target-coords)
(debug-view nil)
- (jvv (instantiate float-vector (calc-target-joint-dimension joint-list))) ;; [rad/s] or [m/s]
- (jav (instantiate float-vector (calc-target-joint-dimension joint-list))) ;; [rad/s^2] or [m/s^2]
+ (dt 0.005)
+ (av (send self :angle-vector))
+ (root-coords (send (car (send self :links)) :copy-worldcoords))
(calc-torque-buffer-args (send self :calc-torque-buffer-args)))
(unless (every #'null (send self :legs)) ;; for legged robot, set default external force and moment at the end-effectors
(let ((weight-force (* (send self :weight) 1e-6 (elt *g-vec* 2)))) ;; weight[g] * 1e-6 * gvec[mm/s^2] = force[N]
@@ -348,7 +349,7 @@
(length target-coords))
(warn ";; ERROR : list length differ : force-list ~A moment-list ~A target-coords ~A~%"
(length force-list) (length moment-list) (length target-coords))
- (return-from :torque-vector jvv))
+ (return-from :torque-vector (instantiate float-vector (length joint-list))))
(mapcar #'(lambda (fv mv tc)
(send (send tc :parent) :ext-force fv)
;; calc moment-offset caused by fv ;
@@ -358,7 +359,8 @@
force-list moment-list target-coords)
(send self :calc-torque
:debug-view debug-view
- :jvv jvv :jav jav :calc-torque-buffer-args calc-torque-buffer-args)
+ :av av :root-coords root-coords :dt dt
+ :calc-torque-buffer-args calc-torque-buffer-args)
)
(:calc-force-from-joint-torque
(limb all-torque &key (move-target (send self limb :end-coords)) (use-torso))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2013-12-15 07:54:57
|
Revision: 1014
http://sourceforge.net/p/jskeus/code/1014
Author: snozawa
Date: 2013-12-15 07:54:54 +0000 (Sun, 15 Dec 2013)
Log Message:
-----------
add root-spacial-velocity and root-angular-velocity arguments
Modified Paths:
--------------
trunk/irteus/irtdyna.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2013-12-15 07:49:30 UTC (rev 1013)
+++ trunk/irteus/irtdyna.l 2013-12-15 07:54:54 UTC (rev 1014)
@@ -788,6 +788,8 @@
(send self :calc-torque
:jvv (cadr (memq :joint-velocity-vector ret-av))
:jav (cadr (memq :joint-acceleration-vector ret-av))
+ :root-spacial-velocity (cadr (memq :root-spacial-velocity ret-rc))
+ :root-angular-velocity (cadr (memq :root-angular-velocity ret-rc))
:root-spacial-acceleration (cadr (memq :root-spacial-acceleration ret-rc))
:root-angular-acceleration (cadr (memq :root-angular-acceleration ret-rc))
:debug-view debug-view
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
