jskeus-commit — jskeus commit mailing list

[jskeus-commit] SF.net SVN: jskeus:[667] trunk/irteus/irtutil.l

[<sn...@us...>]

Revision: 667
          http://jskeus.svn.sourceforge.net/jskeus/?rev=667&view=rev
Author:   snozawa
Date:     2011-07-07 12:45:59 +0000 (Thu, 07 Jul 2011)

Log Message:
-----------
add use-line-break argument to neglect linebreak

Modified Paths:
--------------
    trunk/irteus/irtutil.l

Modified: trunk/irteus/irtutil.l
===================================================================
--- trunk/irteus/irtutil.l	2011-07-06 23:49:47 UTC (rev 666)
+++ trunk/irteus/irtutil.l	2011-07-07 12:45:59 UTC (rev 667)
@@ -161,7 +161,7 @@
 	   nil))
     ))
 
-(defun format-array (arr &optional (header "") (in 7) (fl 3) (strm *error-output*))
+(defun format-array (arr &optional (header "") (in 7) (fl 3) (strm *error-output*) (use-line-break t))
   "arr &optional (header \"\") (in 7) (fl 3) (strm *error-output*);; for format-print array"
   (let* ((val-format (case (send arr :element-type)
                            (:integer (format nil "~~~dd " in))
@@ -172,13 +172,13 @@
      ((derivedp arr vector)
       (dotimes (i (length arr))
         (push (format nil val-format (elt arr i)) str-l))
-      (push "~%" str-l))
+      (if use-line-break (push "~%" str-l)))
      ((derivedp arr array)
       (dotimes (j (car (array-dimensions arr)))
         (if (/= j 0) (push (format nil str-format " ") str-l))
         (dotimes (i (cadr (array-dimensions arr)))
           (push (format nil val-format (aref arr j i)) str-l))
-        (push "~%" str-l))))
+        (if use-line-break (push "~%" str-l)))))
     (let ((ret (format strm (apply #'concatenate string (nreverse str-l)))))
       (if strm arr ret))))
 


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

[jskeus-commit] SF.net SVN: jskeus:[666] trunk/irteus/irtrobot.l

[<k-...@us...>]

Revision: 666
          http://jskeus.svn.sourceforge.net/jskeus/?rev=666&view=rev
Author:   k-okada
Date:     2011-07-06 23:49:47 +0000 (Wed, 06 Jul 2011)

Log Message:
-----------
fix typo warnig-color -> warning-color

Modified Paths:
--------------
    trunk/irteus/irtrobot.l

Modified: trunk/irteus/irtrobot.l
===================================================================
--- trunk/irteus/irtrobot.l	2011-07-04 04:36:55 UTC (rev 665)
+++ trunk/irteus/irtrobot.l	2011-07-06 23:49:47 UTC (rev 666)
@@ -285,7 +285,7 @@
 		   (v* (float-vector 0 0 (if (> rtorque 0.0) 1.0 -1.0)) paxis))))
 	       (default-width width) (default-color color))
 	  (when (and torque-threshold (> (abs rtorque) torque-threshold))
-	    (setq width (* 2 width) color warnig-color))
+	    (setq width (* 2 width) color warning-color))
 	  (send vwer :viewsurface :line-width width)
 	  (send vwer :viewsurface :color color)
 	  (send vwer :draw-circle


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

[jskeus-commit] SF.net SVN: jskeus:[665] trunk/irteus/irtrobot.l

[<k-...@us...>]

Revision: 665
          http://jskeus.svn.sourceforge.net/jskeus/?rev=665&view=rev
Author:   k-okada
Date:     2011-07-04 04:36:55 +0000 (Mon, 04 Jul 2011)

Log Message:
-----------
add draw-torque method to robot-model class

Modified Paths:
--------------
    trunk/irteus/irtrobot.l

Modified: trunk/irteus/irtrobot.l
===================================================================
--- trunk/irteus/irtrobot.l	2011-06-24 05:57:02 UTC (rev 664)
+++ trunk/irteus/irtrobot.l	2011-07-04 04:36:55 UTC (rev 665)
@@ -265,6 +265,37 @@
                :debug-view debug-view
                :jvv jvv :jav jav)
    )
