Menu
▾
▴
jskeus-commit — jskeus commit mailing list
You can subscribe to this list here.
| 2011 |
Jan
|
Feb
|
Mar
|
Apr
|
May
|
Jun
(9) |
Jul
(31) |
Aug
|
Sep
(15) |
Oct
(11) |
Nov
(15) |
Dec
(10) |
|---|---|---|---|---|---|---|---|---|---|---|---|---|
| 2012 |
Jan
(11) |
Feb
(15) |
Mar
(36) |
Apr
(8) |
May
(11) |
Jun
(14) |
Jul
(16) |
Aug
(1) |
Sep
(8) |
Oct
(37) |
Nov
(4) |
Dec
(3) |
| 2013 |
Jan
(1) |
Feb
(7) |
Mar
(17) |
Apr
(29) |
May
(23) |
Jun
(45) |
Jul
(8) |
Aug
(13) |
Sep
(7) |
Oct
(11) |
Nov
(25) |
Dec
(40) |
| 2014 |
Jan
(23) |
Feb
(34) |
Mar
(1) |
Apr
(8) |
May
(50) |
Jun
|
Jul
(2) |
Aug
|
Sep
(7) |
Oct
|
Nov
|
Dec
|
| 2015 |
Jan
(6) |
Feb
|
Mar
|
Apr
|
May
|
Jun
|
Jul
|
Aug
|
Sep
|
Oct
|
Nov
|
Dec
|
|
From: <sn...@us...> - 2012-02-06 07:42:17
|
Revision: 759
http://jskeus.svn.sourceforge.net/jskeus/?rev=759&view=rev
Author: snozawa
Date: 2012-02-06 07:42:11 +0000 (Mon, 06 Feb 2012)
Log Message:
-----------
do not update mass property in :calc-inertia-matrix-from-link-list
Modified Paths:
--------------
trunk/irteus/irtdyna.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2012-02-06 07:40:23 UTC (rev 758)
+++ trunk/irteus/irtdyna.l 2012-02-06 07:42:11 UTC (rev 759)
@@ -400,7 +400,7 @@
(send* (elt link-list l) :calc-inertia-matrix-column column :inertia-matrix inertia-matrix
;; axis-for-angular is the axis for calculation of angular variables
;; for example, we calculate angular momentum around the whole-body Center Of Mass as follows.
- :axis-for-angular (send self :centroid)
+ :axis-for-angular (send self :centroid nil)
:translation-axis translation-axis :rotation-axis rotation-axis args)))
inertia-matrix)
;; return ;; COG jacobian [m]
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2012-02-06 07:40:29
|
Revision: 758
http://jskeus.svn.sourceforge.net/jskeus/?rev=758&view=rev
Author: snozawa
Date: 2012-02-06 07:40:23 +0000 (Mon, 06 Feb 2012)
Log Message:
-----------
add comment for axis-for-angular
Modified Paths:
--------------
trunk/irteus/irtdyna.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2012-02-06 07:36:39 UTC (rev 757)
+++ trunk/irteus/irtdyna.l 2012-02-06 07:40:23 UTC (rev 758)
@@ -398,7 +398,9 @@
((>= l (length link-list)))
(setq j (send (elt link-list l) :joint))
(send* (elt link-list l) :calc-inertia-matrix-column column :inertia-matrix inertia-matrix
- :axis-for-angular (send (car (send self :links)) :get :c-til)
+ ;; axis-for-angular is the axis for calculation of angular variables
+ ;; for example, we calculate angular momentum around the whole-body Center Of Mass as follows.
+ :axis-for-angular (send self :centroid)
:translation-axis translation-axis :rotation-axis rotation-axis args)))
inertia-matrix)
;; return ;; COG jacobian [m]
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2012-02-06 07:36:46
|
Revision: 757
http://jskeus.svn.sourceforge.net/jskeus/?rev=757&view=rev
Author: snozawa
Date: 2012-02-06 07:36:39 +0000 (Mon, 06 Feb 2012)
Log Message:
-----------
remove unnecessary debug-view argument
Modified Paths:
--------------
trunk/irteus/irtdyna.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2012-02-06 07:26:27 UTC (rev 756)
+++ trunk/irteus/irtdyna.l 2012-02-06 07:36:39 UTC (rev 757)
@@ -317,8 +317,7 @@
(:calc-inertia-matrix-column
(column
&rest args
- &key (debug-view nil)
- (rotation-axis nil)
+ &key (rotation-axis nil)
(translation-axis t)
((:inertia-matrix im))
(axis-for-angular (float-vector 0 0 0))
@@ -367,7 +366,6 @@
(axis-dim (send self :calc-target-axis-dimension rotation-axis translation-axis))
(inertia-matrix
(make-matrix axis-dim (send self :calc-target-joint-dimension link-list)))
- (debug-view nil)
(update-mass-properties t)
;; buffers for calculation
(tmp-v0 (instantiate float-vector 0))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2012-02-06 07:26:33
|
Revision: 756
http://jskeus.svn.sourceforge.net/jskeus/?rev=756&view=rev
Author: snozawa
Date: 2012-02-06 07:26:27 +0000 (Mon, 06 Feb 2012)
Log Message:
-----------
remove default parameter for target-centroid-pos ;; use :centroid method to calculate whole-body centroid
Modified Paths:
--------------
trunk/irteus/irtdyna.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2012-02-02 07:51:42 UTC (rev 755)
+++ trunk/irteus/irtdyna.l 2012-02-06 07:26:27 UTC (rev 756)
@@ -430,7 +430,7 @@
&rest args
&key (cog-gain 1.0)
(translation-axis :z)
- (target-centroid-pos (send (car (send self :links)) :get :c-til))
+ (target-centroid-pos)
(centroid-offset-func)
&allow-other-keys)
(transform
@@ -444,7 +444,7 @@
(v- target-centroid-pos
(if (functionp centroid-offset-func)
(funcall centroid-offset-func)
- (send (car (send self :links)) :get :c-til)))
+ (send self :centroid)))
translation-axis))
)
)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sai...@us...> - 2012-02-02 07:51:48
|
Revision: 755
http://jskeus.svn.sourceforge.net/jskeus/?rev=755&view=rev
Author: saito-manabu
Date: 2012-02-02 07:51:42 +0000 (Thu, 02 Feb 2012)
Log Message:
-----------
add irtgraph.l to irteus, this is originally euslib/jsk/graph.l
Modified Paths:
--------------
trunk/irteus/Makefile
trunk/irteus/compile_irt.l
trunk/irteus/irtext.l
Added Paths:
-----------
trunk/irteus/irtgraph.l
Modified: trunk/irteus/Makefile
===================================================================
--- trunk/irteus/Makefile 2012-02-01 12:42:52 UTC (rev 754)
+++ trunk/irteus/Makefile 2012-02-02 07:51:42 UTC (rev 755)
@@ -35,7 +35,7 @@
MODULES.L=irt_modules.l
EUSLIB_MODULES.L=$(addprefix $(EUSDIR)/lib/,$(MODULES.L))
-IRTEUS=irtmath irtutil pgsql
+IRTEUS=irtmath irtutil irtgraph pgsql
IRTEUSG=irtgeo pqp irtscene irtmodel irtsensor irtdyna irtrobot irtbvh irtcollada
IRTEUSX=irtx
IRTEUSIMG=irtimage eusjpeg png
@@ -164,6 +164,7 @@
$(OBJDIR)/irtmath.$(OSFX): irtmath.l
$(OBJDIR)/irtutil.$(OSFX): irtutil.l
+$(OBJDIR)/irtgraph.$(OSFX): irtgraph.l
$(OBJDIR)/irtgeo.$(OSFX): irtgeo.l
$(OBJDIR)/irtscene.$(OSFX): irtscene.l
$(OBJDIR)/irtmodel.$(OSFX): irtmodel.l
Modified: trunk/irteus/compile_irt.l
===================================================================
--- trunk/irteus/compile_irt.l 2012-02-01 12:42:52 UTC (rev 754)
+++ trunk/irteus/compile_irt.l 2012-02-02 07:51:42 UTC (rev 755)
@@ -30,6 +30,7 @@
(comp:compile-file-if-src-newer "irtmath.l" *objdir*)
(comp:compile-file-if-src-newer "irtutil.l" *objdir*)
+(comp:compile-file-if-src-newer "irtgraph.l" *objdir*)
(comp:compile-file-if-src-newer (format nil "~A/lib/llib/pgsql.l" *eusdir*) *objdir*)
Modified: trunk/irteus/irtext.l
===================================================================
--- trunk/irteus/irtext.l 2012-02-01 12:42:52 UTC (rev 754)
+++ trunk/irteus/irtext.l 2012-02-02 07:51:42 UTC (rev 755)
@@ -28,7 +28,7 @@
(load-library
(format nil "~A~A/lib/libirteus"
*eusdir* (unix:getenv "ARCHDIR"))
- '("irtmath" "irtutil" "irtc" "pgsql")))
+ '("irtmath" "irtutil" "irtc" "irtgraph" "pgsql")))
(defun load-irteusg ()
(in-package "GEOMETRY")
(load-library
Added: trunk/irteus/irtgraph.l
===================================================================
--- trunk/irteus/irtgraph.l (rev 0)
+++ trunk/irteus/irtgraph.l 2012-02-02 07:51:42 UTC (rev 755)
@@ -0,0 +1,598 @@
+;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+;;;
+;;; $Id: irtgraph.l $
+;;;
+;;; Copyright (c) 1987- JSK, The University of Tokyo. All Rights Reserved.
+;;;
+;;; This software is a collection of EusLisp code for robot applications,
+;;; which has been developed by the JSK Laboratory for the IRT project.
+;;; For more information on EusLisp and its application to the robotics,
+;;; please refer to the following papers.
+;;;
+;;; Toshihiro Matsui
+;;; Multithread object-oriented language euslisp for parallel and
+;;; asynchronous programming in robotics
+;;; Workshop on Concurrent Object-based Systems,
+;;; IEEE 6th Symposium on Parallel and Distributed Processing, 1994
+;;;
+;;; Permission to use this software for educational, research
+;;; and non-profit purposes, without fee, and without a written
+;;; agreement is hereby granted to all researchers working on
+;;; the IRT project at the University of Tokyo, provided that the
+;;; above copyright notice remains intact.
+;;;
+
+;; this file is copied from euslib/jsk...
+
+;;=======================================================
+;; irt graph class
+;;
+;; You have to create subclass of graph class depending on
+;; Your Application.
+;;
+
+;;=======================================
+;;
+;; pure node/arc class for directional graph
+;;
+
+(defclass node
+ :super propertied-object
+ :slots (arc-list))
+(defmethod node
+ (:init (n)
+ (send self :name n)
+ self)
+ (:arc-list ()
+ arc-list)
+ (:successors ()
+ (mapcar #'(lambda(a)(cons a (send a :to))) arc-list))
+ (:add-arc (a)
+ (push a arc-list))
+ (:remove-arc (a)
+ (setq arc-list (remove a arc-list)))
+ (:remove-all-arcs ()
+ (setq arc-list nil))
+ (:unlink (n)
+ (setq arc-list
+ (remove-if #'(lambda(a)(eq n (send a :to))) arc-list)))
+ )
+
+(defclass arc
+ :super propertied-object
+ :slots (from to))
+(defmethod arc
+ (:init (from_ to_)
+ (setq from from_ to to_)
+ (send from :add-arc self)
+ self)
+ (:from () from)
+ (:to () to)
+ (:prin1 (&optional (strm t) &rest msgs)
+ (send-super :prin1 strm
+ (format nil "~A->~A~A" from to (or msgs "")))))
+
+;;
+;; directed graph
+;;
+(defclass directed-graph
+ :super propertied-object
+ :slots (nodes))
+(defmethod directed-graph
+ (:init ()
+ self)
+ ;;
+ (:successors (node &rest args)
+ (send node :successors))
+ (:node (name)
+ (find name nodes :key #'(lambda (x) (send x :name)) :test #'equal))
+ ;; manipulate nodes/arcs
+ (:nodes (&optional arg)
+ (when arg
+ (dolist (rmnode (set-difference nodes arg))
+ (send-all arg :unlink rmnode))
+ (setq nodes arg))
+ nodes)
+ (:add-node (n)
+ (push n nodes)
+ n )
+ (:remove-node (n)
+ (setq nodes (remove n nodes))
+ (send-all nodes :unlink n)
+ nodes)
+ (:clear-nodes ()
+ (send-all nodes :remove-all-arcs)
+ (setq nodes nil))
+ (:add-arc-from-to (from to)
+ (instance arc :init from to))
+ (:remove-arc (arc)
+ (send (send arc :from) :remove-arc arc))
+ ;; calcurate graph property
+ (:adjacency-matrix ()
+ (let ((size (length nodes)) mat j)
+ (setq mat (make-matrix size size))
+ (dotimes (i size)
+ (dolist (next (send-all (send (elt nodes i) :arc-list) :to))
+ (setq j (position next nodes))
+ (when j (incf (aref mat i j)))
+ ))
+ mat))
+ (:adjacency-list ()
+ (mapcar #'(lambda(n)
+ (sort (mapcar #'(lambda(m)(position m nodes))
+ (send-all (send n :arc-list) :to)) #'<))
+ nodes))
+ )
+
+
+(defclass costed-arc
+ :super arc
+ :slots (cost))
+(defmethod costed-arc
+ (:init (from to c)
+ (setq cost c)
+ (send-super :init from to))
+ (:cost () cost))
+
+(defclass costed-graph
+ :super directed-graph
+ :slots ())
+(defmethod costed-graph
+ (:add-arc (from to cost &key (both nil))
+ (send self :add-arc-from-to from to cost :both both))
+ (:add-arc-from-to (from to cost &key (both nil))
+ (let ((ar (instance costed-arc :init from to cost)))
+ (if both
+ (list ar (instance costed-arc :init to from cost))
+ ar)))
+ (:path-cost (from arc to)
+ (send arc :cost)))
+
+;;=======================================
+;; jsk standard graph
+;; . costed-graph with start and goal
+;; . cost of arc = 1
+;; . nodes has :cost method
+
+(defclass graph
+ :super costed-graph
+ :slots (start-state goal-state))
+
+(defmethod graph
+ ;; :goal-test method need to be overriden according to your application.
+ ;; :goal-test must be fast.
+ (:goal-test (gs)
+ (eq goal-state gs))
+ ;; :path-cost returns cost from 'from-node' to 'to-node' by 'action'.
+ (:path-cost (from arc to)
+ (+ (send from :cost) (send arc :cost)))
+ ;;accessors
+ (:start-state (&optional arg)
+ (if arg (setq start-state arg))
+ start-state)
+ (:goal-state (&optional arg)
+ (if arg (setq goal-state arg))
+ goal-state)
+ (:add-arc (from to &key (both nil))
+ (unless (listp to) (setq to (list to)))
+ (mapcar #'(lambda (next)
+ (send self :add-arc-from-to from next :both both)) to))
+ (:add-arc-from-to (from to &key (both nil))
+ (send-super :add-arc-from-to from to 1 :both both))
+ )
+
+;;
+;; write methods of directed-graph
+;;
+(defmethod directed-graph
+ ;; :write-to-dot method is supposed to be meaningful
+ ;; in only static graph.
+ (:write-to-dot (fname &optional result-path (title "output"))
+ (let ((node-alist ; ((node . symbol) (node . symbol) ...)
+ (mapcar #'(lambda (n)
+ (cons n (string (gensym))))
+ nodes)))
+ (labels ((graphviz-node-name
+ (n)
+ (cdr (assoc n node-alist))))
+ (with-open-file (f fname :direction :output)
+ (format f "digraph ~A {~%" title)
+ (dolist (target-node nodes)
+ (format f " ")
+ ;; hoge = "hoge";
+ (format f "~A [label = \"~s\"];~%"
+ (graphviz-node-name target-node)
+ (send target-node :name))
+ )
+ ;; first of all, write result-path
+ (let ((drawed-arc nil))
+ (let (anode)
+ (while (and (setq anode (pop result-path)) result-path)
+ (format f " ")
+ (format f "~A -> ~A [color = red];~%"
+ (graphviz-node-name (send anode :state))
+ (graphviz-node-name (send (car result-path) :state)))))
+ (dolist (target-node nodes)
+ (let ((neighbors
+ (mapcar #'cdr (send self :successors target-node))))
+ (dolist (neighbor neighbors)
+ (when (not (or (find (cons target-node neighbor) drawed-arc
+ :test #'equal)
+ (find (cons neighbor target-node) drawed-arc
+ :test #'equal)))
+ (if (member target-node
+ (mapcar #'cdr (send self :successors neighbor)))
+ (progn
+ (format f " ")
+ (format f "~A -> ~A [dir = both];~%"
+ (graphviz-node-name target-node)
+ (graphviz-node-name neighbor)))
+ (progn
+ (format f " ")
+ (format f "~A -> ~A;~%"
+ (graphviz-node-name target-node)
+ (graphviz-node-name neighbor))))
+ (push (cons target-node neighbor) drawed-arc)))))
+ (format f "}~%")
+ t)))))
+ (:write-to-pdf (fname &optional result-path
+ (title (string-right-trim ".pdf" fname)))
+ (let ((dot-fname
+ (format nil "~A.dot" (string-right-trim ".pdf" fname))))
+ (send self :write-to-dot dot-fname result-path title)
+ (unix:system (format nil "dot ~A -Tpdf -o ~A" dot-fname fname))
+ t))
+ )
+
+;;=======================================
+;; obsolated methods and class for backward compatibility
+;;
+(defmethod node
+ (:add-neighbor (n &optional a)
+ (let ((ar (instance arc :init self n)))
+ (when a (send ar :name a))
+ (send self :neighbors)))
+ (:neighbors (&optional args)
+ (if args (dolist (n args) (instance arc :init self n)))
+ (mapcar #'cdr (send self :successors)))
+ )
+(defclass arced-node
+ :super node
+ :slots ())
+(defmethod arced-node
+ (:init (&key name)
+ (send-super :init name)
+ self)
+ (:find-action (n)
+ (let ((act (find n arc-list :key #'(lambda(a)(send a :to)))))
+ (when act (send act :name))))
+ (:neighbor-action-alist ()
+ (mapcar #'(lambda(a)(cons (send a :name) (send a :to))) arc-list))
+ )
+
+;;=======================================
+;; solver-node class
+;;
+;; solver-node has history of the past and so on...
+;; state slot is a solver-node's information.
+;; So it is a mistake that you make a subclass of solver-node.
+;; You have to device node class and graph(problem) class.
+(defclass solver-node
+ :super propertied-object
+ :slots (state cost parent action memorized-path))
+
+(defmethod solver-node
+ (:init (st &key ((:cost c) 0)
+ ((:parent p) nil)
+ ((:action a) nil))
+ (setq state st)
+ (setq cost c
+ parent p
+ action a)
+ self)
+ ;; returns path to this node
+ (:path (&optional (prev nil))
+ (if prev
+ (if (send self :parent)
+ (send (send self :parent)
+ :path (cons self prev))
+ (cons self prev))
+ (if memorized-path
+ memorized-path
+ (setq memorized-path (if (send self :parent)
+ (send (send self :parent)
+ :path (cons self prev))
+ (cons self prev))))))
+ ;; expand the next nodes using :successors method of prblm
+ (:expand (prblm &rest args)
+ (let ((successors (send* prblm :successors state args)))
+ ;; successor = (action . next-state)
+ (let ((ret nil))
+ (dolist (successor successors)
+ (push (instance solver-node :init (cdr successor)
+ :parent self
+ :action (car successor)
+ :cost (send prblm :path-cost self
+ (car successor) (cdr successor)))
+ ret))
+ ret)))
+ (:state (&optional arg)
+ (if arg (setq state arg))
+ state)
+ (:cost (&optional arg)
+ (if arg (setq cost arg))
+ cost)
+ (:parent (&optional arg)
+ (if arg (setq parent arg))
+ parent)
+ (:action (&optional arg)
+ (if arg (setq action arg))
+ action)
+ )
+
+;;=======================================
+;; solver
+;;
+;;solver class
+;;
+(defclass solver
+ :super propertied-object
+ :slots ())
+
+(defmethod solver
+ (:init ()
+ self)
+ (:solve (prblm)
+ nil)
+ (:solve-by-name (prblm s g &key (verbose nil))
+ (send prblm :start-state (send prblm :node s))
+ (send prblm :goal-state (send prblm :node g))
+ (send self :solve prblm :verbose verbose)
+ )
+ )
+
+;;=======================================
+;; uninformed search algorithms
+(defclass graph-search-solver
+ :super solver
+ :slots (open-list close-list))
+
+(defmethod graph-search-solver
+ (:solve-init (prblm)
+ (setq close-list nil)
+ (send self :add-to-open-list
+ (instance solver-node :init (send prblm :start-state) :cost 0)))
+ (:find-node-in-close-list (n)
+ (find (send n :state) close-list))
+ (:solve (prblm &key (verbose nil))
+ (send self :solve-init prblm)
+ (while (not (send self :null-open-list?))
+;;; (if verbose
+;;; (warn "current open-list num -> ~A -- ~A --~%"
+;;; (length open-list)
+;;; :solve))
+ (let ((target-node (send self :pop-from-open-list :debug verbose)))
+ ;; here, target-node is removed from open-list in :pop-from-open-list.
+;;; (if verbose
+;;; (warn "target-node is ~A -- ~A -- ~%"
+;;; (send target-node :state) :solve))
+ (cond ((send prblm :goal-test (send target-node :state))
+;;; (if verbose
+;;; (warn "arrived at goal! -- ~A --~%" :solve))
+ (return-from :solve (send target-node :path)))
+ ((not (send self :find-node-in-close-list target-node))
+ (push (send target-node :state) close-list)
+ (send self :add-list-to-open-list
+ (send target-node :expand prblm :verbose verbose)))
+ )))
+ (warn "open-list is nil... -- ~A --~%" :solve)
+ (warn "search was missed!! -- ~A --~%" :solve)
+ (send self :clear-open-list)
+ (setq close-list nil)
+ nil)
+ ;; open-list functions
+ (:add-to-open-list (obj/list)
+ (if (listp obj/list)
+ (send self :add-list-to-open-list obj/list)
+ (send self :add-object-to-open-list obj/list)))
+ (:null-open-list? ()
+ (null open-list))
+ (:clear-open-list ()
+ (warn "you have to override :clear-open-list method~%")
+ nil)
+ (:add-list-to-open-list (lst)
+ (warn "you have to override :add-list-to-open-list method~%")
+ nil)
+ (:add-object-to-open-list (lst)
+ (warn "you have to override :add-object-to-open-list method~%")
+ nil)
+ (:pop-from-open-list ()
+ (warn "you have to override :pop-from-open-list method~%")
+ nil)
+ ;; accessors
+ (:open-list (&optional arg)
+ (if arg (setq open-list arg))
+ open-list)
+ (:close-list (&optional arg)
+ (if arg (setq close-list arg))
+ close-list)
+ )
+
+;;======================================
+;; bredth first
+(defclass breadth-first-graph-search-solver
+ :super graph-search-solver
+ :slots ())
+
+(defmethod breadth-first-graph-search-solver
+ (:init ()
+ (setq open-list (list))
+ self)
+ (:clear-open-list ()
+ (setq open-list nil)
+ nil)
+ (:add-list-to-open-list (lst)
+ ;; add to the tail of open-list
+ (setq open-list (nconc open-list lst))
+ open-list)
+ (:add-object-to-open-list (obj)
+ ;; add to the tail of open-list
+ (setq open-list (nconc open-list (list obj)))
+ open-list)
+ (:pop-from-open-list ()
+ (pop open-list))
+ )
+
+;;=======================================
+;; depth first
+(defclass depth-first-graph-search-solver
+ :super graph-search-solver
+ :slots ())
+
+(defmethod depth-first-graph-search-solver
+ (:init ()
+ (setq open-list (list))
+ self)
+ (:clear-open-list ()
+ (setq open-list nil)
+ nil)
+ (:add-list-to-open-list (lst)
+ ;; add to the head of open-list
+ (setq open-list (nconc lst open-list))
+ open-list)
+ (:add-object-to-open-list (obj)
+ ;; add to the head of open-list
+ (push obj open-list)
+ open-list)
+ (:pop-from-open-list ()
+ (pop open-list))
+ )
+
+;;=======================================
+;; informed search
+
+;;=======================================
+;; best first
+(defclass best-first-graph-search-solver
+ :super graph-search-solver
+ :slots (aproblem))
+
+(defmethod best-first-graph-search-solver
+ (:init (p)
+ (setq aproblem p)
+ (setq open-list (list))
+ self)
+ (:clear-open-list ()
+ (setq open-list nil)
+ nil)
+ (:add-list-to-open-list (lst)
+ (setq open-list (nconc open-list lst))
+ open-list)
+ (:add-object-to-open-list (obj)
+ (push obj open-list)
+ open-list)
+ (:pop-from-open-list (&key (debug nil))
+ ;; returns the element which has minimum priority-value.
+ ;; => Priority Queue.
+ ;; priority-value is calculated by :fn.
+ ;; In AIMA , priority-value is equal to f(n).
+ ;;
+ ;; elements of open-list must be propertied-objects
+ (let* ((min-x (car open-list)))
+ (let ((min-value (or (send min-x :get :priority-value)
+ (progn ; if does not have :priority-value
+ (send min-x :put :priority-value
+ (send self :fn min-x aproblem))
+ (send min-x :get :priority-value)))))
+ (when debug
+ (warn "<~A> v -> ~A~%"
+ (send-all (send-all (send min-x :path) :state) :name)
+ min-value))
+ (dolist (x (cdr open-list))
+ (let ((v (or (get x :priority-value)
+ (progn ; if does not have :priority-value
+ (setf (get x :priority-value)
+ (send self :fn x aproblem))
+ (get x :priority-value)))))
+ (when debug
+ (warn "<~A> v -> ~A~%"
+ (send-all (send-all (send x :path) :state) :name)
+ v))
+ (if (< v min-value)
+ (progn
+ (setq min-value v)
+ (setq min-x x)))
+ ))
+ (when debug
+ (warn ":pop-from-open-list result...~%")
+ (warn "min-value -> ~A~%" min-value)
+ (warn "min-x -> ~A~%"
+ (send-all (send-all (send min-x :path) :state) :name)))
+ ;; remove from open-list
+ (setq open-list (delete min-x open-list :count 1))
+ min-x)))
+ (:fn (n p)
+ ;; calculate f(n)
+ ;; in greedy search, f(n) = g(n), h(n) = 0.
+ (send n :cost))
+ )
+
+;;=======================================
+;; A*
+;; f(n) = g(n) + h(n)
+;; where g(n) is the cost from start node to n.
+;; h(n) is the estimated cost from n to goal node.
+(defclass a*-graph-search-solver
+ :super best-first-graph-search-solver
+ :slots ())
+
+(defmethod a*-graph-search-solver
+ (:init (p)
+ (send-super :init p)
+ self)
+ ;; n -> next node
+ ;; p -> prnoblem
+ (:fn (n p &key (debug nil))
+ (when debug
+ (warn "g(n) -> ~A -- F(N) --~%" (send self :gn n p))
+ (warn "h(n) -> ~A -- F(N) --~%" (send self :hn n p)))
+ (+ (send self :gn n p)
+ (send self :hn n p)))
+ (:gn (n p)
+ (send n :cost))
+ (:hn (n p)
+ (warn "You have to override :hn~%")
+ 0.0)
+ )
+
+
+;;=======================================
+;; samples
+#|
+(let ((gr (instance graph :init)) a b c ar1 ar2)
+ (setq a (instance node :init 'a)
+ b (instance node :init 'b)
+ c (instance node :init 'c))
+ (send gr :add-node a)
+ (send gr :add-node b)
+ (send gr :add-node c)
+ (setq ar1 (send gr :add-arc-from-to a b)
+ ar2 (send gr :add-arc-from-to b c))
+ ;;
+ (format t "~a" (send gr :successors a)) ;; -> (arc . b)
+ (send gr :remove-arc ar1)
+ (format t "~a" (send gr :successors a)) ;; -> nil
+ ;;
+ (send gr :remove-node b)
+ (send gr :add-arc-from-to a c)
+ (format t "~a" (send gr :node 'a)) ;; -> a
+ ;;
+ (send gr :clear-nodes)
+ )
+
+;; To see more samples, please check euslisp/test/graph.l
+
+|#
+
+(provide :irtgraph "$Id$")
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <je...@js...> - 2012-02-02 03:26:06
|
See <http://jenkins.jsk.imi.i.u-tokyo.ac.jp:8080/job/jskeus/38/> |
|
From: <je...@js...> - 2012-02-01 12:44:35
|
See <http://jenkins.jsk.imi.i.u-tokyo.ac.jp:8080/job/jskeus/37/> ------------------------------------------ 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 AU irteus/irtgl.l AU irteus/Makefile.Linux AU irteus/Makefile.Cygwin AU irteus/Makefile AU irteus/png.l A irteus/euspng.c AERROR: 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:784) at hudson.FilePath.act(FilePath.java:766) 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:1404) 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:185) at java.io.BufferedInputStream.read1(BufferedInputStream.java:273) at java.io.BufferedInputStream.read(BufferedInputStream.java:334) at org.tmatesoft.svn.core.internal.util.ChunkedInputStream.read(ChunkedInputStream.java:70) at sun.nio.cs.StreamDecoder.readBytes(StreamDecoder.java:282) at sun.nio.cs.StreamDecoder.implRead(StreamDecoder.java:324) at sun.nio.cs.StreamDecoder.read(StreamDecoder.java:176) at java.io.InputStreamReader.read(InputStreamReader.java:184) 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:1719) at com.sun.org.apache.xerces.internal.impl.XMLEntityScanner.peekChar(XMLEntityScanner.java:464) at com.sun.org.apache.xerces.internal.impl.XMLDocumentFragmentScannerImpl$FragmentContentDriver.next(XMLDocumentFragmentScannerImpl.java:2664) at com.sun.org.apache.xerces.internal.impl.XMLDocumentScannerImpl.next(XMLDocumentScannerImpl.java:625) at com.sun.org.apache.xerces.internal.impl.XMLNSDocumentScannerImpl.next(XMLNSDocumentScannerImpl.java:116) at com.sun.org.apache.xerces.internal.impl.XMLDocumentFragmentScannerImpl.scanDocument(XMLDocumentFragmentScannerImpl.java:488) at com.sun.org.apache.xerces.internal.parsers.XML11Configuration.parse(XML11Configuration.java:819) at com.sun.org.apache.xerces.internal.parsers.XML11Configuration.parse(XML11Configuration.java:748) at com.sun.org.apache.xerces.internal.parsers.XMLParser.parse(XMLParser.java:123) at com.sun.org.apache.xerces.internal.parsers.AbstractSAXParser.parse(AbstractSAXParser.java:1208) at com.sun.org.apache.xerces.internal.jaxp.SAXParserImpl$JAXPSAXParser.parse(SAXParserImpl.java:525) 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 U 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 FATAL: null java.lang.NullPointerException at java.util.ArrayList.addAll(ArrayList.java:497) 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:1404) 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: <sn...@us...> - 2012-02-01 12:43:03
|
Revision: 754
http://jskeus.svn.sourceforge.net/jskeus/?rev=754&view=rev
Author: snozawa
Date: 2012-02-01 12:42:52 +0000 (Wed, 01 Feb 2012)
Log Message:
-----------
remove trace of :parent in :find-link-route <- this is obsolete
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2012-02-01 12:29:47 UTC (rev 753)
+++ trunk/irteus/irtmodel.l 2012-02-01 12:42:52 UTC (rev 754)
@@ -685,10 +685,7 @@
;; if link-route, just return "from" link
((and pl (eq to from))
(list from))
- ;; if parent-link is not prepared adequately, just trace parent
- ((and (not pl) (derivedp (send to :parent) bodyset-link))
- (send self :find-link-route (send to :parent) from)
- ))))
+ )))
(:link-list
(to &optional from)
(let (ret1 ret2)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2012-02-01 12:29:57
|
Revision: 753
http://jskeus.svn.sourceforge.net/jskeus/?rev=753&view=rev
Author: snozawa
Date: 2012-02-01 12:29:47 +0000 (Wed, 01 Feb 2012)
Log Message:
-----------
check weight == 0 in calculation of centroid
Modified Paths:
--------------
trunk/irteus/irtdyna.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2012-02-01 11:49:13 UTC (rev 752)
+++ trunk/irteus/irtdyna.l 2012-02-01 12:29:47 UTC (rev 753)
@@ -190,13 +190,15 @@
&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) self-centroid tmp-vb)))
- (dotimes (i len)
- (v+ ret (scale (elt additional-weights i) (elt additional-centroids i) tmp-va) ret))
- ret)
- tmp-vb))
+ (if (eps= new-weight 0.0)
+ self-centroid ;; if new-weight is 0, return self-centroid ;; in this case, this centroid value is not used latter calculation.
+ (scale
+ (/ 1.0 new-weight)
+ (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)
+ tmp-vb)))
(:append-inertia-no-update
(additional-weights
additional-centroids
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2012-02-01 11:49:23
|
Revision: 752
http://jskeus.svn.sourceforge.net/jskeus/?rev=752&view=rev
Author: snozawa
Date: 2012-02-01 11:49:13 +0000 (Wed, 01 Feb 2012)
Log Message:
-----------
trace all child-links from root link to calculate mass property information
Modified Paths:
--------------
trunk/irteus/irtdyna.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2012-01-28 12:24:31 UTC (rev 751)
+++ trunk/irteus/irtdyna.l 2012-02-01 11:49:13 UTC (rev 752)
@@ -350,6 +350,13 @@
;; inertia matrix and COG jacobian is at the world coordinates
;; link-list should be union-link-list (cannot use list of link-list)
(defmethod cascaded-link
+ (:exec-func-for-all-links
+ (func)
+ (labels ((tmp-exec-func-for-all-links
+ (l tmp-func)
+ (funcall tmp-func l)
+ (dolist (cl (send l :child-links)) (tmp-exec-func-for-all-links cl tmp-func))))
+ (tmp-exec-func-for-all-links (car (send self :links)) func)))
;; return ;; inertia-matrix [m * kg]
(:calc-inertia-matrix-from-link-list
(&rest args
@@ -374,13 +381,14 @@
&allow-other-keys)
(when update-mass-properties
;; parameter initialization
- (dolist (l (send self :links))
- (send l :put :m-til 0)
- (dotimes (i 3)
- (setf (elt (send l :get :c-til) i) 0.0))
- (dotimes (j 3)
- (dotimes (i 3)
- (setf (aref (send l :get :I-til) i j) 0.0))))
+ (send self :exec-func-for-all-links
+ #'(lambda (l)
+ (send l :put :m-til 0)
+ (dotimes (i 3)
+ (setf (elt (send l :get :c-til) i) 0.0))
+ (dotimes (j 3)
+ (dotimes (i 3)
+ (setf (aref (send l :get :I-til) i j) 0.0)))))
;; calc all links' mass properties
(send* (car (send self :links)) :propagate-mass-properties args))
;; calc inertia matrix
@@ -644,9 +652,10 @@
(tmp-mc (make-matrix 3 3))
(tmp-md (make-matrix 3 3)))
(let ((torque-vector (instantiate float-vector (length joint-list))))
- (dolist (l links)
- (send l :put :spacial-velocity-jacobian (float-vector 0 0 0))
- (send l :put :angular-velocity-jacobian (float-vector 0 0 0)))
+ (send self :exec-func-for-all-links
+ #'(lambda (l)
+ (send l :put :spacial-velocity-jacobian (float-vector 0 0 0))
+ (send l :put :angular-velocity-jacobian (float-vector 0 0 0))))
;; joint-angle update
(dotimes (i (length joint-list))
(let* ((jnt (elt joint-list i)))
@@ -662,8 +671,10 @@
:tmp-va tmp-va :tmp-vb tmp-vb :tmp-vc tmp-vc
:tmp-ma tmp-ma :tmp-mb tmp-mb :tmp-mc tmp-mc :tmp-md tmp-md)
;; reset ext force and ext moment
- (send-all (send self :links) :ext-force (float-vector 0 0 0))
- (send-all (send self :links) :ext-moment (float-vector 0 0 0))
+ (send self :exec-func-for-all-links
+ #'(lambda (l)
+ (send l :ext-force (float-vector 0 0 0))
+ (send l :ext-moment (float-vector 0 0 0))))
(dotimes (i (length torque-vector))
(setf (elt torque-vector i) (send (elt joint-list i) :joint-torque)))
torque-vector))
@@ -681,9 +692,10 @@
(unless update (return-from :calc-zmp (send self :get :zmp)))
;; under no external forces and moments
- (dolist (l links)
- (send l :put :ext-force (float-vector 0 0 0))
- (send l :put :ext-moment (float-vector 0 0 0)))
+ (send self :exec-func-for-all-links
+ #'(lambda (l)
+ (send l :ext-force (float-vector 0 0 0))
+ (send l :ext-moment (float-vector 0 0 0))))
;; set spacial-acceleration and angular-acceleration of root-link
(let* ((dt-1 (/ 1.0 dt))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2012-01-28 12:24:38
|
Revision: 751
http://jskeus.svn.sourceforge.net/jskeus/?rev=751&view=rev
Author: snozawa
Date: 2012-01-28 12:24:31 +0000 (Sat, 28 Jan 2012)
Log Message:
-----------
update find-link-route to support robots which has links not included in (send robot :links)
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2012-01-28 11:59:47 UTC (rev 750)
+++ trunk/irteus/irtmodel.l 2012-01-28 12:24:31 UTC (rev 751)
@@ -676,11 +676,16 @@
(to &optional from)
(let ((pl (send to :parent-link)))
(cond
+ ;; if to is not included in (send self :links), just trace parent-link
+ ((and pl (not (find to (send self :links))))
+ (send self :find-link-route pl from))
+ ;; if (send self :links), append "to" link
((and pl (not (eq to from)))
(append (send self :find-link-route pl from) (list to)))
+ ;; if link-route, just return "from" link
((and pl (eq to from))
(list from))
- ;; if parent-link is not prepared adequately, trace parent
+ ;; if parent-link is not prepared adequately, just trace parent
((and (not pl) (derivedp (send to :parent) bodyset-link))
(send self :find-link-route (send to :parent) from)
))))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2012-01-28 11:59:54
|
Revision: 750
http://jskeus.svn.sourceforge.net/jskeus/?rev=750&view=rev
Author: snozawa
Date: 2012-01-28 11:59:47 +0000 (Sat, 28 Jan 2012)
Log Message:
-----------
use error instead of wrning-message for negative max-joint-torque and negative max-joint-velocity
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2012-01-28 09:09:24 UTC (rev 749)
+++ trunk/irteus/irtmodel.l 2012-01-28 11:59:47 UTC (rev 750)
@@ -46,11 +46,11 @@
(if (or (and (float-vector-p mjv)
(not (v> mjv (instantiate float-vector (length mjv)))))
(and (not (float-vector-p mjv)) (< mjv 0.0)))
- (warning-message 3 "[joint ~A] warning negative max-joint-velocity value ~A~%" name mjv))
+ (error "[joint ~A] warning negative max-joint-velocity value ~A~%" name mjv))
(if (or (and (float-vector-p mjt)
(not (v> mjt (instantiate float-vector (length mjt)))))
(and (not (float-vector-p mjt)) (< mjt 0.0)))
- (warning-message 3 "[joint ~A] warning negative max-joint-torque value ~A~%" name mjt))
+ (error "[joint ~A] warning negative max-joint-torque value ~A~%" name mjt))
(send self :max-joint-velocity mjv)
(send self :max-joint-torque mjt)
(setq default-coords (send child-link :copy-coords))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <k-...@us...> - 2012-01-28 09:09:30
|
Revision: 749
http://jskeus.svn.sourceforge.net/jskeus/?rev=749&view=rev
Author: k-okada
Date: 2012-01-28 09:09:24 +0000 (Sat, 28 Jan 2012)
Log Message:
-----------
add find-parent for when parent-link is not included in the link-list :calc-jacobian-from-link-list
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2012-01-28 08:31:03 UTC (rev 748)
+++ trunk/irteus/irtmodel.l 2012-01-28 09:09:24 UTC (rev 749)
@@ -816,7 +816,7 @@
((or
(and (< (+ l 1) len)
(not (eq (send j :child-link)
- (send (elt link-list (+ l 1)) :parent-link))))
+ (find-parent (send (elt link-list (+ l 1)) :parent-link) link-list))))
(and (= len (+ l 1))
(not (eq (send j :child-link)
(find-parent (send move-target :parent) link-list)))))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <k-...@us...> - 2012-01-28 08:31:09
|
Revision: 748
http://jskeus.svn.sourceforge.net/jskeus/?rev=748&view=rev
Author: k-okada
Date: 2012-01-28 08:31:03 +0000 (Sat, 28 Jan 2012)
Log Message:
-----------
fix negative max-joint-velocity/torque for non scalar joint
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2012-01-28 08:17:21 UTC (rev 747)
+++ trunk/irteus/irtmodel.l 2012-01-28 08:31:03 UTC (rev 748)
@@ -43,8 +43,14 @@
(send self :name name)
(setq parent-link plink child-link clink
min-angle min max-angle max)
- (if (< mjv 0.0) (warning-message 3 "[joint ~A] warning negative max-joint-velocity value ~A~%" name mjv))
- (if (< mjt 0.0) (warning-message 3 "[joint ~A] warning negative max-joint-torque value ~A~%" name mjt))
+ (if (or (and (float-vector-p mjv)
+ (not (v> mjv (instantiate float-vector (length mjv)))))
+ (and (not (float-vector-p mjv)) (< mjv 0.0)))
+ (warning-message 3 "[joint ~A] warning negative max-joint-velocity value ~A~%" name mjv))
+ (if (or (and (float-vector-p mjt)
+ (not (v> mjt (instantiate float-vector (length mjt)))))
+ (and (not (float-vector-p mjt)) (< mjt 0.0)))
+ (warning-message 3 "[joint ~A] warning negative max-joint-torque value ~A~%" name mjt))
(send self :max-joint-velocity mjv)
(send self :max-joint-torque mjt)
(setq default-coords (send child-link :copy-coords))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <k-...@us...> - 2012-01-28 08:17:27
|
Revision: 747
http://jskeus.svn.sourceforge.net/jskeus/?rev=747&view=rev
Author: k-okada
Date: 2012-01-28 08:17:21 +0000 (Sat, 28 Jan 2012)
Log Message:
-----------
fix clac-angle-speed-gain-scalar/vector #704
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2012-01-28 08:13:11 UTC (rev 746)
+++ trunk/irteus/irtmodel.l 2012-01-28 08:17:21 UTC (rev 747)
@@ -105,8 +105,8 @@
(defun calc-angle-speed-gain-scalar
(j dav i periodic-time)
(let ((dav-gain
- (/ (send j :max-joint-velocity)
- (/ (abs (elt dav i)) periodic-time))))
+ (abs (/ (send j :max-joint-velocity)
+ (/ (elt dav i) periodic-time)))))
(if (< dav-gain 1.0) dav-gain 1.0)))
;; calc-angle-speed-gain-vector -> for wheel-joint, omniwheel-joint, sphere-joint and 6dof-joint
@@ -115,8 +115,8 @@
(let ((dav-gain 1.0))
(dotimes (ii (send j :joint-dof))
(let ((tmp-gain
- (/ (elt (send j :max-joint-velocity) ii)
- (/ (abs (elt dav (+ ii i))) periodic-time))))
+ (abs (/ (elt (send j :max-joint-velocity) ii)
+ (/ (elt dav (+ ii i)) periodic-time)))))
(if (< tmp-gain dav-gain) (setq dav-gain tmp-gain))))
dav-gain))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <k-...@us...> - 2012-01-28 08:13:18
|
Revision: 746
http://jskeus.svn.sourceforge.net/jskeus/?rev=746&view=rev
Author: k-okada
Date: 2012-01-28 08:13:11 +0000 (Sat, 28 Jan 2012)
Log Message:
-----------
print warning when max-joint-velocity and max-torque-value is minus
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2012-01-24 16:01:30 UTC (rev 745)
+++ trunk/irteus/irtmodel.l 2012-01-28 08:13:11 UTC (rev 746)
@@ -43,6 +43,8 @@
(send self :name name)
(setq parent-link plink child-link clink
min-angle min max-angle max)
+ (if (< mjv 0.0) (warning-message 3 "[joint ~A] warning negative max-joint-velocity value ~A~%" name mjv))
+ (if (< mjt 0.0) (warning-message 3 "[joint ~A] warning negative max-joint-torque value ~A~%" name mjt))
(send self :max-joint-velocity mjv)
(send self :max-joint-torque mjt)
(setq default-coords (send child-link :copy-coords))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2012-01-24 16:01:41
|
Revision: 745
http://jskeus.svn.sourceforge.net/jskeus/?rev=745&view=rev
Author: snozawa
Date: 2012-01-24 16:01:30 +0000 (Tue, 24 Jan 2012)
Log Message:
-----------
fix bug of fix leg to coords in full-body-ik sample
Modified Paths:
--------------
trunk/irteus/demo/full-body-ik.l
Modified: trunk/irteus/demo/full-body-ik.l
===================================================================
--- trunk/irteus/demo/full-body-ik.l 2012-01-20 13:09:45 UTC (rev 744)
+++ trunk/irteus/demo/full-body-ik.l 2012-01-24 16:01:30 UTC (rev 745)
@@ -11,7 +11,7 @@
(if (= (length (car (send *robot* :legs))) 6)
(send *robot* :legs :angle-vector #f(0 0 -10 20 0 -10)))
(if (send *robot* :lleg)
- (send *robot* :transform (send (send *robot* :lleg :end-coords) :transformation (make-coords)))
+ (send *robot* :newcoords (send (make-coords) :transform (send (send *robot* :lleg :end-coords) :transformation *robot*))) ;fix-leg
(send *robot* :newcoords (make-coords)))
(send *robot* :update-descendants)
(let* ((move-target (send *robot* :larm :end-coords))
@@ -37,9 +37,9 @@
(send *robot* :head :look-at
(send *robot* :larm :end-coords :worldpos))
(if (send *robot* :lleg)
- (send *robot* :transform (send (send *robot* :lleg :end-coords)
- :transformation (make-coords))) ;fix-leg
+ (send *robot* :newcoords (send (make-coords) :transform (send (send *robot* :lleg :end-coords) :transformation *robot*))) ;fix-leg
(send *robot* :newcoords (make-coords)))
+ (if use-leg (send *irtviewer* :draw-objects :flush nil))
(send *irtviewer* :viewer :viewsurface :color #f(1 1 1))
(send *irtviewer* :viewer :viewsurface :line-width 2)
(send *irtviewer* :viewer :viewsurface :3d-line
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2012-01-20 13:09:54
|
Revision: 744
http://jskeus.svn.sourceforge.net/jskeus/?rev=744&view=rev
Author: snozawa
Date: 2012-01-20 13:09:45 +0000 (Fri, 20 Jan 2012)
Log Message:
-----------
enable to set faces as an argument for pqp collision model
Modified Paths:
--------------
trunk/irteus/pqp.l
Modified: trunk/irteus/pqp.l
===================================================================
--- trunk/irteus/pqp.l 2012-01-20 09:38:57 UTC (rev 743)
+++ trunk/irteus/pqp.l 2012-01-20 13:09:45 UTC (rev 744)
@@ -31,12 +31,12 @@
(defmethod cascaded-coords
(:make-pqpmodel
- (&key (fat 0))
+ (&key (fat 0) ((:faces fs) (send self :faces)))
(let ((m (pqpmakemodel))
vs v1 v2 v3 (id 0))
(setf (get self :pqpmodel) m)
(pqpbeginmodel m)
- (dolist (f (send self :faces))
+ (dolist (f fs)
(dolist (poly (face-to-triangle-aux f))
(setq vs (send poly :vertices)
v1 (send self :inverse-transform-vector (first vs))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <k-...@us...> - 2012-01-20 09:39:08
|
Revision: 743
http://jskeus.svn.sourceforge.net/jskeus/?rev=743&view=rev
Author: k-okada
Date: 2012-01-20 09:38:57 +0000 (Fri, 20 Jan 2012)
Log Message:
-----------
add debug message on :inverse-kinematics
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2012-01-20 04:01:24 UTC (rev 742)
+++ trunk/irteus/irtmodel.l 2012-01-20 09:38:57 UTC (rev 743)
@@ -1590,7 +1590,10 @@
method-args)))
(when (and debug-view (not (memq :no-message debug-view)))
(warn "loop: ~3d~%" loop)
- (warn "union-link-list: ~A~%" (send-all union-link-list :name)))
+ (warn "union-link-list: ~A~%" (send-all union-link-list :name))
+ (warn "move-target: ~A~%" move-target)
+ (warn "targe-coordst: ~A~%" target-coords)
+ )
;; convergence check
(setq success
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2012-01-20 04:01:30
|
Revision: 742
http://jskeus.svn.sourceforge.net/jskeus/?rev=742&view=rev
Author: snozawa
Date: 2012-01-20 04:01:24 +0000 (Fri, 20 Jan 2012)
Log Message:
-----------
set 6dof-joint's weight by default in :fullbody-inverse-kinematics ;; note that i defined additional weight set by using (memq :weight args) instead of weight argument because (memq :weight args) does not require default weight value
Modified Paths:
--------------
trunk/irteus/irtrobot.l
Modified: trunk/irteus/irtrobot.l
===================================================================
--- trunk/irteus/irtrobot.l 2012-01-16 12:11:12 UTC (rev 741)
+++ trunk/irteus/irtrobot.l 2012-01-20 04:01:24 UTC (rev 742)
@@ -327,11 +327,6 @@
(min (float-vector -500 -500 -500 -20 -20 -10))
(max (float-vector 500 500 25 20 20 10))
(root-link-virtual-joint-weight #f(0.1 0.1 0.1 0.1 0.5 0.5))
- (weight
- #'(lambda (ul)
- (let ((weight (fill (instantiate float-vector (send self :calc-target-joint-dimension ul)) 1.0)))
- (dotimes (i 6) (setf (elt weight i) (elt root-link-virtual-joint-weight i)))
- weight)))
;; default cog-jacobian parameters : target-centroid-pos, cog-gain, and centroid-thre
(target-centroid-pos (apply #'midpoint 0.5 (send self :legs :end-coords :worldpos))) ;; <- target centroid position.
(cog-gain 1.0) ;; <- cog gain for null-space calculation. cog-gain should be over zero.
@@ -344,7 +339,13 @@
(send* self :inverse-kinematics target-coords
:move-target move-target :link-list link-list-with-robot-6dof
:cog-gain cog-gain :centroid-thre centroid-thre :target-centroid-pos target-centroid-pos
- :weight weight
+ :weight #'(lambda (union-link-list)
+ (let ((tmp-weight (fill (instantiate float-vector (send self :calc-target-joint-dimension union-link-list)) 1.0)))
+ (dotimes (i 6) (setf (elt tmp-weight i) (elt root-link-virtual-joint-weight i)))
+ (if (memq :weight args)
+ (let ((tmp-weight2 (funcall (cadr (memq :weight args)) union-link-list)))
+ (dotimes (i (length tmp-weight2)) (setf (elt tmp-weight i) (* (elt tmp-weight i) (elt tmp-weight2 i))))))
+ tmp-weight))
args)
))
)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ky...@us...> - 2012-01-16 12:11:19
|
Revision: 741
http://jskeus.svn.sourceforge.net/jskeus/?rev=741&view=rev
Author: kyouhei
Date: 2012-01-16 12:11:12 +0000 (Mon, 16 Jan 2012)
Log Message:
-----------
add method :inverse-rotate-vector to coordinates
Modified Paths:
--------------
trunk/irteus/irtgeo.l
Modified: trunk/irteus/irtgeo.l
===================================================================
--- trunk/irteus/irtgeo.l 2011-12-13 12:12:06 UTC (rev 740)
+++ trunk/irteus/irtgeo.l 2012-01-16 12:11:12 UTC (rev 741)
@@ -241,11 +241,15 @@
;;;
(defmethod coordinates
- (:rotate-vector (v &optional r)
- (if r (transform rot v r) (transform rot v))))
-(defmethod cascaded-coords
- (:rotate-vector (v &optional r)
- (send (send self :worldcoords) :rotate-vector v r)))
+ (:rotate-vector (v &optional r)
+ (if r (transform rot v r) (transform rot v)))
+ (:inverse-rotate-vector (v &optional r)
+ (if r (transform v rot r) (transform v rot))))
+(defmethod cascaded-coords
+ (:rotate-vector (v &optional r)
+ (send (send self :worldcoords) :rotate-vector v r))
+ (:inverse-rotate-vector (v &optional r)
+ (send (send self :worldcoords) :inverse-rotate-vector v r)))
(defmethod coordinates
(:inverse-transform-vector
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2011-12-13 12:12:17
|
Revision: 740
http://jskeus.svn.sourceforge.net/jskeus/?rev=740&view=rev
Author: snozawa
Date: 2011-12-13 12:12:06 +0000 (Tue, 13 Dec 2011)
Log Message:
-----------
enable to set collision-avoidance-link-pair outside of limb's inverse-kinematics methods
Modified Paths:
--------------
trunk/irteus/irtrobot.l
Modified: trunk/irteus/irtrobot.l
===================================================================
--- trunk/irteus/irtrobot.l 2011-12-08 23:47:10 UTC (rev 739)
+++ trunk/irteus/irtrobot.l 2011-12-13 12:12:06 UTC (rev 740)
@@ -101,18 +101,21 @@
(coerce (mapcar #'(lambda (l) (send l :joint :joint-angle))
(send self limb)) float-vector)))
(:inverse-kinematics
- (let ((link-list (if (memq :link-list args)
- (cadr (memq :link-list args))
- (send self :link-list
- (send self limb :end-coords :parent)
- (send self limb :root-link)))))
+ (let* ((link-list (if (memq :link-list args)
+ (cadr (memq :link-list args))
+ (send self :link-list
+ (send self limb :end-coords :parent)
+ (send self limb :root-link))))
+ (collision-avoidance-link-pair
+ (if (memq :collision-avoidance-link-pair args)
+ (cadr (memq :collision-avoidance-link-pair args))
+ (send self :collision-avoidance-link-pair-from-link-list link-list
+ :collision-avoidance-links (send self limb :collision-avoidance-links)))))
(send* self :inverse-kinematics (car args)
:move-target (if (memq :move-target args)
(cadr (memq :move-target args))
(send self limb :end-coords))
- :collision-avoidance-link-pair
- (send self :collision-avoidance-link-pair-from-link-list link-list
- :collision-avoidance-links (send self limb :collision-avoidance-links))
+ :collision-avoidance-link-pair collision-avoidance-link-pair
:link-list link-list
(cdr args))))
(:move-end
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2011-12-08 23:47:16
|
Revision: 739
http://jskeus.svn.sourceforge.net/jskeus/?rev=739&view=rev
Author: snozawa
Date: 2011-12-08 23:47:10 +0000 (Thu, 08 Dec 2011)
Log Message:
-----------
fix typo ;; cascaded-link -> coordinates
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2011-12-06 13:26:33 UTC (rev 738)
+++ trunk/irteus/irtmodel.l 2011-12-08 23:47:10 UTC (rev 739)
@@ -1642,7 +1642,7 @@
(send self :draw-collision-debug-view)
(when (car target-coords)
(dotimes (i (length target-coords))
- (send-message (elt move-target i) cascaded-link :draw-on :flush nil :size 100)
+ (send-message (elt move-target i) coordinates :draw-on :flush nil :size 100)
(send (elt target-coords i) :draw-on :flush nil :color #f(1 0 0))))
(if (not (memq :no-flush debug-view)) (send *viewer* :viewsurface :flush))
)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2011-12-06 13:26:45
|
Revision: 738
http://jskeus.svn.sourceforge.net/jskeus/?rev=738&view=rev
Author: snozawa
Date: 2011-12-06 13:26:33 +0000 (Tue, 06 Dec 2011)
Log Message:
-----------
remove deprecated argument dt
Modified Paths:
--------------
trunk/irteus/irtdyna.l
Modified: trunk/irteus/irtdyna.l
===================================================================
--- trunk/irteus/irtdyna.l 2011-12-01 13:45:15 UTC (rev 737)
+++ trunk/irteus/irtdyna.l 2011-12-06 13:26:33 UTC (rev 738)
@@ -720,7 +720,7 @@
jvv)) ;; [rad/s]
(jav (scale dt-1 (v- jvv pjvv)))) ;; [rad/s^2]
(send* self calc-torque-method
- :dt dt :jvv jvv :jav jav
+ :jvv jvv :jav jav
:debug-view debug-view
calc-torque-args)
(send self :put :prev-av av)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <je...@js...> - 2011-12-01 16:08:07
|
<http://jenkins.jsk.imi.i.u-tokyo.ac.jp:8080/job/jskeus/10/>を確認してください。 |
