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: <k-...@us...> - 2012-05-06 07:19:39
|
Revision: 805
http://jskeus.svn.sourceforge.net/jskeus/?rev=805&view=rev
Author: k-okada
Date: 2012-05-06 07:19:32 +0000 (Sun, 06 May 2012)
Log Message:
-----------
add :dump-command nil
Modified Paths:
--------------
trunk/irteus/demo/crank-motion.l
trunk/irteus/demo/dual-arm-ik.l
trunk/irteus/demo/dual-manip-ik.l
trunk/irteus/demo/hanoi-arm.l
Modified: trunk/irteus/demo/crank-motion.l
===================================================================
--- trunk/irteus/demo/crank-motion.l 2012-04-26 04:01:06 UTC (rev 804)
+++ trunk/irteus/demo/crank-motion.l 2012-05-06 07:19:32 UTC (rev 805)
@@ -58,7 +58,7 @@
: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)
+ :debug-view :no-flush :dump-command nil)
;; draw
(send *irtviewer* :draw-objects :flush nil)
(mapcar #'(lambda (act ref)
Modified: trunk/irteus/demo/dual-arm-ik.l
===================================================================
--- trunk/irteus/demo/dual-arm-ik.l 2012-04-26 04:01:06 UTC (rev 804)
+++ trunk/irteus/demo/dual-arm-ik.l 2012-05-06 07:19:32 UTC (rev 805)
@@ -53,7 +53,7 @@
(send *robot* :inverse-kinematics target-coords
:link-list link-list :move-target move-target
:stop 500 :thre '(10 10)
- :rotation-axis '(nil nil) :debug-view nil)
+ :rotation-axis '(nil nil) :debug-view nil :dump-command nil)
(send *robot* :head :look-at
(apply #'midpoint 0.5 (send-all target-coords :worldpos)))
(send b0 :orient (* 0.2 (sin (/ i 10.0))) :x :world)
Modified: trunk/irteus/demo/dual-manip-ik.l
===================================================================
--- trunk/irteus/demo/dual-manip-ik.l 2012-04-26 04:01:06 UTC (rev 804)
+++ trunk/irteus/demo/dual-manip-ik.l 2012-05-06 07:19:32 UTC (rev 805)
@@ -75,6 +75,7 @@
tmp-ll tmp-mt ot *robot* :rotation-axis tmp-ra))
;;:debug-view t
:debug-view :no-message
+ :dump-command nil
(append (if (= i 0) '(:stop 100)) args))
(send (send (car ot) :parent) :dissoc *obj*)
))
Modified: trunk/irteus/demo/hanoi-arm.l
===================================================================
--- trunk/irteus/demo/hanoi-arm.l 2012-04-26 04:01:06 UTC (rev 804)
+++ trunk/irteus/demo/hanoi-arm.l 2012-05-06 07:19:32 UTC (rev 805)
@@ -40,6 +40,7 @@
:avoid-collision-joint-gain 2.0 ;; 1.0
:avoid-collision-null-gain 1.0 ;; 1.0
:debug-view :no-message
+ :dump-command nil
)
(break))
(send *irtviewer* :draw-objects)))
@@ -55,7 +56,7 @@
(send *irtviewer* :draw-objects)
(send *sarm* :solve-ik (make-cascoords :pos #f(500 0 100))
:avoid-collision-null-gain 0.0 ;; 1.0
- :rotation-axis t :debug-view nil)
+ :rotation-axis t :debug-view nil :dump-command nil)
(send *sarm* :open-hand)
(send *irtviewer* :draw-objects)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2012-04-26 04:01:12
|
Revision: 804
http://jskeus.svn.sourceforge.net/jskeus/?rev=804&view=rev
Author: snozawa
Date: 2012-04-26 04:01:06 +0000 (Thu, 26 Apr 2012)
Log Message:
-----------
consider all links included in robot model using all-child-links
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2012-04-26 03:42:44 UTC (rev 803)
+++ trunk/irteus/irtmodel.l 2012-04-26 04:01:06 UTC (rev 804)
@@ -1965,7 +1965,7 @@
vel-r))
;; collision check methods
(:collision-check-pairs
- (&key ((:links ls) links))
+ (&key ((:links ls) (cons (car links) (all-child-links (car links)))))
(let (pairs l neighbors)
(while (setq l (pop ls))
(setq neighbors (remove nil
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2012-04-26 03:42:50
|
Revision: 803
http://jskeus.svn.sourceforge.net/jskeus/?rev=803&view=rev
Author: snozawa
Date: 2012-04-26 03:42:44 +0000 (Thu, 26 Apr 2012)
Log Message:
-----------
add self collision check for all links using pqp-collision-check
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2012-04-19 16:08:16 UTC (rev 802)
+++ trunk/irteus/irtmodel.l 2012-04-26 03:42:44 UTC (rev 803)
@@ -1963,6 +1963,34 @@
(setq vel-r (calc-dif-with-axis dif-rot rotation-axis
tmp-v0 tmp-v1 tmp-v2))
vel-r))
+ ;; collision check methods
+ (:collision-check-pairs
+ (&key ((:links ls) links))
+ (let (pairs l neighbors)
+ (while (setq l (pop ls))
+ (setq neighbors (remove nil
+ (append
+ (send l :descendants)
+ (send l :child-links)
+ (list (send l :parent-link) (send l :parent)))))
+ (dolist (l2 ls)
+ (if (not (memq l2 neighbors))
+ (push (cons l l2) pairs))
+ )
+ )
+ pairs))
+ (:self-collision-check
+ (&key (mode :all) (pairs (send self :collision-check-pairs)) (collision-func 'pqp-collision-check))
+ (let ((cpairs) (col-count 0))
+ (dolist (p pairs)
+ (let ((colp (/= (funcall collision-func (car p) (cdr p)) 0)))
+ (when colp
+ (incf col-count)
+ (if (eq mode :first)
+ (return-from :self-collision-check p)
+ (push p cpairs)))
+ ))
+ cpairs))
)
(defun all-child-links (s &optional (pred #'identity))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2012-04-19 16:08:26
|
Revision: 802
http://jskeus.svn.sourceforge.net/jskeus/?rev=802&view=rev
Author: snozawa
Date: 2012-04-19 16:08:16 +0000 (Thu, 19 Apr 2012)
Log Message:
-----------
define max-joint-torque for sample robot and draw torques in crank-motion sample
Modified Paths:
--------------
trunk/irteus/demo/crank-motion.l
trunk/irteus/demo/sample-robot-model.l
Modified: trunk/irteus/demo/crank-motion.l
===================================================================
--- trunk/irteus/demo/crank-motion.l 2012-04-18 02:12:18 UTC (rev 801)
+++ trunk/irteus/demo/crank-motion.l 2012-04-19 16:08:16 UTC (rev 802)
@@ -68,6 +68,7 @@
(setf (elt ac 2) 0) ac))
(send-all move-target :worldpos))
(append (list cog-target-pos) target-coords))
+ (send *robot* :draw-torque *viewer*)
(send *irtviewer* :flush)
))
))))
Modified: trunk/irteus/demo/sample-robot-model.l
===================================================================
--- trunk/irteus/demo/sample-robot-model.l 2012-04-18 02:12:18 UTC (rev 801)
+++ trunk/irteus/demo/sample-robot-model.l 2012-04-19 16:08:16 UTC (rev 802)
@@ -176,6 +176,7 @@
jlr0 jlr1 jlr2 jlr3 jlr4 jlr5
))
(setq collision-avoidance-links (list aroot-link (elt torso 1) (elt larm 3) (elt rarm 3)))
+ (send-all (send self :joint-list) :max-joint-torque 0.1) ;; [Nm]
;; set mass properties
(dolist (l (append (list aroot-link) torso))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <k-...@us...> - 2012-04-18 02:12:24
|
Revision: 801
http://jskeus.svn.sourceforge.net/jskeus/?rev=801&view=rev
Author: k-okada
Date: 2012-04-18 02:12:18 +0000 (Wed, 18 Apr 2012)
Log Message:
-----------
add link/joint method, link/joint name might be string or symbole, thus do use (send robot :link name) instaed of (send robot name)
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2012-04-05 10:29:52 UTC (rev 800)
+++ trunk/irteus/irtmodel.l 2012-04-18 02:12:18 UTC (rev 801)
@@ -671,6 +671,8 @@
(send self :update-descendants))
(:links (&rest args) (user::forward-message-to-all links args))
(:joint-list (&rest args) (user::forward-message-to-all joint-list args))
+ (:link (name) (find name links :test #'equal :key #'(lambda (x) (send x :name))))
+ (:joint (name) (find name joint-list :test #'equal :key #'(lambda (x) (send x :name))))
(:bodies (&rest args) (user::forward-message-to-all bodies args))
(:faces () (flatten (send-all bodies :faces)))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <k-...@us...> - 2012-04-05 10:30:03
|
Revision: 800
http://jskeus.svn.sourceforge.net/jskeus/?rev=800&view=rev
Author: k-okada
Date: 2012-04-05 10:29:52 +0000 (Thu, 05 Apr 2012)
Log Message:
-----------
fix for month
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2012-04-03 14:41:23 UTC (rev 799)
+++ trunk/irteus/irtmodel.l 2012-04-05 10:29:52 UTC (rev 800)
@@ -1760,7 +1760,7 @@
(setq command-directory
(format nil "/tmp/irtmodel-ik-~A" (unix::getpid))
command-id
- (let ((lt (unix::localtime))) (substitute #\0 #\ (format nil "~A-~04d-~02d-~02d-~02d-~02d-~02d" (send (class self) :name) (+ 1900 (elt lt 5)) (elt lt 4) (elt lt 3) (elt lt 2) (elt lt 1) (elt lt 0))))
+ (let ((lt (unix::localtime))) (substitute #\0 #\ (format nil "~A-~04d-~02d-~02d-~02d-~02d-~02d" (send (class self) :name) (+ 1900 (elt lt 5)) (+ 1 (elt lt 4)) (elt lt 3) (elt lt 2) (elt lt 1) (elt lt 0))))
command-filename
(format nil "~A/~A.l" command-directory command-id))
(unix::mkdir command-directory)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ky...@us...> - 2012-04-03 14:41:33
|
Revision: 799
http://jskeus.svn.sourceforge.net/jskeus/?rev=799&view=rev
Author: kyouhei
Date: 2012-04-03 14:41:23 +0000 (Tue, 03 Apr 2012)
Log Message:
-----------
add c-isnan for c implimentation of isnan
Modified Paths:
--------------
trunk/irteus/irtgeoc.c
Modified: trunk/irteus/irtgeoc.c
===================================================================
--- trunk/irteus/irtgeoc.c 2012-04-03 12:49:13 UTC (rev 798)
+++ trunk/irteus/irtgeoc.c 2012-04-03 14:41:23 UTC (rev 799)
@@ -336,6 +336,23 @@
}
//
//
+pointer C_ISNAN (ctx,n,argv)
+ register context *ctx;
+ int n;
+ register pointer argv[];
+{
+ ckarg(1);
+
+ if ( isflt(argv[0]) ) {
+ numunion nu;
+ eusfloat_t f = fltval(argv[0]);
+ if(isnan(f)) return T;
+ return NIL;
+ } else {
+ return NIL;
+ }
+}
+
pointer ___irtgeoc(ctx,n, argv, env)
register context *ctx;int n;pointer *argv;pointer env;
{
@@ -345,4 +362,6 @@
defun(ctx,"VECTOR-ARRAY-VARIANCE",argv[0],VECTOR_ARRAY_VARIANCE);
defun(ctx,"VECTOR-ARRAY-MAX-MIN",argv[0],VECTOR_ARRAY_MAX_MIN);
defun(ctx,"FVECTOR-REPLACE", argv[0], FVECTOR_REPLACE);
+
+ defun(ctx,"C-ISNAN", argv[0], C_ISNAN);
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ky...@us...> - 2012-04-03 12:49:20
|
Revision: 798
http://jskeus.svn.sourceforge.net/jskeus/?rev=798&view=rev
Author: kyouhei
Date: 2012-04-03 12:49:13 +0000 (Tue, 03 Apr 2012)
Log Message:
-----------
add irtpoint.l irtgeoc.c for adding pointcloud class
Modified Paths:
--------------
trunk/irteus/Makefile
trunk/irteus/compile_irtg.l
trunk/irteus/irtext.l
trunk/irteus/irtgl.l
Added Paths:
-----------
trunk/irteus/irtgeoc.c
trunk/irteus/irtpointcloud.l
Modified: trunk/irteus/Makefile
===================================================================
--- trunk/irteus/Makefile 2012-04-02 12:53:20 UTC (rev 797)
+++ trunk/irteus/Makefile 2012-04-03 12:49:13 UTC (rev 798)
@@ -36,7 +36,7 @@
EUSLIB_MODULES.L=$(addprefix $(EUSDIR)/lib/,$(MODULES.L))
IRTEUS=irtmath irtutil irtgraph pgsql
-IRTEUSG=irtgeo pqp irtscene irtmodel irtsensor irtdyna irtrobot irtbvh irtcollada
+IRTEUSG=irtgeo pqp irtscene irtmodel irtsensor irtdyna irtrobot irtbvh irtcollada irtpointcloud
IRTEUSX=irtx
IRTEUSIMG=irtimage eusjpeg png
IRTEUSGL=irtgl irtglrgb irtviewer
@@ -84,7 +84,7 @@
IRTEUSGL_C=$(addsuffix .c,$(IRTEUSGL))
IRTEUSGL_H=$(addsuffix .h,$(IRTEUSGL))
-IRTCOBJECTS=$(OBJDIR)/irtc.$(OSFX)
+IRTCOBJECTS=$(OBJDIR)/irtc.$(OSFX) $(OBJDIR)/irtgeoc.$(OSFX)
IRTGCOBJECTS=$(OBJDIR)/CPQP.$(OSFX) $(OBJDIR)/euspqp.$(OSFX)
IRTIMGCOBJECTS=$(OBJDIR)/euspng.$(OSFX)
NROBJECTS=$(OBJDIR)/nr.$(OSFX)
@@ -171,6 +171,7 @@
$(OBJDIR)/irtdyna.$(OSFX): irtdyna.l
$(OBJDIR)/irtcollada.$(OSFX): irtcollada.l
$(OBJDIR)/irtsensor.$(OSFX): irtsensor.l
+$(OBJDIR)/irtpointcloud.$(OSFX): irtpointcloud.l
$(OBJDIR)/irtrobot.$(OSFX): irtrobot.l
$(OBJDIR)/irtbvh.$(OSFX): irtbvh.l
$(OBJDIR)/irtx.$(OSFX): irtx.l
@@ -183,8 +184,10 @@
$(OBJDIR)/pgsql.$(OSFX): $(EUSDIR)/lib/llib/pgsql.l
$(OBJDIR)/eusjpeg.$(OSFX): $(EUSDIR)/lisp/image/jpeg/eusjpeg.l
-$(OBJDIR)/irtc.$(OSFX): irtc.c $(filter-out $(OBJDIR)/irtc.$(OSFX),$(IRTEUSOBJS) $(IRTCOBJECTS) $(IRTEUSGOBJS) $(IRTGCOBJECTS) PQP/$(ARCHDIR)/libPQP-static.a $(IRTEUSXOBJS) $(LIBDIR)/jpegmemcd.$(LSFX) $(IRTEUSIMGOBJS) $(IRTIMGCOBJECTS) $(IRTEUSGLOBJS))
+$(OBJDIR)/irtc.$(OSFX): irtc.c $(filter-out $(OBJDIR)/irtc.$(OSFX),$(IRTEUSOBJS) $(IRTEUSGOBJS) $(IRTGCOBJECTS) PQP/$(ARCHDIR)/libPQP-static.a $(IRTEUSXOBJS) $(LIBDIR)/jpegmemcd.$(LSFX) $(IRTEUSIMGOBJS) $(IRTIMGCOBJECTS) $(IRTEUSGLOBJS))
$(CC) $(CFLAGS) -c irtc.c $(OBJOPT)$(OBJDIR)/irtc.$(OSFX)
+$(OBJDIR)/irtgeoc.$(OSFX): irtgeoc.c $(filter-out $(OBJDIR)/irtgeoc.$(OSFX), $(IRTEUSOBJS) $(IRTEUSGOBJS) $(IRTGCOBJECTS) PQP/$(ARCHDIR)/libPQP-static.a $(IRTEUSXOBJS) $(LIBDIR)/jpegmemcd.$(LSFX) $(IRTEUSIMGOBJS) $(IRTIMGCOBJECTS) $(IRTEUSGLOBJS))
+ $(CC) $(CFLAGS) -c irtgeoc.c $(OBJOPT)$(OBJDIR)/irtgeoc.$(OSFX)
$(OBJDIR)/CPQP.$(OSFX): CPQP.C
$(CXX) $(CXXFLAGS) -c CPQP.C $(OBJOPT)$(OBJDIR)/CPQP.$(OSFX)
$(OBJDIR)/euspqp.$(OSFX): euspqp.c
Modified: trunk/irteus/compile_irtg.l
===================================================================
--- trunk/irteus/compile_irtg.l 2012-04-02 12:53:20 UTC (rev 797)
+++ trunk/irteus/compile_irtg.l 2012-04-03 12:49:13 UTC (rev 798)
@@ -37,6 +37,7 @@
(comp:compile-file-if-src-newer "irtrobot.l" *objdir*)
(comp:compile-file-if-src-newer "irtbvh.l" *objdir*)
(comp:compile-file-if-src-newer "irtcollada.l" *objdir*)
+(comp:compile-file-if-src-newer "irtpointcloud.l" *objdir*)
(exit 0)
Modified: trunk/irteus/irtext.l
===================================================================
--- trunk/irteus/irtext.l 2012-04-02 12:53:20 UTC (rev 797)
+++ trunk/irteus/irtext.l 2012-04-03 12:49:13 UTC (rev 798)
@@ -28,13 +28,13 @@
(load-library
(format nil "~A~A/lib/libirteus"
*eusdir* (unix:getenv "ARCHDIR"))
- '("irtmath" "irtutil" "irtc" "irtgraph" "pgsql")))
+ '("irtmath" "irtutil" "irtc" "irtgeoc" "irtgraph" "pgsql")))
(defun load-irteusg ()
(in-package "GEOMETRY")
(load-library
(format nil "~A~A/lib/libirteusg"
*eusdir* (unix:getenv "ARCHDIR"))
- '("irtgeo" "euspqp" "pqp" "irtscene" "irtmodel" "irtdyna" "irtrobot" "irtsensor" "irtbvh" "irtcollada"))
+ '("irtgeo" "euspqp" "pqp" "irtscene" "irtmodel" "irtdyna" "irtrobot" "irtsensor" "irtbvh" "irtcollada" "irtpointcloud"))
(in-package "USER")
(import '(collada::convert-irtmodel-to-collada)))
(defun load-irteusx ()
Added: trunk/irteus/irtgeoc.c
===================================================================
--- trunk/irteus/irtgeoc.c (rev 0)
+++ trunk/irteus/irtgeoc.c 2012-04-03 12:49:13 UTC (rev 798)
@@ -0,0 +1,348 @@
+#pragma init (register_irtgeoc)
+#include "eus.h"
+
+extern pointer ___irtgeoc();
+static register_irtgeoc()
+{ add_module_initializer("___irtgeoc", ___irtgeoc);}
+
+//
+//
+#define colsize(p) (intval(p->c.ary.dim[1]))
+#define rowsize(p) (intval(p->c.ary.dim[0]))
+#define ismatrix(p) ((isarray(p) && \
+ p->c.ary.rank==makeint(2) && \
+ elmtypeof(p->c.ary.entity)==ELM_FLOAT))
+
+pointer C_COORDS_TRNSFORM_VECTOR(ctx,n,argv)
+ register context *ctx;
+ int n;
+ register pointer argv[];
+{
+ numunion nu;
+ register pointer result;
+ eusfloat_t *pos, *rot, *mat, *ret;
+ int inversep = 0, fill = 0;
+ int srcsize, dstsize;
+ int i,j;
+
+ // 0, 1, 2, 3, 4
+ // pos, rot, matrix, (matrix), (inverse)
+
+ ckarg2(3,5);
+ if ( (!isfltvector(argv[0])) || (!ismatrix(argv[1])) || (!ismatrix(argv[2]))) error(E_TYPEMISMATCH);
+ pos = argv[0]->c.fvec.fv;
+ rot = argv[1]->c.ary.entity->c.fvec.fv;
+ mat = argv[2]->c.ary.entity->c.fvec.fv;
+ if (n==5) {
+ if(!ismatrix(argv[3])) error(E_TYPEMISMATCH);
+ result = argv[3];
+ inversep = 1;
+ } else if (n==4) {
+ if(ismatrix(argv[3])) {
+ result = argv[3];
+ } else {
+ result = makematrix(ctx,rowsize(argv[2]), colsize(argv[2]));
+ inversep = 1;
+ fill = 1;
+ }
+ } else { // n == 3
+ result = makematrix(ctx,rowsize(argv[2]), colsize(argv[2]));
+ fill = 1;
+ }
+ ret = result->c.ary.entity->c.fvec.fv;
+
+ srcsize = colsize(argv[2]);
+ dstsize = colsize(result);
+ if ((srcsize < 3) && (dstsize < 3)) error(E_TYPEMISMATCH);
+ if (inversep) {
+ for(i=0;i<rowsize(result);i++){
+ eusfloat_t x = mat[i*srcsize+0] - pos[0],
+ y = mat[i*srcsize+1] - pos[1],
+ z = mat[i*srcsize+2] - pos[2];
+ ret[i*dstsize+0] = rot[0]*x+rot[3]*y+rot[6]*z;
+ ret[i*dstsize+1] = rot[1]*x+rot[4]*y+rot[7]*z;
+ ret[i*dstsize+2] = rot[2]*x+rot[5]*y+rot[8]*z;
+ if(fill) {
+ for(j=3;j<dstsize;j++) {
+ ret[i*dstsize + j] = mat[i*srcsize + j];
+ }
+ }
+ }
+ } else {
+ for(i=0;i<rowsize(result);i++){
+ eusfloat_t x = mat[i*srcsize+0], y = mat[i*srcsize+1], z = mat[i*srcsize+2];
+ ret[i*dstsize+0] = rot[0]*x+rot[1]*y+rot[2]*z+pos[0];
+ ret[i*dstsize+1] = rot[3]*x+rot[4]*y+rot[5]*z+pos[1];
+ ret[i*dstsize+2] = rot[6]*x+rot[7]*y+rot[8]*z+pos[2];
+ if(fill) {
+ for(j=3;j<dstsize;j++) {
+ ret[i*dstsize + j] = mat[i*srcsize + j];
+ }
+ }
+ }
+ }
+
+ return(result);
+}
+
+pointer C_MATRIX_ROW(ctx,n,argv)
+ register context *ctx;
+ int n;
+ register pointer argv[];
+{
+ numunion nu;
+ register pointer result;
+ register eusfloat_t *mat, *ret;
+ register eusinteger_t pos,cols,i;
+ int setp = 0;
+
+ // 0, 1, 2, 3
+ // matrix, row_num, (vector), (set?)
+ // return float-vector
+ // if vector is set, elements in vector is over written by row-vector
+ // if vector and set? are set, value of vector is copied to matrix-row
+
+ ckarg2(2,4);
+ //if ( (!ismatrix(argv[0])) || (!isint(argv[1])) ) error(E_TYPEMISMATCH); //no check
+ mat = argv[0]->c.ary.entity->c.fvec.fv;
+ cols = colsize(argv[0]);
+ pos = cols*intval(argv[1]);
+
+ if (n==4) {
+ //if (!(isfltvector(argv[2]))) error(E_TYPEMISMATCH); //no check
+ result = argv[2];
+ setp=1;
+ } else if (n==3) {
+ //if (!(isfltvector(argv[2]))) error(E_TYPEMISMATCH); //no check
+ result = argv[2];
+ } else { // n == 2
+ result = makefvector(cols);
+ }
+ ret = result->c.fvec.fv;
+
+ if(setp) {
+ mat += pos;
+ for(i=0;i<cols;i++) {
+ *mat++ = *ret++;
+ }
+ } else {
+ mat += pos;
+ for(i=0;i<cols;i++) {
+ *ret++ = *mat++;
+ }
+ }
+
+ return(result);
+}
+
+// utility for using matrix as vector-array
+static pointer VECTOR_ARRAY_MEAN(ctx,n,argv)
+ register context *ctx;
+ int n;
+ register pointer *argv;
+{
+ int i,j,size,dim,pc=0;
+ eusfloat_t *m, *fv;
+ pointer mat;
+
+ // 0, 1
+ // matrix-array, (average)
+
+ ckarg2(1,2);
+ if(! ismatrix(argv[0])) {
+ error(E_NOVECTOR);
+ }
+ m = argv[0]->c.ary.entity->c.fvec.fv;
+ size = rowsize(argv[0]);
+ dim = colsize(argv[0]);
+
+ if(n>1 && isfltvector(argv[1])) {
+ mat = argv[1];
+ } else {
+ mat = makevector(C_FLTVECTOR, dim); vpush(mat); pc++;
+ }
+ fv = mat->c.fvec.fv;
+
+ for(i=0;i<size;i++) {
+ for(j=0;j<dim;j++) {
+ fv[j] += *m++;
+ }
+ }
+
+ for(j=0;j<dim;j++) {
+ fv[j] /= size;
+ }
+
+ while(pc-->0) vpop();
+ return mat;
+}
+
+static pointer VECTOR_ARRAY_VARIANCE(ctx,n,argv)
+ register context *ctx;
+ int n;
+ register pointer *argv;
+{
+ int i,j,size,dim,pc=0, free_ave=1;
+ eusfloat_t *m, *fv, *ave;
+ pointer mat, amat;
+
+ // 0, 1, 2
+ // matrix-array, (variance), (average)
+
+ ckarg2(1,3);
+ if(! ismatrix(argv[0])) {
+ error(E_NOVECTOR);
+ }
+
+ size = rowsize(argv[0]);
+ dim = colsize(argv[0]);
+
+ if(n>1 && isfltvector(argv[1])) {
+ mat = argv[1];
+ } else {
+ mat = makevector(C_FLTVECTOR, dim); vpush(mat); pc++;
+ }
+ fv = mat->c.fvec.fv;
+
+ if(n>2 && isfltvector(argv[2])) {
+ amat = argv[2];
+ ave = amat->c.fvec.fv;
+ free_ave=0;
+ } else {
+ ave = (eusfloat_t *) malloc(sizeof(eusfloat_t)*dim);
+ }
+
+ for(i=0;i<dim;i++) {
+ fv[i] = 0.0;
+ ave[i] = 0.0;
+ }
+
+ m = argv[0]->c.ary.entity->c.fvec.fv;
+ for(i=0;i<size;i++) {
+ for(j=0;j<dim;j++) {
+ ave[j] += *m++;
+ }
+ }
+ for(j=0;j<dim;j++) {
+ ave[j] /= size;
+ }
+
+ m = argv[0]->c.ary.entity->c.fvec.fv;
+ for(i=0;i<size;i++) {
+ for(j=0;j<dim;j++) {
+ fv[j] += pow((*m++ - ave[j]), 2);
+ }
+ }
+ for(j=0;j<dim;j++) {
+ fv[j] /= size;
+ }
+
+ if(free_ave) free(ave);
+
+ while(pc-->0) vpop();
+ return mat;
+}
+static pointer VECTOR_ARRAY_MAX_MIN(ctx,n,argv)
+ register context *ctx;
+ int n;
+ register pointer *argv;
+{
+ int i,j,size,dim,pc=0;
+ eusfloat_t *m, *fvmin, *fvmax;
+ pointer fmax, fmin, ret;
+
+ // 0, 1, 2
+ // matrix-array, (max-vector), (min-vector)
+
+ ckarg2(1,3);
+ if(! ismatrix(argv[0])) {
+ error(E_NOVECTOR);
+ }
+ m = argv[0]->c.ary.entity->c.fvec.fv;
+ size = rowsize(argv[0]);
+ dim = colsize(argv[0]);
+
+ if(n == 1 && isfltvector(argv[1])) {
+ fmax = argv[1];
+ fmin = makevector(C_FLTVECTOR, dim); vpush(fmin); pc++;
+ } else if(n > 2 && isfltvector(argv[1]) && isfltvector(argv[2])) {
+ fmax = argv[1];
+ fmin = argv[2];
+ } else {
+ fmax = makevector(C_FLTVECTOR, dim); vpush(fmax); pc++;
+ fmin = makevector(C_FLTVECTOR, dim); vpush(fmin); pc++;
+ }
+ fvmax = fmax->c.fvec.fv;
+ fvmin = fmin->c.fvec.fv;
+
+ // set initial value
+ for(j=0;j<dim;j++) {
+ eusfloat_t val = *m++;
+ fvmax[j] = val;
+ fvmin[j] = val;
+ }
+ for(i=1;i<size;i++) {
+ for(j=0;j<dim;j++) {
+ eusfloat_t val = *m++;
+ if (val > fvmax[j])
+ fvmax[j] = val;
+ if (val < fvmin[j])
+ fvmin[j] = val;
+ }
+ }
+
+ ret=cons(ctx, fmin, NIL);
+ vpush(ret);
+ ret=cons(ctx, fmax, ret);
+ vpop();
+ while(pc-->0) vpop();
+ return ret;
+}
+
+static pointer FVECTOR_REPLACE(ctx,n,argv)
+ register context *ctx;
+ int n;
+ register pointer *argv;
+{
+ register int i,count;
+ register eusfloat_t *src, *dest;
+ eusinteger_t ss,ds,se,de;
+ numunion nu;
+
+ // 0, 1, 2, 3, 4, 5
+ // dst_vec, src_vec, (start_src), (end_src), (start_dst), (end_dst)
+
+ ckarg2(2,6);
+ if (!isfltvector(argv[0])) error(E_NOVECTOR);
+ if (!isfltvector(argv[1])) error(E_NOVECTOR);
+
+ dest = argv[0]->c.fvec.fv;
+ src = argv[1]->c.fvec.fv;
+
+ ds = (n==2) ? 0 : ckintval(argv[2]);
+ de = (n<=3) ? vecsize(argv[0]) : ckintval(argv[3]);
+ ss = (n<=4) ? 0 : ckintval(argv[4]);
+ se = (n<=5) ? vecsize(argv[1]) : ckintval(argv[5]);
+
+ count = min(de-ds, se-ss);
+ dest += ds;
+ src += ss;
+
+ for(i = 0; i < count; i++) {
+ *dest++ = *src++;
+ }
+
+ return argv[0];
+}
+//
+//
+pointer ___irtgeoc(ctx,n, argv, env)
+ register context *ctx;int n;pointer *argv;pointer env;
+{
+ defun(ctx,"C-COORDS-TRANSFORM-VECTOR",argv[0],C_COORDS_TRNSFORM_VECTOR);
+ defun(ctx,"C-MATRIX-ROW",argv[0],C_MATRIX_ROW);
+ defun(ctx,"VECTOR-ARRAY-MEAN",argv[0],VECTOR_ARRAY_MEAN);
+ defun(ctx,"VECTOR-ARRAY-VARIANCE",argv[0],VECTOR_ARRAY_VARIANCE);
+ defun(ctx,"VECTOR-ARRAY-MAX-MIN",argv[0],VECTOR_ARRAY_MAX_MIN);
+ defun(ctx,"FVECTOR-REPLACE", argv[0], FVECTOR_REPLACE);
+}
Modified: trunk/irteus/irtgl.l
===================================================================
--- trunk/irteus/irtgl.l 2012-04-02 12:53:20 UTC (rev 797)
+++ trunk/irteus/irtgl.l 2012-04-03 12:49:13 UTC (rev 798)
@@ -48,6 +48,22 @@
(defconstant GL_POLYGON_OFFSET_EXT #x8037)
(defconstant GL_POLYGON_OFFSET_FACTOR_EXT #x8038)
(defconstant GL_POLYGON_OFFSET_BIAS_EXT #x8039)
+
+ ;; for using array in OpenGL
+ (defforeign glEnableClientState gl-lib "glEnableClientState" () :integer)
+ (defforeign glDisableClientState gl-lib "glDisableClientState" () :integer)
+ (defforeign glVertexPointer gl-lib "glVertexPointer" () :integer)
+ (defforeign glColorPointer gl-lib "glColorPointer" () :integer)
+ (defforeign glNormalPointer gl-lib "glNormalPointer" () :integer)
+ (defforeign glDrawElements gl-lib "glDrawElements" () :integer)
+ (defforeign glArrayElement gl-lib "glArrayElement" () :integer)
+ (defforeign glDrawArrays gl-lib "glDrawArrays" () :integer)
+ (defconstant GL_VERTEX_ARRAY #x8074)
+ (defconstant GL_NORMAL_ARRAY #x8075)
+ (defconstant GL_TEXTURE_COORD_ARRAY #x8078)
+ (defconstant GL_EDGE_FLAG_ARRAY #x8079)
+ (defconstant GL_COLOR_ARRAY #x8076)
+ (defconstant GL_INDEX_ARRAY #x8077)
)
(unless (assoc :color-org (send glviewsurface :methods))
Added: trunk/irteus/irtpointcloud.l
===================================================================
--- trunk/irteus/irtpointcloud.l (rev 0)
+++ trunk/irteus/irtpointcloud.l 2012-04-03 12:49:13 UTC (rev 798)
@@ -0,0 +1,453 @@
+;;
+;; pointcloud class
+;;
+
+(in-package "USER")
+
+(defclass pointcloud
+ :super cascaded-coords
+ :slots (parray carray narray
+ pcolor psize awidth asize
+ box height width view-coords
+ drawnormalmode transparent tcarray))
+
+(defmethod pointcloud
+ (:init (&rest args
+ &key ((:points mat)) ((:colors cary)) ((:normals nary))
+ ((:height ht)) ((:width wd))
+ (point-color (float-vector 0 1 0))
+ (point-size 2.0)
+ (arrow-width 2.0) (arrow-size 0.0))
+ ;; matrix was not be copied
+ (cond
+ ((and mat (listp mat))
+ (send self :points mat))
+ (mat
+ (setq parray mat)))
+ (cond
+ ((and cary (listp cary))
+ (send self :colors cary))
+ (cary
+ (setq carray cary))
+ (t
+ (setq pcolor point-color)))
+ (cond
+ ((and nary (listp narray))
+ (send self :normals nary))
+ (nary
+ (setq narray nary)))
+
+ (setq psize point-size
+ awidth arrow-width
+ asize arrow-size
+ view-coords (make-coords)
+ drawnormalmode :normal)
+
+ (cond
+ ((and ht wd)
+ (send self :size-change wd ht))
+ (t
+ (send self :size-change (if mat (array-dimension mat 0)))))
+ (send-super* :init args)
+ self)
+ (:reset-box () (if (= (send self :size) 0)
+ (make-bounding-box (list #f(0 0 0) #f(1000 1000 1000)) 0.0)
+ (make-bounding-box (send self :point-list) 0.0)))
+ (:box () (unless box (setq box (send self :reset-box))) box)
+ (:vertices () (list (send (send self :box) :maxpoint) (send (send self :box) :minpoint)))
+ (:size () (if parray (array-dimension parray 0) 0))
+ (:width () width)
+ (:height () height)
+ (:size-change
+ (&optional wd ht)
+ (cond
+ ((and wd ht)
+ (setq width wd
+ height ht))
+ (wd
+ (setq width wd
+ height 1))
+ (ht
+ (setq height ht
+ width 1))
+ (t )))
+ (:view-coords (&optional vc) (if vc (setq view-coords vc)) view-coords)
+ (:points (&optional pts wd ht) ;; copy from pts
+ (when pts
+ (send self :size-change wd ht)
+ (cond
+ ((listp pts)
+ (let ((m (make-matrix (length pts) 3))
+ (cntr 0))
+ (dolist (p pts)
+ ;;(setf (matrix-row m cntr) p)
+ (c-matrix-row m cntr p t)
+ (incf cntr))
+ (setq parray m)))
+ ((matrixp pts)
+ (let ((m (make-matrix (array-dimension pts 0) 3)))
+ (sys::vector-replace (array-entity m) (array-entity pts))
+ (setq parray m)))))
+ parray)
+ (:colors (&optional cls) ;; copy from cls
+ (when cls
+ (cond
+ ((listp cls)
+ (setq pcolor nil)
+ (let ((m (make-matrix (length cls) 3))
+ (cntr 0))
+ (dolist (p cls)
+ ;;(setf (matrix-row m cntr) p)
+ (c-matrix-row m cntr p t)
+ (incf cntr))
+ (setq carray m)))
+ ((matrixp cls)
+ (setq pcolor nil)
+ (let ((m (make-matrix (array-dimension cls 0) 3)))
+ (sys::vector-replace (array-entity m) (array-entity cls))
+ (setq carray m)))))
+ carray)
+ (:normals (&optional nmls) ;; copy from nmls
+ (when nmls
+ (cond
+ ((listp nmls)
+ (let ((m (make-matrix (length nmls) 3))
+ (cntr 0))
+ (dolist (p nmls)
+ ;;(setf (matrix-row m cntr) p)
+ (c-matrix-row m cntr p t)
+ (incf cntr))
+ (setq narray m)))
+ ((matrixp nmls)
+ (let ((m (make-matrix (array-dimension nmls 0) 3)))
+ (sys::vector-replace (array-entity m) (array-entity nmls))
+ (setq narray m)))))
+ narray)
+ (:point-list ()
+ (let (lst)
+ (dotimes (i (send self :size))
+ ;;(push (matrix-row parray i) lst)
+ (push (c-matrix-row parray i) lst)
+ )
+ (reverse lst)))
+ (:color-list ()
+ (if carray
+ (let (lst)
+ (dotimes (i (array-dimension carray 0))
+ ;;(push (matrix-row carray i) lst)
+ (push (c-matrix-row carray i) lst)
+ )
+ (reverse lst))))
+ (:normal-list ()
+ (if narray
+ (let (lst)
+ (dotimes (i (array-dimension narray 0))
+ ;;(push (matrix-row narray i) lst)
+ (push (c-matrix-row narray i) lst)
+ )
+ (reverse lst))))
+ (:centroid ()
+ (let ((ret (float-vector 0 0 0)))
+ (vector-array-mean parray ret)
+ ret))
+ (:point-color (&optional pc) (if pc (setq pcolor pc)) pcolor)
+ (:point-size (&optional ps) (if ps (setq psize ps)) psize)
+ (:axis-length (&optioanl al) (if al (setq asize al)) asize)
+ (:axis-width (&optional aw) (if aw (setq awidth aw)) awidth)
+ (:clear-color () (warn ";; this method has not been implemented !!!"))
+ (:clear-normal () (warn ";; this method has not been implemented !!!"))
+ (:append () (warn ";; this method has not been implemented !!!"))
+ (:append-list () (warn ";; this method has not been implemented !!!"))
+ (:nfilter (&rest args) (send* self :filter args)) ;; compatibility to 3dpointcloud
+ (:filter (&rest args &key create &allow-other-keys)
+ (let ((indices (send* self :filtered-indices args)))
+ (send self :filter-with-indices indices :create create)))
+ (:filter-with-indices
+ (idx-lst &key (create) (negative))
+ (let* ((size (send self :size))
+ new-mat new-col new-nom
+ (p (float-vector 0 0 0))
+ (c (if carray (float-vector 0 0 0)))
+ (n (if narray (float-vector 0 0 0)))
+ (cntr 0))
+ (when negative
+ (let (tmp)
+ (dotimes (i size) (push (- size i 1) tmp))
+ (setq idx-lst (set-difference tmp idx-lst))
+ ))
+ (setq new-mat (make-matrix (length idx-lst) 3)
+ new-col (if carray (make-matrix (length idx-lst) 3))
+ new-nom (if narray (make-matrix (length idx-lst) 3)))
+ (cond
+ ((listp idx-lst)
+ (dolist (i idx-lst)
+ #|
+ (setq p (matrix-row parray i))
+ (if carray (setq c (matrix-row carray i)))
+ (if narray (setq n (matrix-row narray i)))
+ (setf (matrix-row new-mat cntr) p)
+ (if carray (setf (matrix-row new-col cntr) c))
+ (if narray (setf (matrix-row new-nom cntr) n))
+ |#
+ (c-matrix-row parray i p)
+ (c-matrix-row new-mat cntr p t)
+ (when carray
+ (c-matrix-row carray i c)
+ (c-matrix-row new-col cntr c t))
+ (when narray
+ (c-matrix-row narray i n)
+ (c-matrix-row new-nom cntr n t))
+ (incf cntr)))
+ (t ;; vector
+ (let (i)
+ (dotimes (idx (length idx-lst))
+ (setq i (elt idx-lst idx))
+ #|
+ (setq p (matrix-row parray i))
+ (if carray (setq c (matrix-row carray i)))
+ (if narray (setq n (matrix-row narray i)))
+ (setf (matrix-row new-mat cntr) p)
+ (if carray (setf (matrix-row new-col cntr) c))
+ (if narray (setf (matrix-row new-nom cntr) n))
+ |#
+ (c-matrix-row parray i p)
+ (c-matrix-row new-mat cntr p t)
+ (when carray
+ (c-matrix-row carray i c)
+ (c-matrix-row new-col cntr c t))
+ (when narray
+ (c-matrix-row narray i n)
+ (c-matrix-row new-nom cntr n t))
+ (incf cntr)))))
+ (if create
+ (let ((ret (instance pointcloud :init
+ :points new-mat
+ :colors new-col
+ :normals new-nom)))
+ (setq (ret . view-coords) (send view-coords :copy-worldcoords))
+ (send ret :transform (send self :worldcoords))
+ ret)
+ (progn
+ (setq parray new-mat)
+ (if carray (setq carray new-col))
+ (if parray (setq narray new-nom))
+ self)
+ )))
+ (:filtered-indices
+ (&key key ckey nkey pckey pnkey pcnkey negative &allow-other-keys)
+ (let* (ret-lst
+ (points-num (send self :size))
+ (p (instantiate float-vector 3))
+ (c (if carray (instantiate float-vector 3)))
+ (n (if narray (instantiate float-vector 3))))
+ (dotimes (i points-num)
+ (c-matrix-row parray i p)
+ (if c (c-matrix-row carray i c))
+ (if n (n-matrix-row narray i n))
+
+ (if (and (or (null key) (funcall key p))
+ (or (null ckey) (or (null c) (funcall ckey c)))
+ (or (null nkey) (or (null n) (funcall nkey n)))
+ (or (null pckey) (or (null c) (funcall pckey p c)))
+ (or (null pnkey) (or (null n) (funcall pnkey p n)))
+ (or (null pcnkey) (or (null c) (null n) (funcall pcnkey p c n))))
+ (unless negative (push i ret-lst))
+ (if negative (push i ret-lst))))
+ (nreverse ret-lst)
+ ))
+
+ (:viewangle-inlier () (warn ";; this method has not been implemented !!!"))
+ ;;(:remove-outlier ())
+ (:copy-from (pc)
+ (send self :points (send pc :points))
+ (send self :colors (send pc :colors))
+ (send self :normals (send pc :normals))
+ (send self :reset-coords)
+ (send self :transform (send pc :worldcoords))
+ (setq view-coords (send (pc . view-coords) :copy-worldcoords))
+ (send self :worldcoords)
+ self)
+ (:transform-points
+ (coords &key (create))
+ (let ((ret (if create (instance pointcloud :init) self)))
+ (if create (send ret :copy-from self))
+ (send (ret . view-coords) :transform
+ (send coords :worldcoords) :world)
+ ;; transform pts
+ (let ((mat (send ret :points)))
+ (c-coords-transform-vector
+ (send coords :worldpos)
+ (send coords :worldrot)
+ mat mat))
+ (when (send ret :normals)
+ ;; rotate normals
+ (let ((mat (send ret :normals)))
+ (c-coords-transform-vector
+ (float-vector 0 0 0)
+ (send coords :worldrot)
+ mat mat)))
+ ret))
+ (:convert-to-world (&key (create))
+ (prog1
+ (send self :transform-points (send self :worldcoords) :create create)
+ (unless create
+ (send self :reset-coords)
+ (send self :worldcoords))))
+ ;; (:add-normal ())
+ (:drawnormalmode (&optional mode)
+ (case mode
+ (:reset (setq drawnormalmode nil))
+ (t (setq drawnormalmode mode)))
+ drawnormalmode)
+ (:transparent (&optional trs)
+ (setq transparent trs)
+ (when (and trs carray)
+ (let ((size (send self :size)))
+ (setq tcarray (make-matrix size 4))
+ (dotimes (i size)
+ (setf (matrix-row tcarray i)
+ (concatenate float-vector (matrix-row carray i) (list trs))))))
+ trs)
+ (:draw (vwer)
+ (when transparent
+ (gl::glDepthMask gl::GL_FALSE)
+ (gl::glEnable gl::GL_BLEND)
+ (gl::glBlendFunc gl::GL_SRC_ALPHA gl::GL_ONE_MINUS_SRC_ALPHA))
+
+ (gl::glPushAttrib gl::GL_ALL_ATTRIB_BITS)
+ (if vwer (send vwer :viewsurface :makecurrent))
+ (gl::glDisable gl::GL_LIGHTING)
+
+ (gl::glpushmatrix)
+ (gl::glmultmatrixf (array-entity (transpose (send worldcoords :4x4) gl::*temp-matrix*)))
+ ;; draw coords
+ (when (> asize 0.1)
+ (gl::glLineWidth (float awidth))
+ (gl::glBegin gl::GL_LINES)
+ (gl::glColor3fv (float-vector 1 0 0))
+ (gl::glVertex3fv (float-vector 0 0 0))
+ (gl::glVertex3fv (float-vector asize 0 0))
+ (gl::glColor3fv (float-vector 0 1 0))
+ (gl::glVertex3fv (float-vector 0 0 0))
+ (gl::glVertex3fv (float-vector 0 asize 0))
+ (gl::glColor3fv (float-vector 0 0 1))
+ (gl::glVertex3fv (float-vector 0 0 0))
+ (gl::glVertex3fv (float-vector 0 0 asize))
+ (gl::glEnd))
+
+ ;; draw features
+ (gl::glPointSize (float psize))
+
+ (when (/= (send self :size) 0)
+ ;; check carray
+ (let ((tmp-color carray))
+ (cond
+ ((vectorp pcolor) (setq tmp-color nil) (gl::glColor3fv pcolor))
+ ((member pcolor (list :rainbow :rainbow-x :rainbow-y :rainbow-z))
+ (let (idx)
+ (case pcolor
+ ((:rainbow :rainbow-z) (setq idx 2))
+ (:rainbow-x (setq idx 0))
+ (:rainbow-y (setq idx 1)))
+ (setq tmp-color (copy-matrix parray))
+ (let ((carray-entity (array-entity tmp-color))
+ (v (float-vector 0 0 0))
+ min range)
+ (setq min (elt (send (send self :box) :minpoint) idx)
+ range (- (elt (send (send self :box) :maxpoint) idx) min))
+ (dotimes (i (array-dimension tmp-color 0))
+ (replace carray-entity
+ (normalize-vector
+ (his2rgb
+ (* -280 (/ (- (aref parray i idx) min) range))
+ 1.0 1.0 v) v) :start1 (* i 3)))))))
+
+ (gl::glEnableClientState gl::GL_VERTEX_ARRAY)
+ (if tmp-color (gl::glEnableClientState gl::GL_COLOR_ARRAY))
+ #-:x86_64
+ (if tmp-color
+ (gl::glColorPointer (if transparent 4 3)
+ gl::GL_FLOAT 0 (array-entity (if transparent tcarray tmp-color))))
+ #+:x86_64
+ (if tmp-color
+ (gl::glColorPointer (if transparent 4 3)
+ gl::GL_DOUBLE 0 (array-entity (if transparent tcarray tmp-color))))
+ #-:x86_64
+ (gl::glVertexPointer 3 gl::GL_FLOAT 0 (array-entity parray))
+ #+:x86_64
+ (gl::glVertexPointer 3 gl::GL_DOUBLE 0 (array-entity parray))
+ (gl::glDrawArrays gl::GL_POINTS 0 (array-dimension parray 0))
+ (gl::glDisableClientState gl::GL_VERTEX_ARRAY)
+ (if tmp-color (gl::glDisableClientState gl::GL_COLOR_ARRAY))
+
+ ;; draw normal
+ (when (and narray drawnormalmode)
+ (let ((tmp (float-vector 0 0 0))
+ (p (float-vector 0 0 0))
+ (n (float-vector 0 0 0))
+ (c (float-vector 0 0 0)))
+ (gl::glBegin gl::GL_LINES)
+ (unless tmp-color
+ (setq c pcolor))
+ (dotimes (i (array-dimension narray 0))
+ (c-matrix-row narray i n)
+ (cond
+ ((< (norm n) 0.1)
+ (gl::glColor3fv #f(1 1 1))
+ (dolist (o (list #f(0 0 -1) #f(0 0 1)
+ #f(0 -1 0) #f(0 1 0)
+ #f(-1 0 0) #f(1 0 0)))
+ #|(gl::glVertex3fv (matrix-row parray i))
+ (gl::glVertex3fv (v+ (matrix-row parray i) o tmp))|#
+ (c-matrix-row parray i p)
+ (gl::glVertex3fv p)
+ (gl::glVertex3fv (v+ p o p))
+ ))
+ (t
+ ;;(setq c (if tmp-color (matrix-row tmp-color i) pcolor))
+ (if tmp-color (c-matrix-row tmp-color i c))
+ (case
+ drawnormalmode
+ (:normal
+ (if (= (elt c 0) (elt c 1) (elt c 2))
+ (gl::glColor3fv c)
+ (gl::glColor3fv #f(0 0 1))))
+ (:normal-mono
+ (gl::glColor3f #f(0.8 0.8 0.8)))
+ (t
+ (gl::glColor3fv c)))
+ #|
+ (gl::glVertex3fv (matrix-row parray i))
+ (gl::glVertex3fv (v+ (matrix-row parray i)
+ (scale 10.0 (matrix-row narray i) tmp) tmp))
+ |#
+ (c-matrix-row parray i p)
+ ;;(c-matrix-row narray i n)
+ (gl::glVertex3fv p)
+ (gl::glVertex3fv (v+ p (scale 10.0 n n) p))
+ )))
+ (gl::glEnd)))
+ ))
+
+ (gl::glpopmatrix)
+ (gl::glEnable gl::GL_LIGHTING)
+ (gl::glPopAttrib)
+ (when transparent
+ (gl::glDepthMask gl::GL_TRUE)
+ (gl::glDisable gl::GL_BLEND)))
+ )
+(in-package "GEOMETRY")
+
+(provide :irtpointcloud "$Id: $")
+;;
+;; test
+;;
+#|
+(defun pointcloud-data ()
+ (setq m (make-matrix 1000 3))
+ (setq c (make-matrix 1000 3))
+ (dotimes (i 1000)
+ (setf (matrix-row m i) (scale 1000 (random-vector)))
+ (setf (matrix-row c i) (v+ (random-vector) #f(0.5 0.5 0.5))))
+ (instance pointcloud :init :points m :colors c))
+|#
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <k-...@us...> - 2012-04-02 12:53:31
|
Revision: 797
http://jskeus.svn.sourceforge.net/jskeus/?rev=797&view=rev
Author: k-okada
Date: 2012-04-02 12:53:20 +0000 (Mon, 02 Apr 2012)
Log Message:
-----------
fix dump-command
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2012-03-29 21:20:31 UTC (rev 796)
+++ trunk/irteus/irtmodel.l 2012-04-02 12:53:20 UTC (rev 797)
@@ -1760,12 +1760,12 @@
(setq command-directory
(format nil "/tmp/irtmodel-ik-~A" (unix::getpid))
command-id
- (let ((lt (unix::localtime))) (substitute #\0 #\ (format nil "~04d-~02d-~02d-~02d-~02d-~02d" (+ 1900 (elt lt 5)) (elt lt 4) (elt lt 3) (elt lt 2) (elt lt 1) (elt lt 0))))
+ (let ((lt (unix::localtime))) (substitute #\0 #\ (format nil "~A-~04d-~02d-~02d-~02d-~02d-~02d" (send (class self) :name) (+ 1900 (elt lt 5)) (elt lt 4) (elt lt 3) (elt lt 2) (elt lt 1) (elt lt 0))))
command-filename
- (format nil "~A-~A" (send (class self) :name) command-id))
+ (format nil "~A/~A.l" command-directory command-id))
(unix::mkdir command-directory)
(with-open-file
- (f (format nil "~A/~A.l" command-directory command-filename) :direction :output)
+ (f (format nil "~A/~A.l" command-directory command-id) :direction :output)
(format f ";; ik ~A log at ~A on ~A~%;;~%" (if success "success" "fail") (string-trim '(10) (unix:asctime (unix:localtime))) lisp::lisp-implementation-version)
(format f ";; link-list ~A~%" link-list)
(format f ";; move-target ~A~%" move-target)
@@ -1852,7 +1852,7 @@
(mapcar #'(lambda (l a) (send l :analysis-level a)) union-link-list old-analysis-level)
;; rename log file
(when dump-command
- (unix::rename (format nil "~A/~A.l" command-directory command-filename ) (format nil "~A/~A-~A.l" command-directory command-filename (if (or success (not revert-if-fail)) "success" "failure"))))
+ (unix::rename (format nil "~A/~A.l" command-directory command-id ) (setq command-filename (format nil "~A/~A-~A.l" command-directory command-id (if (or success (not revert-if-fail)) "success" "failure")))))
;; check solved or not
(if (or success (not revert-if-fail))
(send self :angle-vector)
@@ -1896,13 +1896,13 @@
command-setup `(progn (send r :newcoords (make-coords :4x4 ,(send self :4x4))) (mapc #'(lambda (j a) (send* j :joint-angle a ,joint-args)) (list . ,(mapcar #'(lambda (x) `(send r ,(intern (string-upcase x) *keyword-package*))) (send-all (remove-duplicates (append (send-all union-link-list :joint) joint-list)) :name))) ,(list 'quote av0)) (objects (list r)))
command-args (nconc `(list . ,(mapcar #'(lambda (x) `(make-coords :pos ,(send (if (functionp x) (funcall x) x) :worldpos) :rot ,(send (if (functionp x) (funcall x) x) :worldrot))) target-coords)) (list :dump-command nil :debug-view t) print-args))
(warn ";; command : ~a~%" `(let ((r ,command-init)) ,command-setup (send* r :inverse-kinematics ,command-args)))
- (warn ";; dump debug command to ~A/~A~%" command-directory command-filename)
- (warn ";; (progn (load \"~A\")(ik-setup)(ik-check))~%" command-directory command-filename)
+ (warn ";; dump debug command to ~A~%" command-filename)
+ (warn ";; (progn (load \"~A\")(ik-setup)(ik-check))~%" command-filename)
;; dump
(with-open-file
- (f (format nil "~A/~A-failure.l" command-directory command-filename) :direction :output :if-exists :append)
+ (f command-filename :direction :output :if-exists :append)
(format f "(defun ~A-setup () (let ((r ~A)) (setq *robot* r) ~A (objects (list *robot*))))~%" command-id command-init command-setup)
- (format f "(defun ~A-check () (send* *robot* :inverse-kinematics ~A))~%" command-id command-args)
+ (format f "(defun ~A-check () (let ((r *robot*)) (send* r :inverse-kinematics ~A)))~%" command-id command-args)
(format f "(defun ik-setup () (~A-setup))~%" command-id)
(format f "(defun ik-check () (~A-check))~%" command-id)
)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2012-03-29 21:20:37
|
Revision: 796
http://jskeus.svn.sourceforge.net/jskeus/?rev=796&view=rev
Author: snozawa
Date: 2012-03-29 21:20:31 +0000 (Thu, 29 Mar 2012)
Log Message:
-----------
add funcall because target-coords support lambda definition;; TODO -> Should lambda function be dumped??
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2012-03-29 12:38:33 UTC (rev 795)
+++ trunk/irteus/irtmodel.l 2012-03-29 21:20:31 UTC (rev 796)
@@ -1894,7 +1894,7 @@
(elt print-args (+ i 1)))))))))
(setq command-init `(instance ,(send (class self) :name) :init)
command-setup `(progn (send r :newcoords (make-coords :4x4 ,(send self :4x4))) (mapc #'(lambda (j a) (send* j :joint-angle a ,joint-args)) (list . ,(mapcar #'(lambda (x) `(send r ,(intern (string-upcase x) *keyword-package*))) (send-all (remove-duplicates (append (send-all union-link-list :joint) joint-list)) :name))) ,(list 'quote av0)) (objects (list r)))
- command-args (nconc `(list . ,(mapcar #'(lambda (x) `(make-coords :pos ,(send x :worldpos) :rot ,(send x :worldrot))) target-coords)) (list :dump-command nil :debug-view t) print-args))
+ command-args (nconc `(list . ,(mapcar #'(lambda (x) `(make-coords :pos ,(send (if (functionp x) (funcall x) x) :worldpos) :rot ,(send (if (functionp x) (funcall x) x) :worldrot))) target-coords)) (list :dump-command nil :debug-view t) print-args))
(warn ";; command : ~a~%" `(let ((r ,command-init)) ,command-setup (send* r :inverse-kinematics ,command-args)))
(warn ";; dump debug command to ~A/~A~%" command-directory command-filename)
(warn ";; (progn (load \"~A\")(ik-setup)(ik-check))~%" command-directory command-filename)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <k-...@us...> - 2012-03-29 12:38:44
|
Revision: 795
http://jskeus.svn.sourceforge.net/jskeus/?rev=795&view=rev
Author: k-okada
Date: 2012-03-29 12:38:33 +0000 (Thu, 29 Mar 2012)
Log Message:
-----------
dump ik log both sucess/fail
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2012-03-29 08:17:30 UTC (rev 794)
+++ trunk/irteus/irtmodel.l 2012-03-29 12:38:33 UTC (rev 795)
@@ -1732,7 +1732,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 "/tmp")
+ (dump-command t)
&allow-other-keys)
;; target-coords, move-target, rotation-axis, translation-axis, link-list
;; -> both list and atom OK.
@@ -1747,7 +1747,7 @@
;;
(success t)
(old-analysis-level (send-all union-link-list :analysis-level))
- ik-args)
+ command-directory command-filename command-id ik-args)
(send-all union-link-list :analysis-level :coords)
;; argument check
(when (or (null link-list) (null move-target))
@@ -1755,6 +1755,27 @@
(return-from :inverse-kinematics t))
(if (and (null translation-axis) (null rotation-axis))
(return-from :inverse-kinematics t))
+ ;; setup fname for log
+ (when dump-command
+ (setq command-directory
+ (format nil "/tmp/irtmodel-ik-~A" (unix::getpid))
+ command-id
+ (let ((lt (unix::localtime))) (substitute #\0 #\ (format nil "~04d-~02d-~02d-~02d-~02d-~02d" (+ 1900 (elt lt 5)) (elt lt 4) (elt lt 3) (elt lt 2) (elt lt 1) (elt lt 0))))
+ command-filename
+ (format nil "~A-~A" (send (class self) :name) command-id))
+ (unix::mkdir command-directory)
+ (with-open-file
+ (f (format nil "~A/~A.l" command-directory command-filename) :direction :output)
+ (format f ";; ik ~A log at ~A on ~A~%;;~%" (if success "success" "fail") (string-trim '(10) (unix:asctime (unix:localtime))) lisp::lisp-implementation-version)
+ (format f ";; link-list ~A~%" link-list)
+ (format f ";; move-target ~A~%" move-target)
+ (format f ";; rotatoin-axis ~A, translation-axis ~A~%" rotation-axis translation-axis)
+ (format f ";; thre ~A, rthre ~A, stop ~A~%" thre rthre stop)
+ (if (atom target-coords)
+ (dump-structure f `(setq c0 ,target-coords))
+ (dump-structure f `(setq c0 ',(mapcar #'(lambda (x) x) target-coords))))
+ (dump-structure f `(setq av0 ,(send self :angle-vector)))
+ ))
;; atom -> list
(when debug-view
(if (atom debug-view) (setq debug-view (list debug-view)))
@@ -1829,6 +1850,9 @@
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)
+ ;; rename log file
+ (when dump-command
+ (unix::rename (format nil "~A/~A.l" command-directory command-filename ) (format nil "~A/~A-~A.l" command-directory command-filename (if (or success (not revert-if-fail)) "success" "failure"))))
;; check solved or not
(if (or success (not revert-if-fail))
(send self :angle-vector)
@@ -1846,7 +1870,7 @@
(warn ";; angles : ~a~%" av0)
(warn ";; args : ~a~%" (append (list target-coords) args))
(when dump-command
- (let (i (print-args (copy-list args)) command-init command-setup command-args command-id)
+ (let (i (print-args (copy-list args)) command-init command-setup command-args)
(dotimes (j (count :link-list print-args))
(if (setq i (position :link-list print-args :count (1+ j)))
(cond ((atom (car (elt print-args (+ i 1))))
@@ -1870,15 +1894,13 @@
(elt print-args (+ i 1)))))))))
(setq command-init `(instance ,(send (class self) :name) :init)
command-setup `(progn (send r :newcoords (make-coords :4x4 ,(send self :4x4))) (mapc #'(lambda (j a) (send* j :joint-angle a ,joint-args)) (list . ,(mapcar #'(lambda (x) `(send r ,(intern (string-upcase x) *keyword-package*))) (send-all (remove-duplicates (append (send-all union-link-list :joint) joint-list)) :name))) ,(list 'quote av0)) (objects (list r)))
- command-args (nconc `(list . ,(mapcar #'(lambda (x) `(make-coords :pos ,(send x :worldpos) :rot ,(send x :worldrot))) target-coords)) (list :dump-command nil :debug-view t) print-args)
- command-id (let ((lt (unix::localtime))) (substitute #\0 #\ (format nil "~A-~04d-~02d-~02d-~02d-~02d-~02d" (send (class self) :name) (+ 1900 (elt lt 5)) (elt lt 4) (elt lt 3) (elt lt 2) (elt lt 1) (elt lt 0)))))
+ command-args (nconc `(list . ,(mapcar #'(lambda (x) `(make-coords :pos ,(send x :worldpos) :rot ,(send x :worldrot))) target-coords)) (list :dump-command nil :debug-view t) print-args))
(warn ";; command : ~a~%" `(let ((r ,command-init)) ,command-setup (send* r :inverse-kinematics ,command-args)))
- (warn ";; dump debug command to /~A/~A.l~%" dump-command command-id)
- (warn ";; (progn (load \"/~A/~A.l\")(ik-setup)(ik-check))~%" dump-command command-id)
+ (warn ";; dump debug command to ~A/~A~%" command-directory command-filename)
+ (warn ";; (progn (load \"~A\")(ik-setup)(ik-check))~%" command-directory command-filename)
;; dump
(with-open-file
- (f (format nil "/~A/~A.l" dump-command command-id) :direction :output)
- (format f ";; ik ~A log at ~A on ~A~%~%" (if success "success" "fail") (string-trim '(10) (unix:asctime (unix:localtime))) lisp::lisp-implementation-version)
+ (f (format nil "~A/~A-failure.l" command-directory command-filename) :direction :output :if-exists :append)
(format f "(defun ~A-setup () (let ((r ~A)) (setq *robot* r) ~A (objects (list *robot*))))~%" command-id command-init command-setup)
(format f "(defun ~A-check () (send* *robot* :inverse-kinematics ~A))~%" command-id command-args)
(format f "(defun ik-setup () (~A-setup))~%" command-id)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <k-...@us...> - 2012-03-29 08:17:42
|
Revision: 794
http://jskeus.svn.sourceforge.net/jskeus/?rev=794&view=rev
Author: k-okada
Date: 2012-03-29 08:17:30 +0000 (Thu, 29 Mar 2012)
Log Message:
-----------
add calc-weight from joint max velocity
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2012-03-29 04:21:07 UTC (rev 793)
+++ trunk/irteus/irtmodel.l 2012-03-29 08:17:30 UTC (rev 794)
@@ -916,12 +916,10 @@
(format-array (map float-vector #'rad2deg tmp-len2) "Ny :")
(format-array (map float-vector #'rad2deg J#x) "dav :")))
J#x))
- (:calc-joint-max-velocity
- (union-link-list periodic-time)
+ (:calc-joint-angle-speed-gain
+ (union-link-list dav periodic-time)
(let* ((fik-len (send self :calc-target-joint-dimension union-link-list))
- (av (instantiate float-vector fik-len))
- (dav (instantiate float-vector fik-len)) j)
- (fill dav 1)
+ (av (instantiate float-vector fik-len)) j)
(do* ((i 0 (+ i (send j :joint-dof)))
(l 0 (1+ l)))
((>= l (length union-link-list)))
@@ -1179,13 +1177,12 @@
(setq dav (send* self :calc-joint-angle-speed union-vel args))
;; truncate to speed limit
- (let ((speed-limit (send self :calc-joint-max-velocity union-link-list periodic-time))
- (tmp-gain) (max-gain 1.0))
- (dotimes (i (length dav))
- (setq tmp-gain (/ (abs (elt dav i)) (elt speed-limit i)) )
- (if (> tmp-gain max-gain)
- (setq max-gain tmp-gain)))
- (setq dav (scale (/ 1.0 max-gain) dav dav)))
+ (let ((tmp-gain (send self :calc-joint-angle-speed-gain union-link-list dav periodic-time))
+ (min-gain 1.0))
+ (dotimes (i (length tmp-gain))
+ (if (< (elt tmp-gain i) min-gain)
+ (setq min-gain (elt tmp-gain i))))
+ (setq dav (scale min-gain dav dav)))
(when (and debug-view (not (memq :no-message debug-view)))
(format-array (map float-vector #'rad2deg dav) "dav^ :"))
@@ -1269,12 +1266,23 @@
(cond
((functionp weight) (setq weight (funcall weight union-link-list)))
((listp weight) (setq weight (eval weight))))
+ ;; calc weight from joint limit
(setq tmp-weight
(send self :calc-weight-from-joint-limit
avoid-weight-gain fik-len link-list union-link-list
debug-view weight tmp-weight tmp-len))
(dotimes (i fik-len)
(setf (elt tmp-weight i) (* (elt weight i) (elt tmp-weight i))))
+#|
+ ;; calc weight from joint max velocity
+ (let* ((dav (fill (instantiate float-vector fik-len) 1))
+ (speed-limit (send self :calc-joint-angle-speed-gain union-link-list dav 0.5))
+ (max-gain 1.0))
+ (dotimes (i (length speed-limit)) (setq max-gain (max (elt speed-limit i) max-gain)))
+ (setq speed-limit (scale (/ 1.0 max-gain) speed-limit speed-limit))
+ (setq tmp-weight (transform (diagonal speed-limit) tmp-weight tmp-weight)))
+|#
+ ;;
(when (and debug-view (not (memq :no-message debug-view)))
(format-array tmp-weight "weight:"))
tmp-weight)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <k-...@us...> - 2012-03-29 04:21:13
|
Revision: 793
http://jskeus.svn.sourceforge.net/jskeus/?rev=793&view=rev
Author: k-okada
Date: 2012-03-29 04:21:07 +0000 (Thu, 29 Mar 2012)
Log Message:
-----------
add :calc-joint-max-velocity, and update truncate to speed limit algorithm in :move-joints, set periodic-time is 0.5 (10 times faster than default speed) when :inverse-kinemtaics
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2012-03-28 18:10:16 UTC (rev 792)
+++ trunk/irteus/irtmodel.l 2012-03-29 04:21:07 UTC (rev 793)
@@ -916,6 +916,19 @@
(format-array (map float-vector #'rad2deg tmp-len2) "Ny :")
(format-array (map float-vector #'rad2deg J#x) "dav :")))
J#x))
+ (:calc-joint-max-velocity
+ (union-link-list periodic-time)
+ (let* ((fik-len (send self :calc-target-joint-dimension union-link-list))
+ (av (instantiate float-vector fik-len))
+ (dav (instantiate float-vector fik-len)) j)
+ (fill dav 1)
+ (do* ((i 0 (+ i (send j :joint-dof)))
+ (l 0 (1+ l)))
+ ((>= l (length union-link-list)))
+ (setq j (send (elt union-link-list l) :joint))
+ (dotimes (k (send j :joint-dof))
+ (setf (elt av (+ i k)) (send j :calc-angle-speed-gain dav i periodic-time))))
+ av))
(:collision-avoidance-links
(&optional l)
(if l (setq collision-avoidance-links l)) collision-avoidance-links)
@@ -1158,21 +1171,22 @@
(joint-args)
(debug-view nil)
&allow-other-keys)
- (let (dav dtheta j (gain 1.0))
+ (let (dav dtheta j)
(if (and debug-view (atom debug-view)) (setq debug-view (list debug-view)))
(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))
- (do ((i 0 (+ i (send j :joint-dof)))
- (l 0 (1+ l)))
- ((>= l (length union-link-list)))
- (setq j (send (elt union-link-list l) :joint))
- (let ((tmp-gain
- (send (send (elt union-link-list l) :joint)
- :calc-angle-speed-gain dav i periodic-time)))
- (if (< tmp-gain gain) (setq gain tmp-gain))))
- (setq dav (scale gain dav dav))
+
+ ;; truncate to speed limit
+ (let ((speed-limit (send self :calc-joint-max-velocity union-link-list periodic-time))
+ (tmp-gain) (max-gain 1.0))
+ (dotimes (i (length dav))
+ (setq tmp-gain (/ (abs (elt dav i)) (elt speed-limit i)) )
+ (if (> tmp-gain max-gain)
+ (setq max-gain tmp-gain)))
+ (setq dav (scale (/ 1.0 max-gain) dav dav)))
+
(when (and debug-view (not (memq :no-message debug-view)))
(format-array (map float-vector #'rad2deg dav) "dav^ :"))
@@ -1778,6 +1792,7 @@
(setq success (send* self :inverse-kinematics-loop
dif-pos dif-rot
:target-coords target-coords
+ :periodic-time 0.5
: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: <k-...@us...> - 2012-03-28 18:10:27
|
Revision: 792
http://jskeus.svn.sourceforge.net/jskeus/?rev=792&view=rev
Author: k-okada
Date: 2012-03-28 18:10:16 +0000 (Wed, 28 Mar 2012)
Log Message:
-----------
print scaled dav as debug message
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2012-03-28 17:44:20 UTC (rev 791)
+++ trunk/irteus/irtmodel.l 2012-03-28 18:10:16 UTC (rev 792)
@@ -1173,6 +1173,8 @@
:calc-angle-speed-gain dav i periodic-time)))
(if (< tmp-gain gain) (setq gain tmp-gain))))
(setq dav (scale gain dav dav))
+ (when (and debug-view (not (memq :no-message debug-view)))
+ (format-array (map float-vector #'rad2deg dav) "dav^ :"))
;; update body
(do ((i 0 (+ i (send j :joint-dof)))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <k-...@us...> - 2012-03-28 17:44:30
|
Revision: 791
http://jskeus.svn.sourceforge.net/jskeus/?rev=791&view=rev
Author: k-okada
Date: 2012-03-28 17:44:20 +0000 (Wed, 28 Mar 2012)
Log Message:
-----------
add angle-to-speed, calc-joint-angle-min-max-for-limit-calculation uses angle-to-speed
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2012-03-28 16:08:21 UTC (rev 790)
+++ trunk/irteus/irtmodel.l 2012-03-28 17:44:20 UTC (rev 791)
@@ -62,6 +62,7 @@
(:calc-dav-gain (dav i periodic-time) (warn "subclass's respoinsibility (send ~s :calc-dav-gain)~%" self))
(:joint-dof () (warn "subclass's respoinsibility (send ~s :joint-dof)~%" self))
(:speed-to-angle (&rest args) (warn "subclass's respoinsibility (send ~s :speed-to-angle)~%" self))
+ (:angle-to-speed (&rest args) (warn "subclass's respoinsibility (send ~s :angle-to-speed)~%" self))
(:calc-jacobian (&rest args) (warn "subclass's respoinsibility (send ~s :calc-jacobian)~%" self))
(:joint-velocity (&optional jv) (if jv (setq joint-velocity jv) joint-velocity))
(:joint-acceleration (&optional ja) (if ja (setq joint-acceleration ja) joint-acceleration))
@@ -172,7 +173,8 @@
joint-angle))
(:joint-dof () 1)
(:calc-angle-speed-gain (dav i periodic-time) (calc-angle-speed-gain-scalar self dav i periodic-time))
- (:speed-to-angle (dav i) (rad2deg (elt dav i)))
+ (:speed-to-angle (v) (rad2deg v))
+ (:angle-to-speed (v) (deg2rad v))
(:calc-jacobian (&rest args) (apply #'calc-jacobian-rotational args))
)
@@ -221,7 +223,8 @@
joint-angle))
(:joint-dof () 1)
(:calc-angle-speed-gain (dav i periodic-time) (calc-angle-speed-gain-scalar self dav i periodic-time))
- (:speed-to-angle (dav i) (* 1000.0 (elt dav i)))
+ (:speed-to-angle (v) (* 1000 v))
+ (:angle-to-speed (v) (* 0.001 v))
(:calc-jacobian (&rest args) (apply #'calc-jacobian-linear args))
)
@@ -262,9 +265,12 @@
joint-angle))
(:joint-dof () 2)
(:calc-angle-speed-gain (dav i periodic-time) (calc-angle-speed-gain-vector self dav i periodic-time))
- (:speed-to-angle (dav i)
- (float-vector (* 1000 (elt dav i))
- (rad2deg (elt dav (incf i)))))
+ (:speed-to-angle (dv)
+ (float-vector (* 1000 (elt dv 0))
+ (rad2deg (elt dv 1))))
+ (:angle-to-speed (dv)
+ (float-vector (* 0.001 (elt dv 0))
+ (deg2rad (elt dv 1))))
(:calc-jacobian
(fik row column joint paxis child-link world-default-coords child-reverse
move-target transform-coords rotation-axis translation-axis
@@ -315,10 +321,14 @@
joint-angle)
(:joint-dof () 3)
(:calc-angle-speed-gain (dav i periodic-time) (calc-angle-speed-gain-vector self dav i periodic-time))
- (:speed-to-angle (dav i)
- (float-vector (* 1000 (elt dav i))
- (* 1000 (elt dav (incf i)))
- (rad2deg (elt dav (incf i)))))
+ (:speed-to-angle (dv)
+ (float-vector (* 1000 (elt dv 0))
+ (* 1000 (elt dv 1))
+ (rad2deg (elt dv 2))))
+ (:angle-to-speed (dv)
+ (float-vector (* 0.001 (elt dv 0))
+ (* 0.001 (elt dv 1))
+ (deg2rad (elt dv 2))))
(:calc-jacobian
(fik row column joint paxis child-link world-default-coords child-reverse
move-target transform-coords rotation-axis translation-axis
@@ -396,10 +406,14 @@
(map float-vector #'rad2deg (car (rpy-angle (matrix-exponent (map float-vector #'deg2rad joint-angle))))))
(:joint-dof () 3)
(:calc-angle-speed-gain (dav i periodic-time) (calc-angle-speed-gain-vector self dav i periodic-time))
- (:speed-to-angle (dav i)
- (float-vector (rad2deg (elt dav i))
- (rad2deg (elt dav (incf i)))
- (rad2deg (elt dav (incf i)))))
+ (:speed-to-angle (dv)
+ (float-vector (rad2deg (elt dv 0))
+ (rad2deg (elt dv 1))
+ (rad2deg (elt dv 2))))
+ (:angle-to-speed (dv)
+ (float-vector (deg2rad (elt dv 0))
+ (deg2rad (elt dv 1))
+ (deg2rad (elt dv 2))))
(:calc-jacobian
(fik row column joint paxis child-link world-default-coords child-reverse
move-target transform-coords rotation-axis translation-axis
@@ -527,13 +541,20 @@
(map float-vector #'rad2deg (car (rpy-angle (matrix-exponent (map float-vector #'deg2rad (subseq joint-angle 3 6))))))))
(:joint-dof () 6)
(:calc-angle-speed-gain (dav i periodic-time) (calc-angle-speed-gain-vector self dav i periodic-time))
- (:speed-to-angle (dav i)
- (float-vector (* 1000 (elt dav i))
- (* 1000 (elt dav (incf i)))
- (* 1000 (elt dav (incf i)))
- (rad2deg (elt dav (incf i)))
- (rad2deg (elt dav (incf i)))
- (rad2deg (elt dav (incf i)))))
+ (:speed-to-angle (dv)
+ (float-vector (* 1000 (elt dv 0))
+ (* 1000 (elt dv 1))
+ (* 1000 (elt dv 2))
+ (rad2deg (elt dv 3))
+ (rad2deg (elt dv 4))
+ (rad2deg (elt dv 5))))
+ (:angle-to-speed (dv)
+ (float-vector (* 0.001 (elt dv 0))
+ (* 0.001 (elt dv 1))
+ (* 0.001 (elt dv 2))
+ (deg2rad (elt dv 3))
+ (deg2rad (elt dv 4))
+ (deg2rad (elt dv 5))))
(:calc-jacobian
(fik row column joint paxis child-link world-default-coords child-reverse
move-target transform-coords rotation-axis translation-axis
@@ -1158,7 +1179,9 @@
(l 0 (1+ l)))
((>= l (length union-link-list)))
(setq j (send (elt union-link-list l) :joint))
- (setq dtheta (send j :speed-to-angle dav i))
+ (case (send j :joint-dof)
+ (1 (setq dtheta (send j :speed-to-angle (elt dav i))))
+ (t (setq dtheta (send j :speed-to-angle (subseq dav i (+ i (send j :joint-dof)))))))
(send* (elt union-link-list l) :joint :joint-angle dtheta :relative t joint-args))
t))
;; calc weight according to joint limit
@@ -1949,18 +1972,16 @@
(defun calc-joint-angle-min-max-for-limit-calculation (j kk jamm)
;; fix unit system ;; [mm] -> [m], [deg] -> [rad]
- (labels ((mm2m (x) (* 0.001 x)))
- (cond
- ((vectorp (send j :joint-angle)) ;; multi-dof joint such as sphere-joint, *wheel-joint and 6dof-joint
- (let ((vp (vectorp (elt (j . axis) kk))))
- (setf (elt jamm 0) (funcall (if vp #'mm2m #'deg2rad) (elt (send j :joint-angle) kk)))
- (setf (elt jamm 1) (funcall (if vp #'mm2m #'deg2rad) (elt (send j :max-angle) kk)))
- (setf (elt jamm 2) (funcall (if vp #'mm2m #'deg2rad) (elt (send j :min-angle) kk)))))
- (t ;; 1-dof joint such as rotational-joint and linear-joint
- (let ((vp (case (send (class j) :name) ('rotational-joint nil) ('linear-joint t) (t (vectorp (j . axis))))))
- (setf (elt jamm 0) (funcall (if vp #'mm2m #'deg2rad) (send j :joint-angle)))
- (setf (elt jamm 1) (funcall (if vp #'mm2m #'deg2rad) (send j :max-angle)))
- (setf (elt jamm 2) (funcall (if vp #'mm2m #'deg2rad) (send j :min-angle)))))))
+ (cond
+ ((vectorp (send j :joint-angle)) ;; multi-dof joint such as sphere-joint, *wheel-joint and 6dof-joint
+ (setf (elt jamm 0) (elt (send j :angle-to-speed (send j :joint-angle)) kk))
+ (setf (elt jamm 1) (elt (send j :angle-to-speed (send j :max-angle)) kk))
+ (setf (elt jamm 2) (elt (send j :angle-to-speed (send j :min-angle)) kk)))
+ (t ;; 1-dof joint such as rotational-joint and linear-joint
+ (setf (elt jamm 0) (send j :angle-to-speed (send j :joint-angle)))
+ (setf (elt jamm 1) (send j :angle-to-speed (send j :max-angle)))
+ (setf (elt jamm 2) (send j :angle-to-speed (send j :min-angle))))
+ )
jamm)
(defun joint-angle-limit-weight (j-l &optional (res (instantiate float-vector (calc-target-joint-dimension j-l))))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <k-...@us...> - 2012-03-28 16:08:27
|
Revision: 790
http://jskeus.svn.sourceforge.net/jskeus/?rev=790&view=rev
Author: k-okada
Date: 2012-03-28 16:08:21 +0000 (Wed, 28 Mar 2012)
Log Message:
-----------
joint-angle violation warning only when non-relative mode
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2012-03-28 16:04:50 UTC (rev 789)
+++ trunk/irteus/irtmodel.l 2012-03-28 16:08:21 UTC (rev 790)
@@ -158,13 +158,13 @@
(setq v (mod v 360))
(if (> v 180.0) (setq v (- v 360.0))))
((> v max-angle)
- (warning-message 3 "~A :joint-angle(~A) violate max-angle(~A)~%" self v max-angle)
+ (unless relative (warning-message 3 "~A :joint-angle(~A) violate max-angle(~A)~%" self v max-angle))
(setq v max-angle)))
(cond ((and (= min-angle *-inf*) (<= v -180.0))
(setq v (mod v 360))
(if (<= v -180.0) (setq v (+ v 360.0))))
((< v min-angle)
- (warning-message 3 "~A :joint-angle(~A) violate min-angle(~A)~%" self v min-angle)
+ (unless relative (warning-message 3 "~A :joint-angle(~A) violate min-angle(~A)~%" self v min-angle))
(setq v min-angle)))
(setq joint-angle v)
(send child-link :replace-coords default-coords)
@@ -210,10 +210,10 @@
(when v
(if relative (setq v (+ v joint-angle)))
(when (> v max-angle)
- (warning-message 3 "~A :joint-angle(~A) violate max-angle(~A)~%" self v max-angle)
+ (unless relative (warning-message 3 "~A :joint-angle(~A) violate max-angle(~A)~%" self v max-angle))
(setq v max-angle))
(when (< v min-angle)
- (warning-message 3 "~A :joint-angle(~A) violate min-angle(~A)~%" self v min-angle)
+ (unless relative (warning-message 3 "~A :joint-angle(~A) violate min-angle(~A)~%" self v min-angle))
(setq v min-angle))
(setq joint-angle v)
(send child-link :replace-coords default-coords)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <k-...@us...> - 2012-03-28 16:04:59
|
Revision: 789
http://jskeus.svn.sourceforge.net/jskeus/?rev=789&view=rev
Author: k-okada
Date: 2012-03-28 16:04:50 +0000 (Wed, 28 Mar 2012)
Log Message:
-----------
j . axis is not always vector for rotational-joint
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2012-03-28 14:12:08 UTC (rev 788)
+++ trunk/irteus/irtmodel.l 2012-03-28 16:04:50 UTC (rev 789)
@@ -1957,10 +1957,11 @@
(setf (elt jamm 1) (funcall (if vp #'mm2m #'deg2rad) (elt (send j :max-angle) kk)))
(setf (elt jamm 2) (funcall (if vp #'mm2m #'deg2rad) (elt (send j :min-angle) kk)))))
(t ;; 1-dof joint such as rotational-joint and linear-joint
- (let ((vp (vectorp (j . axis))))
+ (let ((vp (case (send (class j) :name) ('rotational-joint nil) ('linear-joint t) (t (vectorp (j . axis))))))
(setf (elt jamm 0) (funcall (if vp #'mm2m #'deg2rad) (send j :joint-angle)))
(setf (elt jamm 1) (funcall (if vp #'mm2m #'deg2rad) (send j :max-angle)))
- (setf (elt jamm 2) (funcall (if vp #'mm2m #'deg2rad) (send j :min-angle))))))))
+ (setf (elt jamm 2) (funcall (if vp #'mm2m #'deg2rad) (send j :min-angle)))))))
+ jamm)
(defun joint-angle-limit-weight (j-l &optional (res (instantiate float-vector (calc-target-joint-dimension j-l))))
(let ((k 0) (kk 0) (jamm (float-vector 0 0 0)))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <k-...@us...> - 2012-03-28 14:12:18
|
Revision: 788
http://jskeus.svn.sourceforge.net/jskeus/?rev=788&view=rev
Author: k-okada
Date: 2012-03-28 14:12:08 +0000 (Wed, 28 Mar 2012)
Log Message:
-----------
warning-message with yellow color and use > instead of >= for min/max check
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2012-03-28 14:10:06 UTC (rev 787)
+++ trunk/irteus/irtmodel.l 2012-03-28 14:12:08 UTC (rev 788)
@@ -157,14 +157,14 @@
(cond ((and (eq max-angle *inf*) (>= v 180.0))
(setq v (mod v 360))
(if (> v 180.0) (setq v (- v 360.0))))
- ((>= v max-angle)
- (warning-message 1 "~A :joint-angle(~A) violate max-angle(~A)~%" self v max-angle)
+ ((> v max-angle)
+ (warning-message 3 "~A :joint-angle(~A) violate max-angle(~A)~%" self v max-angle)
(setq v max-angle)))
(cond ((and (= min-angle *-inf*) (<= v -180.0))
(setq v (mod v 360))
(if (<= v -180.0) (setq v (+ v 360.0))))
- ((<= v min-angle)
- (warning-message 1 "~A :joint-angle(~A) violate min-angle(~A)~%" self v min-angle)
+ ((< v min-angle)
+ (warning-message 3 "~A :joint-angle(~A) violate min-angle(~A)~%" self v min-angle)
(setq v min-angle)))
(setq joint-angle v)
(send child-link :replace-coords default-coords)
@@ -209,11 +209,11 @@
(let ()
(when v
(if relative (setq v (+ v joint-angle)))
- (when (>= v max-angle)
- (warning-message 1 "~A :joint-angle(~A) violate max-angle(~A)~%" self v max-angle)
+ (when (> v max-angle)
+ (warning-message 3 "~A :joint-angle(~A) violate max-angle(~A)~%" self v max-angle)
(setq v max-angle))
- (when (<= v min-angle)
- (warning-message 1 "~A :joint-angle(~A) violate min-angle(~A)~%" self v min-angle)
+ (when (< v min-angle)
+ (warning-message 3 "~A :joint-angle(~A) violate min-angle(~A)~%" self v min-angle)
(setq v min-angle))
(setq joint-angle v)
(send child-link :replace-coords default-coords)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <k-...@us...> - 2012-03-28 14:10:17
|
Revision: 787
http://jskeus.svn.sourceforge.net/jskeus/?rev=787&view=rev
Author: k-okada
Date: 2012-03-28 14:10:06 +0000 (Wed, 28 Mar 2012)
Log Message:
-----------
use copy-list instead of copy-object for args->print-args
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2012-03-28 12:47:03 UTC (rev 786)
+++ trunk/irteus/irtmodel.l 2012-03-28 14:10:06 UTC (rev 787)
@@ -1798,7 +1798,7 @@
(warn ";; angles : ~a~%" av0)
(warn ";; args : ~a~%" (append (list target-coords) args))
(when dump-command
- (let (i (print-args (copy-object args)) command-init command-setup command-args command-id)
+ (let (i (print-args (copy-list args)) command-init command-setup command-args command-id)
(dotimes (j (count :link-list print-args))
(if (setq i (position :link-list print-args :count (1+ j)))
(cond ((atom (car (elt print-args (+ i 1))))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <k-...@us...> - 2012-03-28 12:47:09
|
Revision: 786
http://jskeus.svn.sourceforge.net/jskeus/?rev=786&view=rev
Author: k-okada
Date: 2012-03-28 12:47:03 +0000 (Wed, 28 Mar 2012)
Log Message:
-----------
dump ik-command under /tmp if :inverse-kinematics filed
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2012-03-28 11:52:26 UTC (rev 785)
+++ trunk/irteus/irtmodel.l 2012-03-28 12:47:03 UTC (rev 786)
@@ -1685,6 +1685,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 "/tmp")
&allow-other-keys)
;; target-coords, move-target, rotation-axis, translation-axis, link-list
;; -> both list and atom OK.
@@ -1796,34 +1797,48 @@
(send (let ((p self)) (while (send p :parent) (setq p (send p :parent))) p) :worldcoords))
(warn ";; angles : ~a~%" av0)
(warn ";; args : ~a~%" (append (list target-coords) args))
- (let (i (print-args (copy-object args)) command-init command-setup command-ik)
- (dotimes (j (count :link-list print-args))
- (if (setq i (position :link-list print-args :count (1+ j)))
- (cond ((atom (car (elt print-args (+ i 1))))
- (setf (elt print-args (+ i 1)) (append '(list) (mapcar #'(lambda (x) `(send r ,(send x :name))) (elt print-args (+ i 1))))))
- (t
- (setf (elt print-args (+ i 1))
- (append '(list)
- (mapcar #'(lambda (y)
- (append '(list) (mapcar #'(lambda (x) `(send r ,(send x :name))) y)))
+ (when dump-command
+ (let (i (print-args (copy-object args)) command-init command-setup command-args command-id)
+ (dotimes (j (count :link-list print-args))
+ (if (setq i (position :link-list print-args :count (1+ j)))
+ (cond ((atom (car (elt print-args (+ i 1))))
+ (setf (elt print-args (+ i 1)) (append '(list) (mapcar #'(lambda (x) `(send r ,(send x :name))) (elt print-args (+ i 1))))))
+ (t
+ (setf (elt print-args (+ i 1))
+ (append '(list)
+ (mapcar #'(lambda (y)
+ (append '(list) (mapcar #'(lambda (x) `(send r ,(send x :name))) y)))
(elt print-args (+ i 1)))))))))
- (dotimes (j (count :move-target print-args))
- (if (setq i (position :move-target print-args :count (1+ j)))
- (cond ((atom (elt print-args (+ i 1)))
- (let ((x (elt print-args (+ i 1))))
- (setf (elt print-args (+ i 1)) `(let* ((p (send r ,(send (send x :parent) :name))) (c (make-cascoords :coords (send (send p :copy-worldcoords) :transform (make-cascoords :4x4 ,(send x :4x4))) :parent p))) c))))
- (t
- (setf (elt print-args (+ i 1))
- (append '(list)
- (mapcar #'(lambda (x)
- `(let* ((p (send r ,(send (send x :parent) :name))) (c (make-cascoords :coords (send (send p :copy-worldcoords) :transform (make-cascoords :4x4 ,(send x :4x4))) :parent p))) c))
- (elt print-args (+ i 1)))))))))
- (setq command-init `(instance ,(send (class self) :name) :init)
- command-setup `(progn (send r :newcoords (make-coords :4x4 ,(send self :4x4))) (mapc #'(lambda (j a) (send* j :joint-angle a ,joint-args)) (list . ,(mapcar #'(lambda (x) `(send r ,(intern (string-upcase x) *keyword-package*))) (send-all (remove-duplicates (append (send-all union-link-list :joint) joint-list)) :name))) ,(list 'quote av0)) (objects (list r)))
- command-ik `(send r :inverse-kinematics ,(cons 'list (mapcar #'(lambda (x) `(make-coords :pos ,(send x :worldpos) :rot ,(send x :worldrot))) target-coords)) . ,print-args))
- (warn ";; command : ~a~%" `(let ((r ,command-init)) ,command-setup ,command-ik))
- )
- )
+ (dotimes (j (count :move-target print-args))
+ (if (setq i (position :move-target print-args :count (1+ j)))
+ (cond ((atom (elt print-args (+ i 1)))
+ (let ((x (elt print-args (+ i 1))))
+ (setf (elt print-args (+ i 1)) `(let* ((p (send r ,(send (send x :parent) :name))) (c (make-cascoords :coords (send (send p :copy-worldcoords) :transform (make-cascoords :4x4 ,(send x :4x4))) :parent p))) c))))
+ (t
+ (setf (elt print-args (+ i 1))
+ (append '(list)
+ (mapcar #'(lambda (x)
+ `(let* ((p (send r ,(send (send x :parent) :name))) (c (make-cascoords :coords (send (send p :copy-worldcoords) :transform (make-cascoords :4x4 ,(send x :4x4))) :parent p))) c))
+ (elt print-args (+ i 1)))))))))
+ (setq command-init `(instance ,(send (class self) :name) :init)
+ command-setup `(progn (send r :newcoords (make-coords :4x4 ,(send self :4x4))) (mapc #'(lambda (j a) (send* j :joint-angle a ,joint-args)) (list . ,(mapcar #'(lambda (x) `(send r ,(intern (string-upcase x) *keyword-package*))) (send-all (remove-duplicates (append (send-all union-link-list :joint) joint-list)) :name))) ,(list 'quote av0)) (objects (list r)))
+ command-args (nconc `(list . ,(mapcar #'(lambda (x) `(make-coords :pos ,(send x :worldpos) :rot ,(send x :worldrot))) target-coords)) (list :dump-command nil :debug-view t) print-args)
+ command-id (let ((lt (unix::localtime))) (substitute #\0 #\ (format nil "~A-~04d-~02d-~02d-~02d-~02d-~02d" (send (class self) :name) (+ 1900 (elt lt 5)) (elt lt 4) (elt lt 3) (elt lt 2) (elt lt 1) (elt lt 0)))))
+ (warn ";; command : ~a~%" `(let ((r ,command-init)) ,command-setup (send* r :inverse-kinematics ,command-args)))
+ (warn ";; dump debug command to /~A/~A.l~%" dump-command command-id)
+ (warn ";; (progn (load \"/~A/~A.l\")(ik-setup)(ik-check))~%" dump-command command-id)
+ ;; dump
+ (with-open-file
+ (f (format nil "/~A/~A.l" dump-command command-id) :direction :output)
+ (format f ";; ik ~A log at ~A on ~A~%~%" (if success "success" "fail") (string-trim '(10) (unix:asctime (unix:localtime))) lisp::lisp-implementation-version)
+ (format f "(defun ~A-setup () (let ((r ~A)) (setq *robot* r) ~A (objects (list *robot*))))~%" command-id command-init command-setup)
+ (format f "(defun ~A-check () (send* *robot* :inverse-kinematics ~A))~%" command-id command-args)
+ (format f "(defun ik-setup () (~A-setup))~%" command-id)
+ (format f "(defun ik-check () (~A-check))~%" command-id)
+ )
+ ) ;;let
+ ) ;; dump
+ ) ;; warnp
(mapc #'(lambda (j a) (send* j :joint-angle a joint-args))
(remove-duplicates
(append (send-all union-link-list :joint) joint-list)) av0)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <k-...@us...> - 2012-03-28 11:52:32
|
Revision: 785
http://jskeus.svn.sourceforge.net/jskeus/?rev=785&view=rev
Author: k-okada
Date: 2012-03-28 11:52:26 +0000 (Wed, 28 Mar 2012)
Log Message:
-----------
clean up :command debug code in :inverse-kinematics
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2012-03-28 11:40:29 UTC (rev 784)
+++ trunk/irteus/irtmodel.l 2012-03-28 11:52:26 UTC (rev 785)
@@ -1796,7 +1796,7 @@
(send (let ((p self)) (while (send p :parent) (setq p (send p :parent))) p) :worldcoords))
(warn ";; angles : ~a~%" av0)
(warn ";; args : ~a~%" (append (list target-coords) args))
- (let (i (print-args (copy-object args)))
+ (let (i (print-args (copy-object args)) command-init command-setup command-ik)
(dotimes (j (count :link-list print-args))
(if (setq i (position :link-list print-args :count (1+ j)))
(cond ((atom (car (elt print-args (+ i 1))))
@@ -1818,7 +1818,10 @@
(mapcar #'(lambda (x)
`(let* ((p (send r ,(send (send x :parent) :name))) (c (make-cascoords :coords (send (send p :copy-worldcoords) :transform (make-cascoords :4x4 ,(send x :4x4))) :parent p))) c))
(elt print-args (+ i 1)))))))))
- (warn ";; command : ~a~%" `(let ((r (instance ,(send (class self) :name) :init))) (send r :newcoords (make-coords :4x4 ,(send self :4x4))) (mapc #'(lambda (j a) (send* j :joint-angle a ,joint-args)) ,(append '(list) (mapcar #'(lambda (x) `(send r ,(intern (string-upcase x) *keyword-package*))) (send-all (remove-duplicates (append (send-all union-link-list :joint) joint-list)) :name))) ,(append '(list) av0)) (objects (list r)) (send r :inverse-kinematics ,(append '(list) (mapcar #'(lambda (x) `(make-coords :pos ,(send x :worldpos) :rot ,(send x :worldrot))) target-coords)) . ,print-args)))
+ (setq command-init `(instance ,(send (class self) :name) :init)
+ command-setup `(progn (send r :newcoords (make-coords :4x4 ,(send self :4x4))) (mapc #'(lambda (j a) (send* j :joint-angle a ,joint-args)) (list . ,(mapcar #'(lambda (x) `(send r ,(intern (string-upcase x) *keyword-package*))) (send-all (remove-duplicates (append (send-all union-link-list :joint) joint-list)) :name))) ,(list 'quote av0)) (objects (list r)))
+ command-ik `(send r :inverse-kinematics ,(cons 'list (mapcar #'(lambda (x) `(make-coords :pos ,(send x :worldpos) :rot ,(send x :worldrot))) target-coords)) . ,print-args))
+ (warn ";; command : ~a~%" `(let ((r ,command-init)) ,command-setup ,command-ik))
)
)
(mapc #'(lambda (j a) (send* j :joint-angle a joint-args))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <je...@js...> - 2012-03-28 11:45:59
|
See <http://jenkins.jsk.imi.i.u-tokyo.ac.jp:8080/job/jskeus/74/> |
|
From: <k-...@us...> - 2012-03-28 11:40:38
|
Revision: 784
http://jskeus.svn.sourceforge.net/jskeus/?rev=784&view=rev
Author: k-okada
Date: 2012-03-28 11:40:29 +0000 (Wed, 28 Mar 2012)
Log Message:
-----------
do not destroy arguments copy args to print-args
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2012-03-28 10:31:17 UTC (rev 783)
+++ trunk/irteus/irtmodel.l 2012-03-28 11:40:29 UTC (rev 784)
@@ -1694,6 +1694,7 @@
(append (send-all union-link-list :joint) joint-list))
:joint-angle))
(c0 (unless (send self :parent) (send self :copy-worldcoords)))
+ (target-coords0 target-coords)
dif-pos dif-rot
;;
(success t)
@@ -1795,29 +1796,29 @@
(send (let ((p self)) (while (send p :parent) (setq p (send p :parent))) p) :worldcoords))
(warn ";; angles : ~a~%" av0)
(warn ";; args : ~a~%" (append (list target-coords) args))
- (let (i)
- (dotimes (j (count :link-list args))
- (if (setq i (position :link-list args :count (1+ j)))
- (cond ((atom (car (elt args (+ i 1))))
- (setf (elt args (+ i 1)) (append '(list) (mapcar #'(lambda (x) `(send r ,(send x :name))) (elt args (+ i 1))))))
+ (let (i (print-args (copy-object args)))
+ (dotimes (j (count :link-list print-args))
+ (if (setq i (position :link-list print-args :count (1+ j)))
+ (cond ((atom (car (elt print-args (+ i 1))))
+ (setf (elt print-args (+ i 1)) (append '(list) (mapcar #'(lambda (x) `(send r ,(send x :name))) (elt print-args (+ i 1))))))
(t
- (setf (elt args (+ i 1))
+ (setf (elt print-args (+ i 1))
(append '(list)
(mapcar #'(lambda (y)
(append '(list) (mapcar #'(lambda (x) `(send r ,(send x :name))) y)))
- (elt args (+ i 1)))))))))
- (dotimes (j (count :move-target args))
- (if (setq i (position :move-target args :count (1+ j)))
- (cond ((atom (elt args (+ i 1)))
- (let ((x (elt args (+ i 1))))
- (setf (elt args (+ i 1)) `(let* ((p (send r ,(send (send x :parent) :name))) (c (make-cascoords :coords (send (send p :copy-worldcoords) :transform (make-cascoords :4x4 ,(send x :4x4))) :parent p))) c))))
+ (elt print-args (+ i 1)))))))))
+ (dotimes (j (count :move-target print-args))
+ (if (setq i (position :move-target print-args :count (1+ j)))
+ (cond ((atom (elt print-args (+ i 1)))
+ (let ((x (elt print-args (+ i 1))))
+ (setf (elt print-args (+ i 1)) `(let* ((p (send r ,(send (send x :parent) :name))) (c (make-cascoords :coords (send (send p :copy-worldcoords) :transform (make-cascoords :4x4 ,(send x :4x4))) :parent p))) c))))
(t
- (setf (elt args (+ i 1))
+ (setf (elt print-args (+ i 1))
(append '(list)
(mapcar #'(lambda (x)
`(let* ((p (send r ,(send (send x :parent) :name))) (c (make-cascoords :coords (send (send p :copy-worldcoords) :transform (make-cascoords :4x4 ,(send x :4x4))) :parent p))) c))
- (elt args (+ i 1)))))))))
- (warn ";; command : ~a~%" `(let ((r (instance ,(send (class self) :name) :init))) (send r :newcoords (make-coords :4x4 ,(send self :4x4))) (mapc #'(lambda (j a) (send* j :joint-angle a ,joint-args)) ,(append '(list) (mapcar #'(lambda (x) `(send r ,(intern (string-upcase x) *keyword-package*))) (send-all (remove-duplicates (append (send-all union-link-list :joint) joint-list)) :name))) ,(append '(list) av0)) (objects (list r)) (send r :inverse-kinematics ,(append '(list) (mapcar #'(lambda (x) `(make-coords :pos ,(send x :worldpos) :rot ,(send x :worldrot))) target-coords)) . ,args)))
+ (elt print-args (+ i 1)))))))))
+ (warn ";; command : ~a~%" `(let ((r (instance ,(send (class self) :name) :init))) (send r :newcoords (make-coords :4x4 ,(send self :4x4))) (mapc #'(lambda (j a) (send* j :joint-angle a ,joint-args)) ,(append '(list) (mapcar #'(lambda (x) `(send r ,(intern (string-upcase x) *keyword-package*))) (send-all (remove-duplicates (append (send-all union-link-list :joint) joint-list)) :name))) ,(append '(list) av0)) (objects (list r)) (send r :inverse-kinematics ,(append '(list) (mapcar #'(lambda (x) `(make-coords :pos ,(send x :worldpos) :rot ,(send x :worldrot))) target-coords)) . ,print-args)))
)
)
(mapc #'(lambda (j a) (send* j :joint-angle a joint-args))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <je...@js...> - 2012-03-28 10:33:04
|
See <http://jenkins.jsk.imi.i.u-tokyo.ac.jp:8080/job/jskeus/73/> ------------------------------------------ Started by an SCM change Building on master in workspace <http://jenkins.jsk.imi.i.u-tokyo.ac.jp:8080/job/jskeus/ws/> 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 A irteus/irtgraph.l AU irteus/Makefile.Linux AU irteus/irtgl.l 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 A irteus/irtcollada.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:298) at org.tmatesoft.svn.core.internal.io.dav.http.HTTPConnection.request(HTTPConnection.java:283) at org.tmatesoft.svn.core.internal.io.dav.http.HTTPConnection.request(HTTPConnection.java:271) at org.tmatesoft.svn.core.internal.io.dav.DAVConnection.doReport(DAVConnection.java:283) at org.tmatesoft.svn.core.internal.io.dav.DAVRepository.runReport(DAVRepository.java:1282) at org.tmatesoft.svn.core.internal.io.dav.DAVRepository.update(DAVRepository.java:830) 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:84) at hudson.scm.subversion.WorkspaceUpdater$UpdateTask.delegateTo(WorkspaceUpdater.java:136) at hudson.scm.SubversionSCM$CheckOutTask.perform(SubversionSCM.java:787) at hudson.scm.SubversionSCM$CheckOutTask.invoke(SubversionSCM.java:768) at hudson.scm.SubversionSCM$CheckOutTask.invoke(SubversionSCM.java:752) at hudson.FilePath.act(FilePath.java:788) at hudson.FilePath.act(FilePath.java:770) at hudson.scm.SubversionSCM.checkout(SubversionSCM.java:742) at hudson.scm.SubversionSCM.checkout(SubversionSCM.java:684) at hudson.model.AbstractProject.checkout(AbstractProject.java:1195) at hudson.model.AbstractBuild$AbstractRunner.checkout(AbstractBuild.java:576) at hudson.model.AbstractBuild$AbstractRunner.run(AbstractBuild.java:465) at hudson.model.Run.run(Run.java:1409) at hudson.model.FreeStyleBuild.run(FreeStyleBuild.java:46) at hudson.model.ResourceController.execute(ResourceController.java:88) at hudson.model.Executor.run(Executor.java:238) 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:662) at org.tmatesoft.svn.core.internal.io.dav.http.HTTPConnection.request(HTTPConnection.java:292) ... 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:660) ... 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:426) ... 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.peekChar(XMLEntityScanner.java:487) at com.sun.org.apache.xerces.internal.impl.XMLDocumentFragmentScannerImpl$FragmentContentDriver.next(XMLDocumentFragmentScannerImpl.java:2687) 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:776) at org.tmatesoft.svn.core.internal.io.dav.http.HTTPConnection.readData(HTTPConnection.java:741) at org.tmatesoft.svn.core.internal.io.dav.http.HTTPRequest.dispatch(HTTPRequest.java:218) at org.tmatesoft.svn.core.internal.io.dav.http.HTTPConnection._request(HTTPConnection.java:379) ... 24 more FATAL: null java.lang.NullPointerException at java.util.ArrayList.addAll(ArrayList.java:472) at hudson.scm.SubversionSCM.checkout(SubversionSCM.java:742) at hudson.scm.SubversionSCM.checkout(SubversionSCM.java:684) at hudson.model.AbstractProject.checkout(AbstractProject.java:1195) at hudson.model.AbstractBuild$AbstractRunner.checkout(AbstractBuild.java:576) at hudson.model.AbstractBuild$AbstractRunner.run(AbstractBuild.java:465) at hudson.model.Run.run(Run.java:1409) at hudson.model.FreeStyleBuild.run(FreeStyleBuild.java:46) at hudson.model.ResourceController.execute(ResourceController.java:88) at hudson.model.Executor.run(Executor.java:238) |
|
From: <k-...@us...> - 2012-03-28 10:31:24
|
Revision: 783
http://jskeus.svn.sourceforge.net/jskeus/?rev=783&view=rev
Author: k-okada
Date: 2012-03-28 10:31:17 +0000 (Wed, 28 Mar 2012)
Log Message:
-----------
minor fix : print s-expresion that re-produce failed ik commmand #666
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2012-03-28 10:20:23 UTC (rev 782)
+++ trunk/irteus/irtmodel.l 2012-03-28 10:31:17 UTC (rev 783)
@@ -1817,7 +1817,7 @@
(mapcar #'(lambda (x)
`(let* ((p (send r ,(send (send x :parent) :name))) (c (make-cascoords :coords (send (send p :copy-worldcoords) :transform (make-cascoords :4x4 ,(send x :4x4))) :parent p))) c))
(elt args (+ i 1)))))))))
- (warn ";; command : ~a~%" `(let ((r (instance ,(send (class self) :name) :init))) (send r :newcoords (make-coords :4x4 ,(send self :4x4))) (mapc #'(lambda (j a) (send* j :joint-angle a ,joint-args)) ,(append '(list) (mapcar #'(lambda (x) `(send r ,(intern (string-upcase x) *keyword-package*))) (send-all (remove-duplicates (append (send-all union-link-list :joint) joint-list)) :name))) ,(append '(list) av0)) (objects (list r)) (send* r :inverse-kinematics ,(append '(list) (mapcar #'(lambda (x) `(make-coords :pos ,(send x :worldpos) :rot ,(send x :worldrot))) target-coords)) ,(append '(list) args))))
+ (warn ";; command : ~a~%" `(let ((r (instance ,(send (class self) :name) :init))) (send r :newcoords (make-coords :4x4 ,(send self :4x4))) (mapc #'(lambda (j a) (send* j :joint-angle a ,joint-args)) ,(append '(list) (mapcar #'(lambda (x) `(send r ,(intern (string-upcase x) *keyword-package*))) (send-all (remove-duplicates (append (send-all union-link-list :joint) joint-list)) :name))) ,(append '(list) av0)) (objects (list r)) (send r :inverse-kinematics ,(append '(list) (mapcar #'(lambda (x) `(make-coords :pos ,(send x :worldpos) :rot ,(send x :worldrot))) target-coords)) . ,args)))
)
)
(mapc #'(lambda (j a) (send* j :joint-angle a joint-args))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