+  (:draw-torque
+   (vwer &key flush (width 2) (size 100) (color (float-vector 1 0.3 0)) (warning-color (float-vector 1 0 0)) (torque-threshold nil) (torque-vector (send self :torque-vector)))
+   (mapcar
+    #'(lambda (j r)
+	(let* ((rtorque (/ r (send j :max-joint-torque)))
+	       (paxis (send j :child-link :rotate-vector
+			    (case (j . axis)
+			      (:x #f(1 0 0)) (:y #f(0 1 0))
+			      (:z #f(0 0 1)) (:-x #f(-1 0 0))
+			      (:-y #f(0 -1 0)) (:-z #f(0 0 -1))
+			      (t (j . axis)))))
+	       (rot-th (acos (v. (float-vector 0 0 1) paxis)))
+	       (rot
+		(if (eps= rot-th 0.0)
+		    (unit-matrix 3)
+		  (rotation-matrix
+		   rot-th
+		   (v* (float-vector 0 0 (if (> rtorque 0.0) 1.0 -1.0)) paxis))))
+	       (default-width width) (default-color color))
+	  (when (and torque-threshold (> (abs rtorque) torque-threshold))
+	    (setq width (* 2 width) color warnig-color))
+	  (send vwer :viewsurface :line-width width)
+	  (send vwer :viewsurface :color color)
+	  (send vwer :draw-circle
+		(make-coords :pos (send j :child-link :worldpos) :rot rot)
+		:radius (* size (abs rtorque)) :arrow t :arc (deg2rad 330))
+	  ))
+    (send self :joint-list)
+    (coerce torque-vector cons))
+   (if flush (send vwer :viewsurface :flush))
+   );; draw-torque
   )
 
 (in-package "GEOMETRY")


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

[jskeus-commit] SF.net SVN: jskeus:[664] trunk/irteus/irtdyna.l

[<sn...@us...>]

Revision: 664
          http://jskeus.svn.sourceforge.net/jskeus/?rev=664&view=rev
Author:   snozawa
Date:     2011-06-24 05:57:02 +0000 (Fri, 24 Jun 2011)

Log Message:
-----------
add comments and add buffer reuse codes

Modified Paths:
--------------
    trunk/irteus/irtdyna.l

Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l	2011-06-24 05:20:21 UTC (rev 663)
+++ trunk/irteus/irtdyna.l	2011-06-24 05:57:02 UTC (rev 664)
@@ -175,6 +175,7 @@
   )
 
 (defmethod bodyset-link
+  ;; user shuold not call :append-weight-no-update, :append-centroid-no-update, and :append-inertia-no-update directly because these methods are for :append-mass-properties
   (:append-weight-no-update
    (additional-weights
     &key (len (length additional-weights)))
@@ -185,49 +186,58 @@
   (:append-centroid-no-update
    (additional-weights
     additional-centroids
-    new-weight
+    self-centroid new-weight
     &key (tmp-va (float-vector 0 0 0))
+         (tmp-vb (float-vector 0 0 0))
          (len (length additional-weights)))
    (scale
     (/ 1.0 new-weight)
-    (let ((ret (scale (send self :weight) (send self :centroid))))
+    (let ((ret (scale (send self :weight) self-centroid tmp-vb)))
       (dotimes (i len)
 	(v+ ret (scale (elt additional-weights i) (elt additional-centroids i) tmp-va) ret))
-      ret)))
+      ret)
+    tmp-vb))
   (:append-inertia-no-update
    (additional-weights
     additional-centroids
     additional-inertias
-    new-centroid
+    self-centroid new-centroid
     &key (tmp-ma (make-matrix 3 3))
          (tmp-mb (make-matrix 3 3))
          (tmp-mc (make-matrix 3 3))
+	 (tmp-md (make-matrix 3 3))
+	 (tmp-va (float-vector 0 0 0))
          (len (length additional-weights)))
    (labels
        ((DD (r)
 	    (let ((r2 (outer-product-matrix r tmp-ma)))
 	      (m* (transpose r2 tmp-mb) r2 tmp-mc))))
      (let ((ret (m* (m* (send self :worldrot) (send self :inertia-tensor) tmp-ma)
-		    (transpose (send self :worldrot) tmp-mb))))
-       (m+ ret
-	   (scale-matrix
+		    (transpose (send self :worldrot) tmp-mb)
+		    tmp-md)))
+       (m+ (scale-matrix
 	    (send self :weight)
-	    (DD (v- (send self :centroid) new-centroid)) tmp-ma)
-	   ret)
+	    (DD (v- self-centroid new-centroid tmp-va)) tmp-ma)
+	   ret ret)
        (dotimes (i len)
 	 (m+ (elt additional-inertias i) ret ret)
 	 (m+ (scale-matrix (elt additional-weights i)
-			   (DD (v- (elt additional-centroids i) new-centroid))
+			   (DD (v- (elt additional-centroids i) new-centroid tmp-va))
 			   tmp-ma)
 	     ret ret))
        ret)))
