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...> - 2011-12-01 13:45:24
|
Revision: 737
http://jskeus.svn.sourceforge.net/jskeus/?rev=737&view=rev
Author: snozawa
Date: 2011-12-01 13:45:15 +0000 (Thu, 01 Dec 2011)
Log Message:
-----------
update dual-manip-ik according to r725 commit
Revision Links:
--------------
http://jskeus.svn.sourceforge.net/jskeus/?rev=725&view=rev
Modified Paths:
--------------
trunk/irteus/demo/dual-manip-ik.l
Modified: trunk/irteus/demo/dual-manip-ik.l
===================================================================
--- trunk/irteus/demo/dual-manip-ik.l 2011-12-01 12:57:53 UTC (rev 736)
+++ trunk/irteus/demo/dual-manip-ik.l 2011-12-01 13:45:15 UTC (rev 737)
@@ -49,14 +49,6 @@
robot-ll (mapcar #'(lambda (l) (send *obj* :get l)) '(:rarm :larm))
:joint-class 6dof-joint))
(ll (car ret)) (ot (cadr ret))
- (ul (apply #'append
- (mapcar
- #'(lambda (f)
- (send *robot* :calc-union-link-list
- (mapcar #'(lambda (l)
- (funcall f #'(lambda (x) (member x (send *robot* :links))) l))
- ll)))
- (list #'remove-if-not #'remove-if))))
(rotation-axis (cond
((= (mod i 4) 1) '(:x t))
((= (mod i 4) 3) '(t :x))
@@ -64,18 +56,27 @@
(weight (fill (instantiate float-vector (send *robot* :calc-target-joint-dimension ll)) 1.0)))
(dotimes (i 3)
(setf (elt weight (+ (- (send *robot* :calc-target-joint-dimension ll) 6) i)) 0.001))
- (send (car (last ul)) :assoc *obj*)
+ (send (send (car ot) :parent) :assoc *obj*)
(send* *robot* :inverse-kinematics ot
- :union-link-list ul :link-list ll :move-target mt
+ :link-list ll :move-target mt
+ :rotation-axis rotation-axis
:weight weight
- :rotation-axis rotation-axis
- :jacobi #'(lambda ()
+ :union-link-list #'(lambda (tmp-ll)
+ (apply #'append
+ (mapcar
+ #'(lambda (f)
+ (send *robot* :calc-union-link-list
+ (mapcar #'(lambda (l)
+ (funcall f #'(lambda (x) (member x (send *robot* :links))) l))
+ tmp-ll)))
+ (list #'remove-if-not #'remove-if))))
+ :jacobi #'(lambda (tmp-ll tmp-mt tmp-ta tmp-ra)
(calc-jacobian-from-link-list-including-robot-and-obj-virtual-joint
- ll mt ot *robot* :rotation-axis rotation-axis))
+ tmp-ll tmp-mt ot *robot* :rotation-axis tmp-ra))
;;:debug-view t
:debug-view :no-message
(append (if (= i 0) '(:stop 100)) args))
- (send (car (last ul)) :dissoc *obj*)
+ (send (send (car ot) :parent) :dissoc *obj*)
))
(incf i)
(send *robot* :head :look-at (send *obj* :worldpos))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2011-12-01 12:58:03
|
Revision: 736
http://jskeus.svn.sourceforge.net/jskeus/?rev=736&view=rev
Author: snozawa
Date: 2011-12-01 12:57:53 +0000 (Thu, 01 Dec 2011)
Log Message:
-----------
add arguments for jacobi and take function as union-link-list ;; update calculation of jacobian for object manip ik
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2011-12-01 10:39:24 UTC (rev 735)
+++ trunk/irteus/irtmodel.l 2011-12-01 12:57:53 UTC (rev 736)
@@ -1581,7 +1581,7 @@
(setq tmp-dim (cadr (memq :tmp-dim ik-args)))
(setq tmp-dim (instantiate float-vector (send self :calc-target-axis-dimension rotation-axis translation-axis))))
- (if (functionp jacobi) (setq jacobi (funcall jacobi)))
+ (if (functionp jacobi) (setq jacobi (funcall jacobi link-list move-target translation-axis rotation-axis)))
(unless jacobi
(setq jacobi (send* self :calc-jacobian-from-link-list link-list
:translation-axis translation-axis
@@ -1672,7 +1672,7 @@
;; target-coords, move-target, rotation-axis, translation-axis, link-list
;; -> both list and atom OK.
(let* ((loop 0)
- (union-link-list (if union-link-list union-link-list (send self :calc-union-link-list link-list)))
+ (union-link-list (if (functionp union-link-list) (funcall union-link-list link-list) (send self :calc-union-link-list link-list)))
(av0 (send-all (remove-duplicates
(append (send-all union-link-list :joint) joint-list))
:joint-angle))
@@ -1975,9 +1975,10 @@
(let* ((robot-ll (mapcar #'(lambda (l) ;; link-list for object link
(remove-if-not #'(lambda (x) (member x (send robot :links))) l))
link-list))
- (obj-ll (mapcar #'(lambda (l) ;; link-list for robot link
- (remove-if #'(lambda (x) (member x (send robot :links))) l))
- link-list))
+ (obj-ll (remove nil ;; obj-move-target should corresponds to head of link-list
+ (mapcar #'(lambda (l) ;; link-list for robot link
+ (remove-if #'(lambda (x) (member x (send robot :links))) l))
+ link-list)))
(robot-ll-len (send robot :calc-target-joint-dimension robot-ll)))
(send robot :calc-jacobian-from-link-list
obj-ll :col-offset robot-ll-len :fik fik
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <je...@js...> - 2011-12-01 12:41:07
|
<http://jenkins.jsk.imi.i.u-tokyo.ac.jp:8080/job/jskeus/9/>を確認してください。 ------------------------------------------ SCMのポーリングが実行 masterでビルドします。 Cleaning local Directory jskeus Checking out http://jskeus.svn.sourceforge.net/svnroot/jskeus/trunk A irteus AU irteus/irtx.l A irteus/irtsensor.l A irteus/demo A irteus/demo/hand-grasp-ik.l A irteus/demo/sample-hand-model.l A irteus/demo/dual-manip-ik.l A irteus/demo/hanoi-arm.l A irteus/demo/sample-arm-model.l A irteus/demo/full-body-ik.l A irteus/demo/hanoi.l A irteus/demo/crank-motion.l A irteus/demo/sample-robot-model.l A irteus/demo/dual-arm-ik.l A irteus/demo/particle.l AU irteus/demo/demo.l A irteus/Makefile.Linux64 AU irteus/compile_irtx.l A irteus/irtbvh.l AU irteus/pqp.l AU irteus/irtimage.l AU irteus/euspqp.c A irteus/Makefile.Darwin A irteus/PQP A irteus/PQP/Linux AU irteus/PQP/Linux/.keepme AU irteus/PQP/Linux/.cvsignore A irteus/PQP/Cygwin AU irteus/PQP/Cygwin/.keepme AU irteus/PQP/Cygwin/.cvsignore A irteus/PQP/Makefile.Darwin A irteus/PQP/Darwin A irteus/PQP/Makefile.Linux64 A irteus/PQP/src AU irteus/PQP/src/Build.cpp AU irteus/PQP/src/PQP.cpp AU irteus/PQP/src/PQP_Internal.h AU irteus/PQP/src/GetTime.h AU irteus/PQP/src/Tri.h AU irteus/PQP/src/Build.h AU irteus/PQP/src/PQP.h AU irteus/PQP/src/BV.cpp AU irteus/PQP/src/OBB_Disjoint.h AU irteus/PQP/src/BV.h AU irteus/PQP/src/PQP_Compile.h AU irteus/PQP/src/BVTQ.h AU irteus/PQP/src/TriDist.cpp AU irteus/PQP/src/MatVec.h AU irteus/PQP/src/RectDist.h AU irteus/PQP/src/TriDist.h A irteus/PQP/Linux64 AU irteus/PQP/Makefile.Linux AU irteus/PQP/README.txt AU irteus/PQP/Makefile.Cygwin AU irteus/PQP/Makefile AU irteus/irtdyna.l AU irteus/irtmodel.l AU irteus/irtext.l AU irteus/irtgl.l AU irteus/Makefile.Linux AU irteus/Makefile.Cygwin AU irteus/Makefile AU irteus/png.l A irteus/euspng.c AU irteus/irtc.c AU irteus/compile_irtgl.l AU irteus/irtgeo.l AU irteus/CPQP.C AU irteus/irtutil.l AU irteus/compile_irtimg.l AU irteus/irtrobot.l AU irteus/nr.c AU irteus/compile_irt.l AU irteus/irtmath.l AU irteus/nr.h A irteus/irtscene.l AU irteus/irtglrgb.l AU irteus/compile_irtg.l ERROR: Failed to check out http://jskeus.svn.sourceforge.net/svnroot/jskeus/trunk org.tmatesoft.svn.core.SVNException: svn: REPORT /svnroot/jskeus/!svn/vcc/default failed at org.tmatesoft.svn.core.internal.io.dav.http.HTTPConnection.request(HTTPConnection.java:291) at org.tmatesoft.svn.core.internal.io.dav.http.HTTPConnection.request(HTTPConnection.java:276) at org.tmatesoft.svn.core.internal.io.dav.http.HTTPConnection.request(HTTPConnection.java:264) at org.tmatesoft.svn.core.internal.io.dav.DAVConnection.doReport(DAVConnection.java:266) at org.tmatesoft.svn.core.internal.io.dav.DAVRepository.runReport(DAVRepository.java:1263) at org.tmatesoft.svn.core.internal.io.dav.DAVRepository.update(DAVRepository.java:820) at org.tmatesoft.svn.core.wc.SVNUpdateClient.update(SVNUpdateClient.java:564) at org.tmatesoft.svn.core.wc.SVNUpdateClient.doCheckout(SVNUpdateClient.java:942) at hudson.scm.subversion.CheckoutUpdater$1.perform(CheckoutUpdater.java:86) at hudson.scm.subversion.WorkspaceUpdater$UpdateTask.delegateTo(WorkspaceUpdater.java:136) at hudson.scm.SubversionSCM$CheckOutTask.perform(SubversionSCM.java:780) at hudson.scm.SubversionSCM$CheckOutTask.invoke(SubversionSCM.java:761) at hudson.scm.SubversionSCM$CheckOutTask.invoke(SubversionSCM.java:745) at hudson.FilePath.act(FilePath.java:783) at hudson.FilePath.act(FilePath.java:765) at hudson.scm.SubversionSCM.checkout(SubversionSCM.java:735) at hudson.scm.SubversionSCM.checkout(SubversionSCM.java:677) at hudson.model.AbstractProject.checkout(AbstractProject.java:1195) at hudson.model.AbstractBuild$AbstractRunner.checkout(AbstractBuild.java:568) at hudson.model.AbstractBuild$AbstractRunner.run(AbstractBuild.java:457) at hudson.model.Run.run(Run.java:1404) at hudson.model.FreeStyleBuild.run(FreeStyleBuild.java:46) at hudson.model.ResourceController.execute(ResourceController.java:88) at hudson.model.Executor.run(Executor.java:230) Caused by: org.tmatesoft.svn.core.SVNErrorMessage: svn: REPORT /svnroot/jskeus/!svn/vcc/default failed at org.tmatesoft.svn.core.SVNErrorMessage.create(SVNErrorMessage.java:200) at org.tmatesoft.svn.core.SVNErrorMessage.create(SVNErrorMessage.java:146) at org.tmatesoft.svn.core.SVNErrorMessage.create(SVNErrorMessage.java:89) ... 24 more Caused by: org.tmatesoft.svn.core.SVNException: svn: REPORT request failed on '/svnroot/jskeus/!svn/vcc/default' svn: Connection reset at org.tmatesoft.svn.core.internal.wc.SVNErrorManager.error(SVNErrorManager.java:64) at org.tmatesoft.svn.core.internal.wc.SVNErrorManager.error(SVNErrorManager.java:51) at org.tmatesoft.svn.core.internal.io.dav.http.HTTPConnection._request(HTTPConnection.java:644) at org.tmatesoft.svn.core.internal.io.dav.http.HTTPConnection.request(HTTPConnection.java:285) ... 23 more Caused by: org.tmatesoft.svn.core.SVNErrorMessage: svn: REPORT request failed on '/svnroot/jskeus/!svn/vcc/default' at org.tmatesoft.svn.core.SVNErrorMessage.create(SVNErrorMessage.java:200) at org.tmatesoft.svn.core.internal.io.dav.http.HTTPConnection._request(HTTPConnection.java:642) ... 24 more Caused by: org.tmatesoft.svn.core.SVNErrorMessage: svn: Connection reset at org.tmatesoft.svn.core.SVNErrorMessage.create(SVNErrorMessage.java:101) at org.tmatesoft.svn.core.internal.io.dav.http.HTTPConnection._request(HTTPConnection.java:408) ... 24 more Caused by: java.net.SocketException: Connection reset at java.net.SocketInputStream.read(SocketInputStream.java:168) at java.io.BufferedInputStream.read1(BufferedInputStream.java:256) at java.io.BufferedInputStream.read(BufferedInputStream.java:317) at org.tmatesoft.svn.core.internal.util.ChunkedInputStream.read(ChunkedInputStream.java:70) at sun.nio.cs.StreamDecoder.readBytes(StreamDecoder.java:264) at sun.nio.cs.StreamDecoder.implRead(StreamDecoder.java:306) at sun.nio.cs.StreamDecoder.read(StreamDecoder.java:158) at java.io.InputStreamReader.read(InputStreamReader.java:167) at org.tmatesoft.svn.core.internal.io.dav.http.XMLReader.read(XMLReader.java:39) at com.sun.org.apache.xerces.internal.impl.XMLEntityScanner.load(XMLEntityScanner.java:1742) at com.sun.org.apache.xerces.internal.impl.XMLEntityScanner.skipChar(XMLEntityScanner.java:1416) at com.sun.org.apache.xerces.internal.impl.XMLDocumentFragmentScannerImpl$FragmentContentDriver.next(XMLDocumentFragmentScannerImpl.java:2792) at com.sun.org.apache.xerces.internal.impl.XMLDocumentScannerImpl.next(XMLDocumentScannerImpl.java:648) at com.sun.org.apache.xerces.internal.impl.XMLNSDocumentScannerImpl.next(XMLNSDocumentScannerImpl.java:140) at com.sun.org.apache.xerces.internal.impl.XMLDocumentFragmentScannerImpl.scanDocument(XMLDocumentFragmentScannerImpl.java:511) at com.sun.org.apache.xerces.internal.parsers.XML11Configuration.parse(XML11Configuration.java:808) at com.sun.org.apache.xerces.internal.parsers.XML11Configuration.parse(XML11Configuration.java:737) at com.sun.org.apache.xerces.internal.parsers.XMLParser.parse(XMLParser.java:119) at com.sun.org.apache.xerces.internal.parsers.AbstractSAXParser.parse(AbstractSAXParser.java:1205) at com.sun.org.apache.xerces.internal.jaxp.SAXParserImpl$JAXPSAXParser.parse(SAXParserImpl.java:522) at org.tmatesoft.svn.core.internal.io.dav.http.HTTPConnection.readData(HTTPConnection.java:754) at org.tmatesoft.svn.core.internal.io.dav.http.HTTPConnection.readData(HTTPConnection.java:719) at org.tmatesoft.svn.core.internal.io.dav.http.HTTPRequest.dispatch(HTTPRequest.java:216) at org.tmatesoft.svn.core.internal.io.dav.http.HTTPConnection._request(HTTPConnection.java:364) ... 24 more FATAL: null java.lang.NullPointerException at java.util.ArrayList.addAll(ArrayList.java:472) at hudson.scm.SubversionSCM.checkout(SubversionSCM.java:735) at hudson.scm.SubversionSCM.checkout(SubversionSCM.java:677) at hudson.model.AbstractProject.checkout(AbstractProject.java:1195) at hudson.model.AbstractBuild$AbstractRunner.checkout(AbstractBuild.java:568) at hudson.model.AbstractBuild$AbstractRunner.run(AbstractBuild.java:457) at hudson.model.Run.run(Run.java:1404) at hudson.model.FreeStyleBuild.run(FreeStyleBuild.java:46) at hudson.model.ResourceController.execute(ResourceController.java:88) at hudson.model.Executor.run(Executor.java:230) |
|
From: <sn...@us...> - 2011-12-01 10:39:30
|
Revision: 735
http://jskeus.svn.sourceforge.net/jskeus/?rev=735&view=rev
Author: snozawa
Date: 2011-12-01 10:39:24 +0000 (Thu, 01 Dec 2011)
Log Message:
-----------
add comments for :fullbody-inverse-kinematics and :cog-jacobian functions
Modified Paths:
--------------
trunk/irteus/irtdyna.l
trunk/irteus/irtrobot.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2011-12-01 09:20:40 UTC (rev 734)
+++ trunk/irteus/irtdyna.l 2011-12-01 10:39:24 UTC (rev 735)
@@ -410,6 +410,11 @@
(dotimes (j (cadr (array-dimensions inertia-matrix)))
(setf (aref inertia-matrix i j) (/ (aref inertia-matrix i j) all-M))))
inertia-matrix))
+ ;; cog jacobian methods : :cog-jacobian-balance-nspace, :difference-cog-position, and :cog-convergence-check
+ ;; arguments
+ ;; translation-axis : use X and Y components of cog vector by default
+ ;; target-centroid-pos : 3D vector of target centroid position
+ ;; centroid-offset-func : offset for centroid
(:cog-jacobian-balance-nspace
(link-list
&rest args
Modified: trunk/irteus/irtrobot.l
===================================================================
--- trunk/irteus/irtrobot.l 2011-12-01 09:20:40 UTC (rev 734)
+++ trunk/irteus/irtrobot.l 2011-12-01 10:39:24 UTC (rev 735)
@@ -313,6 +313,9 @@
(coerce torque-vector cons))
(if flush (send vwer :viewsurface :flush))
);; draw-torque
+ ;; fullbody inverse kinematics for legged robot
+ ;; necessary args : target-coords, move-target, and link-list must include legs' (or leg's) parameters
+ ;; ex. (send *robot* :fullbody-inverse-kinematics (list rarm-tc rleg-tc lleg-tc) :move-target (list rarm-mt rleg-mt lleg-mt) :link-list (list rarm-ll rleg-ll lleg-ll))
(:fullbody-inverse-kinematics
(target-coords
&rest args
@@ -327,8 +330,9 @@
(dotimes (i 6) (setf (elt weight i) (elt root-link-virtual-joint-weight i)))
weight)))
;; default cog-jacobian parameters : target-centroid-pos, cog-gain, and centroid-thre
- (target-centroid-pos (apply #'midpoint 0.5 (send self :legs :end-coords :worldpos)))
- (cog-gain 1.0) (centroid-thre 5.0) ;; [mm]
+ (target-centroid-pos (apply #'midpoint 0.5 (send self :legs :end-coords :worldpos))) ;; <- target centroid position.
+ (cog-gain 1.0) ;; <- cog gain for null-space calculation. cog-gain should be over zero.
+ (centroid-thre 5.0) ;; cog convergence threshould [mm]
&allow-other-keys)
(with-append-root-joint ;; inverse-kinematics with base-link
(link-list-with-robot-6dof self link-list
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2011-12-01 09:20:50
|
Revision: 734
http://jskeus.svn.sourceforge.net/jskeus/?rev=734&view=rev
Author: snozawa
Date: 2011-12-01 09:20:40 +0000 (Thu, 01 Dec 2011)
Log Message:
-----------
fix *viewer* existence check ;; *viewer* is always bound in eus/lisp/geo/viewport.l ;; if viewer is available, *viewer* does not nil
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2011-12-01 06:39:51 UTC (rev 733)
+++ trunk/irteus/irtmodel.l 2011-12-01 09:20:40 UTC (rev 734)
@@ -1125,7 +1125,7 @@
&allow-other-keys)
(let (dav dtheta j (gain 1.0))
(if (and debug-view (atom debug-view)) (setq debug-view (list debug-view)))
- (if (and debug-view (not (equal debug-view :no-clear)) (boundp '*viewer*))
+ (if (and debug-view (not (equal debug-view :no-clear)) *viewer*)
(send *viewer* :viewsurface :clear))
(setq dav (send* self :calc-joint-angle-speed union-vel args))
@@ -1550,7 +1550,7 @@
(setq union-link-list (send self :calc-union-link-list link-list)))
;; atom -> list
(if (and debug-view (atom debug-view)) (setq debug-view (list debug-view)))
- (if (and debug-view (not (equal debug-view :no-clear)) (boundp '*viewer*))
+ (if (and debug-view (not (equal debug-view :no-clear)) *viewer*)
(send *viewer* :viewsurface :clear))
(if (atom (car link-list)) (setq link-list (list link-list)))
(if (atom move-target) (setq move-target (list move-target)))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2011-12-01 06:39:57
|
Revision: 733
http://jskeus.svn.sourceforge.net/jskeus/?rev=733&view=rev
Author: snozawa
Date: 2011-12-01 06:39:51 +0000 (Thu, 01 Dec 2011)
Log Message:
-----------
add :fullbody-inverse-kinematics using root-link virtual joint and cog-jacobian balancing
Modified Paths:
--------------
trunk/irteus/demo/crank-motion.l
trunk/irteus/irtrobot.l
Modified: trunk/irteus/demo/crank-motion.l
===================================================================
--- trunk/irteus/demo/crank-motion.l 2011-11-30 17:36:31 UTC (rev 732)
+++ trunk/irteus/demo/crank-motion.l 2011-12-01 06:39:51 UTC (rev 733)
@@ -49,39 +49,28 @@
(link-list (mapcar #'(lambda (l) (send *robot* :link-list (send l :parent)))
move-target))
(thre (list 15 1 1))
- (rotation-axis (list nil t t)))
- (with-append-root-joint
- (ll *robot* link-list
- :joint-class 6dof-joint
- :joint-args
- (list :min (float-vector -300 -300 -25 -10 -15 -15)
- :max (float-vector 300 300 25 10 15 15)))
- (let ((weight (fill (instantiate float-vector (send *robot* :calc-target-joint-dimension ll)) 1.0))
- (6dof-weight (float-vector 0.1 0.1 0.1 0.1 0.5 0.5))
- (fp (apply #'midpoint 0.5 (send-all fix-leg-coords :worldpos))))
- (dotimes (i 6) (setf (elt weight i) (elt 6dof-weight i)))
- (do-until-key
- (send crank :rotate (deg2rad 15) :z)
- (let* ((target-coords (append (list (send crank :get :handle)) fix-leg-coords)))
- (send *robot* :inverse-kinematics target-coords
- :link-list ll :move-target move-target
- :look-at-target t :thre thre :rotation-axis rotation-axis
- :weight weight
- :cog-gain 1.0 :target-centroid-pos fp :centroid-thre 10.0
- :debug-view :no-flush)
- ;; draw
- (send *irtviewer* :draw-objects :flush nil)
- (mapcar #'(lambda (act ref)
- (send act :draw-on :flush nil :size 100)
- (send ref :draw-on :flush nil :color #f(1 0 0)))
- (append (list (let ((ac (send (car (send *robot* :links)) :get :c-til)))
- (setf (elt ac 2) 0) ac))
- (send-all move-target :worldpos))
- (append (list cog-target-pos) target-coords))
- (send *irtviewer* :flush)
- )))
- ))
- )))
+ (rotation-axis (list nil t t))
+ (fp (apply #'midpoint 0.5 (send-all fix-leg-coords :worldpos))))
+ (do-until-key
+ (send crank :rotate (deg2rad 15) :z)
+ (let* ((target-coords (append (list (send crank :get :handle)) fix-leg-coords)))
+ (send *robot* :fullbody-inverse-kinematics target-coords
+ :move-target move-target :link-list link-list
+ :rotation-axis rotation-axis :thre thre
+ :look-at-target t :centroid-thre 10.0
+ :debug-view :no-flush)
+ ;; draw
+ (send *irtviewer* :draw-objects :flush nil)
+ (mapcar #'(lambda (act ref)
+ (send act :draw-on :flush nil :size 100)
+ (send ref :draw-on :flush nil :color #f(1 0 0)))
+ (append (list (let ((ac (send (car (send *robot* :links)) :get :c-til)))
+ (setf (elt ac 2) 0) ac))
+ (send-all move-target :worldpos))
+ (append (list cog-target-pos) target-coords))
+ (send *irtviewer* :flush)
+ ))
+ ))))
(unless (boundp '*irtviewer*) (make-irtviewer))
(warn "(crank-motion) for fullbody motion~%")
Modified: trunk/irteus/irtrobot.l
===================================================================
--- trunk/irteus/irtrobot.l 2011-11-30 17:36:31 UTC (rev 732)
+++ trunk/irteus/irtrobot.l 2011-12-01 06:39:51 UTC (rev 733)
@@ -313,6 +313,33 @@
(coerce torque-vector cons))
(if flush (send vwer :viewsurface :flush))
);; draw-torque
+ (:fullbody-inverse-kinematics
+ (target-coords
+ &rest args
+ &key (move-target) (link-list)
+ ;; default robot root-link 6dof parameters : min, max, root-link-virtual-joint-weight, and weight
+ (min (float-vector -500 -500 -500 -20 -20 -10))
+ (max (float-vector 500 500 25 20 20 10))
+ (root-link-virtual-joint-weight #f(0.1 0.1 0.1 0.1 0.5 0.5))
+ (weight
+ #'(lambda (ul)
+ (let ((weight (fill (instantiate float-vector (send self :calc-target-joint-dimension ul)) 1.0)))
+ (dotimes (i 6) (setf (elt weight i) (elt root-link-virtual-joint-weight i)))
+ weight)))
+ ;; default cog-jacobian parameters : target-centroid-pos, cog-gain, and centroid-thre
+ (target-centroid-pos (apply #'midpoint 0.5 (send self :legs :end-coords :worldpos)))
+ (cog-gain 1.0) (centroid-thre 5.0) ;; [mm]
+ &allow-other-keys)
+ (with-append-root-joint ;; inverse-kinematics with base-link
+ (link-list-with-robot-6dof self link-list
+ :joint-class 6dof-joint
+ :joint-args (list :min min :max max))
+ (send* self :inverse-kinematics target-coords
+ :move-target move-target :link-list link-list-with-robot-6dof
+ :cog-gain cog-gain :centroid-thre centroid-thre :target-centroid-pos target-centroid-pos
+ :weight weight
+ args)
+ ))
)
(in-package "GEOMETRY")
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2011-11-30 17:36:37
|
Revision: 732
http://jskeus.svn.sourceforge.net/jskeus/?rev=732&view=rev
Author: snozawa
Date: 2011-11-30 17:36:31 +0000 (Wed, 30 Nov 2011)
Log Message:
-----------
fix typoes ;; cog-thre -> centroid-thre
Modified Paths:
--------------
trunk/irteus/demo/crank-motion.l
trunk/irteus/irtmodel.l
Modified: trunk/irteus/demo/crank-motion.l
===================================================================
--- trunk/irteus/demo/crank-motion.l 2011-11-30 17:34:44 UTC (rev 731)
+++ trunk/irteus/demo/crank-motion.l 2011-11-30 17:36:31 UTC (rev 732)
@@ -67,7 +67,7 @@
:link-list ll :move-target move-target
:look-at-target t :thre thre :rotation-axis rotation-axis
:weight weight
- :cog-gain 1.0 :target-centroid-pos fp :cog-thre 10.0
+ :cog-gain 1.0 :target-centroid-pos fp :centroid-thre 10.0
:debug-view :no-flush)
;; draw
(send *irtviewer* :draw-objects :flush nil)
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2011-11-30 17:34:44 UTC (rev 731)
+++ trunk/irteus/irtmodel.l 2011-11-30 17:36:31 UTC (rev 732)
@@ -1537,7 +1537,7 @@
target-coords ;required for debug-view
(jacobi)
(additional-check) ;; to add optional convergence conditions
- (cog-thre 1.0) (target-centroid-pos) (centroid-offset-func)
+ (centroid-thre 1.0) (target-centroid-pos) (centroid-offset-func)
debug-view ik-args
&allow-other-keys)
;; dif-pos, dif-rot, move-target, rotation-axis, translation-axis, link-list
@@ -1597,7 +1597,7 @@
(send self :ik-convergence-check
(>= loop (/ stop 10)) dif-pos dif-rot
rotation-axis translation-axis thre rthre
- cog-thre target-centroid-pos centroid-offset-func))
+ centroid-thre target-centroid-pos centroid-offset-func))
(if (and additional-check success)
(setq success (and success (funcall additional-check))))
@@ -1667,7 +1667,7 @@
((atom move-target) (deg2rad 1))
(t (make-list (length move-target) :initial-element (deg2rad 1)))))
(union-link-list)
- (cog-thre 1.0) (target-centroid-pos) (centroid-offset-func)
+ (centroid-thre 1.0) (target-centroid-pos) (centroid-offset-func)
&allow-other-keys)
;; target-coords, move-target, rotation-axis, translation-axis, link-list
;; -> both list and atom OK.
@@ -1759,7 +1759,7 @@
(send self :ik-convergence-check
success dif-pos dif-rot
rotation-axis translation-axis thre rthre
- cog-thre target-centroid-pos centroid-offset-func)))
+ centroid-thre target-centroid-pos centroid-offset-func)))
;; update difference
(mapcar #'(lambda (l a) (send l :analysis-level a)) union-link-list old-analysis-level)
;; check solved or not
@@ -1773,7 +1773,7 @@
(warn ";; dif-rot : ~a/(~a/~a)~%" (elt dif-rot i) (norm (elt dif-rot i)) (elt rthre i)))
(if target-centroid-pos
(let ((dif-cog (send self :difference-cog-position target-centroid-pos centroid-offset-func)))
- (warn ";; cog-dif : ~a/(~a/~a)~%" dif-cog (norm dif-cog) cog-thre)))
+ (warn ";; cog-dif : ~a/(~a/~a)~%" dif-cog (norm dif-cog) centroid-thre)))
(warn ";; coords : ~a~%"
(send (let ((p self)) (while (send p :parent) (setq p (send p :parent))) p) :worldcoords))
(warn ";; angles : ~a~%" av0)
@@ -1788,13 +1788,13 @@
(:ik-convergence-check
(success dif-pos dif-rot
rotation-axis translation-axis thre rthre
- cog-thre target-centroid-pos centroid-offset-func)
+ centroid-thre target-centroid-pos centroid-offset-func)
(dotimes (i (length dif-pos))
(setq success (and success
(if (elt translation-axis i) (< (norm (elt dif-pos i)) (elt thre i)) t)
(if (elt rotation-axis i) (< (norm (elt dif-rot i)) (elt rthre i)) t))))
(if target-centroid-pos
- (setq success (and success (send self :cog-convergence-check cog-thre target-centroid-pos centroid-offset-func))))
+ (setq success (and success (send self :cog-convergence-check centroid-thre target-centroid-pos centroid-offset-func))))
success)
(:calc-vel-from-pos
(dif-pos translation-axis
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2011-11-30 17:34:50
|
Revision: 731
http://jskeus.svn.sourceforge.net/jskeus/?rev=731&view=rev
Author: snozawa
Date: 2011-11-30 17:34:44 +0000 (Wed, 30 Nov 2011)
Log Message:
-----------
add union-link-list for funcall argument in calc weight by lambda form
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2011-11-30 17:31:10 UTC (rev 730)
+++ trunk/irteus/irtmodel.l 2011-11-30 17:34:44 UTC (rev 731)
@@ -1214,7 +1214,7 @@
(tmp-weight (instantiate float-vector fik-len))
(tmp-len (instantiate float-vector fik-len)))
(cond
- ((functionp weight) (setq weight (funcall weight)))
+ ((functionp weight) (setq weight (funcall weight union-link-list)))
((listp weight) (setq weight (eval weight))))
(setq tmp-weight
(send self :calc-weight-from-joint-limit
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2011-11-30 17:31:17
|
Revision: 730
http://jskeus.svn.sourceforge.net/jskeus/?rev=730&view=rev
Author: snozawa
Date: 2011-11-30 17:31:10 +0000 (Wed, 30 Nov 2011)
Log Message:
-----------
add cog-convergence debug message if ik fail
Modified Paths:
--------------
trunk/irteus/irtdyna.l
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2011-11-30 16:42:44 UTC (rev 729)
+++ trunk/irteus/irtdyna.l 2011-11-30 17:31:10 UTC (rev 730)
@@ -433,14 +433,18 @@
translation-axis))
)
)
+ (:difference-cog-position
+ (target-centroid-pos &optional (centroid-offset-func) (translation-axis :z))
+ (calc-dif-with-axis
+ (v- target-centroid-pos
+ (if (functionp centroid-offset-func)
+ (funcall centroid-offset-func)
+ (send self :centroid)))
+ translation-axis))
(:cog-convergence-check
(centroid-thre target-centroid-pos &optional centroid-offset-func)
(> centroid-thre
- (norm
- (subseq (v- target-centroid-pos
- (if (functionp centroid-offset-func)
- (funcall centroid-offset-func)
- (send self :centroid))) 0 2))))
+ (norm (send self :difference-cog-position target-centroid-pos centroid-offset-func))))
)
;; recursive computation for inverse-dynamics
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2011-11-30 16:42:44 UTC (rev 729)
+++ trunk/irteus/irtmodel.l 2011-11-30 17:31:10 UTC (rev 730)
@@ -1771,6 +1771,9 @@
(dotimes (i (length move-target))
(warn ";; dif-pos : ~a/(~a/~a)~%" (elt dif-pos i) (norm (elt dif-pos i)) (elt thre i))
(warn ";; dif-rot : ~a/(~a/~a)~%" (elt dif-rot i) (norm (elt dif-rot i)) (elt rthre i)))
+ (if target-centroid-pos
+ (let ((dif-cog (send self :difference-cog-position target-centroid-pos centroid-offset-func)))
+ (warn ";; cog-dif : ~a/(~a/~a)~%" dif-cog (norm dif-cog) cog-thre)))
(warn ";; coords : ~a~%"
(send (let ((p self)) (while (send p :parent) (setq p (send p :parent))) p) :worldcoords))
(warn ";; angles : ~a~%" av0)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2011-11-30 16:42:51
|
Revision: 729
http://jskeus.svn.sourceforge.net/jskeus/?rev=729&view=rev
Author: snozawa
Date: 2011-11-30 16:42:44 +0000 (Wed, 30 Nov 2011)
Log Message:
-----------
move centroid convergence check codes to check centroid convergence at the ending of IK
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2011-11-30 16:35:09 UTC (rev 728)
+++ trunk/irteus/irtmodel.l 2011-11-30 16:42:44 UTC (rev 729)
@@ -1596,9 +1596,8 @@
(setq success
(send self :ik-convergence-check
(>= loop (/ stop 10)) dif-pos dif-rot
- rotation-axis translation-axis thre rthre))
- (if target-centroid-pos
- (setq success (and success (send self :cog-convergence-check cog-thre target-centroid-pos centroid-offset-func))))
+ rotation-axis translation-axis thre rthre
+ cog-thre target-centroid-pos centroid-offset-func))
(if (and additional-check success)
(setq success (and success (funcall additional-check))))
@@ -1668,6 +1667,7 @@
((atom move-target) (deg2rad 1))
(t (make-list (length move-target) :initial-element (deg2rad 1)))))
(union-link-list)
+ (cog-thre 1.0) (target-centroid-pos) (centroid-offset-func)
&allow-other-keys)
;; target-coords, move-target, rotation-axis, translation-axis, link-list
;; -> both list and atom OK.
@@ -1758,7 +1758,8 @@
success
(send self :ik-convergence-check
success dif-pos dif-rot
- rotation-axis translation-axis thre rthre)))
+ rotation-axis translation-axis thre rthre
+ cog-thre target-centroid-pos centroid-offset-func)))
;; update difference
(mapcar #'(lambda (l a) (send l :analysis-level a)) union-link-list old-analysis-level)
;; check solved or not
@@ -1783,11 +1784,14 @@
))
(:ik-convergence-check
(success dif-pos dif-rot
- rotation-axis translation-axis thre rthre)
+ rotation-axis translation-axis thre rthre
+ cog-thre target-centroid-pos centroid-offset-func)
(dotimes (i (length dif-pos))
(setq success (and success
(if (elt translation-axis i) (< (norm (elt dif-pos i)) (elt thre i)) t)
(if (elt rotation-axis i) (< (norm (elt dif-rot i)) (elt rthre i)) t))))
+ (if target-centroid-pos
+ (setq success (and success (send self :cog-convergence-check cog-thre target-centroid-pos centroid-offset-func))))
success)
(:calc-vel-from-pos
(dif-pos translation-axis
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2011-11-30 16:35:18
|
Revision: 728
http://jskeus.svn.sourceforge.net/jskeus/?rev=728&view=rev
Author: snozawa
Date: 2011-11-30 16:35:09 +0000 (Wed, 30 Nov 2011)
Log Message:
-----------
add :ik-convergence-check and remove duplicated codes
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2011-11-30 16:13:45 UTC (rev 727)
+++ trunk/irteus/irtmodel.l 2011-11-30 16:35:09 UTC (rev 728)
@@ -1591,34 +1591,32 @@
(when (and debug-view (not (memq :no-message debug-view)))
(warn "loop: ~3d~%" loop)
(warn "union-link-list: ~A~%" (send-all union-link-list :name)))
- (setq success (>= loop (/ stop 10)))
+
+ ;; convergence check
+ (setq success
+ (send self :ik-convergence-check
+ (>= loop (/ stop 10)) dif-pos dif-rot
+ rotation-axis translation-axis thre rthre))
+ (if target-centroid-pos
+ (setq success (and success (send self :cog-convergence-check cog-thre target-centroid-pos centroid-offset-func))))
+ (if (and additional-check success)
+ (setq success (and success (funcall additional-check))))
+
+ ;; calculation of move-coords velocities from vel-p and vel-r
(dotimes (i (length move-target))
- (let* ((move-target (elt move-target i))
- (dif-pos (elt dif-pos i))
- (dif-rot (elt dif-rot i))
- (rotation-axis (elt rotation-axis i))
- (translation-axis (elt translation-axis i))
- (thre (elt thre i)) (rthre (elt rthre i))
- (tmp-dim (elt tmp-dims i))
- (vel-p (send* self :calc-vel-from-pos dif-pos translation-axis
+ (let* ((tmp-dim (elt tmp-dims i))
+ (vel-p (send* self :calc-vel-from-pos (elt dif-pos i) (elt translation-axis i)
(if p-limit (list :p-limit p-limit))))
- (vel-r (send* self :calc-vel-from-rot dif-rot rotation-axis
+ (vel-r (send* self :calc-vel-from-rot (elt dif-rot i) (elt rotation-axis i)
(if r-limit (list :r-limit r-limit)))))
(when (and debug-view (not (memq :no-message debug-view)))
(format-array (scale 1000.0 vel-p) "vel-pos :")
(format-array vel-r "vel-rot :")
(warn "vel-pos-norm : ~7,3f/~7,3f~%vel-rot-norm : ~7,3f/~7,3f~%"
- (* 1000.0 (norm vel-p)) thre (norm vel-r) rthre))
- (setq success (and success
- (if translation-axis (< (norm dif-pos) thre) t)
- (if rotation-axis (< (norm dif-rot) rthre) t)))
+ (* 1000.0 (norm vel-p)) (elt thre i) (norm vel-r) (elt rthre i)))
(dotimes (j (length vel-p)) (setf (elt tmp-dim j) (elt vel-p j)))
(dotimes (j (length vel-r)) (setf (elt tmp-dim (+ j (length vel-p))) (elt vel-r j)))
))
- (if target-centroid-pos
- (setq success (and success (send self :cog-convergence-check cog-thre target-centroid-pos centroid-offset-func))))
- (if (and additional-check success)
- (setq success (and success (funcall additional-check))))
(dotimes (i (length tmp-dims))
(dotimes (j (length (elt tmp-dims i)))
(setf (elt tmp-dim (+ j vec-count)) (elt (elt tmp-dims i) j)))
@@ -1747,21 +1745,21 @@
(when (eq success :ik-succeed)
(setq success t) (return nil))
))
+ (let* ((target-coords
+ (mapcar #'(lambda (x)
+ (if (functionp x) (funcall x) x))
+ target-coords)))
+ (setq dif-pos (mapcar #'(lambda (mt tc ta)
+ (send mt :difference-position tc :translation-axis ta))
+ move-target target-coords translation-axis)
+ dif-rot (mapcar #'(lambda (mt tc ra)
+ (send mt :difference-rotation tc :rotation-axis ra))
+ move-target target-coords rotation-axis)
+ success
+ (send self :ik-convergence-check
+ success dif-pos dif-rot
+ rotation-axis translation-axis thre rthre)))
;; update difference
- (dotimes (i (length move-target))
- (let ((move-target (elt move-target i))
- (target-coords (if (functionp (elt target-coords i))
- (funcall (elt target-coords i))
- (elt target-coords i)))
- (rotation-axis (elt rotation-axis i))
- (translation-axis (elt translation-axis i))
- (thre (elt thre i)) (rthre (elt rthre i)))
- (setq dif-pos (send move-target :difference-position target-coords :translation-axis translation-axis)
- dif-rot (send move-target :difference-rotation target-coords :rotation-axis rotation-axis))
- (setq success (and success
- (if translation-axis (< (norm dif-pos) thre) t)
- (if rotation-axis (< (norm dif-rot) rthre) t)))
- ))
(mapcar #'(lambda (l a) (send l :analysis-level a)) union-link-list old-analysis-level)
;; check solved or not
(if (or success (not revert-if-fail))
@@ -1770,18 +1768,8 @@
(when warnp
(warn ";; inverse-kinematics failed.~%")
(dotimes (i (length move-target))
- (let ((move-target (elt move-target i))
- (target-coords (if (functionp (elt target-coords i))
- (funcall (elt target-coords i))
- (elt target-coords i)))
- (rotation-axis (elt rotation-axis i))
- (translation-axis (elt translation-axis i))
- (thre (elt thre i)) (rthre (elt rthre i)))
- (setq dif-pos (send move-target :difference-position target-coords :translation-axis translation-axis)
- dif-rot (send move-target :difference-rotation target-coords :rotation-axis rotation-axis))
- (warn ";; dif-pos : ~a/(~a/~a)~%" dif-pos (norm dif-pos) thre)
- (warn ";; dif-rot : ~a/(~a/~a)~%" dif-rot (norm dif-rot) rthre)
- ))
+ (warn ";; dif-pos : ~a/(~a/~a)~%" (elt dif-pos i) (norm (elt dif-pos i)) (elt thre i))
+ (warn ";; dif-rot : ~a/(~a/~a)~%" (elt dif-rot i) (norm (elt dif-rot i)) (elt rthre i)))
(warn ";; coords : ~a~%"
(send (let ((p self)) (while (send p :parent) (setq p (send p :parent))) p) :worldcoords))
(warn ";; angles : ~a~%" av0)
@@ -1793,6 +1781,14 @@
(if c0 (send self :newcoords c0))
nil))
))
+ (:ik-convergence-check
+ (success dif-pos dif-rot
+ rotation-axis translation-axis thre rthre)
+ (dotimes (i (length dif-pos))
+ (setq success (and success
+ (if (elt translation-axis i) (< (norm (elt dif-pos i)) (elt thre i)) t)
+ (if (elt rotation-axis i) (< (norm (elt dif-rot i)) (elt rthre i)) t))))
+ success)
(:calc-vel-from-pos
(dif-pos translation-axis
&rest args
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2011-11-30 16:13:51
|
Revision: 727
http://jskeus.svn.sourceforge.net/jskeus/?rev=727&view=rev
Author: snozawa
Date: 2011-11-30 16:13:45 +0000 (Wed, 30 Nov 2011)
Log Message:
-----------
extract loop count check from convergence check dotimes loop
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2011-11-30 14:12:37 UTC (rev 726)
+++ trunk/irteus/irtmodel.l 2011-11-30 16:13:45 UTC (rev 727)
@@ -1591,6 +1591,7 @@
(when (and debug-view (not (memq :no-message debug-view)))
(warn "loop: ~3d~%" loop)
(warn "union-link-list: ~A~%" (send-all union-link-list :name)))
+ (setq success (>= loop (/ stop 10)))
(dotimes (i (length move-target))
(let* ((move-target (elt move-target i))
(dif-pos (elt dif-pos i))
@@ -1609,7 +1610,6 @@
(warn "vel-pos-norm : ~7,3f/~7,3f~%vel-rot-norm : ~7,3f/~7,3f~%"
(* 1000.0 (norm vel-p)) thre (norm vel-r) rthre))
(setq success (and success
- (>= loop (/ stop 10))
(if translation-axis (< (norm dif-pos) thre) t)
(if rotation-axis (< (norm dif-rot) rthre) t)))
(dotimes (j (length vel-p)) (setf (elt tmp-dim j) (elt vel-p j)))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2011-11-30 14:12:46
|
Revision: 726
http://jskeus.svn.sourceforge.net/jskeus/?rev=726&view=rev
Author: snozawa
Date: 2011-11-30 14:12:37 +0000 (Wed, 30 Nov 2011)
Log Message:
-----------
add centroid-offset-func ;; this can be used for balancing against simulation external force
Modified Paths:
--------------
trunk/irteus/irtdyna.l
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2011-11-30 13:15:13 UTC (rev 725)
+++ trunk/irteus/irtdyna.l 2011-11-30 14:12:37 UTC (rev 726)
@@ -416,6 +416,7 @@
&key (cog-gain 1.0)
(translation-axis :z)
(target-centroid-pos (send (car (send self :links)) :get :c-til))
+ (centroid-offset-func)
&allow-other-keys)
(transform
(send* self :calc-inverse-jacobian
@@ -424,15 +425,22 @@
:translation-axis translation-axis args)
args)
(scale (* 0.001 cog-gain) ;; [mm] -> [m]
- (calc-dif-with-axis (v- target-centroid-pos (send (car (send self :links)) :get :c-til))
- translation-axis))
+ (calc-dif-with-axis
+ (v- target-centroid-pos
+ (if (functionp centroid-offset-func)
+ (funcall centroid-offset-func)
+ (send (car (send self :links)) :get :c-til)))
+ translation-axis))
)
)
(:cog-convergence-check
- (centroid-thre target-centroid-pos)
+ (centroid-thre target-centroid-pos &optional centroid-offset-func)
(> centroid-thre
(norm
- (subseq (v- target-centroid-pos (send self :centroid)) 0 2))))
+ (subseq (v- target-centroid-pos
+ (if (functionp centroid-offset-func)
+ (funcall centroid-offset-func)
+ (send self :centroid))) 0 2))))
)
;; recursive computation for inverse-dynamics
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2011-11-30 13:15:13 UTC (rev 725)
+++ trunk/irteus/irtmodel.l 2011-11-30 14:12:37 UTC (rev 726)
@@ -1252,7 +1252,7 @@
(fik-len (send self :calc-target-joint-dimension union-link-list))
(null-space) (debug-view)
;; if user wants to use cog-jacobian as null-space, please set cog-gain(> 0.0) and target-centroid-pos
- (cog-gain 0.0) (target-centroid-pos)
+ (cog-gain 0.0) (target-centroid-pos) (centroid-offset-func)
(weight (fill (instantiate float-vector fik-len) 1.0))
(tmp-nspace (instantiate float-vector fik-len)))
;; calc null-space from joint-limit
@@ -1263,7 +1263,7 @@
(if (and (> cog-gain 0.0) target-centroid-pos)
(setq tmp-nspace
(v+ (send self :cog-jacobian-balance-nspace union-link-list
- :weight weight
+ :weight weight :centroid-offset-func centroid-offset-func
:target-centroid-pos target-centroid-pos :cog-gain cog-gain)
tmp-nspace tmp-nspace)))
;; add null-space from arguments
@@ -1288,7 +1288,7 @@
(avoid-collision-joint-gain 1.0)
((:collision-avoidance-link-pair pair-list)
(send self :collision-avoidance-link-pair-from-link-list link-list :obstacles (cadr (memq :obstacles args)) :debug (cadr (memq :debug-view args))))
- (cog-gain 0.0) (target-centroid-pos)
+ (cog-gain 0.0) (target-centroid-pos) (centroid-offset-func)
(tmp-len (instantiate float-vector fik-len))
(tmp-len2 (instantiate float-vector fik-len))
(tmp-weight (instantiate float-vector fik-len))
@@ -1383,7 +1383,7 @@
(send self :calc-inverse-kinematics-nspace-from-link-list
link-list :union-link-list union-link-list
:avoid-nspace-gain avoid-nspace-gain :debug-view debug-view
- :cog-gain cog-gain :target-centroid-pos target-centroid-pos
+ :cog-gain cog-gain :target-centroid-pos target-centroid-pos :centroid-offset-func centroid-offset-func
:weight weight :fik-len fik-len :null-space null-space :tmp-nspace tmp-nspace))
(if (send self :get :collision-avoidance-null-vector)
(v+ tmp-nspace (send self :get :collision-avoidance-null-vector) tmp-nspace))
@@ -1537,7 +1537,7 @@
target-coords ;required for debug-view
(jacobi)
(additional-check) ;; to add optional convergence conditions
- (cog-thre 1.0) (target-centroid-pos)
+ (cog-thre 1.0) (target-centroid-pos) (centroid-offset-func)
debug-view ik-args
&allow-other-keys)
;; dif-pos, dif-rot, move-target, rotation-axis, translation-axis, link-list
@@ -1616,7 +1616,7 @@
(dotimes (j (length vel-r)) (setf (elt tmp-dim (+ j (length vel-p))) (elt vel-r j)))
))
(if target-centroid-pos
- (setq success (and success (send self :cog-convergence-check cog-thre target-centroid-pos))))
+ (setq success (and success (send self :cog-convergence-check cog-thre target-centroid-pos centroid-offset-func))))
(if (and additional-check success)
(setq success (and success (funcall additional-check))))
(dotimes (i (length tmp-dims))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2011-11-30 13:15:19
|
Revision: 725
http://jskeus.svn.sourceforge.net/jskeus/?rev=725&view=rev
Author: snozawa
Date: 2011-11-30 13:15:13 +0000 (Wed, 30 Nov 2011)
Log Message:
-----------
just fix arrangement of local variables and indents
Modified Paths:
--------------
trunk/irteus/demo/crank-motion.l
Modified: trunk/irteus/demo/crank-motion.l
===================================================================
--- trunk/irteus/demo/crank-motion.l 2011-11-30 12:57:08 UTC (rev 724)
+++ trunk/irteus/demo/crank-motion.l 2011-11-30 13:15:13 UTC (rev 725)
@@ -32,57 +32,56 @@
(send (send b1 :copy-worldcoords) :translate (float-vector 0 0 50))))
(send br :assoc (send br :get :handle))
br))))
- (let* ((cog-target-pos
- (if (some #'null (send *robot* :legs))
- (send (car (send *robot* :links)) :worldpos)
- (apply #'midpoint 0.5 (send *robot* :legs :end-coords :worldpos))))
- (fix-leg-coords
- (unless (some #'null (send *robot* :legs))
- (send *robot* :legs :end-coords :copy-worldcoords)))
- (crank (make-crank))
- ;; append legs' parameters for move-target, link-list, thre, rotation-axis, and target-coords
- ;; all parameter list -> (list larm rleg lleg)
- (move-target (append (list (send *robot* :larm :end-coords))
- (send *robot* :legs :end-coords)))
- (link-list (mapcar #'(lambda (l) (send *robot* :link-list (send l :parent)))
- move-target))
- (thre (list 15 1 1))
- (rotation-axis (list nil t t)))
- (send crank :locate #f(350 100 500) :world)
- (objects (list crank *robot*))
- ;;
- (with-append-root-joint
- (ll *robot* link-list
- :joint-class 6dof-joint
- :joint-args
- (list :min (float-vector -300 -300 -25 -10 -15 -15)
- :max (float-vector 300 300 25 10 15 15)))
- (let ((weight (fill (instantiate float-vector (send *robot* :calc-target-joint-dimension ll)) 1.0))
- (6dof-weight (float-vector 0.1 0.1 0.1 0.1 0.5 0.5))
- (fp (apply #'midpoint 0.5 (send-all fix-leg-coords :worldpos))))
- (dotimes (i 6) (setf (elt weight i) (elt 6dof-weight i)))
- (do-until-key
- (send crank :rotate (deg2rad 15) :z)
- (let* ((target-coords (append (list (send crank :get :handle)) fix-leg-coords)))
- (send *robot* :inverse-kinematics target-coords
- :link-list ll :move-target move-target
- :look-at-target t :thre thre :rotation-axis rotation-axis
- :weight weight
- :cog-gain 1.0 :target-centroid-pos fp :cog-thre 10.0
- :debug-view :no-flush)
- ;; draw
- (send *irtviewer* :draw-objects :flush nil)
- (mapcar #'(lambda (act ref)
- (send act :draw-on :flush nil :size 100)
- (send ref :draw-on :flush nil :color #f(1 0 0)))
- (append (list (let ((ac (send (car (send *robot* :links)) :get :c-til)))
- (setf (elt ac 2) 0) ac))
- (send-all move-target :worldpos))
- (append (list cog-target-pos) target-coords))
- (send *irtviewer* :flush)
- )))
- ))
- ))
+ (let ((crank (make-crank)))
+ (send crank :locate #f(350 100 500) :world)
+ (objects (list crank *robot*))
+ (let* ((cog-target-pos
+ (if (some #'null (send *robot* :legs))
+ (send (car (send *robot* :links)) :worldpos)
+ (apply #'midpoint 0.5 (send *robot* :legs :end-coords :worldpos))))
+ (fix-leg-coords
+ (unless (some #'null (send *robot* :legs))
+ (send *robot* :legs :end-coords :copy-worldcoords)))
+ ;; append legs' parameters for move-target, link-list, thre, rotation-axis, and target-coords
+ ;; all parameter list -> (list larm rleg lleg)
+ (move-target (append (list (send *robot* :larm :end-coords))
+ (send *robot* :legs :end-coords)))
+ (link-list (mapcar #'(lambda (l) (send *robot* :link-list (send l :parent)))
+ move-target))
+ (thre (list 15 1 1))
+ (rotation-axis (list nil t t)))
+ (with-append-root-joint
+ (ll *robot* link-list
+ :joint-class 6dof-joint
+ :joint-args
+ (list :min (float-vector -300 -300 -25 -10 -15 -15)
+ :max (float-vector 300 300 25 10 15 15)))
+ (let ((weight (fill (instantiate float-vector (send *robot* :calc-target-joint-dimension ll)) 1.0))
+ (6dof-weight (float-vector 0.1 0.1 0.1 0.1 0.5 0.5))
+ (fp (apply #'midpoint 0.5 (send-all fix-leg-coords :worldpos))))
+ (dotimes (i 6) (setf (elt weight i) (elt 6dof-weight i)))
+ (do-until-key
+ (send crank :rotate (deg2rad 15) :z)
+ (let* ((target-coords (append (list (send crank :get :handle)) fix-leg-coords)))
+ (send *robot* :inverse-kinematics target-coords
+ :link-list ll :move-target move-target
+ :look-at-target t :thre thre :rotation-axis rotation-axis
+ :weight weight
+ :cog-gain 1.0 :target-centroid-pos fp :cog-thre 10.0
+ :debug-view :no-flush)
+ ;; draw
+ (send *irtviewer* :draw-objects :flush nil)
+ (mapcar #'(lambda (act ref)
+ (send act :draw-on :flush nil :size 100)
+ (send ref :draw-on :flush nil :color #f(1 0 0)))
+ (append (list (let ((ac (send (car (send *robot* :links)) :get :c-til)))
+ (setf (elt ac 2) 0) ac))
+ (send-all move-target :worldpos))
+ (append (list cog-target-pos) target-coords))
+ (send *irtviewer* :flush)
+ )))
+ ))
+ )))
(unless (boundp '*irtviewer*) (make-irtviewer))
(warn "(crank-motion) for fullbody motion~%")
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2011-11-30 12:57:14
|
Revision: 724
http://jskeus.svn.sourceforge.net/jskeus/?rev=724&view=rev
Author: snozawa
Date: 2011-11-30 12:57:08 +0000 (Wed, 30 Nov 2011)
Log Message:
-----------
fix typo ;; cog-target-pos -> target-centroid-pos ;; add additional check for target-centroid-pos
Modified Paths:
--------------
trunk/irteus/demo/crank-motion.l
trunk/irteus/irtdyna.l
trunk/irteus/irtmodel.l
Modified: trunk/irteus/demo/crank-motion.l
===================================================================
--- trunk/irteus/demo/crank-motion.l 2011-11-30 12:37:37 UTC (rev 723)
+++ trunk/irteus/demo/crank-motion.l 2011-11-30 12:57:08 UTC (rev 724)
@@ -68,12 +68,7 @@
:link-list ll :move-target move-target
:look-at-target t :thre thre :rotation-axis rotation-axis
:weight weight
- :cog-gain 1.0 :cog-target-pos fp
- :additional-check
- #'(lambda ()
- (> 10.0
- (norm
- (subseq (v- fp (send (car (send *robot* :links)) :get :c-til)) 0 2))))
+ :cog-gain 1.0 :target-centroid-pos fp :cog-thre 10.0
:debug-view :no-flush)
;; draw
(send *irtviewer* :draw-objects :flush nil)
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2011-11-30 12:37:37 UTC (rev 723)
+++ trunk/irteus/irtdyna.l 2011-11-30 12:57:08 UTC (rev 724)
@@ -428,6 +428,11 @@
translation-axis))
)
)
+ (:cog-convergence-check
+ (centroid-thre target-centroid-pos)
+ (> centroid-thre
+ (norm
+ (subseq (v- target-centroid-pos (send self :centroid)) 0 2))))
)
;; recursive computation for inverse-dynamics
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2011-11-30 12:37:37 UTC (rev 723)
+++ trunk/irteus/irtmodel.l 2011-11-30 12:57:08 UTC (rev 724)
@@ -1251,8 +1251,8 @@
(union-link-list (send self :calc-union-link-list link-list))
(fik-len (send self :calc-target-joint-dimension union-link-list))
(null-space) (debug-view)
- ;; if user wants to use cog-jacobian as null-space, please set cog-gain(> 0.0) and cog-target-pos
- (cog-gain 0.0) (cog-target-pos)
+ ;; if user wants to use cog-jacobian as null-space, please set cog-gain(> 0.0) and target-centroid-pos
+ (cog-gain 0.0) (target-centroid-pos)
(weight (fill (instantiate float-vector fik-len) 1.0))
(tmp-nspace (instantiate float-vector fik-len)))
;; calc null-space from joint-limit
@@ -1260,11 +1260,11 @@
(send self :calc-nspace-from-joint-limit
avoid-nspace-gain union-link-list weight debug-view tmp-nspace))
;; add null-space from cog-jacobian
- (if (and (> cog-gain 0.0) cog-target-pos)
+ (if (and (> cog-gain 0.0) target-centroid-pos)
(setq tmp-nspace
(v+ (send self :cog-jacobian-balance-nspace union-link-list
:weight weight
- :target-centroid-pos cog-target-pos :cog-gain cog-gain)
+ :target-centroid-pos target-centroid-pos :cog-gain cog-gain)
tmp-nspace tmp-nspace)))
;; add null-space from arguments
(cond
@@ -1288,7 +1288,7 @@
(avoid-collision-joint-gain 1.0)
((:collision-avoidance-link-pair pair-list)
(send self :collision-avoidance-link-pair-from-link-list link-list :obstacles (cadr (memq :obstacles args)) :debug (cadr (memq :debug-view args))))
- (cog-gain 0.0) (cog-target-pos)
+ (cog-gain 0.0) (target-centroid-pos)
(tmp-len (instantiate float-vector fik-len))
(tmp-len2 (instantiate float-vector fik-len))
(tmp-weight (instantiate float-vector fik-len))
@@ -1383,7 +1383,7 @@
(send self :calc-inverse-kinematics-nspace-from-link-list
link-list :union-link-list union-link-list
:avoid-nspace-gain avoid-nspace-gain :debug-view debug-view
- :cog-gain cog-gain :cog-target-pos cog-target-pos
+ :cog-gain cog-gain :target-centroid-pos target-centroid-pos
:weight weight :fik-len fik-len :null-space null-space :tmp-nspace tmp-nspace))
(if (send self :get :collision-avoidance-null-vector)
(v+ tmp-nspace (send self :get :collision-avoidance-null-vector) tmp-nspace))
@@ -1537,6 +1537,7 @@
target-coords ;required for debug-view
(jacobi)
(additional-check) ;; to add optional convergence conditions
+ (cog-thre 1.0) (target-centroid-pos)
debug-view ik-args
&allow-other-keys)
;; dif-pos, dif-rot, move-target, rotation-axis, translation-axis, link-list
@@ -1614,6 +1615,8 @@
(dotimes (j (length vel-p)) (setf (elt tmp-dim j) (elt vel-p j)))
(dotimes (j (length vel-r)) (setf (elt tmp-dim (+ j (length vel-p))) (elt vel-r j)))
))
+ (if target-centroid-pos
+ (setq success (and success (send self :cog-convergence-check cog-thre target-centroid-pos))))
(if (and additional-check success)
(setq success (and success (funcall additional-check))))
(dotimes (i (length tmp-dims))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2011-11-30 12:37:43
|
Revision: 723
http://jskeus.svn.sourceforge.net/jskeus/?rev=723&view=rev
Author: snozawa
Date: 2011-11-30 12:37:37 +0000 (Wed, 30 Nov 2011)
Log Message:
-----------
add comments to use cog-jacobian in :calc-inverse-kinematics-nspace-from-link-list
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2011-11-30 12:28:56 UTC (rev 722)
+++ trunk/irteus/irtmodel.l 2011-11-30 12:37:37 UTC (rev 723)
@@ -1251,6 +1251,7 @@
(union-link-list (send self :calc-union-link-list link-list))
(fik-len (send self :calc-target-joint-dimension union-link-list))
(null-space) (debug-view)
+ ;; if user wants to use cog-jacobian as null-space, please set cog-gain(> 0.0) and cog-target-pos
(cog-gain 0.0) (cog-target-pos)
(weight (fill (instantiate float-vector fik-len) 1.0))
(tmp-nspace (instantiate float-vector fik-len)))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2011-11-30 12:29:02
|
Revision: 722
http://jskeus.svn.sourceforge.net/jskeus/?rev=722&view=rev
Author: snozawa
Date: 2011-11-30 12:28:56 +0000 (Wed, 30 Nov 2011)
Log Message:
-----------
add cog-jacobian null-space to :calc-inverse-kinematics-nspace-from-link-list ;; cog-jacobian is not used by default for non-legged robots
Modified Paths:
--------------
trunk/irteus/demo/crank-motion.l
trunk/irteus/irtmodel.l
Modified: trunk/irteus/demo/crank-motion.l
===================================================================
--- trunk/irteus/demo/crank-motion.l 2011-11-30 10:53:37 UTC (rev 721)
+++ trunk/irteus/demo/crank-motion.l 2011-11-30 12:28:56 UTC (rev 722)
@@ -68,10 +68,7 @@
:link-list ll :move-target move-target
:look-at-target t :thre thre :rotation-axis rotation-axis
:weight weight
- :null-space ;; for balancing
- #'(lambda ()
- (send *robot* :cog-jacobian-balance-nspace (send *robot* :calc-union-link-list ll)
- :target-centroid-pos cog-target-pos :cog-gain 1.5))
+ :cog-gain 1.0 :cog-target-pos fp
:additional-check
#'(lambda ()
(> 10.0
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2011-11-30 10:53:37 UTC (rev 721)
+++ trunk/irteus/irtmodel.l 2011-11-30 12:28:56 UTC (rev 722)
@@ -1251,11 +1251,21 @@
(union-link-list (send self :calc-union-link-list link-list))
(fik-len (send self :calc-target-joint-dimension union-link-list))
(null-space) (debug-view)
+ (cog-gain 0.0) (cog-target-pos)
(weight (fill (instantiate float-vector fik-len) 1.0))
(tmp-nspace (instantiate float-vector fik-len)))
+ ;; calc null-space from joint-limit
(setq tmp-nspace
(send self :calc-nspace-from-joint-limit
avoid-nspace-gain union-link-list weight debug-view tmp-nspace))
+ ;; add null-space from cog-jacobian
+ (if (and (> cog-gain 0.0) cog-target-pos)
+ (setq tmp-nspace
+ (v+ (send self :cog-jacobian-balance-nspace union-link-list
+ :weight weight
+ :target-centroid-pos cog-target-pos :cog-gain cog-gain)
+ tmp-nspace tmp-nspace)))
+ ;; add null-space from arguments
(cond
((functionp null-space) (setq null-space (funcall null-space)))
((listp null-space) (setq null-space (eval null-space))))
@@ -1277,6 +1287,7 @@
(avoid-collision-joint-gain 1.0)
((:collision-avoidance-link-pair pair-list)
(send self :collision-avoidance-link-pair-from-link-list link-list :obstacles (cadr (memq :obstacles args)) :debug (cadr (memq :debug-view args))))
+ (cog-gain 0.0) (cog-target-pos)
(tmp-len (instantiate float-vector fik-len))
(tmp-len2 (instantiate float-vector fik-len))
(tmp-weight (instantiate float-vector fik-len))
@@ -1371,6 +1382,7 @@
(send self :calc-inverse-kinematics-nspace-from-link-list
link-list :union-link-list union-link-list
:avoid-nspace-gain avoid-nspace-gain :debug-view debug-view
+ :cog-gain cog-gain :cog-target-pos cog-target-pos
:weight weight :fik-len fik-len :null-space null-space :tmp-nspace tmp-nspace))
(if (send self :get :collision-avoidance-null-vector)
(v+ tmp-nspace (send self :get :collision-avoidance-null-vector) tmp-nspace))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2011-11-30 10:53:43
|
Revision: 721
http://jskeus.svn.sourceforge.net/jskeus/?rev=721&view=rev
Author: snozawa
Date: 2011-11-30 10:53:37 +0000 (Wed, 30 Nov 2011)
Log Message:
-----------
update crank-motion sample to use legs' constraint in fullbody :inverse-kinematics
Modified Paths:
--------------
trunk/irteus/demo/crank-motion.l
Modified: trunk/irteus/demo/crank-motion.l
===================================================================
--- trunk/irteus/demo/crank-motion.l 2011-11-25 10:26:48 UTC (rev 720)
+++ trunk/irteus/demo/crank-motion.l 2011-11-30 10:53:37 UTC (rev 721)
@@ -32,16 +32,22 @@
(send (send b1 :copy-worldcoords) :translate (float-vector 0 0 50))))
(send br :assoc (send br :get :handle))
br))))
- (let* ((move-target (send *robot* :larm :end-coords))
- (cog-target-pos
+ (let* ((cog-target-pos
(if (some #'null (send *robot* :legs))
(send (car (send *robot* :links)) :worldpos)
(apply #'midpoint 0.5 (send *robot* :legs :end-coords :worldpos))))
(fix-leg-coords
(unless (some #'null (send *robot* :legs))
(send *robot* :legs :end-coords :copy-worldcoords)))
- (link-list (send *robot* :link-list (send move-target :parent)))
- (crank (make-crank)))
+ (crank (make-crank))
+ ;; append legs' parameters for move-target, link-list, thre, rotation-axis, and target-coords
+ ;; all parameter list -> (list larm rleg lleg)
+ (move-target (append (list (send *robot* :larm :end-coords))
+ (send *robot* :legs :end-coords)))
+ (link-list (mapcar #'(lambda (l) (send *robot* :link-list (send l :parent)))
+ move-target))
+ (thre (list 15 1 1))
+ (rotation-axis (list nil t t)))
(send crank :locate #f(350 100 500) :world)
(objects (list crank *robot*))
;;
@@ -52,19 +58,19 @@
(list :min (float-vector -300 -300 -25 -10 -15 -15)
:max (float-vector 300 300 25 10 15 15)))
(let ((weight (fill (instantiate float-vector (send *robot* :calc-target-joint-dimension ll)) 1.0))
- (fp (apply #'midpoint 0.5 (send *robot* :legs :end-coords :worldpos))))
- (dotimes (i 3) (setf (elt weight i) i) 0.01)
+ (6dof-weight (float-vector 0.1 0.1 0.1 0.1 0.5 0.5))
+ (fp (apply #'midpoint 0.5 (send-all fix-leg-coords :worldpos))))
+ (dotimes (i 6) (setf (elt weight i) (elt 6dof-weight i)))
(do-until-key
(send crank :rotate (deg2rad 15) :z)
- (let* ((target (send crank :get :handle)))
- (send *robot* :inverse-kinematics target
- :link-list (car ll) :move-target move-target
- :look-at-target t :thre 15
- :rotation-axis nil :translation-axis t
+ (let* ((target-coords (append (list (send crank :get :handle)) fix-leg-coords)))
+ (send *robot* :inverse-kinematics target-coords
+ :link-list ll :move-target move-target
+ :look-at-target t :thre thre :rotation-axis rotation-axis
:weight weight
:null-space ;; for balancing
#'(lambda ()
- (send *robot* :cog-jacobian-balance-nspace (car ll)
+ (send *robot* :cog-jacobian-balance-nspace (send *robot* :calc-union-link-list ll)
:target-centroid-pos cog-target-pos :cog-gain 1.5))
:additional-check
#'(lambda ()
@@ -72,22 +78,15 @@
(norm
(subseq (v- fp (send (car (send *robot* :links)) :get :c-til)) 0 2))))
:debug-view :no-flush)
- ;; leg inverse kinematics
- (unless (some #'null (send *robot* :legs))
- (mapcar #'(lambda (l tc)
- (send *robot* :inverse-kinematics tc
- :move-target (send *robot* l :end-coords)
- :link-list (send *robot* l)))
- '(:lleg :rleg) fix-leg-coords))
;; draw
(send *irtviewer* :draw-objects :flush nil)
(mapcar #'(lambda (act ref)
(send act :draw-on :flush nil :size 100)
(send ref :draw-on :flush nil :color #f(1 0 0)))
- (list (let ((ac (send (car (send *robot* :links)) :get :c-til)))
- (setf (elt ac 2) 0) ac)
- (send move-target :worldpos))
- (list cog-target-pos target))
+ (append (list (let ((ac (send (car (send *robot* :links)) :get :c-til)))
+ (setf (elt ac 2) 0) ac))
+ (send-all move-target :worldpos))
+ (append (list cog-target-pos) target-coords))
(send *irtviewer* :flush)
)))
))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2011-11-25 10:26:54
|
Revision: 720
http://jskeus.svn.sourceforge.net/jskeus/?rev=720&view=rev
Author: snozawa
Date: 2011-11-25 10:26:48 +0000 (Fri, 25 Nov 2011)
Log Message:
-----------
just fix indent according to r718 commit
Revision Links:
--------------
http://jskeus.svn.sourceforge.net/jskeus/?rev=718&view=rev
Modified Paths:
--------------
trunk/irteus/irtrobot.l
Modified: trunk/irteus/irtrobot.l
===================================================================
--- trunk/irteus/irtrobot.l 2011-11-25 10:25:46 UTC (rev 719)
+++ trunk/irteus/irtrobot.l 2011-11-25 10:26:48 UTC (rev 720)
@@ -234,50 +234,50 @@
(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]
(let ((weight-force (* (send self :weight) 1e-6 (elt *g-vec* 2)))) ;; weight[g] * 1e-6 * gvec[mm/s^2] = force[N]
- ;; set default external force and moment at the end-effectors
- (if (every #'null (send self :legs)) ;; for not legged robot
- (let ((fv (float-vector 0 0 weight-force))) ;; for non-legged robot
- (send (car (send self :links)) :ext-force fv)
- (let* ((c (send (car (send self :links)) :centroid))
- (moment-offset (v* (scale 1e-3 (send (car (send self :links)) :worldpos)) fv)))
- (send (car (send self :links)) :ext-moment moment-offset)
- ))
- (progn ;; for legged robot
- (unless target-coords
- (dolist (limb '(:rleg :lleg))
- (push (send self limb :end-coords) target-coords)))
- (unless force-list ;; force [N]
- (let ((total-force/2 (* weight-force 0.5)))
- ;; hypothesis : lleg force = rleg force, lleg force + rleg force + gravity force = 0
+ ;; set default external force and moment at the end-effectors
+ (if (every #'null (send self :legs)) ;; for not legged robot
+ (let ((fv (float-vector 0 0 weight-force))) ;; for non-legged robot
+ (send (car (send self :links)) :ext-force fv)
+ (let* ((c (send (car (send self :links)) :centroid))
+ (moment-offset (v* (scale 1e-3 (send (car (send self :links)) :worldpos)) fv)))
+ (send (car (send self :links)) :ext-moment moment-offset)
+ ))
+ (progn ;; for legged robot
+ (unless target-coords
(dolist (limb '(:rleg :lleg))
- (push (float-vector 0.0 0.0 (case limb
- ((:rleg :lleg) total-force/2)
- (t 0.0))) force-list))
- ))
- (unless moment-list ;; moment [Nm]
- (let ((total-moment (v* (scale 1e-3
- (v- (send self :centroid nil)
- (apply #'midpoint 0.5 (send-all target-coords :worldpos))))
- (float-vector 0 0 weight-force))))
- ;; hypothesis : gravity force moment + rleg moment + lleg moment = 0, minimal internal moments <-> pseudo-inverse <-> lleg moment = rleg moment
- (dolist (limb '(:rleg :lleg))
- (push (scale 0.5 total-moment) 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~%"
- (length force-list) (length moment-list) (length target-coords))
- (return-from :torque-vector jvv))
- (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* ((c (send (send tc :parent) :centroid))
- (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)
- )
- ))
+ (push (send self limb :end-coords) target-coords)))
+ (unless force-list ;; force [N]
+ (let ((total-force/2 (* weight-force 0.5)))
+ ;; hypothesis : lleg force = rleg force, lleg force + rleg force + gravity force = 0
+ (dolist (limb '(:rleg :lleg))
+ (push (float-vector 0.0 0.0 (case limb
+ ((:rleg :lleg) total-force/2)
+ (t 0.0))) force-list))
+ ))
+ (unless moment-list ;; moment [Nm]
+ (let ((total-moment (v* (scale 1e-3
+ (v- (send self :centroid nil)
+ (apply #'midpoint 0.5 (send-all target-coords :worldpos))))
+ (float-vector 0 0 weight-force))))
+ ;; hypothesis : gravity force moment + rleg moment + lleg moment = 0, minimal internal moments <-> pseudo-inverse <-> lleg moment = rleg moment
+ (dolist (limb '(:rleg :lleg))
+ (push (scale 0.5 total-moment) 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~%"
+ (length force-list) (length moment-list) (length target-coords))
+ (return-from :torque-vector jvv))
+ (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* ((c (send (send tc :parent) :centroid))
+ (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-super :calc-torque
:debug-view debug-view
:jvv jvv :jav jav)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2011-11-25 10:25:52
|
Revision: 719
http://jskeus.svn.sourceforge.net/jskeus/?rev=719&view=rev
Author: snozawa
Date: 2011-11-25 10:25:46 +0000 (Fri, 25 Nov 2011)
Log Message:
-----------
use *g-vec* instead of using 9.8
Modified Paths:
--------------
trunk/irteus/irtrobot.l
Modified: trunk/irteus/irtrobot.l
===================================================================
--- trunk/irteus/irtrobot.l 2011-11-02 08:30:47 UTC (rev 718)
+++ trunk/irteus/irtrobot.l 2011-11-25 10:25:46 UTC (rev 719)
@@ -233,9 +233,10 @@
(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]
+ (let ((weight-force (* (send self :weight) 1e-6 (elt *g-vec* 2)))) ;; weight[g] * 1e-6 * gvec[mm/s^2] = force[N]
;; set default external force and moment at the end-effectors
(if (every #'null (send self :legs)) ;; for not legged robot
- (let ((fv (float-vector 0 0 (* (send self :weight) 0.0098)))) ;; for non-legged robot
+ (let ((fv (float-vector 0 0 weight-force))) ;; for non-legged robot
(send (car (send self :links)) :ext-force fv)
(let* ((c (send (car (send self :links)) :centroid))
(moment-offset (v* (scale 1e-3 (send (car (send self :links)) :worldpos)) fv)))
@@ -246,7 +247,7 @@
(dolist (limb '(:rleg :lleg))
(push (send self limb :end-coords) target-coords)))
(unless force-list ;; force [N]
- (let ((total-force/2 (* (send self :weight) 1e-3 9.8 0.5)))
+ (let ((total-force/2 (* weight-force 0.5)))
;; hypothesis : lleg force = rleg force, lleg force + rleg force + gravity force = 0
(dolist (limb '(:rleg :lleg))
(push (float-vector 0.0 0.0 (case limb
@@ -257,7 +258,7 @@
(let ((total-moment (v* (scale 1e-3
(v- (send self :centroid nil)
(apply #'midpoint 0.5 (send-all target-coords :worldpos))))
- (float-vector 0 0 (* 1e-3 (send self :weight nil) 9.8)))))
+ (float-vector 0 0 weight-force))))
;; hypothesis : gravity force moment + rleg moment + lleg moment = 0, minimal internal moments <-> pseudo-inverse <-> lleg moment = rleg moment
(dolist (limb '(:rleg :lleg))
(push (scale 0.5 total-moment) moment-list))
@@ -276,7 +277,7 @@
(send (send tc :parent) :ext-moment (v+ mv moment-offset))))
force-list moment-list target-coords)
)
- )
+ ))
(send-super :calc-torque
:debug-view debug-view
:jvv jvv :jav jav)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2011-11-02 08:30:53
|
Revision: 718
http://jskeus.svn.sourceforge.net/jskeus/?rev=718&view=rev
Author: snozawa
Date: 2011-11-02 08:30:47 +0000 (Wed, 02 Nov 2011)
Log Message:
-----------
fix bug when rotation angle between v and axis equals to 180[deg] ;; support :-x, :-y, and :-z for axis
Modified Paths:
--------------
trunk/irteus/irtgeo.l
Modified: trunk/irteus/irtgeo.l
===================================================================
--- trunk/irteus/irtgeo.l 2011-10-31 17:01:01 UTC (rev 717)
+++ trunk/irteus/irtgeo.l 2011-11-02 08:30:47 UTC (rev 718)
@@ -265,15 +265,30 @@
;; "v" must be non-zero vector.
(defun orient-coords-to-axis (target-coords v &optional (axis :z))
(let* ((nv (normalize-vector v))
- (ax (send target-coords :rotate-vector
+ (ax (send target-coords :rotate-vector ;; axis in the worldcoords
(case axis
- (:x #f(1 0 0))
- (:y #f(0 1 0))
- (:z #f(0 0 1))
+ (:x #f(1 0 0)) (:-x #f(-1 0 0))
+ (:y #f(0 1 0)) (:-y #f(0 -1 0))
+ (:z #f(0 0 1)) (:-z #f(0 0 -1))
(t axis))))
- (vcr (v* ax nv)))
- (if (not (eps= (norm vcr) 0.0)) ;; check vcr ;; if vcr is zero-vector, it is unnecessary to rotate target-coords.
- (send target-coords :rotate (acos (v. nv ax)) vcr :world))
+ (rot-axis (v* ax nv)) ;; axis to rotate ax -> nv
+ (rot-angle-cos (v. nv ax))) ;; angle to rotate ax -> nv
+ ;; check rot-angle-cos
+ (cond
+ ;; if th = 0[deg] -> no need to rotate target-coords
+ ((eps= rot-angle-cos 1.0)
+ (return-from orient-coords-to-axis target-coords))
+ ;; if th = 180[deg] -> previous rot-axis = 0 vector and rot-axis is ambiguous. overwrite rot-axis by the axis orthogonal to ax
+ ((eps= rot-angle-cos -1.0)
+ (block :calc-for-th-180
+ (dolist (rot-axis2 (list #f(1 0 0) #f(0 1 0)))
+ (let ((rot-angle-cos2 (v. ax rot-axis2)))
+ (unless (eps= (abs rot-angle-cos2) 1.0)
+ (setq rot-axis (v- rot-axis2 (scale rot-angle-cos2 ax))) ;; use only vertical component of rot-axis2 by removing parallel component of rot-axis2
+ (return-from :calc-for-th-180 nil))
+ ))))
+ (t ))
+ (send target-coords :rotate (acos rot-angle-cos) rot-axis :world)
target-coords))
;;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <k-...@us...> - 2011-10-31 17:01:07
|
Revision: 717
http://jskeus.svn.sourceforge.net/jskeus/?rev=717&view=rev
Author: k-okada
Date: 2011-10-31 17:01:01 +0000 (Mon, 31 Oct 2011)
Log Message:
-----------
fix automatic change of perspective-far and perspective-near
Modified Paths:
--------------
trunk/irteus/irtviewer.l
Modified: trunk/irteus/irtviewer.l
===================================================================
--- trunk/irteus/irtviewer.l 2011-10-17 04:56:36 UTC (rev 716)
+++ trunk/irteus/irtviewer.l 2011-10-31 17:01:01 UTC (rev 717)
@@ -168,7 +168,7 @@
(setq sc (/ (apply #'max (coerce (send bbox :diagonal) cons))
(* 0.5 (tan (send self :viewer :viewing :view-angle)))))
(when (or (> (/ sc gl::*perspective-far*) 2)
- (< (/ sc gl::*perspective-far*) 0.01))
+ (< (/ sc gl::*perspective-far*) 0.001))
(warning-message 2 "Detect very large/small objects(~A), change perspective~%" (/ sc gl::*perspective-far*) )
(setq gl::*perspective-near* (* (/ sc gl::*perspective-far*) gl::*perspective-near*)
gl::*perspective-far* (* (/ sc gl::*perspective-far*) gl::*perspective-far*)))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2011-10-17 04:56:43
|
Revision: 716
http://jskeus.svn.sourceforge.net/jskeus/?rev=716&view=rev
Author: snozawa
Date: 2011-10-17 04:56:36 +0000 (Mon, 17 Oct 2011)
Log Message:
-----------
:spots returns all spots in sub-scenes ;; patching saito's diff
Modified Paths:
--------------
trunk/irteus/irtscene.l
Modified: trunk/irteus/irtscene.l
===================================================================
--- trunk/irteus/irtscene.l 2011-10-13 13:15:23 UTC (rev 715)
+++ trunk/irteus/irtscene.l 2011-10-17 04:56:36 UTC (rev 716)
@@ -44,13 +44,24 @@
(:objects () objs)
(:find-object
(name)
- (mapcan #'(lambda (o)
- (if (string= name (send o :name)) (list o)))
- objs))
+ (append
+ (mapcan
+ #'(lambda(x)(if (derivedp x scene-model) (send x :find-object name) nil))
+ objs)
+ (mapcan #'(lambda (o)
+ (if (string= name (send o :name)) (list o)))
+ objs)))
(:spots
(&optional name)
- (mapcan #'(lambda (o) (if (eq (class o) cascaded-coords) (list o)))
- (if name (send self :find-object name) (send self :objects))))
+ (append
+ (mapcan
+ #'(lambda(x)(if (derivedp x scene-model) (send x :spots name) nil))
+ objs)
+ (mapcan #'(lambda (o)
+ (if (and (eq (class o) cascaded-coords)
+ (or (null name) (string= name (send o :name))))
+ (list o)))
+ objs)))
;;
(:object
(name)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2011-10-13 13:15:33
|
Revision: 715
http://jskeus.svn.sourceforge.net/jskeus/?rev=715&view=rev
Author: snozawa
Date: 2011-10-13 13:15:23 +0000 (Thu, 13 Oct 2011)
Log Message:
-----------
assoc both spots and other objects to scene model
Modified Paths:
--------------
trunk/irteus/irtscene.l
Modified: trunk/irteus/irtscene.l
===================================================================
--- trunk/irteus/irtscene.l 2011-10-13 13:03:58 UTC (rev 714)
+++ trunk/irteus/irtscene.l 2011-10-13 13:15:23 UTC (rev 715)
@@ -35,8 +35,8 @@
(setq name :name)
(setq objs o)
(dolist (obj objs)
+ (send self :assoc obj)
(when (eq (class obj) cascaded-coords)
- (send self :assoc obj)
(setf (get obj :width) 2)
(setf (get obj :color) #f(0.294118 0.572549 0.858823))
(setf (get obj :size) 300)))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2011-10-13 13:04:09
|
Revision: 714
http://jskeus.svn.sourceforge.net/jskeus/?rev=714&view=rev
Author: snozawa
Date: 2011-10-13 13:03:58 +0000 (Thu, 13 Oct 2011)
Log Message:
-----------
assoc spot to scene model
Modified Paths:
--------------
trunk/irteus/irtscene.l
Modified: trunk/irteus/irtscene.l
===================================================================
--- trunk/irteus/irtscene.l 2011-10-13 06:53:58 UTC (rev 713)
+++ trunk/irteus/irtscene.l 2011-10-13 13:03:58 UTC (rev 714)
@@ -36,6 +36,7 @@
(setq objs o)
(dolist (obj objs)
(when (eq (class obj) cascaded-coords)
+ (send self :assoc obj)
(setf (get obj :width) 2)
(setf (get obj :color) #f(0.294118 0.572549 0.858823))
(setf (get obj :size) 300)))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
