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: <je...@js...> - 2014-02-07 02:15:46
|
See <http://jenkins.jsk.imi.i.u-tokyo.ac.jp:8080/job/jskeus/199/> ------------------------------------------ Started by user k-okada Building remotely on Jenkins EC2 server (i-f20867dc) in workspace <http://jenkins.jsk.imi.i.u-tokyo.ac.jp:8080/job/jskeus/ws/> java.io.IOException: Failed to mkdirs: <http://jenkins.jsk.imi.i.u-tokyo.ac.jp:8080/job/jskeus/ws/> at hudson.FilePath.mkdirs(FilePath.java:1069) at hudson.model.AbstractProject.checkout(AbstractProject.java:1404) at hudson.model.AbstractBuild$AbstractBuildExecution.defaultCheckout(AbstractBuild.java:651) at jenkins.scm.SCMCheckoutStrategy.checkout(SCMCheckoutStrategy.java:88) at hudson.model.AbstractBuild$AbstractBuildExecution.run(AbstractBuild.java:560) at hudson.model.Run.execute(Run.java:1670) at hudson.model.FreeStyleBuild.run(FreeStyleBuild.java:46) at hudson.model.ResourceController.execute(ResourceController.java:88) at hudson.model.Executor.run(Executor.java:231) |
|
From: <je...@js...> - 2014-02-07 02:15:44
|
See <http://jenkins.jsk.imi.i.u-tokyo.ac.jp:8080/job/jskeus/198/> ------------------------------------------ Started by user k-okada Building remotely on Jenkins EC2 server (i-f20867dc) in workspace <http://jenkins.jsk.imi.i.u-tokyo.ac.jp:8080/job/jskeus/ws/> java.io.IOException: Failed to mkdirs: <http://jenkins.jsk.imi.i.u-tokyo.ac.jp:8080/job/jskeus/ws/> at hudson.FilePath.mkdirs(FilePath.java:1069) at hudson.model.AbstractProject.checkout(AbstractProject.java:1404) at hudson.model.AbstractBuild$AbstractBuildExecution.defaultCheckout(AbstractBuild.java:651) at jenkins.scm.SCMCheckoutStrategy.checkout(SCMCheckoutStrategy.java:88) at hudson.model.AbstractBuild$AbstractBuildExecution.run(AbstractBuild.java:560) at hudson.model.Run.execute(Run.java:1670) at hudson.model.FreeStyleBuild.run(FreeStyleBuild.java:46) at hudson.model.ResourceController.execute(ResourceController.java:88) at hudson.model.Executor.run(Executor.java:231) |
|
From: <sn...@us...> - 2014-02-04 18:21:14
|
Revision: 1059
http://sourceforge.net/p/jskeus/code/1059
Author: snozawa
Date: 2014-02-04 18:21:11 +0000 (Tue, 04 Feb 2014)
Log Message:
-----------
just fix indent
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2014-02-04 18:20:43 UTC (rev 1058)
+++ trunk/irteus/irtmodel.l 2014-02-04 18:21:11 UTC (rev 1059)
@@ -1843,33 +1843,33 @@
(if success
:ik-succeed
(progn
- (when (and debug-view (not (memq :no-clear debug-view)) *viewer*)
- (send *viewer* :viewsurface :clear))
- (send self :put :collision-pair-list nil)
- (send-message* self cascaded-link
- :move-joints-avoidance tmp-dim
- :union-link-list union-link-list
- :rotation-axis rotation-axis
- :translation-axis translation-axis
- :jacobi jacobi :debug-view debug-view
- ;; buffer for calculation
- method-args)
- (when (and debug-view *viewer*)
- (send *viewer* :draw-objects :clear nil :flush nil)
- ;;
- ;; done
- (send self :draw-collision-debug-view)
- (when (car target-coords)
- (dotimes (i (length target-coords))
- (send-message (elt move-target i) coordinates :draw-on :flush nil :size 100)
- (send (elt target-coords i) :draw-on :flush nil :color #f(1 0 0))))
- (dolist (p (send self :get :ik-draw-on-params))
- (send* (car p) :draw-on :flush nil (cdr p)))
- (send self :put :ik-draw-on-params nil)
- (if (not (memq :no-flush debug-view)) (send *viewer* :viewsurface :flush))
+ (when (and debug-view (not (memq :no-clear debug-view)) *viewer*)
+ (send *viewer* :viewsurface :clear))
+ (send self :put :collision-pair-list nil)
+ (send-message* self cascaded-link
+ :move-joints-avoidance tmp-dim
+ :union-link-list union-link-list
+ :rotation-axis rotation-axis
+ :translation-axis translation-axis
+ :jacobi jacobi :debug-view debug-view
+ ;; buffer for calculation
+ method-args)
+ (when (and debug-view *viewer*)
+ (send *viewer* :draw-objects :clear nil :flush nil)
+ ;;
+ ;; done
+ (send self :draw-collision-debug-view)
+ (when (car target-coords)
+ (dotimes (i (length target-coords))
+ (send-message (elt move-target i) coordinates :draw-on :flush nil :size 100)
+ (send (elt target-coords i) :draw-on :flush nil :color #f(1 0 0))))
+ (dolist (p (send self :get :ik-draw-on-params))
+ (send* (car p) :draw-on :flush nil (cdr p)))
+ (send self :put :ik-draw-on-params nil)
+ (if (not (memq :no-flush debug-view)) (send *viewer* :viewsurface :flush))
+ )
+ :ik-continues)
)
- :ik-continues)
- )
))
(:inverse-kinematics
(target-coords &rest args
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2014-02-04 18:20:46
|
Revision: 1058
http://sourceforge.net/p/jskeus/code/1058
Author: snozawa
Date: 2014-02-04 18:20:43 +0000 (Tue, 04 Feb 2014)
Log Message:
-----------
use 'if' instead of 'return-from'
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2014-02-04 17:15:36 UTC (rev 1057)
+++ trunk/irteus/irtmodel.l 2014-02-04 18:20:43 UTC (rev 1058)
@@ -1841,7 +1841,8 @@
;; check loop end
(if success
- (return-from :inverse-kinematics-loop :ik-succeed))
+ :ik-succeed
+ (progn
(when (and debug-view (not (memq :no-clear debug-view)) *viewer*)
(send *viewer* :viewsurface :clear))
(send self :put :collision-pair-list nil)
@@ -1867,7 +1868,9 @@
(send self :put :ik-draw-on-params nil)
(if (not (memq :no-flush debug-view)) (send *viewer* :viewsurface :flush))
)
- :ik-continues))
+ :ik-continues)
+ )
+ ))
(:inverse-kinematics
(target-coords &rest args
&key (stop 50)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2014-02-04 17:15:39
|
Revision: 1057
http://sourceforge.net/p/jskeus/code/1057
Author: snozawa
Date: 2014-02-04 17:15:36 +0000 (Tue, 04 Feb 2014)
Log Message:
-----------
remove I-J#J which is not used in :collision-avoidance
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2014-02-03 21:23:53 UTC (rev 1056)
+++ trunk/irteus/irtmodel.l 2014-02-04 17:15:36 UTC (rev 1057)
@@ -1176,7 +1176,7 @@
avoid-collision-joint-gain
avoid-collision-null-gain
((:collision-avoidance-link-pair pair-list))
- (union-link-list) (link-list) (I-J#J) (weight)
+ (union-link-list) (link-list) (weight)
;; link-list -> both list and atom OK.
(fik-len (send self :calc-target-joint-dimension union-link-list))
debug-view
@@ -1235,9 +1235,6 @@
)))
))
)))
- (when (null I-J#J)
- (warn ";; ERROR : I-J#J is required in :collision-avoidance~%")
- (return-from :collision-avoidance dav-col))
(dotimes (i fik-len) (setf (elt dav-col i) (* (elt dav-col i) (elt weight i))))
(send self :put :collision-avoidance-null-vector
@@ -1572,7 +1569,6 @@
(> avoid-collision-null-gain 0.0)))
(setq angle-speed-collision-avoidance
(send* self :collision-avoidance
- :I-J#J I-J#J
:avoid-collision-distance avoid-collision-distance
:avoid-collision-null-gain avoid-collision-null-gain
:avoid-collision-joint-gain avoid-collision-joint-gain
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2014-02-03 21:23:58
|
Revision: 1056
http://sourceforge.net/p/jskeus/code/1056
Author: snozawa
Date: 2014-02-03 21:23:53 +0000 (Mon, 03 Feb 2014)
Log Message:
-----------
remove x::wind-w-main-one ;; enable to use x::wind-w-main-one by user
Modified Paths:
--------------
trunk/irteus/irtrobot.l
Modified: trunk/irteus/irtrobot.l
===================================================================
--- trunk/irteus/irtrobot.l 2014-02-01 04:21:20 UTC (rev 1055)
+++ trunk/irteus/irtrobot.l 2014-02-03 21:23:53 UTC (rev 1056)
@@ -648,7 +648,6 @@
#f(1 1 1)
#'(lambda () (send *viewer* :viewsurface :string 20 110 "white = calc zmp"))))
(send *viewer* :viewsurface :flush)
- (x::window-main-one)
)
;; generate footstep parameter
;; currently only default, forward and outside
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2014-02-01 04:21:23
|
Revision: 1055
http://sourceforge.net/p/jskeus/code/1055
Author: snozawa
Date: 2014-02-01 04:21:20 +0000 (Sat, 01 Feb 2014)
Log Message:
-----------
add :calc-vel-for-cog
Modified Paths:
--------------
trunk/irteus/irtdyna.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2014-02-01 04:15:22 UTC (rev 1054)
+++ trunk/irteus/irtdyna.l 2014-02-01 04:21:20 UTC (rev 1055)
@@ -448,20 +448,27 @@
:link-list link-list
:translation-axis translation-axis args)
args)
- (scale (* 0.001 cog-gain) ;; [mm] -> [m]
- (calc-dif-with-axis
- (let ((current-centroid-pos
- (if (functionp centroid-offset-func)
- (funcall centroid-offset-func)
- (send self :centroid nil))))
- (send self :put :ik-draw-on-params
- (append (send self :get :ik-draw-on-params)
- (list (list target-centroid-pos :color #f(0 0 1))
- (list (float-vector (elt current-centroid-pos 0) (elt current-centroid-pos 1) (elt target-centroid-pos 2)) :size 100 :color #f(0 1 0)))))
- (v- target-centroid-pos current-centroid-pos))
- translation-axis))
+ (send self :calc-vel-for-cog cog-gain translation-axis target-centroid-pos centroid-offset-func)
)
)
+ (:calc-vel-for-cog
+ (cog-gain
+ translation-axis
+ target-centroid-pos
+ centroid-offset-func)
+ (scale (* 0.001 cog-gain) ;; [mm] -> [m]
+ (calc-dif-with-axis
+ (let ((current-centroid-pos
+ (if (functionp centroid-offset-func)
+ (funcall centroid-offset-func)
+ (send self :centroid nil))))
+ (send self :put :ik-draw-on-params
+ (append (send self :get :ik-draw-on-params)
+ (list (list target-centroid-pos :color #f(0 0 1))
+ (list (float-vector (elt current-centroid-pos 0) (elt current-centroid-pos 1) (elt target-centroid-pos 2)) :size 100 :color #f(0 1 0)))))
+ (v- target-centroid-pos current-centroid-pos))
+ translation-axis)
+ ))
(:difference-cog-position
(target-centroid-pos &optional (centroid-offset-func) (translation-axis :z))
(calc-dif-with-axis
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2014-02-01 04:15:24
|
Revision: 1054
http://sourceforge.net/p/jskeus/code/1054
Author: snozawa
Date: 2014-02-01 04:15:22 +0000 (Sat, 01 Feb 2014)
Log Message:
-----------
check collicion for all link which distance <= avoid-collision-distance ;; at previous revision, a link which has min-distance was checked. But, in the case that several links ware near the min-distance, min-distance links ware frequently switched and that cause joint velocity discontinuousness
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2014-01-31 12:17:52 UTC (rev 1053)
+++ trunk/irteus/irtmodel.l 2014-02-01 04:15:22 UTC (rev 1054)
@@ -1187,18 +1187,16 @@
(dav-col (instantiate float-vector fik-len)))
(send self :put :collision-pair-list pair-list)
(if (atom (car link-list)) (setq link-list (list link-list)))
- (while (and (< n pair-len)
- (eps= min-distance (car (elt (send self :get :collision-distance) n)) 1.0))
+ (while (< n pair-len)
(let* ((np (elt (send self :get :collision-distance) n))
(pair (elt pair-list (car (last np)))))
(incf n)
(when (and debug-view (not (memq :no-message debug-view)))
(let ((v (normalize-vector (v- (elt np 1) (elt np 2)))))
- (warn "min-distance : ~7,3f(<~7,3f) #f(" min-distance avoid-collision-distance)
+ (warn "col-distance : ~7,3f(<~7,3f) #f(" (elt np 0) avoid-collision-distance)
(dotimes (i 3) (warn "~7,3f " (elt v i)))
(warn ") ~A~%" (send-all pair :name))))
- (if (>= min-distance avoid-collision-distance)
- (return-from :collision-avoidance dav-col))
+ (unless (>= (elt np 0) avoid-collision-distance)
;; extract a-link-list from link-list
;; link-list = (list link-list0 link-list1 .. )
@@ -1214,7 +1212,8 @@
(let* ((col-jacobi (send* self :calc-jacobian-from-link-list
col-link-list :move-target drag-coords
col-args))
- (ddav-col (transform (transpose col-jacobi) (elt np 3))))
+ (ddav-col (transform (transpose col-jacobi) (elt np 3)))
+ (distance-gain (- (/ avoid-collision-distance (elt np 0)) 1)))
(send (car pair) :dissoc drag-coords)
(dolist (al col-link-list)
(labels ((calc-target-joint-dimension-from-subseq
@@ -1231,26 +1230,24 @@
(dotimes (i (calc-target-joint-dimension (list (send al :joint))))
(setf (elt dav-col (+ i dav-col-idx))
(+ (elt dav-col (+ i dav-col-idx))
- (elt ddav-col (+ ddav-col-idx i))))
+ (* distance-gain (elt ddav-col (+ ddav-col-idx i)))))
)
)))
))
- ))
+ )))
(when (null I-J#J)
(warn ";; ERROR : I-J#J is required in :collision-avoidance~%")
(return-from :collision-avoidance dav-col))
- (let ((distance-gain
- (- (/ avoid-collision-distance min-distance) 1)))
(dotimes (i fik-len) (setf (elt dav-col i) (* (elt dav-col i) (elt weight i))))
(send self :put :collision-avoidance-null-vector
- (scale (* distance-gain avoid-collision-null-gain) dav-col))
+ (scale avoid-collision-null-gain dav-col))
(send self :put :collision-avoidance-joint-vector
- (scale (* distance-gain avoid-collision-joint-gain) dav-col))
+ (scale avoid-collision-joint-gain dav-col))
(when (and debug-view (not (memq :no-message debug-view)))
(format-array (map float-vector #'rad2deg (send self :get :collision-avoidance-joint-vector)) "coljnt:")
(format-array (map float-vector #'rad2deg (send self :get :collision-avoidance-null-vector)) "colnul:")))
- (send self :get :collision-avoidance-joint-vector)))
+ (send self :get :collision-avoidance-joint-vector))
(:move-joints
(union-vel &rest args
&key
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2014-01-31 12:17:54
|
Revision: 1053
http://sourceforge.net/p/jskeus/code/1053
Author: snozawa
Date: 2014-01-31 12:17:52 +0000 (Fri, 31 Jan 2014)
Log Message:
-----------
add darwin example and fix comment
Modified Paths:
--------------
trunk/irteus/demo/walk-motion.l
Modified: trunk/irteus/demo/walk-motion.l
===================================================================
--- trunk/irteus/demo/walk-motion.l 2014-01-31 12:17:16 UTC (rev 1052)
+++ trunk/irteus/demo/walk-motion.l 2014-01-31 12:17:52 UTC (rev 1053)
@@ -35,7 +35,7 @@
(if is-small-robot (send robot :gen-footstep-parameter :ratio 0.3))
(send robot :calc-walk-pattern-from-footstep-list
(send robot :go-pos-params->footstep-list
- 500 150 45) ;; x[mm] y[mm] th[rad]
+ 500 150 45) ;; x[mm] y[mm] th[deg]
:debug-view :no-message
:default-step-height default-step-height)
))
@@ -154,10 +154,12 @@
(defun walk-motion-for-robots ()
(unless (boundp '*robots*)
(setq *robots*
- (mapcar #'(lambda (form)
- (load (format nil "models/~A-robot.l" form))
- (funcall form))
- '(h7 h6 kaz3))))
+ (append
+ (mapcar #'(lambda (form)
+ (load (format nil "models/~A-robot.l" form))
+ (funcall form))
+ '(h7 h6 kaz3))
+ (list (progn (load (format nil "models/darwin.l")) (darwin))))))
(mapcar #'(lambda
(rb)
(format t ";; ~A walking~%" (send rb :name))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2014-01-31 12:17:20
|
Revision: 1052
http://sourceforge.net/p/jskeus/code/1052
Author: snozawa
Date: 2014-01-31 12:17:16 +0000 (Fri, 31 Jan 2014)
Log Message:
-----------
remove move centorid on foot ;; add limitation of rotation
Modified Paths:
--------------
trunk/irteus/irtrobot.l
Modified: trunk/irteus/irtrobot.l
===================================================================
--- trunk/irteus/irtrobot.l 2014-01-31 11:10:25 UTC (rev 1051)
+++ trunk/irteus/irtrobot.l 2014-01-31 12:17:16 UTC (rev 1052)
@@ -661,7 +661,7 @@
(send self :fix-leg-to-coords (make-coords) '(:rleg :lleg))
(let ((dol (abs (elt (apply #'v- (send self :legs :end-coords :worldpos)) 1))))
(labels ((ik-test
- (target-coords-func diff-func)
+ (target-coords-func diff-func &optional (limit-func))
(send self :reset-pose)
(send self :fix-leg-to-coords (make-coords) '(:rleg :lleg))
(let* ((tc (send self :rleg :end-coords :copy-worldcoords))
@@ -671,14 +671,17 @@
(funcall target-coords-func tc)
:link-list (send self :link-list (send self :rleg :end-coords :parent))
:move-target (send self :rleg :end-coords) :warnp nil)
- (send self :move-centroid-on-foot :lleg '(:rleg :lleg) :warnp nil)))
+ (if limit-func (funcall limit-func tc init-tc) t)
+ ))
(funcall diff-func tc init-tc))))
(let ((fol (ik-test #'(lambda (tc) (send tc :translate #f(10 0 0)))
#'(lambda (tc init-tc) (abs (elt (v- (send tc :worldpos) (send init-tc :worldpos)) 0)))))
(ool (ik-test #'(lambda (tc) (send tc :translate #f(0 -10 0)))
#'(lambda (tc init-tc) (abs (elt (v- (send tc :worldpos) (send init-tc :worldpos)) 1)))))
(frr (ik-test #'(lambda (tc) (send tc :rotate (deg2rad -2.5) :z))
- #'(lambda (tc init-tc) (abs (rad2deg (- (caar (send tc :rpy-angle)) (caar (send init-tc :rpy-angle)))))))))
+ #'(lambda (tc init-tc) (abs (rad2deg (- (caar (send tc :rpy-angle)) (caar (send init-tc :rpy-angle))))))
+ #'(lambda (tc init-tc) (> 90 (abs (rad2deg (- (caar (send tc :rpy-angle)) (caar (send init-tc :rpy-angle)))))))
+ )))
(send self :newcoords pc)
(send self :angle-vector pav)
(warn ";; generating footstep-parameter... done.~%")
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2014-01-31 11:10:28
|
Revision: 1051
http://sourceforge.net/p/jskeus/code/1051
Author: snozawa
Date: 2014-01-31 11:10:25 +0000 (Fri, 31 Jan 2014)
Log Message:
-----------
add move-centroid-on-foot in calculating foot reachability
Modified Paths:
--------------
trunk/irteus/irtrobot.l
Modified: trunk/irteus/irtrobot.l
===================================================================
--- trunk/irteus/irtrobot.l 2014-01-29 09:58:06 UTC (rev 1050)
+++ trunk/irteus/irtrobot.l 2014-01-31 11:10:25 UTC (rev 1051)
@@ -666,10 +666,12 @@
(send self :fix-leg-to-coords (make-coords) '(:rleg :lleg))
(let* ((tc (send self :rleg :end-coords :copy-worldcoords))
(init-tc (send tc :copy-worldcoords)))
- (while (send self :inverse-kinematics
- (funcall target-coords-func tc)
- :link-list (send self :link-list (send self :rleg :end-coords :parent))
- :move-target (send self :rleg :end-coords) :warnp nil))
+ (while (and
+ (send self :inverse-kinematics
+ (funcall target-coords-func tc)
+ :link-list (send self :link-list (send self :rleg :end-coords :parent))
+ :move-target (send self :rleg :end-coords) :warnp nil)
+ (send self :move-centroid-on-foot :lleg '(:rleg :lleg) :warnp nil)))
(funcall diff-func tc init-tc))))
(let ((fol (ik-test #'(lambda (tc) (send tc :translate #f(10 0 0)))
#'(lambda (tc init-tc) (abs (elt (v- (send tc :worldpos) (send init-tc :worldpos)) 0)))))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2014-01-29 09:58:09
|
Revision: 1050
http://sourceforge.net/p/jskeus/code/1050
Author: snozawa
Date: 2014-01-29 09:58:06 +0000 (Wed, 29 Jan 2014)
Log Message:
-----------
add endlink I-til setting
Modified Paths:
--------------
trunk/irteus/irtdyna.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2014-01-22 07:13:23 UTC (rev 1049)
+++ trunk/irteus/irtdyna.l 2014-01-29 09:58:06 UTC (rev 1050)
@@ -307,6 +307,9 @@
(let ((cen (send self :centroid)))
(dotimes (i 3) (setf (elt (send self :get :c-til) i) (elt cen i)))
(send self :put :m-til (send self :weight))
+ (m* (m* (send self :worldrot) (send self :inertia-tensor) tmp-ma)
+ (transpose (send self :worldrot) tmp-mb)
+ (send self :get :I-til))
))
(if debug-view
(warn ";; joint = ~A ;; m-til = ~A[g], c-til = ~A[mm]~%"
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <je...@js...> - 2014-01-23 05:38:38
|
See <http://jenkins.jsk.imi.i.u-tokyo.ac.jp:8080/job/jskeus/182/> |
|
From: <je...@js...> - 2014-01-23 05:31:34
|
See <http://jenkins.jsk.imi.i.u-tokyo.ac.jp:8080/job/jskeus/181/changes> Changes: [k-okada] generate toc for jmanual ------------------------------------------ [...truncated 863 lines...] A eus/models/room73b2-panelwall-0-object.l A eus/models/room73b2-door-right-object.l A eus/models/room602-bariera-1200-right6-object.l A eus/models/kettle-object.l A eus/models/elevator_inside_panel-object.l AU eus/models/elevator_inside_panel_eng2.png A eus/models/room602-door-left-object.l A eus/models/room73b2-kitchen-object.l A eus/models/room83b1-askul-1400-desk-3-object.l A eus/models/zero-robot.l A eus/models/room73a3-bariera-1400-middle-7-object.l A eus/models/bariera-1200-right-object.l A eus/models/bariera-1200-middle2-object.l A eus/models/room73b2-foldable-desk-object.l A eus/models/gifuplastic-900-cart-object.l A eus/models/room602-askul-1200x700-desk-7-object.l A eus/models/room73b2-locker1-object.l A eus/models/toshiba-clacio-refrigerator-object.l A eus/models/room73b2-askul-1200x700-desk-4-object.l A eus/models/room73a3-village-chair-0-object.l A eus/models/room73b2-unknown-1200-desk-4-object.l A eus/models/room602-chair5-object.l A eus/models/sushi-cup-object.l A eus/models/room83b1-83b1-ground-object.l A eus/models/room602-wall4-object.l A eus/models/coe-1200-shelf-object.l A eus/models/room73b2-uchida-shelf-1300-object.l A eus/models/kaz2-robot.l A eus/models/uchida-monitor-leg-object.l A eus/models/room73a3-bariera-1400-middle-0-object.l A eus/models/room602-tableware-shelf2-object.l A eus/models/room73a3-askul-1400-desk-0-object.l A eus/models/arrow-object.l A eus/models/square-600-closet-object.l A eus/models/room73b2-askul-1400-desk-object.l A eus/models/room602-askul-1200x700-desk-0-object.l A eus/models/room83b1-scene.l A eus/models/akira-robot.l A eus/models/bariera-900-middle-object.l A eus/models/room73b2-sharp-52-aquostv-object.l A eus/models/single-arrow-object.l A eus/models/room602-trashbox0-object.l A eus/models/askul-gdh-cupboard-object.l A eus/models/room73b2-coe-450-shelf-object.l A eus/models/tot-robot.l A eus/models/room602-3t-600-clean-locker-object.l A eus/models/askul-corner-desk-object.l A eus/models/trashbox-object.l A eus/models/room83b1-askul-1600-desk-1-object.l A eus/models/room602-bariera-1200-corner1-object.l A eus/models/uchida-1000-desk-object.l A eus/models/coffee-cup-cup-object.l AU eus/models/elevator_call_panel_eng8.png A eus/models/room73a3-bariera-1400-middle-9-object.l A eus/models/room73b2-bottle-object.l A eus/models/room73a3-clock-object.l A eus/models/coe-demospace-object.l A eus/models/kokuyo-450-locker-object.l A eus/models/uchida-1500-desk-object.l A eus/models/room602-bariera-1200-middle3-object.l A eus/models/uchida-1800-desk-object.l A eus/models/room602-aquos1-object.l A eus/models/bariera-1200-corner-object.l A eus/models/room73b2-bariera-900-left-object.l A eus/models/itoki-900-closet-object.l A eus/models/askul-ntr-cupboard-object.l A eus/models/room73b2-askul-1200x700-desk-6-object.l A eus/models/broom-object.l A eus/models/room73a3-village-chair-2-object.l A eus/models/sakae-900-desk-object.l A eus/models/maruman-55-analog-clock-object.l A eus/models/room83b1-door-right-object.l A eus/models/room602-wall6-object.l A eus/models/room73b2-bariera-1200-middle-1-object.l A eus/models/akira3-robot.l A eus/models/room73a3-bariera-1400-middle-2-object.l A eus/models/book-object.l A eus/models/room602-tableware-shelf4-object.l A eus/models/room73a3-askul-1400-desk-2-object.l A eus/models/knife-object.l A eus/models/room73b2-cup-object.l A eus/models/sharp-52-aquostv-object.l A eus/models/white-bread-salad-dish-object.l A eus/models/room610-610-ground-object.l A eus/models/room602-askul-1200x700-desk-2-object.l A eus/models/room73b2-door-left-object.l A eus/models/uchida-2400-desk-object.l A eus/models/room602-door-object.l A eus/models/ape3-robot.l A eus/models/room73a3-officeprinter-object.l A eus/models/room73a3-shelf-1-object.l AU eus/models/wheelchair-seat2.jpg A eus/models/coe-stairs-object.l A eus/models/ball-object.l A eus/models/room73b2-sushi-cup2-object.l A eus/models/hitachi-fiesta-refrigerator-object.l A eus/models/h3s-robot.l A eus/models/plus-1800-pdesk-object.l A eus/models/room602-trashbox2-object.l A eus/models/room83b1-uchida-1800-desk-object.l Fetching 'https://svn.code.sf.net/p/euslisp/code/trunk/EusLisp/models' at 659 into '<http://jenkins.jsk.imi.i.u-tokyo.ac.jp:8080/job/jskeus/ws/jskeus/eus/models'> At revision 659 A eus/doc/latex/opengl.tex A eus/doc/latex/mars-pre.tex A eus/doc/latex/symbols.tex A eus/doc/latex/sysfunc.tex A eus/doc/latex/mytabbing.sty A eus/doc/latex/contact.tex AU eus/doc/latex/intro.tex A eus/doc/latex/sequences.tex A eus/doc/latex/voronoi.tex A eus/doc/latex/datetime.tex A eus/doc/latex/manual.ps A eus/doc/latex/man.idx A eus/doc/latex/generals.tex A eus/doc/latex/graphics.tex A eus/doc/latex/types.tex A eus/doc/latex/preface.tex A eus/doc/latex/fig A eus/doc/latex/fig/textviewpanel.ps A eus/doc/latex/fig/simst.ps A eus/doc/latex/fig/filepanel.ps A eus/doc/latex/fig/eta3colavo.ps A eus/doc/latex/fig/threadpool.ps A eus/doc/latex/fig/threadobj.ps A eus/doc/latex/fig/mars.eps A eus/doc/latex/fig/synchports.ps A eus/doc/latex/fig/robot-ui2.ps A eus/doc/latex/fig/eta3hiru.ps A eus/doc/latex/fig/viewcoords.ps A eus/doc/latex/fig/xdraw.ps A eus/doc/latex/fig/fig-peg-naname-m2.ps A eus/doc/latex/fig/threadmodel.ps A eus/doc/latex/fig/fig-peg-in-hole1.ps A eus/doc/latex/fig/fig-peg-naname-m4.ps A eus/doc/latex/fig/fig-peg-in-hole3.ps A eus/doc/latex/fig/robot-st2.ps A eus/doc/latex/fig/parathreads.ps A eus/doc/latex/fig/beam.ps A eus/doc/latex/fig/eta3coords.ps A eus/doc/latex/fig/object.ps A eus/doc/latex/fig/block1.edg.ps A eus/doc/latex/fig/loop.ps A eus/doc/latex/fig/torus.ps A eus/doc/latex/fig/panelitem.ps A eus/doc/latex/fig/pointer.ps A eus/doc/latex/fig/simwindow.ps A eus/doc/latex/fig/fig1.ps A eus/doc/latex/fig/fig-peg-naname-m1.ps A eus/doc/latex/fig/cup.ps A eus/doc/latex/fig/fig-peg-naname-m3.ps A eus/doc/latex/fig/fig-peg-in-hole2.ps A eus/doc/latex/fig/fig-peg-in-hole4.ps A eus/doc/latex/fig/threadfig.ps A eus/doc/latex/fig/panellayout.ps A eus/doc/latex/evaluation.tex A eus/doc/latex/io.tex A eus/doc/latex/hyph.tex A eus/doc/latex/man-title.ps A eus/doc/latex/xtoolkit.tex A eus/doc/latex/manual.tex A eus/doc/latex/Makefile A eus/doc/latex/http.tex A eus/doc/latex/image.tex A eus/doc/latex/text.tex A eus/doc/latex/mthread.tex AU eus/doc/latex/manual.pdf A eus/doc/latex/loadforeign.tex A eus/doc/latex/objects.tex A eus/doc/latex/manipulator.tex A eus/doc/latex/geometry.tex AU eus/doc/latex/manual.dvi A eus/doc/latex/xwindow.tex A eus/doc/latex/manual.idx A eus/doc/latex/methods.tex A eus/doc/latex/controls.tex A eus/doc/latex/matrix.tex A eus/doc/latex/database.tex A eus/doc/latex/arith.tex A eus/doc/latex/euslisp.hlp Fetching 'https://svn.code.sf.net/p/euslisp/code/trunk/EusLisp/doc/latex' at 659 into '<http://jenkins.jsk.imi.i.u-tokyo.ac.jp:8080/job/jskeus/ws/jskeus/eus/doc/latex'> At revision 659 A eus/doc/jlatex/jevaluation.tex A eus/doc/jlatex/jio.tex A eus/doc/jlatex/jxtoolkit.tex A eus/doc/jlatex/jmanual.tex A eus/doc/jlatex/tape.tex A eus/doc/jlatex/mytabbing.sty A eus/doc/jlatex/jimage.tex A eus/doc/jlatex/jmthread.tex AU eus/doc/jlatex/jmanual.pdf AU eus/doc/jlatex/test.dvi A eus/doc/jlatex/jobjects.tex A eus/doc/jlatex/jgeometry.tex A eus/doc/jlatex/jmanipulator.tex AU eus/doc/jlatex/fig A eus/doc/jlatex/jxwindow.tex AU eus/doc/jlatex/jmanual.dvi A eus/doc/jlatex/jmanual.idx A eus/doc/jlatex/jcontrols.tex AU eus/doc/jlatex/Makefile A eus/doc/jlatex/jmatrix.tex A eus/doc/jlatex/jarith.tex A eus/doc/jlatex/jvxw.tex A eus/doc/jlatex/jmars-pre.tex A eus/doc/jlatex/test.ps A eus/doc/jlatex/jsymbols.tex A eus/doc/jlatex/jsysfunc.tex A eus/doc/jlatex/jintro.tex A eus/doc/jlatex/jcontact.tex A eus/doc/jlatex/jsequences.tex A eus/doc/jlatex/jvoronoi.tex A eus/doc/jlatex/server.tex A eus/doc/jlatex/euslisp.hlp A eus/doc/jlatex/jgenerals.tex A eus/doc/jlatex/jgraphics.tex A eus/doc/jlatex/test.tex Fetching 'https://svn.code.sf.net/p/euslisp/code/trunk/EusLisp/doc/jlatex' at 659 into '<http://jenkins.jsk.imi.i.u-tokyo.ac.jp:8080/job/jskeus/ws/jskeus/eus/doc/jlatex'> At revision 659 At revision 1049 no change for http://svn.code.sf.net/p/jskeus/code/trunk since the previous build no change for https://svn.code.sf.net/p/euslisp/code/trunk/EusLisp/lisp since the previous build no change for https://svn.code.sf.net/p/euslisp/code/trunk/EusLisp/lib since the previous build no change for https://svn.code.sf.net/p/euslisp/code/trunk/EusLisp/models since the previous build no change for https://svn.code.sf.net/p/euslisp/code/trunk/EusLisp/doc/latex since the previous build [jskeus] $ /bin/sh -xe /tmp/hudson6027702608662149.sh + sudo apt-get install -y subversion gcc g++ libjpeg-dev libxext-dev libx11-dev libgl1-mesa-dev libglu1-mesa-dev libpq-dev libpng12-dev xfonts-100dpi xfonts-75dpi msttcorefonts Reading package lists... Building dependency tree... Reading state information... g++ is already the newest version. gcc is already the newest version. libjpeg-dev is already the newest version. libpng12-dev is already the newest version. xfonts-100dpi is already the newest version. xfonts-75dpi is already the newest version. ttf-mscorefonts-installer is already the newest version. libgl1-mesa-dev is already the newest version. libglu1-mesa-dev is already the newest version. libpq-dev is already the newest version. libx11-dev is already the newest version. libxext-dev is already the newest version. subversion is already the newest version. You might want to run 'apt-get -f install' to correct these: The following packages have unmet dependencies: ros-hydro-roscpp : Depends: ros-hydro-cpp-common (>= 0.3.17) but 0.3.16-0precise-20131010-0127-+0000 is to be installed Depends: ros-hydro-roscpp-traits (>= 0.3.17) but 0.3.16-0precise-20131010-0148-+0000 is to be installed E: Unmet dependencies. Try 'apt-get -f install' with no packages (or specify a solution). Build step 'Execute shell' marked build as failure |
|
From: <sn...@us...> - 2014-01-22 07:13:27
|
Revision: 1049
http://sourceforge.net/p/jskeus/code/1049
Author: snozawa
Date: 2014-01-22 07:13:23 +0000 (Wed, 22 Jan 2014)
Log Message:
-----------
remove nil in limb-cop-fz-list reported by kuroiwa
Modified Paths:
--------------
trunk/irteus/irtrobot.l
Modified: trunk/irteus/irtrobot.l
===================================================================
--- trunk/irteus/irtrobot.l 2014-01-18 02:29:58 UTC (rev 1048)
+++ trunk/irteus/irtrobot.l 2014-01-22 07:13:23 UTC (rev 1049)
@@ -473,7 +473,8 @@
(send self :calc-cop-from-force-moment
nf nm fs cc :fz-thre 0 :return-all-values t)))
force-sensors forces moments cop-coords)))
- (let* ((limb-fz (reduce #'+ (mapcar #'(lambda (x) (cadr (memq :fz x))) limb-cop-fz-list) :initial-value 0))
+ (let* ((limb-cop-fz-list2 (remove nil limb-cop-fz-list))
+ (limb-fz (reduce #'+ (mapcar #'(lambda (x) (cadr (memq :fz x))) limb-cop-fz-list) :initial-value 0))
(tmpzmp (scale (/ 1.0 limb-fz) (reduce #'v+ (mapcar #'(lambda (x) (scale (cadr (memq :fz x)) (cadr (memq :cop x)))) limb-cop-fz-list)))))
(cond
((< limb-fz fz-thre) nil)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2014-01-18 02:30:02
|
Revision: 1048
http://sourceforge.net/p/jskeus/code/1048
Author: snozawa
Date: 2014-01-18 02:29:58 +0000 (Sat, 18 Jan 2014)
Log Message:
-----------
fix typo ;; end-coorde -> end-coords
Modified Paths:
--------------
trunk/irteus/irtrobot.l
Modified: trunk/irteus/irtrobot.l
===================================================================
--- trunk/irteus/irtrobot.l 2014-01-16 03:07:11 UTC (rev 1047)
+++ trunk/irteus/irtrobot.l 2014-01-18 02:29:58 UTC (rev 1048)
@@ -463,7 +463,7 @@
;; :wrt is :world => calc world zmp for robot
(limbs '(:rleg :lleg))
(force-sensors (mapcar #'(lambda (l) (send self :force-sensor l)) limbs))
- (cop-coords (mapcar #'(lambda (l) (send self l :end-coorde)) limbs))
+ (cop-coords (mapcar #'(lambda (l) (send self l :end-coords)) limbs))
(fz-thre 50) ;; [N]
(limb-cop-fz-list
(mapcar #'(lambda (fs f m cc)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2014-01-16 03:07:16
|
Revision: 1047
http://sourceforge.net/p/jskeus/code/1047
Author: snozawa
Date: 2014-01-16 03:07:11 +0000 (Thu, 16 Jan 2014)
Log Message:
-----------
enable to set periodic-time
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2014-01-14 21:45:47 UTC (rev 1046)
+++ trunk/irteus/irtmodel.l 2014-01-16 03:07:11 UTC (rev 1047)
@@ -1896,7 +1896,7 @@
(t (make-list (length move-target) :initial-element (deg2rad 1)))))
(union-link-list)
(centroid-thre 1.0) (target-centroid-pos) (centroid-offset-func)
- (dump-command t)
+ (dump-command t) (periodic-time 0.5) ;; [s]
&allow-other-keys)
;; target-coords, move-target, rotation-axis, translation-axis, link-list
;; -> both list and atom OK.
@@ -1986,7 +1986,7 @@
(setq success (send* self :inverse-kinematics-loop
dif-pos dif-rot
:target-coords target-coords
- :periodic-time 0.5
+ :periodic-time periodic-time
:stop stop :loop loop
:rotation-axis rotation-axis
:translation-axis translation-axis
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2014-01-14 21:45:52
|
Revision: 1046
http://sourceforge.net/p/jskeus/code/1046
Author: snozawa
Date: 2014-01-14 21:45:47 +0000 (Tue, 14 Jan 2014)
Log Message:
-----------
add preview control example ;; add comment to plot on gnuplot
Modified Paths:
--------------
trunk/irteus/demo/walk-motion.l
Modified: trunk/irteus/demo/walk-motion.l
===================================================================
--- trunk/irteus/demo/walk-motion.l 2014-01-09 17:36:37 UTC (rev 1045)
+++ trunk/irteus/demo/walk-motion.l 2014-01-14 21:45:47 UTC (rev 1046)
@@ -165,3 +165,48 @@
*robots*)
)
(warn "(walk-motion-for-robots) for walking motion for several robot models~%")
+
+;; preview control example
+(defun test-preview-control
+ (&key (preview-class preview-control)
+ (q 1) (r 1e-6))
+ (let* ((dt 0.010) (max-tm 10.0)
+ (pc (instance preview-class :init dt 800 :q q :r r))
+ (tm-list (mapcar #'(lambda (x) (* x dt))
+ (let ((i 0)) (mapcar #'(lambda (x) (incf i)) (make-list (round (/ max-tm dt)))))))
+ (ref-zmp-list
+ (mapcar #'(lambda (tm)
+ (cond ((< tm 2) #f(0 0 0))
+ ((< tm 4) #f(-20 20 0))
+ ((< tm 6) #f(20 -20 0))
+ (t #f(0 -20 0))))
+ tm-list))
+ zmp-list cog-list ref-zmp-list2)
+ (mapcar
+ #'(lambda (ref-zmp tm)
+ (let ((xk (send pc :update-xk ref-zmp)))
+ (format t ";tm=~7,3f, u=~7,3f, p=~7,3f, xk=~30A~%"
+ tm (elt (matrix-row (send pc :calc-u) 0) 0) (elt (send pc :cart-zmp) 0) (elt (send pc :refcog) 0))
+ (push (elt (send pc :cart-zmp) 0) zmp-list) ;; zmp ;; this zmp is "zmp as a table-cart model"
+ (push (elt (send pc :refcog) 0) cog-list) ;; cog
+ (push (elt (send pc :current-refzmp) 0) ref-zmp-list2) ;; ref zmp ;; ref-zmp-list2 should be equal to ref-zmp-list
+ ))
+ ref-zmp-list tm-list)
+ (with-open-file
+ (f "/tmp/test-preview-control-data.dat" :direction :output)
+ (mapcar #'(lambda (tm zmp cog refzmp)
+ (format f "~A ~A ~A ~A~%" tm zmp cog refzmp))
+ tm-list (reverse zmp-list) (reverse cog-list) (reverse ref-zmp-list2))
+ )
+ ;; gnuplot sample
+ ;; $ gnuplot
+ ;; gnuplot> set xlabel "Time [s]"
+ ;; gnuplot> set ylabel "ZMP X [mm]"
+ ;; gnuplot> plot '/tmp/test-preview-control-data.dat' using 1:2 title "cart zmp" with lines
+ ;; gnuplot> replot '/tmp/test-preview-control-data.dat' using 1:3 title "cog" with lines
+ ;; gnuplot> replot '/tmp/test-preview-control-data.dat' using 1:4 title "refzmp" with lines
+ (list :time-list
+ tm-list
+ :data-list
+ (list (reverse zmp-list) (reverse cog-list) (reverse ref-zmp-list2)))
+ ))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2014-01-09 17:36:47
|
Revision: 1045
http://sourceforge.net/p/jskeus/code/1045
Author: snozawa
Date: 2014-01-09 17:36:37 +0000 (Thu, 09 Jan 2014)
Log Message:
-----------
add piped-fork function which returns list
Modified Paths:
--------------
trunk/irteus/irtutil.l
Modified: trunk/irteus/irtutil.l
===================================================================
--- trunk/irteus/irtutil.l 2014-01-09 07:00:38 UTC (rev 1044)
+++ trunk/irteus/irtutil.l 2014-01-09 17:36:37 UTC (rev 1045)
@@ -354,6 +354,16 @@
)
,flag))))
+;; piped fork returning result as list
+(defun piped-fork-returns-list (cmd &optional args)
+ (let ((strm (apply #'piped-fork cmd args)))
+ (unwind-protect
+ (let ((ret) (buf))
+ (while (setq buf (read-line strm nil))
+ (push buf ret))
+ (reverse ret))
+ (close strm))))
+
(provide :irtutil "$Id$")
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2014-01-09 07:00:40
|
Revision: 1044
http://sourceforge.net/p/jskeus/code/1044
Author: snozawa
Date: 2014-01-09 07:00:38 +0000 (Thu, 09 Jan 2014)
Log Message:
-----------
add comments to specify units
Modified Paths:
--------------
trunk/irteus/irtdyna.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2014-01-08 19:13:35 UTC (rev 1043)
+++ trunk/irteus/irtdyna.l 2014-01-09 07:00:38 UTC (rev 1044)
@@ -883,10 +883,10 @@
;; 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))
+ (let* ((cfs (send cop-coords :inverse-rotate-vector force)) ;; [N]
+ (cms (send cop-coords :inverse-rotate-vector moment)) ;; [Nm]
(cps (scale 1e-3 (send cop-coords :inverse-transform-vector
- (send sensor-coords :worldpos))))
+ (send sensor-coords :worldpos)))) ;; [mm]->[m]
(tmpcopx (+ (* (elt cfs 2) (elt cps 0))
(- (* (elt cps 2) (elt cfs 0)))
(- (elt cms 1))))
@@ -899,10 +899,10 @@
(return-all-values
(list :cop
(send cop-coords :transform-vector
- (scale 1e3 (float-vector (/ tmpcopx tmpfz) (/ tmpcopy tmpfz) 0)))
+ (scale 1e3 (float-vector (/ tmpcopx tmpfz) (/ tmpcopy tmpfz) 0))) ;; [m]->[mm]
:fz tmpfz))
(t (send cop-coords :transform-vector
- (scale 1e3 (float-vector (/ tmpcopx tmpfz) (/ tmpcopy tmpfz) 0))))
+ (scale 1e3 (float-vector (/ tmpcopx tmpfz) (/ tmpcopy tmpfz) 0)))) ;; [m]->[mm]
)))
;; get list of N x force and N x moment from 6 x N wrench vector
;; (list (list f1 ... fN) (list n1 ... nN)) <= [f1 n1 ... fN nN]
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2014-01-08 19:13:38
|
Revision: 1043
http://sourceforge.net/p/jskeus/code/1043
Author: snozawa
Date: 2014-01-08 19:13:35 +0000 (Wed, 08 Jan 2014)
Log Message:
-----------
remove unused zmp-z argument
Modified Paths:
--------------
trunk/irteus/irtrobot.l
Modified: trunk/irteus/irtrobot.l
===================================================================
--- trunk/irteus/irtrobot.l 2014-01-08 19:07:45 UTC (rev 1042)
+++ trunk/irteus/irtrobot.l 2014-01-08 19:13:35 UTC (rev 1043)
@@ -464,7 +464,6 @@
(limbs '(:rleg :lleg))
(force-sensors (mapcar #'(lambda (l) (send self :force-sensor l)) limbs))
(cop-coords (mapcar #'(lambda (l) (send self l :end-coorde)) limbs))
- (zmp-z (elt (apply #'midpoint 0.5 (mapcar #'(lambda (c) (send c :worldpos)) cop-coords)) 2)) ;; [mm]
(fz-thre 50) ;; [N]
(limb-cop-fz-list
(mapcar #'(lambda (fs f m cc)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2014-01-08 19:07:47
|
Revision: 1042
http://sourceforge.net/p/jskeus/code/1042
Author: snozawa
Date: 2014-01-08 19:07:45 +0000 (Wed, 08 Jan 2014)
Log Message:
-----------
use calc-cop-from-force-moment
Modified Paths:
--------------
trunk/irteus/irtrobot.l
Modified: trunk/irteus/irtrobot.l
===================================================================
--- trunk/irteus/irtrobot.l 2014-01-08 18:04:53 UTC (rev 1041)
+++ trunk/irteus/irtrobot.l 2014-01-08 19:07:45 UTC (rev 1042)
@@ -463,31 +463,25 @@
;; :wrt is :world => calc world zmp for robot
(limbs '(:rleg :lleg))
(force-sensors (mapcar #'(lambda (l) (send self :force-sensor l)) limbs))
- (zmp-z (elt (apply #'midpoint 0.5 (mapcar #'(lambda (l) (send self l :end-coords :worldpos)) limbs)) 2)) ;; [mm]
- (fz-thre 50)) ;; [N]
- (let ((tmpzmpx 0) (tmpzmpy 0) (tmpfz 0)
- (tmp-zmp-z (* 1e-3 zmp-z))) ;; [mm]->[m]
- (mapcar #'(lambda (fs f m)
- (let ((fsp (scale 1e-3 (send fs :worldpos))) ;; [mm]->[m]
- (nf (send fs :rotate-vector f))
- (nm (send fs :rotate-vector m)))
- (setq tmpzmpx (+ tmpzmpx
- (* (elt nf 2) (elt fsp 0))
- (- (* (- (elt fsp 2) tmp-zmp-z) (elt nf 0)))
- (- (elt nm 1)))
- tmpzmpy (+ tmpzmpy
- (* (elt nf 2) (elt fsp 1))
- (- (* (- (elt fsp 2) tmp-zmp-z) (elt nf 1)))
- (elt nm 0))
- tmpfz (+ tmpfz (elt nf 2)))))
- force-sensors forces moments)
- (let ((tmpzmp (scale 1e3 (float-vector (/ tmpzmpx tmpfz) (/ tmpzmpy tmpfz) tmp-zmp-z)))) ;; [m]->[mm]
- (cond
- ((< tmpfz fz-thre) nil)
- ((eq wrt :world) tmpzmp)
- ((eq wrt :local) (send (car (send self :links)) :inverse-transform-vector tmpzmp))
- (t )
- ))))
+ (cop-coords (mapcar #'(lambda (l) (send self l :end-coorde)) limbs))
+ (zmp-z (elt (apply #'midpoint 0.5 (mapcar #'(lambda (c) (send c :worldpos)) cop-coords)) 2)) ;; [mm]
+ (fz-thre 50) ;; [N]
+ (limb-cop-fz-list
+ (mapcar #'(lambda (fs f m cc)
+ (let ((fsp (scale 1e-3 (send fs :worldpos))) ;; [mm]->[m]
+ (nf (send fs :rotate-vector f))
+ (nm (send fs :rotate-vector m)))
+ (send self :calc-cop-from-force-moment
+ nf nm fs cc :fz-thre 0 :return-all-values t)))
+ force-sensors forces moments cop-coords)))
+ (let* ((limb-fz (reduce #'+ (mapcar #'(lambda (x) (cadr (memq :fz x))) limb-cop-fz-list) :initial-value 0))
+ (tmpzmp (scale (/ 1.0 limb-fz) (reduce #'v+ (mapcar #'(lambda (x) (scale (cadr (memq :fz x)) (cadr (memq :cop x)))) limb-cop-fz-list)))))
+ (cond
+ ((< limb-fz fz-thre) nil)
+ ((eq wrt :world) tmpzmp)
+ ((eq wrt :local) (send (car (send self :links)) :inverse-transform-vector tmpzmp))
+ (t )
+ )))
)
;;; moved from rbrarin libraries
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2014-01-08 18:04:57
|
Revision: 1041
http://sourceforge.net/p/jskeus/code/1041
Author: snozawa
Date: 2014-01-08 18:04:53 +0000 (Wed, 08 Jan 2014)
Log Message:
-----------
add wrench-vector wrench-list conversion
Modified Paths:
--------------
trunk/irteus/irtdyna.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2014-01-08 17:46:54 UTC (rev 1040)
+++ trunk/irteus/irtdyna.l 2014-01-08 18:04:53 UTC (rev 1041)
@@ -904,6 +904,28 @@
(t (send cop-coords :transform-vector
(scale 1e3 (float-vector (/ tmpcopx tmpfz) (/ tmpcopy tmpfz) 0))))
)))
+ ;; get list of N x force and N x moment from 6 x N wrench vector
+ ;; (list (list f1 ... fN) (list n1 ... nN)) <= [f1 n1 ... fN nN]
+ (:wrench-vector->wrench-list
+ (wrench-vector)
+ (let ((f-list) (m-list))
+ (dotimes (i (/ (length wrench-vector) 6))
+ (push (subseq wrench-vector (* 6 i) (+ 3 (* 6 i))) f-list)
+ (push (subseq wrench-vector (+ 3 (* 6 i)) (+ 6 (* 6 i))) m-list)
+ )
+ (list (reverse f-list) (reverse m-list))
+ ))
+ ;; get 6 x N wrench vector from list of N x force and N x moment
+ ;; [f1 n1 ... fN nN] <= (list (list f1 ... fN) (list n1 ... nN))
+ (:wrench-list->wrench-vector
+ (wrench-list)
+ (apply
+ #'concatenate
+ float-vector
+ (mapcar #'(lambda (f m)
+ (concatenate float-vector f m))
+ (car wrench-list) (cadr wrench-list))
+ ))
;; 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-08 17:46:58
|
Revision: 1040
http://sourceforge.net/p/jskeus/code/1040
Author: snozawa
Date: 2014-01-08 17:46:54 +0000 (Wed, 08 Jan 2014)
Log Message:
-----------
add return-all-values to return both fz and cop
Modified Paths:
--------------
trunk/irteus/irtdyna.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2014-01-08 17:15:38 UTC (rev 1039)
+++ trunk/irteus/irtdyna.l 2014-01-08 17:46:54 UTC (rev 1040)
@@ -878,7 +878,7 @@
(: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))
+ &key (fz-thre 1) (return-all-values))
;; 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}
@@ -894,11 +894,16 @@
(- (* (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))))
- ))
+ (cond
+ ((< tmpfz fz-thre) nil)
+ (return-all-values
+ (list :cop
+ (send cop-coords :transform-vector
+ (scale 1e3 (float-vector (/ tmpcopx tmpfz) (/ tmpcopy tmpfz) 0)))
+ :fz tmpfz))
+ (t (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-08 17:15:41
|
Revision: 1039
http://sourceforge.net/p/jskeus/code/1039
Author: snozawa
Date: 2014-01-08 17:15:38 +0000 (Wed, 08 Jan 2014)
Log Message:
-----------
fix indent in calc-cop-from-force-moment
Modified Paths:
--------------
trunk/irteus/irtdyna.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2014-01-08 17:10:11 UTC (rev 1038)
+++ trunk/irteus/irtdyna.l 2014-01-08 17:15:38 UTC (rev 1039)
@@ -875,30 +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))))
- ))
+ (: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))))
+ (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.
|