+  ;; append mass properties of additional-links to self link
   (:append-mass-properties
    (additional-links
-    &key (update t)
+    &key (update t) ;; if update is nil, mass properties of self link is not updated.
+         ;; buffers
          (tmp-va (float-vector 0 0 0))
+         (tmp-vb (float-vector 0 0 0))
          (tmp-ma (make-matrix 3 3))
          (tmp-mb (make-matrix 3 3))
          (tmp-mc (make-matrix 3 3))
+	 (tmp-md (make-matrix 3 3))
+	 ;;   centroid and inertia-tensor : world
          (additional-weights
 	  (send-all additional-links :weight))
 	 (additional-centroids
@@ -236,18 +246,21 @@
 	  (mapcar #'(lambda (x)
 		      (m* (m* (send x :worldrot) (send x :inertia-tensor) tmp-ma)
 			  (transpose (send x :worldrot) tmp-mb)))
-		  additional-links)))
+		  additional-links))
+	 (self-centroid (send self :centroid)))
    (let* ((len (length additional-links))
 	  (new-weight
 	   (send self :append-weight-no-update additional-weights :len len))
-	  (new-centroid
+	  (new-centroid ;; <- tmp-vb's buffer
 	   (send self :append-centroid-no-update additional-weights additional-centroids
-		 new-weight :tmp-va tmp-va :len len))
-	  (new-inertia
+		 self-centroid new-weight :tmp-va tmp-va :tmp-vb tmp-vb :len len))
+	  (new-inertia ;; <- tmp-md's buffer
 	   (send self :append-inertia-no-update
 		 additional-weights additional-centroids
-		 additional-inertias new-centroid
-		 :tmp-ma tmp-ma :tmp-mb tmp-mb :tmp-mc tmp-mc :len len)))
+		 additional-inertias self-centroid new-centroid
+		 :tmp-ma tmp-ma :tmp-mb tmp-mb :tmp-mc tmp-mc :tmp-md tmp-md :tmp-va tmp-va :len len)))
+     ;; update mass properties of self links
+     ;;   centroid and inertia-tensor : world -> local
      (when update
        (send self :weight new-weight)
        (setq acentroid (send self :inverse-transform-vector new-centroid acentroid tmp-va tmp-ma))
@@ -255,8 +268,7 @@
 	     (m* (m* (transpose (send self :worldrot) tmp-ma) new-inertia tmp-mb)
 		 (send self :worldrot)
 		 (send self :inertia-tensor))))
-     (list new-weight new-centroid new-inertia)
-     ))
+     (list new-weight new-centroid new-inertia)))
   ;; propagate mass properties
   ;;   unit system ;; [g] [mm]
   ;;   m-til -> the mass of all link structure driven by joint of this bodyset-link.
@@ -270,35 +282,30 @@
 	  (tmp-mb (make-matrix 3 3))
 	  (tmp-mc (make-matrix 3 3))
     &allow-other-keys)
-    (let ((cen (send self :centroid)) ;; [mm]
-	  (wei (send self :weight)) ;; [g]
-	  (iner (m* (m* (send self :worldrot) (send self :inertia-tensor) tmp-ma)
-		    (transpose (send self :worldrot) tmp-mb)
-		    (send self :get :I-til)))) ;; [g mm^2]
-      (if child-links
-	  (progn
-	    ;; propagation of m-til and c-til from children
-	    (dolist (child child-links)
-	      (send child :propagate-mass-properties :debug-view debug-view
-		    :tmp-va tmp-va :tmp-vb tmp-vb
-		    :tmp-ma tmp-ma :tmp-mb tmp-mb :tmp-mc tmp-mc))
-	    (let ((ret
-		   (send self :append-mass-properties
-			 child-links
-			 :tmp-ma tmp-ma :tmp-mb tmp-mb :tmp-mc tmp-mc
-			 :tmp-va tmp-va
-			 :additional-weights (send-all child-links :get :m-til)
-			 :additional-centroids (send-all child-links :get :c-til)
-			 :additional-inertias (send-all child-links :get :i-til)
-			 :update nil)))
-	      (send self :put :m-til (car ret))
-	      (send self :put :c-til (cadr ret))
-	      (send self :put :i-til (caddr ret))
-	      ))
-	(progn ;; if end-link
-	  (dotimes (i 3) (setf (elt (send self :get :c-til) i) (elt cen i)))
-	  (send self :put :m-til wei)
+    ;; weight [g], centroid [mm], inertia [g mm^2]
+    (if child-links
+	(progn
+	  ;; propagation of m-til and c-til from children
+	  (dolist (child child-links)
+	    (send child :propagate-mass-properties :debug-view debug-view
+		  :tmp-va tmp-va :tmp-vb tmp-vb
+		  :tmp-ma tmp-ma :tmp-mb tmp-mb :tmp-mc tmp-mc))
+	  ;; update m-til, c-til, and i-til
+	  ;;  c-til and i-til are arleady copied by using tmp-vb and tmp-md buffer
+	  (send self :put :m-til
+		(car (send self :append-mass-properties
+			   child-links
+			   :tmp-ma tmp-ma :tmp-mb tmp-mb :tmp-mc tmp-mc :tmp-md (send self :get :i-til)
+			   :tmp-va tmp-va :tmp-vb (send self :get :c-til)
+			   :additional-weights (send-all child-links :get :m-til)
+			   :additional-centroids (send-all child-links :get :c-til)
+			   :additional-inertias (send-all child-links :get :i-til)
+			   :self-centroid (send self :centroid) :update nil)))
 	  )
+      ;; if end-link
+      (let ((cen (send self :centroid)))
+	(dotimes (i 3) (setf (elt (send self :get :c-til) i) (elt cen i)))
+	(send self :put :m-til (send self :weight))
 	))
     (if debug-view
 	(warn ";; joint = ~A ;; m-til = ~A[g], c-til = ~A[mm]~%"


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

[jskeus-commit] SF.net SVN: jskeus:[663] trunk/irteus/irtdyna.l

[<sn...@us...>]

Revision: 663
          http://jskeus.svn.sourceforge.net/jskeus/?rev=663&view=rev
Author:   snozawa
Date:     2011-06-24 05:20:21 +0000 (Fri, 24 Jun 2011)

Log Message:
-----------
update :propagate-mass-properties method ;; separate append-mass-properties calculation from this method

Modified Paths:
--------------
    trunk/irteus/irtdyna.l

Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l	2011-06-23 10:21:30 UTC (rev 662)
+++ trunk/irteus/irtdyna.l	2011-06-24 05:20:21 UTC (rev 663)
@@ -175,6 +175,88 @@
   )
 
 (defmethod bodyset-link
+  (:append-weight-no-update
+   (additional-weights
+    &key (len (length additional-weights)))
+   (let ((ret (send self :weight)))
+     (dotimes (i len)
+       (setq ret (+ (elt additional-weights i) ret)))
+     ret))
+  (:append-centroid-no-update
+   (additional-weights
+    additional-centroids
+    new-weight
+    &key (tmp-va (float-vector 0 0 0))
+         (len (length additional-weights)))
+   (scale
+    (/ 1.0 new-weight)
+    (let ((ret (scale (send self :weight) (send self :centroid))))
+      (dotimes (i len)
+	(v+ ret (scale (elt additional-weights i) (elt additional-centroids i) tmp-va) ret))
+      ret)))
+  (:append-inertia-no-update
+   (additional-weights
+    additional-centroids
+    additional-inertias
+    new-centroid
+    &key (tmp-ma (make-matrix 3 3))
+         (tmp-mb (make-matrix 3 3))
+         (tmp-mc (make-matrix 3 3))
+         (len (length additional-weights)))
+   (labels
+       ((DD (r)
+	    (let ((r2 (outer-product-matrix r tmp-ma)))
+	      (m* (transpose r2 tmp-mb) r2 tmp-mc))))
+     (let ((ret (m* (m* (send self :worldrot) (send self :inertia-tensor) tmp-ma)
+		    (transpose (send self :worldrot) tmp-mb))))
+       (m+ ret
+	   (scale-matrix
+	    (send self :weight)
+	    (DD (v- (send self :centroid) new-centroid)) tmp-ma)
+	   ret)
+       (dotimes (i len)
+	 (m+ (elt additional-inertias i) ret ret)
+	 (m+ (scale-matrix (elt additional-weights i)
+			   (DD (v- (elt additional-centroids i) new-centroid))
+			   tmp-ma)
+	     ret ret))
+       ret)))
+  (:append-mass-properties
+   (additional-links
+    &key (update t)
+         (tmp-va (float-vector 0 0 0))
+         (tmp-ma (make-matrix 3 3))
+         (tmp-mb (make-matrix 3 3))
+         (tmp-mc (make-matrix 3 3))
+         (additional-weights
+	  (send-all additional-links :weight))
+	 (additional-centroids
+	  (send-all additional-links :centroid))
+	 (additional-inertias
+	  (mapcar #'(lambda (x)
+		      (m* (m* (send x :worldrot) (send x :inertia-tensor) tmp-ma)
+			  (transpose (send x :worldrot) tmp-mb)))
+		  additional-links)))
+   (let* ((len (length additional-links))
+	  (new-weight
+	   (send self :append-weight-no-update additional-weights :len len))
+	  (new-centroid
+	   (send self :append-centroid-no-update additional-weights additional-centroids
+		 new-weight :tmp-va tmp-va :len len))
+	  (new-inertia
+	   (send self :append-inertia-no-update
+		 additional-weights additional-centroids
+		 additional-inertias new-centroid
+		 :tmp-ma tmp-ma :tmp-mb tmp-mb :tmp-mc tmp-mc :len len)))
+     (when update
+       (send self :weight new-weight)
+       (setq acentroid (send self :inverse-transform-vector new-centroid acentroid tmp-va tmp-ma))
+       (send self :inertia-tensor
+	     (m* (m* (transpose (send self :worldrot) tmp-ma) new-inertia tmp-mb)
+		 (send self :worldrot)
+		 (send self :inertia-tensor))))
+     (list new-weight new-centroid new-inertia)
+     ))
   ;; propagate mass properties
   ;;   unit system ;; [g] [mm]
   ;;   m-til -> the mass of all link structure driven by joint of this bodyset-link.
@@ -194,34 +276,25 @@
 		    (transpose (send self :worldrot) tmp-mb)
 		    (send self :get :I-til)))) ;; [g mm^2]
       (if child-links
-	  (labels
-	   ((update-mass-properties
-	     (additional-wei additional-cen)
-	     (let ((tm (+ additional-wei (send self :get :m-til))))
-	       (v+ (scale (/ additional-wei tm) additional-cen tmp-va)
-		   (scale (/ (send self :get :m-til) tm) (send self :get :c-til) tmp-vb)
-		   (send self :get :c-til))
-	       (send self :put :m-til tm)))
-	    (DD (r)
-              (let ((r2 (outer-product-matrix r tmp-ma)))
-		(m* (transpose r2 tmp-mb) r2 tmp-mc))))
-           ;; propagation of m-til and c-til from children
-	   (dolist (child child-links)
-	     (send child :propagate-mass-properties :debug-view debug-view)
-	     (update-mass-properties (send child :get :m-til) (send child :get :c-til)))
-	   ;; calculation of m-til and c-til from children and self
-	   (update-mass-properties wei cen)
-	   ;; calculation of I-til from children and self
-	   (m+ iner (scale-matrix wei (DD (v- cen (send self :get :c-til) tmp-va))
-				  tmp-ma) (send self :get :I-til))
-           ;; propagation of I-til from children
-	   (dolist (child child-links)
-	     (m+ (send self :get :I-til)
-		 (m+ (send child :get :I-til)
-		     (scale-matrix (send child :get :m-til)
-				   (DD (v- (send child :get :c-til) (send self :get :c-til) tmp-va))
-				   tmp-ma) tmp-ma) (send self :get :I-til)))
-	   )
+	  (progn
+	    ;; propagation of m-til and c-til from children
+	    (dolist (child child-links)
+	      (send child :propagate-mass-properties :debug-view debug-view
+		    :tmp-va tmp-va :tmp-vb tmp-vb
+		    :tmp-ma tmp-ma :tmp-mb tmp-mb :tmp-mc tmp-mc))
+	    (let ((ret
+		   (send self :append-mass-properties
+			 child-links
+			 :tmp-ma tmp-ma :tmp-mb tmp-mb :tmp-mc tmp-mc
+			 :tmp-va tmp-va
+			 :additional-weights (send-all child-links :get :m-til)
+			 :additional-centroids (send-all child-links :get :c-til)
+			 :additional-inertias (send-all child-links :get :i-til)
+			 :update nil)))
+	      (send self :put :m-til (car ret))
+	      (send self :put :c-til (cadr ret))
+	      (send self :put :i-til (caddr ret))
+	      ))
 	(progn ;; if end-link
 	  (dotimes (i 3) (setf (elt (send self :get :c-til) i) (elt cen i)))
 	  (send self :put :m-til wei)


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

[jskeus-commit] SF.net SVN: jskeus:[662] trunk/irteus/irtrobot.l

[<sn...@us...>]

Revision: 662
          http://jskeus.svn.sourceforge.net/jskeus/?rev=662&view=rev
Author:   snozawa
Date:     2011-06-23 10:21:30 +0000 (Thu, 23 Jun 2011)

Log Message:
-----------
set wrt to :local if wrt is not specified by user

Modified Paths:
--------------
    trunk/irteus/irtrobot.l

Modified: trunk/irteus/irtrobot.l
===================================================================
--- trunk/irteus/irtrobot.l	2011-06-22 14:13:48 UTC (rev 661)
+++ trunk/irteus/irtrobot.l	2011-06-23 10:21:30 UTC (rev 662)
@@ -110,6 +110,7 @@
        (:move-end-rot
 	(let ((coords (send self limb :end-coords :copy-worldcoords))
 	      (angle (pop args)) (axis (pop args)) (wrt (pop args)))
+	  (unless wrt (setq wrt :local))
 	  (send* self limb :move-end
 		 (send coords :rotate (deg2rad angle) axis wrt) args)))
        (:move-end-pos


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

[jskeus-commit] SF.net SVN: jskeus:[661] trunk/irteus/irtviewer.l

[<k-...@us...>]

Revision: 661
          http://jskeus.svn.sourceforge.net/jskeus/?rev=661&view=rev
Author:   k-okada
Date:     2011-06-22 14:13:48 +0000 (Wed, 22 Jun 2011)

Log Message:
-----------
fix auto far/near adjustment

Modified Paths:
--------------
    trunk/irteus/irtviewer.l

Modified: trunk/irteus/irtviewer.l
===================================================================
--- trunk/irteus/irtviewer.l	2011-06-22 07:43:55 UTC (rev 660)
+++ trunk/irteus/irtviewer.l	2011-06-22 14:13:48 UTC (rev 661)
@@ -168,8 +168,8 @@
        (setq sc (/ (apply #'max (coerce (send bbox :diagonal) cons))
 		   (* 0.5 (tan (send self :viewer :viewing :view-angle)))))
        (when (or (> (/ sc gl::*perspective-far*) 2)
-		 (< (/ sc gl::*perspective-far*) 0.5))
-	 (warning-message 2 "Detect very large/small objects, change perspective~%")
+		 (< (/ sc gl::*perspective-far*) 0.01))
+	 (warning-message 2 "Detect very large/small objects(~A), change perspective~%" (/ sc gl::*perspective-far*) )
 	 (setq gl::*perspective-near* (* (/ sc gl::*perspective-far*) gl::*perspective-near*)
 	       gl::*perspective-far*  (* (/ sc gl::*perspective-far*) gl::*perspective-far*)))
        (setq vp


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

[jskeus-commit] SF.net SVN: jskeus:[660] trunk/irteus/irtrobot.l

[<sn...@us...>]

Revision: 660
          http://jskeus.svn.sourceforge.net/jskeus/?rev=660&view=rev
Author:   snozawa
Date:     2011-06-22 07:43:55 +0000 (Wed, 22 Jun 2011)

Log Message:
-----------
print warning message if joint name not found ;; this is behaviour before revision 644

Revision Links:
--------------
    http://jskeus.svn.sourceforge.net/jskeus/?rev=644&view=rev

Modified Paths:
--------------
    trunk/irteus/irtrobot.l

Modified: trunk/irteus/irtrobot.l
===================================================================
--- trunk/irteus/irtrobot.l	2011-06-22 03:37:27 UTC (rev 659)
+++ trunk/irteus/irtrobot.l	2011-06-22 07:43:55 UTC (rev 660)
@@ -177,10 +177,12 @@
 	      (send-all (cdr (assoc (intern (string-upcase limb)) (send self :slots))) method)
 	    (cdr (assoc (intern (string-upcase limb)) (send self :slots)))))
 	 (t
-	  (setq ret (list (send self (intern (format nil "~A-~A" (string-upcase limb) (string-upcase method)) *keyword-package*))))
-	  (if ret (user::forward-message-to (car ret) args)
-	    (progn (warn ";; error: cannot find method ~A~%" method)))
-	  )))
+	  (let ((limb-joint-name
+		 (intern (format nil "~A-~A" (string-upcase limb) (string-upcase method)) *keyword-package*)))
+	    (if (find-method self limb-joint-name)
+		(user::forward-message-to (send self limb-joint-name) args)
+	      (warn ";; error: cannot find method ~A~%" method))
+	    ))))
        ) ;; case method
      )) ;; defmethod
   (:larm (&rest args) 


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

[jskeus-commit] SF.net SVN: jskeus:[659] trunk/irteus/irtmodel.l

[<sn...@us...>]

Revision: 659
          http://jskeus.svn.sourceforge.net/jskeus/?rev=659&view=rev
Author:   snozawa
Date:     2011-06-22 03:37:27 +0000 (Wed, 22 Jun 2011)

Log Message:
-----------
fix typoes in angle-speed-collision-blending setting

Modified Paths:
--------------
    trunk/irteus/irtmodel.l

Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l	2011-06-22 03:24:53 UTC (rev 658)
+++ trunk/irteus/irtmodel.l	2011-06-22 03:37:27 UTC (rev 659)
@@ -1356,8 +1356,8 @@
 			 :collision-avoidance-link-pair pair-list args))
        (setq min-distance (car (elt (send self :get :collision-distance) 0)))
        (setq angle-speed-collision-blending
-	     (cond ((<= avoid-collision-joint-gain 0.0)
-		    (< min-distance (* 0.1 avoid-collision-distance))
+	     (cond ((<= avoid-collision-joint-gain 0.0) 0.0)
+		   ((< min-distance (* 0.1 avoid-collision-distance))
 		    1.0)
 		   ((< min-distance avoid-collision-distance)
 		    (/ (- avoid-collision-distance min-distance) 


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

[jskeus-commit] SF.net SVN: jskeus:[658] trunk/irteus/irtmodel.l

[<sn...@us...>]

Revision: 658
          http://jskeus.svn.sourceforge.net/jskeus/?rev=658&view=rev
Author:   snozawa
Date:     2011-06-22 03:24:53 +0000 (Wed, 22 Jun 2011)

Log Message:
-----------
fix angle-speed-blending ;; previous -> null-space vector for collision-avoidance is affected by blending coefficient ;; disable angle-speed blending if avoid-collision-joint-gain equals zero

Modified Paths:
--------------
    trunk/irteus/irtmodel.l

Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l	2011-06-21 02:54:40 UTC (rev 657)
+++ trunk/irteus/irtmodel.l	2011-06-22 03:24:53 UTC (rev 658)
@@ -1104,14 +1104,17 @@
        (warn ";; ERROR : I-J#J is required in :collision-avoidance~%")
        (return-from :collision-avoidance dav-col))
 
-     (let ((dav-col-null (transform I-J#J dav-col)))
+     (let ((distance-gain
+	    (- (/ avoid-collision-distance min-distance) 1)))
+       (dotimes (i fik-len) (setf (elt dav-col i) (* (elt dav-col i) (elt weight i))))
+       (send self :put :collision-avoidance-null-vector
+	     (scale (* distance-gain avoid-collision-null-gain) dav-col))
+       (send self :put :collision-avoidance-joint-vector
+	     (scale (* distance-gain avoid-collision-joint-gain) dav-col))
        (when (and debug-view (not (memq :no-message debug-view)))
-         (format-array (scale avoid-collision-joint-gain (map float-vector #'rad2deg dav-col)) "coljnt:")
-         (format-array (scale avoid-collision-null-gain (map float-vector #'rad2deg dav-col-null)) "colnul:"))
-       (dotimes (i fik-len) (setf (elt dav-col i)
-                                  (+ (* avoid-collision-joint-gain (elt dav-col i) (elt weight i))
-                                     (* avoid-collision-null-gain (elt dav-col-null i) (elt weight i))))))
-     (scale (- (* avoid-collision-distance (/ 1.0 min-distance)) 1) dav-col)))
+         (format-array (map float-vector #'rad2deg (send self :get :collision-avoidance-joint-vector)) "coljnt:")
+         (format-array (map float-vector #'rad2deg (send self :get :collision-avoidance-null-vector)) "colnul:")))
+     (send self :get :collision-avoidance-joint-vector)))
   (:move-joints
    (union-vel &rest args
     &key
@@ -1338,6 +1341,8 @@
      ;; Collision Avoidance with Whole Body Motion Control for Humanoid
      ;; Robots", In IROS 2007, 2053--2058
      ;;     
+     (send self :put :collision-avoidance-null-vector nil)
+     (send self :put :collision-avoidance-joint-vector nil)
      (when (and pair-list (> avoid-collision-distance 0.0)
 		(or (> avoid-collision-joint-gain 0.0)
 		    (> avoid-collision-null-gain 0.0)))
@@ -1351,7 +1356,8 @@
 			 :collision-avoidance-link-pair pair-list args))
        (setq min-distance (car (elt (send self :get :collision-distance) 0)))
        (setq angle-speed-collision-blending
-	     (cond ((< min-distance (* 0.1 avoid-collision-distance))
+	     (cond ((<= avoid-collision-joint-gain 0.0)
+		    (< min-distance (* 0.1 avoid-collision-distance))
 		    1.0)
 		   ((< min-distance avoid-collision-distance)
 		    (/ (- avoid-collision-distance min-distance) 
@@ -1366,6 +1372,8 @@
                  link-list :union-link-list union-link-list
                  :avoid-nspace-gain avoid-nspace-gain :debug-view debug-view
                  :weight weight :fik-len fik-len :null-space null-space :tmp-nspace tmp-nspace))
+     (if (send self :get :collision-avoidance-null-vector)
+	 (v+ tmp-nspace (send self :get :collision-avoidance-null-vector) tmp-nspace))
      ;;
      ;; q = f(d) qca + {1 - f(d)} qwbm
      ;;


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

[Jskeus-commit] SF.net SVN: jskeus:[657] trunk/irteus/irtviewer.l

[<k-...@us...>]

Revision: 657
          http://jskeus.svn.sourceforge.net/jskeus/?rev=657&view=rev
Author:   k-okada
Date:     2011-06-21 02:54:40 +0000 (Tue, 21 Jun 2011)

Log Message:
-----------
fix :look-all paramater

Modified Paths:
--------------
    trunk/irteus/irtviewer.l

Modified: trunk/irteus/irtviewer.l
===================================================================
--- trunk/irteus/irtviewer.l	2011-06-21 02:45:42 UTC (rev 656)
+++ trunk/irteus/irtviewer.l	2011-06-21 02:54:40 UTC (rev 657)
@@ -167,8 +167,8 @@
        (setq vt (send bbox :center))
        (setq sc (/ (apply #'max (coerce (send bbox :diagonal) cons))
 		   (* 0.5 (tan (send self :viewer :viewing :view-angle)))))
-       (when (or (> (/ sc gl::*perspective-far*) 10)
-		 (< (/ sc gl::*perspective-far*) 0.1))
+       (when (or (> (/ sc gl::*perspective-far*) 2)
+		 (< (/ sc gl::*perspective-far*) 0.5))
 	 (warning-message 2 "Detect very large/small objects, change perspective~%")
 	 (setq gl::*perspective-near* (* (/ sc gl::*perspective-far*) gl::*perspective-near*)
 	       gl::*perspective-far*  (* (/ sc gl::*perspective-far*) gl::*perspective-far*)))


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

[Jskeus-commit] SF.net SVN: jskeus:[656] trunk/irteus/irtviewer.l

[<k-...@us...>]

Revision: 656
          http://jskeus.svn.sourceforge.net/jskeus/?rev=656&view=rev
Author:   k-okada
Date:     2011-06-21 02:45:42 +0000 (Tue, 21 Jun 2011)

Log Message:
-----------
add code to change perspective-near and paerspective-far according to object size in :look-all

Modified Paths:
--------------
    trunk/irteus/irtviewer.l

Modified: trunk/irteus/irtviewer.l
===================================================================
--- trunk/irteus/irtviewer.l	2011-06-12 07:44:50 UTC (rev 655)
+++ trunk/irteus/irtviewer.l	2011-06-21 02:45:42 UTC (rev 656)
@@ -163,16 +163,20 @@
          (bbox
           (setq bbox (geo::make-bounding-box (flatten (send-all (x::draw-things bbox) :vertices))))))
    (when bbox
-     (let (vt vp)
+     (let (vt vp sc)
        (setq vt (send bbox :center))
+       (setq sc (/ (apply #'max (coerce (send bbox :diagonal) cons))
+		   (* 0.5 (tan (send self :viewer :viewing :view-angle)))))
+       (when (or (> (/ sc gl::*perspective-far*) 10)
+		 (< (/ sc gl::*perspective-far*) 0.1))
+	 (warning-message 2 "Detect very large/small objects, change perspective~%")
+	 (setq gl::*perspective-near* (* (/ sc gl::*perspective-far*) gl::*perspective-near*)
+	       gl::*perspective-far*  (* (/ sc gl::*perspective-far*) gl::*perspective-far*)))
        (setq vp
 	 (scale
           (min
            (* gl::*perspective-far* 0.8)
-           (max
-            (* gl::*perspective-near* 1.5)
-            (/ (apply #'max (coerce (send bbox :diagonal) cons))
-               (* 0.5 (tan (send self :viewer :viewing :view-angle))))))
+           (max (* gl::*perspective-near* 1.5) sc))
           (normalize-vector (v- viewpoint viewtarget))))
        (send self :viewtarget vt)
        (send self :viewpoint (v+ vt vp))


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.