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-07-06 10:43:56
|
Revision: 830
http://jskeus.svn.sourceforge.net/jskeus/?rev=830&view=rev
Author: snozawa
Date: 2012-07-06 10:43:46 +0000 (Fri, 06 Jul 2012)
Log Message:
-----------
remove unnecessary ext-force moment setting for non-legged robot
Modified Paths:
--------------
trunk/irteus/irtrobot.l
Modified: trunk/irteus/irtrobot.l
===================================================================
--- trunk/irteus/irtrobot.l 2012-06-27 07:36:58 UTC (rev 829)
+++ trunk/irteus/irtrobot.l 2012-07-06 10:43:46 UTC (rev 830)
@@ -251,16 +251,8 @@
(debug-view nil)
(jvv (instantiate float-vector (calc-target-joint-dimension joint-list))) ;; [rad/s] or [m/s]
(jav (instantiate float-vector (calc-target-joint-dimension joint-list)))) ;; [rad/s^2] or [m/s^2]
- (let ((weight-force (* (send self :weight) 1e-6 (elt *g-vec* 2)))) ;; weight[g] * 1e-6 * gvec[mm/s^2] = force[N]
- ;; set default external force and moment at the end-effectors
- (if (every #'null (send self :legs)) ;; for not legged robot
- (let ((fv (float-vector 0 0 weight-force))) ;; for non-legged robot
- (send (car (send self :links)) :ext-force fv)
- (let* ((c (send (car (send self :links)) :centroid))
- (moment-offset (v* (scale 1e-3 (send (car (send self :links)) :worldpos)) fv)))
- (send (car (send self :links)) :ext-moment moment-offset)
- ))
- (progn ;; for legged robot
+ (unless (every #'null (send self :legs)) ;; for legged robot, set default external force and moment at the end-effectors
+ (let ((weight-force (* (send self :weight) 1e-6 (elt *g-vec* 2)))) ;; weight[g] * 1e-6 * gvec[mm/s^2] = force[N]
(unless target-coords
(dolist (limb '(:rleg :lleg))
(push (send self limb :end-coords) target-coords)))
@@ -280,7 +272,7 @@
;; hypothesis : gravity force moment + rleg moment + lleg moment = 0, minimal internal moments <-> pseudo-inverse <-> lleg moment = rleg moment
(dolist (limb '(:rleg :lleg))
(push (scale 0.5 total-moment) moment-list))
- ))
+ ))))
(unless (= (length force-list) (length moment-list)
(length target-coords))
(warn ";; ERROR : list length differ : force-list ~A moment-list ~A target-coords ~A~%"
@@ -294,8 +286,6 @@
(moment-offset (v* (scale 1e-3 (send tc :worldpos)) fv)))
(send (send tc :parent) :ext-moment (v+ mv moment-offset))))
force-list moment-list target-coords)
- )
- ))
(send-super :calc-torque
:debug-view debug-view
:jvv jvv :jav jav)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2012-06-27 07:37:08
|
Revision: 829
http://jskeus.svn.sourceforge.net/jskeus/?rev=829&view=rev
Author: snozawa
Date: 2012-06-27 07:36:58 +0000 (Wed, 27 Jun 2012)
Log Message:
-----------
import make-ring function to user package
Modified Paths:
--------------
trunk/irteus/irtgeo.l
Modified: trunk/irteus/irtgeo.l
===================================================================
--- trunk/irteus/irtgeo.l 2012-06-27 07:34:45 UTC (rev 828)
+++ trunk/irteus/irtgeo.l 2012-06-27 07:36:58 UTC (rev 829)
@@ -27,7 +27,7 @@
(in-package "GEOMETRY")
(export '(body-to-faces body-to-triangles midcoords orient-coords-to-axis bodyset *g-vec*
- make-sphere x-of-cube y-of-cube z-of-cube
+ make-sphere make-ring x-of-cube y-of-cube z-of-cube
height-of-cylinder radius-of-cylinder radius-of-sphere
matrix-to-euler-angle))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2012-06-27 07:34:54
|
Revision: 828
http://jskeus.svn.sourceforge.net/jskeus/?rev=828&view=rev
Author: snozawa
Date: 2012-06-27 07:34:45 +0000 (Wed, 27 Jun 2012)
Log Message:
-----------
add make-ring function to generate ring primitive
Modified Paths:
--------------
trunk/irteus/irtgeo.l
Modified: trunk/irteus/irtgeo.l
===================================================================
--- trunk/irteus/irtgeo.l 2012-06-26 08:07:22 UTC (rev 827)
+++ trunk/irteus/irtgeo.l 2012-06-27 07:34:45 UTC (rev 828)
@@ -515,6 +515,14 @@
(send obj :csg `((:sphere ,r)))
obj))
+(defun make-ring (ring-radius pipe-radius &rest args &key (segments 16))
+ (let ((v-list) (dtheta (/ 2pi segments)))
+ (dotimes (i segments)
+ (push (float-vector (+ (* pipe-radius (cos (* i dtheta))) ring-radius)
+ 0 (* pipe-radius (sin (* i dtheta))))
+ v-list))
+ (apply #'make-torus v-list :segments segments args)))
+
;;
;; accessor to primitive bodies
;;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2012-06-26 08:07:33
|
Revision: 827
http://jskeus.svn.sourceforge.net/jskeus/?rev=827&view=rev
Author: snozawa
Date: 2012-06-26 08:07:22 +0000 (Tue, 26 Jun 2012)
Log Message:
-----------
rename handles -> handle according to rbrain robot-object
Modified Paths:
--------------
trunk/irteus/demo/crank-motion.l
trunk/irteus/demo/dual-arm-ik.l
Modified: trunk/irteus/demo/crank-motion.l
===================================================================
--- trunk/irteus/demo/crank-motion.l 2012-06-24 06:45:48 UTC (rev 826)
+++ trunk/irteus/demo/crank-motion.l 2012-06-26 08:07:22 UTC (rev 827)
@@ -2,7 +2,7 @@
(defclass sample-crank
:super cascaded-link
- :slots (handles crank-joint)
+ :slots (handle crank-joint)
)
(defmethod sample-crank
@@ -11,7 +11,7 @@
&key (root-bar-height 550) (handle-base-radius 100) (handle-bar-height 40)
&allow-other-keys)
(send-super* :init args)
- (setq handles nil)
+ (setq handle nil)
;; 1. make links links and assoc all links
(let ((rl (send self :make-root-link root-bar-height))
(cl (send self :make-crank-link handle-base-radius handle-bar-height)))
@@ -58,10 +58,10 @@
:translate (float-vector 0 (* -0.5 handle-base-radius) (/ handle-bar-height 2.0)))
:name :crank-handle)))
(send br :assoc ahandle)
- (push ahandle handles)
+ (push ahandle handle)
br)))
- (:handle () handles)
- (:crank-handle () (car handles))
+ (:handle () handle)
+ (:crank-handle () (car handle))
(:crank-joint (&rest args) (forward-message-to crank-joint args))
)
Modified: trunk/irteus/demo/dual-arm-ik.l
===================================================================
--- trunk/irteus/demo/dual-arm-ik.l 2012-06-24 06:45:48 UTC (rev 826)
+++ trunk/irteus/demo/dual-arm-ik.l 2012-06-26 08:07:22 UTC (rev 827)
@@ -2,7 +2,7 @@
(defclass sample-broom
:super cascaded-link
- :slots (handles)
+ :slots (handle)
)
(defmethod sample-broom
@@ -12,7 +12,7 @@
(bar-length 700) (bar-width 20)
&allow-other-keys)
(send-super* :init args)
- (setq handles nil)
+ (setq handle nil)
;; 1. make links links and assoc all links
(let ((rl (send self :make-broom-link
sweep-height sweep-width sweep-thickness
@@ -52,10 +52,10 @@
(let ((br (instance bodyset-link :init (make-cascoords)
:bodies (list bar sweep))))
(dolist (rate (list 0.8 0.6))
- (push (make-cascoords :pos (float-vector 0 0 (+ (* rate bl) sh))) handles))
- (dolist (hc handles) (send br :assoc hc))
+ (push (make-cascoords :pos (float-vector 0 0 (+ (* rate bl) sh))) handle))
+ (dolist (hc handle) (send br :assoc hc))
br)))
- (:handle () handles)
+ (:handle () handle)
)
(defun dual-arm-ik nil
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2012-06-24 06:45:56
|
Revision: 826
http://jskeus.svn.sourceforge.net/jskeus/?rev=826&view=rev
Author: snozawa
Date: 2012-06-24 06:45:48 +0000 (Sun, 24 Jun 2012)
Log Message:
-----------
update g-vec value ;; 9.8 -> 9.80665
Modified Paths:
--------------
trunk/irteus/irtgeo.l
Modified: trunk/irteus/irtgeo.l
===================================================================
--- trunk/irteus/irtgeo.l 2012-06-20 08:23:02 UTC (rev 825)
+++ trunk/irteus/irtgeo.l 2012-06-24 06:45:48 UTC (rev 826)
@@ -31,7 +31,7 @@
height-of-cylinder radius-of-cylinder radius-of-sphere
matrix-to-euler-angle))
-(defvar *g-vec* (float-vector 0 0 9800)) ;; [mm/s^2]
+(defvar *g-vec* (float-vector 0 0 9806.65)) ;; [mm/s^2]
(defun midcoords (p c1 c2)
(let ()
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2012-06-20 08:23:11
|
Revision: 825
http://jskeus.svn.sourceforge.net/jskeus/?rev=825&view=rev
Author: snozawa
Date: 2012-06-20 08:23:02 +0000 (Wed, 20 Jun 2012)
Log Message:
-----------
enable to set sweep parameter
Modified Paths:
--------------
trunk/irteus/demo/dual-arm-ik.l
Modified: trunk/irteus/demo/dual-arm-ik.l
===================================================================
--- trunk/irteus/demo/dual-arm-ik.l 2012-06-20 08:14:18 UTC (rev 824)
+++ trunk/irteus/demo/dual-arm-ik.l 2012-06-20 08:23:02 UTC (rev 825)
@@ -7,11 +7,16 @@
(defmethod sample-broom
(:init
- (&rest args)
+ (&rest args
+ &key (sweep-height 250) (sweep-width 125) (sweep-thickness 20)
+ (bar-length 700) (bar-width 20)
+ &allow-other-keys)
(send-super* :init args)
(setq handles nil)
;; 1. make links links and assoc all links
- (let ((rl (send self :make-broom-link)))
+ (let ((rl (send self :make-broom-link
+ sweep-height sweep-width sweep-thickness
+ bar-length bar-width)))
;; 2. assoc links
;; Root link should be associated with "self".
(send self :assoc rl)
@@ -27,13 +32,17 @@
self))
;; Methods to define robot links
(:make-broom-link
- ()
- (let* ((sh 250) (sw 125) (length 700)
- (bar (make-cylinder 10 length))
+ (sh ;; sw = Sweep Height
+ sw ;; sw = Sweep Width
+ st ;; st = Sweep Thickness
+ bl ;; bl = Bar Length
+ bw) ;; bw = Bar Width
+ (let* ((bar (make-cylinder (/ bw 2.0) bl))
(sweep (make-prism (list (float-vector sw (- sh) 0)
(float-vector (- sw) (- sh) 0)
- (float-vector -20 0 0)
- (float-vector 20 0 0)) 20)))
+ (float-vector (* -0.5 bw) 0 0)
+ (float-vector (* 0.5 bw) 0 0))
+ st)))
(send bar :locate (float-vector 0 0 sh) :world)
(send sweep :rotate pi/2 :x)
(send sweep :locate (float-vector 0 10 sh) :world)
@@ -43,7 +52,7 @@
(let ((br (instance bodyset-link :init (make-cascoords)
:bodies (list bar sweep))))
(dolist (rate (list 0.8 0.6))
- (push (make-cascoords :pos (float-vector 0 0 (+ (* rate length) sh))) handles))
+ (push (make-cascoords :pos (float-vector 0 0 (+ (* rate bl) sh))) handles))
(dolist (hc handles) (send br :assoc hc))
br)))
(:handle () handles)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2012-06-20 08:14:27
|
Revision: 824
http://jskeus.svn.sourceforge.net/jskeus/?rev=824&view=rev
Author: snozawa
Date: 2012-06-20 08:14:18 +0000 (Wed, 20 Jun 2012)
Log Message:
-----------
enable to set crank model parameter
Modified Paths:
--------------
trunk/irteus/demo/crank-motion.l
Modified: trunk/irteus/demo/crank-motion.l
===================================================================
--- trunk/irteus/demo/crank-motion.l 2012-06-19 11:13:13 UTC (rev 823)
+++ trunk/irteus/demo/crank-motion.l 2012-06-20 08:14:18 UTC (rev 824)
@@ -7,13 +7,15 @@
(defmethod sample-crank
(:init
- (&rest args)
+ (&rest args
+ &key (root-bar-height 550) (handle-base-radius 100) (handle-bar-height 40)
+ &allow-other-keys)
(send-super* :init args)
(setq handles nil)
;; 1. make links links and assoc all links
- (let ((rl (send self :make-root-link))
- (cl (send self :make-crank-link)))
- (send cl :translate #f(0 0 500) :world)
+ (let ((rl (send self :make-root-link root-bar-height))
+ (cl (send self :make-crank-link handle-base-radius handle-bar-height)))
+ (send cl :translate (float-vector 0 0 root-bar-height) :world)
;; 2. assoc links
;; Root link should be associated with "self".
(send self :assoc rl)
@@ -33,24 +35,27 @@
self))
;; Methods to define robot links
(:make-root-link
- ()
+ (root-bar-height)
(instance bodyset-link :init (make-cascoords)
- :bodies (list (make-cylinder 10 500))
+ :bodies (list (make-cylinder 10 root-bar-height))
:name :crank-root-link))
(:make-crank-link
- ()
- (let* ((b0 (make-cylinder 10 50))
- (b1 (make-cylinder 10 70))
- (b2 (make-cube 30 120 10)))
- (send b2 :translate (float-vector 0 -50 55))
- (send b1 :translate (float-vector 0 -100 60))
- (send b0 :assoc b1)
- (send b0 :assoc b2)
+ (handle-base-radius handle-bar-height)
+ (let* ((handle-base-thickness 10)
+ (handle-bar-radius 15)
+ (handle-base (make-cube (* handle-bar-radius 2) handle-base-radius handle-base-thickness))
+ (handle-bar (make-cylinder handle-bar-radius handle-bar-height)))
+ (send handle-bar :translate
+ (float-vector 0 (- handle-bar-radius handle-base-radius) (/ handle-base-thickness 2.0)))
+ (send handle-base :translate (float-vector 0 (/ handle-base-radius -2.0) 0))
+ (send handle-base :assoc handle-bar)
+ (send handle-base :translate (float-vector 0 0 (/ handle-base-thickness 2.0)))
(let* ((br (instance bodyset-link :init (make-cascoords)
- :bodies (list b0 b1 b2) :name :crank-handle-link))
+ :bodies (list handle-base handle-bar) :name :crank-handle-link))
(ahandle
(make-cascoords :coords
- (send (send b1 :copy-worldcoords) :translate (float-vector 0 0 50))
+ (send (send handle-base :copy-worldcoords)
+ :translate (float-vector 0 (* -0.5 handle-base-radius) (/ handle-bar-height 2.0)))
:name :crank-handle)))
(send br :assoc ahandle)
(push ahandle handles)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2012-06-19 11:13:19
|
Revision: 823
http://jskeus.svn.sourceforge.net/jskeus/?rev=823&view=rev
Author: snozawa
Date: 2012-06-19 11:13:13 +0000 (Tue, 19 Jun 2012)
Log Message:
-----------
add deftest to obtain unittest result
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2012-06-19 10:51:58 UTC (rev 822)
+++ trunk/irteus/irtmodel.l 2012-06-19 11:13:13 UTC (rev 823)
@@ -2419,7 +2419,8 @@
(require :unittest "lib/llib/unittest.l")
(init-unit-test)
(setq lisp::*exit-on-fatal-error* nil)
- (eusmodel-validity-check-one robot)
+ (eval `(deftest eusmodel-validity-check-test
+ (eusmodel-validity-check-one ,robot)))
(run-all-tests)
)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2012-06-19 10:52:09
|
Revision: 822
http://jskeus.svn.sourceforge.net/jskeus/?rev=822&view=rev
Author: snozawa
Date: 2012-06-19 10:51:58 +0000 (Tue, 19 Jun 2012)
Log Message:
-----------
rename eusmodel-validity-check -> eusmodel-validity-check-one ;; eusmodel-validity-check becomes single unittest program
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2012-06-19 10:39:14 UTC (rev 821)
+++ trunk/irteus/irtmodel.l 2012-06-19 10:51:58 UTC (rev 822)
@@ -2377,7 +2377,7 @@
,new-move-target)
))))
-(defun eusmodel-validity-check (robot)
+(defun eusmodel-validity-check-one (robot)
;; root-link-validity-check
(let ((root-link (car (send robot :links))))
(assert (null (send root-link :parent-link))
@@ -2415,6 +2415,14 @@
(format nil "definition of child-link and parent-link should consistent in joints and links ~A ~A ~A" j (send j :child-link) (send j :parent-link)))
)))
+(defun eusmodel-validity-check (robot)
+ (require :unittest "lib/llib/unittest.l")
+ (init-unit-test)
+ (setq lisp::*exit-on-fatal-error* nil)
+ (eusmodel-validity-check-one robot)
+ (run-all-tests)
+ )
+
(in-package "GEOMETRY")
(provide :irtmodel "$Id$")
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2012-06-19 10:39:23
|
Revision: 821
http://jskeus.svn.sourceforge.net/jskeus/?rev=821&view=rev
Author: snozawa
Date: 2012-06-19 10:39:14 +0000 (Tue, 19 Jun 2012)
Log Message:
-----------
remove labels function and fix indent
Modified Paths:
--------------
trunk/irteus/demo/dual-arm-ik.l
Modified: trunk/irteus/demo/dual-arm-ik.l
===================================================================
--- trunk/irteus/demo/dual-arm-ik.l 2012-06-19 10:37:55 UTC (rev 820)
+++ trunk/irteus/demo/dual-arm-ik.l 2012-06-19 10:39:14 UTC (rev 821)
@@ -53,44 +53,42 @@
"dual arm ik"
(send *irtviewer* :title "dual-arm-ik")
(let ((i 0) link-list move-target target-coords b0)
- (labels
- ()
- ;;
- (unless (boundp '*robot*)
- (setq *robot* (instance sample-robot :init)))
- (send *robot* :reset-pose)
- (if (= (length (car (send *robot* :arms))) 7)
- (send *robot* :arms :angle-vector #f(10 20 0 -20 10 0 0)))
- (if (some #'null (send *robot* :legs))
- (send *robot* :newcoords (make-coords))
- (send *robot* :transform (send (apply #'midcoords 0.5 (send *robot* :legs :end-coords)) :transformation (make-coords))))
- (send *robot* :update-descendants)
- ;;
- ;; make broom model
- (setq b0 (instance sample-broom :init))
- (send b0 :locate #f(250 0 0))
- ;;
- ;; setup move-target and link-list
- (setq move-target (send *robot* :arms :end-coords)
- link-list (mapcar #'(lambda (mt) (send *robot* :link-list mt)) (send-all move-target :parent)))
- ;;
- ;; look-at
- (send *robot* :head :look-at (apply #'midpoint 0.5 (send-all (send b0 :handle) :worldpos)))
- (objects (list *robot* b0))
- ;;
- ;; do sweep
- (do-until-key
- (send *robot* :inverse-kinematics (send b0 :handle)
- :link-list link-list :move-target move-target
- :stop 500 :thre '(10 10)
- :rotation-axis '(nil nil) :debug-view nil :dump-command nil)
- (send *robot* :head :look-at
- (apply #'midpoint 0.5 (send-all (send b0 :handle) :worldpos)))
- (send b0 :orient (* 0.2 (sin (/ i 10.0))) :x :world)
- (send b0 :locate (float-vector 250 (* 250 (sin (/ (incf i) 10.0))) 0) :world)
- (send *irtviewer* :draw-objects)
- (incf i)
- ))
+ ;;
+ (unless (boundp '*robot*)
+ (setq *robot* (instance sample-robot :init)))
+ (send *robot* :reset-pose)
+ (if (= (length (car (send *robot* :arms))) 7)
+ (send *robot* :arms :angle-vector #f(10 20 0 -20 10 0 0)))
+ (if (some #'null (send *robot* :legs))
+ (send *robot* :newcoords (make-coords))
+ (send *robot* :transform (send (apply #'midcoords 0.5 (send *robot* :legs :end-coords)) :transformation (make-coords))))
+ (send *robot* :update-descendants)
+ ;;
+ ;; make broom model
+ (setq b0 (instance sample-broom :init))
+ (send b0 :locate #f(250 0 0))
+ ;;
+ ;; setup move-target and link-list
+ (setq move-target (send *robot* :arms :end-coords)
+ link-list (mapcar #'(lambda (mt) (send *robot* :link-list mt)) (send-all move-target :parent)))
+ ;;
+ ;; look-at
+ (send *robot* :head :look-at (apply #'midpoint 0.5 (send-all (send b0 :handle) :worldpos)))
+ (objects (list *robot* b0))
+ ;;
+ ;; do sweep
+ (do-until-key
+ (send *robot* :inverse-kinematics (send b0 :handle)
+ :link-list link-list :move-target move-target
+ :stop 500 :thre '(10 10)
+ :rotation-axis '(nil nil) :debug-view nil :dump-command nil)
+ (send *robot* :head :look-at
+ (apply #'midpoint 0.5 (send-all (send b0 :handle) :worldpos)))
+ (send b0 :orient (* 0.2 (sin (/ i 10.0))) :x :world)
+ (send b0 :locate (float-vector 250 (* 250 (sin (/ (incf i) 10.0))) 0) :world)
+ (send *irtviewer* :draw-objects)
+ (incf i)
+ )
))
(unless (boundp '*irtviewer*) (make-irtviewer))
(warn "(dual-arm-ik) for tool usage~%")
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2012-06-19 10:38:05
|
Revision: 820
http://jskeus.svn.sourceforge.net/jskeus/?rev=820&view=rev
Author: snozawa
Date: 2012-06-19 10:37:55 +0000 (Tue, 19 Jun 2012)
Log Message:
-----------
add handle function according to committed irteus models
Modified Paths:
--------------
trunk/irteus/demo/crank-motion.l
Modified: trunk/irteus/demo/crank-motion.l
===================================================================
--- trunk/irteus/demo/crank-motion.l 2012-06-19 10:37:23 UTC (rev 819)
+++ trunk/irteus/demo/crank-motion.l 2012-06-19 10:37:55 UTC (rev 820)
@@ -55,6 +55,7 @@
(send br :assoc ahandle)
(push ahandle handles)
br)))
+ (:handle () handles)
(:crank-handle () (car handles))
(:crank-joint (&rest args) (forward-message-to crank-joint args))
)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2012-06-19 10:37:34
|
Revision: 819
http://jskeus.svn.sourceforge.net/jskeus/?rev=819&view=rev
Author: snozawa
Date: 2012-06-19 10:37:23 +0000 (Tue, 19 Jun 2012)
Log Message:
-----------
add sample-broom class ;; separate labels function make-sweep
Modified Paths:
--------------
trunk/irteus/demo/dual-arm-ik.l
Modified: trunk/irteus/demo/dual-arm-ik.l
===================================================================
--- trunk/irteus/demo/dual-arm-ik.l 2012-06-19 10:08:00 UTC (rev 818)
+++ trunk/irteus/demo/dual-arm-ik.l 2012-06-19 10:37:23 UTC (rev 819)
@@ -1,30 +1,60 @@
(load "sample-robot-model.l")
+(defclass sample-broom
+ :super cascaded-link
+ :slots (handles)
+ )
+
+(defmethod sample-broom
+ (:init
+ (&rest args)
+ (send-super* :init args)
+ (setq handles nil)
+ ;; 1. make links links and assoc all links
+ (let ((rl (send self :make-broom-link)))
+ ;; 2. assoc links
+ ;; Root link should be associated with "self".
+ (send self :assoc rl)
+ ;; 3. make all joints
+ ;; Before making joints, you should :assoc all links.
+
+ ;; 4. define slots for robot class
+ ;; links and joint-list for cascaded-link.
+ (setq links (list rl))
+ (setq joint-list nil)
+ ;; 5. call :init-ending after defining links and joint-list and return "self"
+ (send self :init-ending)
+ self))
+ ;; Methods to define robot links
+ (:make-broom-link
+ ()
+ (let* ((sh 250) (sw 125) (length 700)
+ (bar (make-cylinder 10 length))
+ (sweep (make-prism (list (float-vector sw (- sh) 0)
+ (float-vector (- sw) (- sh) 0)
+ (float-vector -20 0 0)
+ (float-vector 20 0 0)) 20)))
+ (send bar :locate (float-vector 0 0 sh) :world)
+ (send sweep :rotate pi/2 :x)
+ (send sweep :locate (float-vector 0 10 sh) :world)
+ (send bar :set-color :brown)
+ (send sweep :set-color :red)
+ (send bar :assoc sweep)
+ (let ((br (instance bodyset-link :init (make-cascoords)
+ :bodies (list bar sweep))))
+ (dolist (rate (list 0.8 0.6))
+ (push (make-cascoords :pos (float-vector 0 0 (+ (* rate length) sh))) handles))
+ (dolist (hc handles) (send br :assoc hc))
+ br)))
+ (:handle () handles)
+ )
+
(defun dual-arm-ik nil
"dual arm ik"
(send *irtviewer* :title "dual-arm-ik")
(let ((i 0) link-list move-target target-coords b0)
(labels
- ((make-sweep
- nil
- (let ((sh 250) (sw 125) (length 700) sweep bar br)
- (setq bar (make-cylinder 10 length))
- (setq sweep (make-prism (list (float-vector sw (- sh) 0)
- (float-vector (- sw) (- sh) 0)
- (float-vector -20 0 0)
- (float-vector 20 0 0)) 20))
- (send bar :locate (float-vector 0 0 sh) :world)
- (send sweep :rotate pi/2 :x)
- (send sweep :locate (float-vector 0 10 sh) :world)
- (send bar :set-color :brown)
- (send sweep :set-color :red)
- (setq br (instance bodyset :init (make-cascoords)
- :bodies (list bar sweep)))
- (dolist (rate (list 0.8 0.6))
- (push (make-cascoords :pos (float-vector 0 0 (+ (* rate length) sh))) target-coords))
- (dolist (tc (append target-coords (list bar sweep)))
- (send br :assoc tc))
- br)))
+ ()
;;
(unless (boundp '*robot*)
(setq *robot* (instance sample-robot :init)))
@@ -37,7 +67,7 @@
(send *robot* :update-descendants)
;;
;; make broom model
- (setq b0 (make-sweep))
+ (setq b0 (instance sample-broom :init))
(send b0 :locate #f(250 0 0))
;;
;; setup move-target and link-list
@@ -45,17 +75,17 @@
link-list (mapcar #'(lambda (mt) (send *robot* :link-list mt)) (send-all move-target :parent)))
;;
;; look-at
- (send *robot* :head :look-at (apply #'midpoint 0.5 (send-all target-coords :worldpos)))
+ (send *robot* :head :look-at (apply #'midpoint 0.5 (send-all (send b0 :handle) :worldpos)))
(objects (list *robot* b0))
;;
;; do sweep
(do-until-key
- (send *robot* :inverse-kinematics target-coords
+ (send *robot* :inverse-kinematics (send b0 :handle)
:link-list link-list :move-target move-target
:stop 500 :thre '(10 10)
:rotation-axis '(nil nil) :debug-view nil :dump-command nil)
(send *robot* :head :look-at
- (apply #'midpoint 0.5 (send-all target-coords :worldpos)))
+ (apply #'midpoint 0.5 (send-all (send b0 :handle) :worldpos)))
(send b0 :orient (* 0.2 (sin (/ i 10.0))) :x :world)
(send b0 :locate (float-vector 250 (* 250 (sin (/ (incf i) 10.0))) 0) :world)
(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-06-19 10:08:10
|
Revision: 818
http://jskeus.svn.sourceforge.net/jskeus/?rev=818&view=rev
Author: snozawa
Date: 2012-06-19 10:08:00 +0000 (Tue, 19 Jun 2012)
Log Message:
-----------
add eusmodel-validity-check ;; this code is not pr2 dependent and only dependent irtmodel specification, so i copy this function from euscollada-pr2-test.l
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2012-06-15 05:07:05 UTC (rev 817)
+++ trunk/irteus/irtmodel.l 2012-06-19 10:08:00 UTC (rev 818)
@@ -2377,6 +2377,44 @@
,new-move-target)
))))
+(defun eusmodel-validity-check (robot)
+ ;; root-link-validity-check
+ (let ((root-link (car (send robot :links))))
+ (assert (null (send root-link :parent-link))
+ (format nil "root link should have no parent-link!! ~A ~A~%" root-link (send root-link :parent-link)))
+ (assert (equal robot (send root-link :parent))
+ (format nil "root link ~A should be :assoced with robot ~A!!~%" root-link robot))
+ )
+
+ ;;link-joint-length-check-for-serial-link-manipulator
+ (let ((joint-list
+ (remove-duplicates
+ (append (mapcar #'cdr (remove-if-not #'(lambda (s) (derivedp (cdr s) joint)) (send robot :slots)))
+ (send robot :joint-list))))
+ (links (remove-duplicates
+ (append (mapcar #'cdr (remove-if-not #'(lambda (s) (derivedp (cdr s) bodyset-link)) (send robot :slots)))
+ (send robot :links)))))
+ (assert (= (+ (length joint-list) 1) (length links))
+ (format nil ";; link(~A) = joint(=~A) + 1 <- for serial link manipulator!!~%" (length links) (length joint-list)))
+ ;; chain-validity-check
+ (dolist (j joint-list)
+ ;; joint should have child-link derived from bodyset-link class
+ (assert (and (send j :child-link) (derivedp (send j :child-link) bodyset-link))
+ (format nil "joint should have child-link derived from bodyset-link class ~A ~A" j (send j :child-link)))
+ ;; joint should have parent-link derived from bodyset-link class
+ (assert (and (send j :parent-link) (derivedp (send j :parent-link) bodyset-link))
+ (format nil "child-link should associated with parent-link ~A ~A"
+ j (send j :parent-link)))
+ ;; definition of child-link and parent-link should consistent in joints and links
+ (assert (and (member (send j :child-link) (send (send j :parent-link) :descendants))
+ (equal (send j :parent-link) (send (send j :child-link) :parent)))
+ (format nil "definition of child-link and parent-link should consistent in joints and links ~A ~A ~A" j (send j :parent-link) (send j :child-link)))
+ ;; definition of child-link and parent-link should consistent in joints and links
+ (assert (and (equal (send (send j :child-link) :parent-link) (send j :parent-link))
+ (member (send j :child-link) (send (send j :parent-link) :child-links)))
+ (format nil "definition of child-link and parent-link should consistent in joints and links ~A ~A ~A" j (send j :child-link) (send j :parent-link)))
+ )))
+
(in-package "GEOMETRY")
(provide :irtmodel "$Id$")
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <k-...@us...> - 2012-06-15 05:07:11
|
Revision: 817
http://jskeus.svn.sourceforge.net/jskeus/?rev=817&view=rev
Author: k-okada
Date: 2012-06-15 05:07:05 +0000 (Fri, 15 Jun 2012)
Log Message:
-----------
add :calc-force-from-joint-torque
Modified Paths:
--------------
trunk/irteus/irtrobot.l
Modified: trunk/irteus/irtrobot.l
===================================================================
--- trunk/irteus/irtrobot.l 2012-06-10 07:56:40 UTC (rev 816)
+++ trunk/irteus/irtrobot.l 2012-06-15 05:07:05 UTC (rev 817)
@@ -300,6 +300,24 @@
:debug-view debug-view
:jvv jvv :jav jav)
)
+ (:calc-force-from-joint-torque
+ (limb all-torque &key (move-target (send self limb :end-coords)) (use-torso))
+ (let* ((link-list
+ (send self :link-list
+ (send move-target :parent)
+ (unless use-torso (car (send self limb :links)))))
+ (jacobian
+ (send self :calc-jacobian-from-link-list
+ link-list
+ :move-target move-target
+ :rotation-axis (list t)
+ :translation-axis (list t)))
+ (torque (instantiate float-vector (length link-list))))
+ (dotimes (i (length link-list))
+ (setf (elt torque i)
+ (elt all-torque (position (send (elt link-list i) :joint) (send self :joint-list)))))
+ (transform (send self :calc-inverse-jacobian (transpose jacobian))
+ torque)))
(: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
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ky...@us...> - 2012-06-10 07:56:46
|
Revision: 816
http://jskeus.svn.sourceforge.net/jskeus/?rev=816&view=rev
Author: kyouhei
Date: 2012-06-10 07:56:40 +0000 (Sun, 10 Jun 2012)
Log Message:
-----------
add set-stereo-gl-attribute for quad buffer stereo
Modified Paths:
--------------
trunk/irteus/irtgl.l
Modified: trunk/irteus/irtgl.l
===================================================================
--- trunk/irteus/irtgl.l 2012-05-28 12:36:08 UTC (rev 815)
+++ trunk/irteus/irtgl.l 2012-06-10 07:56:40 UTC (rev 816)
@@ -66,6 +66,17 @@
(defconstant GL_INDEX_ARRAY #x8077)
)
+(defun set-stereo-gl-attribute ()
+ (reset-gl-attribute)
+ (let ((iv (make-array (1+ (length gl::*attributelist*)) :element-type :integer)))
+ (sys::vector-replace iv *attributelist*)
+ (setf (elt iv (1- (length *attributelist*))) glx_stereo)
+ (setq *attributelist* iv)))
+(defun reset-gl-attribute ()
+ (setq *attributelist*
+ (integer-vector glx_rgba glx_red_size 1 glx_green_size 1
+ glx_blue_size 1 glx_doublebuffer glx_depth_size 1 0)))
+
(unless (assoc :color-org (send glviewsurface :methods))
(rplaca (assoc :color (send glviewsurface :methods)) :color-org))
(defmethod glviewsurface
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <k-...@us...> - 2012-05-28 12:36:14
|
Revision: 815
http://jskeus.svn.sourceforge.net/jskeus/?rev=815&view=rev
Author: k-okada
Date: 2012-05-28 12:36:08 +0000 (Mon, 28 May 2012)
Log Message:
-----------
fix eus->collada conversion
Modified Paths:
--------------
trunk/irteus/irtcollada.l
Modified: trunk/irteus/irtcollada.l
===================================================================
--- trunk/irteus/irtcollada.l 2012-05-17 05:20:16 UTC (rev 814)
+++ trunk/irteus/irtcollada.l 2012-05-28 12:36:08 UTC (rev 815)
@@ -490,7 +490,7 @@
,@(mapcar #'(lambda (j)
(let ((joint-name (cdr (assoc :name j))))
`(newparam
- (@ (sid ,(format nil "libkinscenes.kinScene_libkinscenes.kinScene_inst_kinmodel.~A_axis0_value" joint-name)))
+ (@ (sid ,(format nil "libkinscenes.kinScene_libkinscenes.kinScene_inst_kinmodel.~A.value" joint-name)))
,(format nil "<SIDREF>kinsystem_motion/kinsystem_motion.kinsystem_inst.inst_~A_value</SIDREF>"
joint-name))))
(eusmodel-joint-description desc))))))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2012-05-17 05:20:23
|
Revision: 814
http://jskeus.svn.sourceforge.net/jskeus/?rev=814&view=rev
Author: snozawa
Date: 2012-05-17 05:20:16 +0000 (Thu, 17 May 2012)
Log Message:
-----------
use :joint-angle method in crank-motion
Modified Paths:
--------------
trunk/irteus/demo/crank-motion.l
Modified: trunk/irteus/demo/crank-motion.l
===================================================================
--- trunk/irteus/demo/crank-motion.l 2012-05-17 04:52:19 UTC (rev 813)
+++ trunk/irteus/demo/crank-motion.l 2012-05-17 05:20:16 UTC (rev 814)
@@ -2,7 +2,7 @@
(defclass sample-crank
:super cascaded-link
- :slots (handles)
+ :slots (handles crank-joint)
)
(defmethod sample-crank
@@ -18,13 +18,17 @@
;; Root link should be associated with "self".
(send self :assoc rl)
(send rl :assoc cl)
- ;; 3. define slots for robot class
+ ;; 3. make all joints
+ ;; Before making joints, you should :assoc all links.
+ (setq crank-joint (instance rotational-joint :init
+ :parent-link rl :child-link cl
+ :name :crank-joint :axis :z
+ :min *-inf* :max *inf*))
+ ;; 4. define slots for robot class
;; links and joint-list for cascaded-link.
(setq links (list rl cl))
- (setq joint-list (list (instance rotational-joint :init
- :parent-link rl :child-link cl
- :name :crank-joint :axis :z)))
- ;; 4. call :init-ending after defining links and joint-list and return "self"
+ (setq joint-list (list crank-joint))
+ ;; 5. call :init-ending after defining links and joint-list and return "self"
(send self :init-ending)
self))
;; Methods to define robot links
@@ -52,6 +56,7 @@
(push ahandle handles)
br)))
(:crank-handle () (car handles))
+ (:crank-joint (&rest args) (forward-message-to crank-joint args))
)
(defun crank-motion
@@ -92,7 +97,7 @@
(rotation-axis (list nil t t))
(fp (apply #'midpoint 0.5 (send-all fix-leg-coords :worldpos))))
(do-until-key
- (send crank :rotate (deg2rad 15) :z)
+ (send crank :crank-joint :joint-angle 15 :relative t)
(let* ((target-coords (append (list (send crank :crank-handle)) fix-leg-coords)))
(send *robot* :fullbody-inverse-kinematics target-coords
:move-target move-target :link-list link-list
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2012-05-17 04:52:26
|
Revision: 813
http://jskeus.svn.sourceforge.net/jskeus/?rev=813&view=rev
Author: snozawa
Date: 2012-05-17 04:52:19 +0000 (Thu, 17 May 2012)
Log Message:
-----------
add comments to model making functions
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-05-17 04:14:14 UTC (rev 812)
+++ trunk/irteus/demo/crank-motion.l 2012-05-17 04:52:19 UTC (rev 813)
@@ -10,17 +10,24 @@
(&rest args)
(send-super* :init args)
(setq handles nil)
+ ;; 1. make links links and assoc all links
(let ((rl (send self :make-root-link))
(cl (send self :make-crank-link)))
(send cl :translate #f(0 0 500) :world)
+ ;; 2. assoc links
+ ;; Root link should be associated with "self".
(send self :assoc rl)
(send rl :assoc cl)
+ ;; 3. define slots for robot class
+ ;; links and joint-list for cascaded-link.
(setq links (list rl cl))
(setq joint-list (list (instance rotational-joint :init
:parent-link rl :child-link cl
:name :crank-joint :axis :z)))
+ ;; 4. call :init-ending after defining links and joint-list and return "self"
(send self :init-ending)
self))
+ ;; Methods to define robot links
(:make-root-link
()
(instance bodyset-link :init (make-cascoords)
Modified: trunk/irteus/demo/sample-robot-model.l
===================================================================
--- trunk/irteus/demo/sample-robot-model.l 2012-05-17 04:14:14 UTC (rev 812)
+++ trunk/irteus/demo/sample-robot-model.l 2012-05-17 04:52:19 UTC (rev 813)
@@ -14,13 +14,15 @@
(crotch-width 75) (foot-depth 200) (foot-width 100) (foot-thickness 25) (foot-offset 50)
(arm-radius 50) (upper-arm-length 275) (lower-arm-length 195) (shoulder-width 150) (hand-length 50))
(send-super* :init args)
- ;; generate links and assoc all links
+ ;; 1. make links links and assoc all links
(let ((aroot-link (send self :make-root-link)))
(setq torso (send self :make-torso-links) head (send self :make-head-links)
rarm (send self :make-arm-links :rarm arm-radius upper-arm-length lower-arm-length shoulder-width hand-length)
larm (send self :make-arm-links :larm arm-radius upper-arm-length lower-arm-length shoulder-width hand-length)
rleg (send self :make-leg-links :rleg leg-radius upper-leg-length lower-leg-length ankle-length crotch-width foot-depth foot-width foot-thickness foot-offset)
lleg (send self :make-leg-links :lleg leg-radius upper-leg-length lower-leg-length ankle-length crotch-width foot-depth foot-width foot-thickness foot-offset))
+ ;; 2. assoc links
+ ;; Root link should be associated with "self".
(send self :assoc aroot-link)
(send aroot-link :assoc (car torso))
(send (cadr torso) :assoc (car head))
@@ -29,7 +31,8 @@
(send aroot-link :assoc (car rleg))
(send aroot-link :assoc (car lleg))
- ;; generate all joints
+ ;; 3. make all joints
+ ;; Before making joints, you should :assoc all links.
(setq jc0 (instance rotational-joint :init :parent-link aroot-link :child-link (car torso) :name :torso-waist-y :axis :z :min -45 :max 45))
(setq jc1 (instance rotational-joint :init :parent-link (car torso) :child-link (cadr torso) :name :torso-waist-p :axis :y))
@@ -66,10 +69,8 @@
(setq jlr4 (instance rotational-joint :init :parent-link (elt rleg 3) :child-link (elt rleg 4) :name :rleg-ankle-p :axis :y))
(setq jlr5 (instance rotational-joint :init :parent-link (elt rleg 4) :child-link (elt rleg 5) :name :rleg-ankle-r :axis :-x))
- ;; define other parameters
- (setq larm-root-link (car larm) rarm-root-link (car rarm)
- lleg-root-link (car lleg) rleg-root-link (car rleg)
- torso-root-link (car torso) head-root-link (car head))
+ ;; 4. define slots for robot class
+ ;; links and joint-list for cascaded-link.
(setq links (append (list aroot-link) torso head larm rarm lleg rleg))
(setq joint-list (list jc0 jc1 jh0 jh1
jal0 jal1 jal2 jal3 jal4 jal5 jal6
@@ -77,10 +78,13 @@
jll0 jll1 jll2 jll3 jll4 jll5
jlr0 jlr1 jlr2 jlr3 jlr4 jlr5
))
+ ;; These are for robot-model.
+ (setq larm-root-link (car larm) rarm-root-link (car rarm)
+ lleg-root-link (car lleg) rleg-root-link (car rleg)
+ torso-root-link (car torso) head-root-link (car head))
(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
+ ;; set mass properties & max torques
(dolist (l (append (list aroot-link) torso))
(send l :weight 100.0))
(dolist (l (append larm rarm lleg rleg head))
@@ -95,9 +99,12 @@
(scale (/ 1.0 (reduce #'+ (mapcar #'(lambda (x) (send x :volume)) valid-bodies)))
(reduce #'v+ (mapcar #'(lambda (x) (scale (send x :volume) (send x :centroid))) valid-bodies)))))
))
+ (send-all (send self :joint-list) :max-joint-torque 0.1) ;; [Nm]
+ ;; 5. call :init-ending after defining links and joint-list and return "self"
(send self :init-ending)
self))
+ ;; Methods to define robot links
(:make-root-link
()
(let ((bc0 (make-cube 100 200 100)))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2012-05-17 04:14:21
|
Revision: 812
http://jskeus.svn.sourceforge.net/jskeus/?rev=812&view=rev
Author: snozawa
Date: 2012-05-17 04:14:14 +0000 (Thu, 17 May 2012)
Log Message:
-----------
just fix indent
Modified Paths:
--------------
trunk/irteus/demo/crank-motion.l
Modified: trunk/irteus/demo/crank-motion.l
===================================================================
--- trunk/irteus/demo/crank-motion.l 2012-05-17 04:13:43 UTC (rev 811)
+++ trunk/irteus/demo/crank-motion.l 2012-05-17 04:14:14 UTC (rev 812)
@@ -65,46 +65,46 @@
(send *robot* :transform (send (apply #'midcoords 0.5 (send *robot* :legs :end-coords)) :transformation (make-coords))))
(send *robot* :update-descendants)
- (let ((crank (instance sample-crank :init)))
- (send crank :locate #f(350 100 0) :world)
- (objects (list crank *robot*))
- (let* ((cog-target-pos
- (if (some #'null (send *robot* :legs))
- (send (car (send *robot* :links)) :worldpos)
- (apply #'midpoint 0.5 (send *robot* :legs :end-coords :worldpos))))
- (fix-leg-coords
- (unless (some #'null (send *robot* :legs))
- (send *robot* :legs :end-coords :copy-worldcoords)))
- ;; append legs' parameters for move-target, link-list, thre, rotation-axis, and target-coords
- ;; all parameter list -> (list larm rleg lleg)
- (move-target (append (list (send *robot* :larm :end-coords))
- (send *robot* :legs :end-coords)))
- (link-list (mapcar #'(lambda (l) (send *robot* :link-list (send l :parent)))
- move-target))
- (thre (list 15 1 1))
- (rotation-axis (list nil t t))
- (fp (apply #'midpoint 0.5 (send-all fix-leg-coords :worldpos))))
- (do-until-key
- (send crank :rotate (deg2rad 15) :z)
- (let* ((target-coords (append (list (send crank :crank-handle)) fix-leg-coords)))
- (send *robot* :fullbody-inverse-kinematics target-coords
- :move-target move-target :link-list link-list
- :rotation-axis rotation-axis :thre thre
- :look-at-target t :centroid-thre 10.0
- :debug-view :no-flush :dump-command nil)
- ;; draw
- (send *irtviewer* :draw-objects :flush nil)
- (mapcar #'(lambda (act ref)
- (send act :draw-on :flush nil :size 100)
- (send ref :draw-on :flush nil :color #f(1 0 0)))
- (append (list (let ((ac (send (car (send *robot* :links)) :get :c-til)))
- (setf (elt ac 2) 0) ac))
- (send-all move-target :worldpos))
- (append (list cog-target-pos) target-coords))
- (send *robot* :draw-torque *viewer*)
- (send *irtviewer* :flush)
- ))
- )))
+ (let ((crank (instance sample-crank :init)))
+ (send crank :locate #f(350 100 0) :world)
+ (objects (list crank *robot*))
+ (let* ((cog-target-pos
+ (if (some #'null (send *robot* :legs))
+ (send (car (send *robot* :links)) :worldpos)
+ (apply #'midpoint 0.5 (send *robot* :legs :end-coords :worldpos))))
+ (fix-leg-coords
+ (unless (some #'null (send *robot* :legs))
+ (send *robot* :legs :end-coords :copy-worldcoords)))
+ ;; append legs' parameters for move-target, link-list, thre, rotation-axis, and target-coords
+ ;; all parameter list -> (list larm rleg lleg)
+ (move-target (append (list (send *robot* :larm :end-coords))
+ (send *robot* :legs :end-coords)))
+ (link-list (mapcar #'(lambda (l) (send *robot* :link-list (send l :parent)))
+ move-target))
+ (thre (list 15 1 1))
+ (rotation-axis (list nil t t))
+ (fp (apply #'midpoint 0.5 (send-all fix-leg-coords :worldpos))))
+ (do-until-key
+ (send crank :rotate (deg2rad 15) :z)
+ (let* ((target-coords (append (list (send crank :crank-handle)) fix-leg-coords)))
+ (send *robot* :fullbody-inverse-kinematics target-coords
+ :move-target move-target :link-list link-list
+ :rotation-axis rotation-axis :thre thre
+ :look-at-target t :centroid-thre 10.0
+ :debug-view :no-flush :dump-command nil)
+ ;; draw
+ (send *irtviewer* :draw-objects :flush nil)
+ (mapcar #'(lambda (act ref)
+ (send act :draw-on :flush nil :size 100)
+ (send ref :draw-on :flush nil :color #f(1 0 0)))
+ (append (list (let ((ac (send (car (send *robot* :links)) :get :c-til)))
+ (setf (elt ac 2) 0) ac))
+ (send-all move-target :worldpos))
+ (append (list cog-target-pos) target-coords))
+ (send *robot* :draw-torque *viewer*)
+ (send *irtviewer* :flush)
+ ))
+ )))
(unless (boundp '*irtviewer*) (make-irtviewer))
(warn "(crank-motion) for fullbody motion~%")
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2012-05-17 04:13:49
|
Revision: 811
http://jskeus.svn.sourceforge.net/jskeus/?rev=811&view=rev
Author: snozawa
Date: 2012-05-17 04:13:43 +0000 (Thu, 17 May 2012)
Log Message:
-----------
define sample-crank class instead of local functoin
Modified Paths:
--------------
trunk/irteus/demo/crank-motion.l
Modified: trunk/irteus/demo/crank-motion.l
===================================================================
--- trunk/irteus/demo/crank-motion.l 2012-05-17 03:38:24 UTC (rev 810)
+++ trunk/irteus/demo/crank-motion.l 2012-05-17 04:13:43 UTC (rev 811)
@@ -1,5 +1,52 @@
(load "sample-robot-model.l")
+(defclass sample-crank
+ :super cascaded-link
+ :slots (handles)
+ )
+
+(defmethod sample-crank
+ (:init
+ (&rest args)
+ (send-super* :init args)
+ (setq handles nil)
+ (let ((rl (send self :make-root-link))
+ (cl (send self :make-crank-link)))
+ (send cl :translate #f(0 0 500) :world)
+ (send self :assoc rl)
+ (send rl :assoc cl)
+ (setq links (list rl cl))
+ (setq joint-list (list (instance rotational-joint :init
+ :parent-link rl :child-link cl
+ :name :crank-joint :axis :z)))
+ (send self :init-ending)
+ self))
+ (:make-root-link
+ ()
+ (instance bodyset-link :init (make-cascoords)
+ :bodies (list (make-cylinder 10 500))
+ :name :crank-root-link))
+ (:make-crank-link
+ ()
+ (let* ((b0 (make-cylinder 10 50))
+ (b1 (make-cylinder 10 70))
+ (b2 (make-cube 30 120 10)))
+ (send b2 :translate (float-vector 0 -50 55))
+ (send b1 :translate (float-vector 0 -100 60))
+ (send b0 :assoc b1)
+ (send b0 :assoc b2)
+ (let* ((br (instance bodyset-link :init (make-cascoords)
+ :bodies (list b0 b1 b2) :name :crank-handle-link))
+ (ahandle
+ (make-cascoords :coords
+ (send (send b1 :copy-worldcoords) :translate (float-vector 0 0 50))
+ :name :crank-handle)))
+ (send br :assoc ahandle)
+ (push ahandle handles)
+ br)))
+ (:crank-handle () (car handles))
+ )
+
(defun crank-motion
()
"crank motion using full body ik"
@@ -18,22 +65,8 @@
(send *robot* :transform (send (apply #'midcoords 0.5 (send *robot* :legs :end-coords)) :transformation (make-coords))))
(send *robot* :update-descendants)
- (labels ((make-crank ()
- (let* ((b0 (make-cylinder 10 50))
- (b1 (make-cylinder 10 70))
- (b2 (make-cube 30 120 10)))
- (send b2 :translate (float-vector 0 -50 55))
- (send b1 :translate (float-vector 0 -100 60))
- (send b0 :assoc b1) (send b0 :assoc b2)
- (let ((br (instance bodyset :init (make-cascoords)
- :bodies (list b0 b1 b2))))
- (send br :put :handle
- (make-cascoords :coords
- (send (send b1 :copy-worldcoords) :translate (float-vector 0 0 50))))
- (send br :assoc (send br :get :handle))
- br))))
- (let ((crank (make-crank)))
- (send crank :locate #f(350 100 500) :world)
+ (let ((crank (instance sample-crank :init)))
+ (send crank :locate #f(350 100 0) :world)
(objects (list crank *robot*))
(let* ((cog-target-pos
(if (some #'null (send *robot* :legs))
@@ -53,7 +86,7 @@
(fp (apply #'midpoint 0.5 (send-all fix-leg-coords :worldpos))))
(do-until-key
(send crank :rotate (deg2rad 15) :z)
- (let* ((target-coords (append (list (send crank :get :handle)) fix-leg-coords)))
+ (let* ((target-coords (append (list (send crank :crank-handle)) fix-leg-coords)))
(send *robot* :fullbody-inverse-kinematics target-coords
:move-target move-target :link-list link-list
:rotation-axis rotation-axis :thre thre
@@ -71,7 +104,7 @@
(send *robot* :draw-torque *viewer*)
(send *irtviewer* :flush)
))
- ))))
+ )))
(unless (boundp '*irtviewer*) (make-irtviewer))
(warn "(crank-motion) for fullbody motion~%")
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2012-05-17 03:38:31
|
Revision: 810
http://jskeus.svn.sourceforge.net/jskeus/?rev=810&view=rev
Author: snozawa
Date: 2012-05-17 03:38:24 +0000 (Thu, 17 May 2012)
Log Message:
-----------
remove labels and fix indent
Modified Paths:
--------------
trunk/irteus/demo/sample-robot-model.l
Modified: trunk/irteus/demo/sample-robot-model.l
===================================================================
--- trunk/irteus/demo/sample-robot-model.l 2012-05-17 03:36:40 UTC (rev 809)
+++ trunk/irteus/demo/sample-robot-model.l 2012-05-17 03:38:24 UTC (rev 810)
@@ -14,92 +14,90 @@
(crotch-width 75) (foot-depth 200) (foot-width 100) (foot-thickness 25) (foot-offset 50)
(arm-radius 50) (upper-arm-length 275) (lower-arm-length 195) (shoulder-width 150) (hand-length 50))
(send-super* :init args)
- ;; define local functions to generate limb links
- (labels ()
- ;; generate links and assoc all links
- (let ((aroot-link (send self :make-root-link)))
- (setq torso (send self :make-torso-links) head (send self :make-head-links)
- rarm (send self :make-arm-links :rarm arm-radius upper-arm-length lower-arm-length shoulder-width hand-length)
- larm (send self :make-arm-links :larm arm-radius upper-arm-length lower-arm-length shoulder-width hand-length)
- rleg (send self :make-leg-links :rleg leg-radius upper-leg-length lower-leg-length ankle-length crotch-width foot-depth foot-width foot-thickness foot-offset)
- lleg (send self :make-leg-links :lleg leg-radius upper-leg-length lower-leg-length ankle-length crotch-width foot-depth foot-width foot-thickness foot-offset))
- (send self :assoc aroot-link)
- (send aroot-link :assoc (car torso))
- (send (cadr torso) :assoc (car head))
- (send (cadr torso) :assoc (car rarm))
- (send (cadr torso) :assoc (car larm))
- (send aroot-link :assoc (car rleg))
- (send aroot-link :assoc (car lleg))
+ ;; generate links and assoc all links
+ (let ((aroot-link (send self :make-root-link)))
+ (setq torso (send self :make-torso-links) head (send self :make-head-links)
+ rarm (send self :make-arm-links :rarm arm-radius upper-arm-length lower-arm-length shoulder-width hand-length)
+ larm (send self :make-arm-links :larm arm-radius upper-arm-length lower-arm-length shoulder-width hand-length)
+ rleg (send self :make-leg-links :rleg leg-radius upper-leg-length lower-leg-length ankle-length crotch-width foot-depth foot-width foot-thickness foot-offset)
+ lleg (send self :make-leg-links :lleg leg-radius upper-leg-length lower-leg-length ankle-length crotch-width foot-depth foot-width foot-thickness foot-offset))
+ (send self :assoc aroot-link)
+ (send aroot-link :assoc (car torso))
+ (send (cadr torso) :assoc (car head))
+ (send (cadr torso) :assoc (car rarm))
+ (send (cadr torso) :assoc (car larm))
+ (send aroot-link :assoc (car rleg))
+ (send aroot-link :assoc (car lleg))
- ;; generate all joints
- (setq jc0 (instance rotational-joint :init :parent-link aroot-link :child-link (car torso) :name :torso-waist-y :axis :z :min -45 :max 45))
- (setq jc1 (instance rotational-joint :init :parent-link (car torso) :child-link (cadr torso) :name :torso-waist-p :axis :y))
+ ;; generate all joints
+ (setq jc0 (instance rotational-joint :init :parent-link aroot-link :child-link (car torso) :name :torso-waist-y :axis :z :min -45 :max 45))
+ (setq jc1 (instance rotational-joint :init :parent-link (car torso) :child-link (cadr torso) :name :torso-waist-p :axis :y))
- (setq jh0 (instance rotational-joint :init :parent-link (cadr torso) :child-link (car head) :name :head-neck-y :axis :z))
- (setq jh1 (instance rotational-joint :init :parent-link (car head) :child-link (cadr head) :name :head-neck-p :axis :y))
+ (setq jh0 (instance rotational-joint :init :parent-link (cadr torso) :child-link (car head) :name :head-neck-y :axis :z))
+ (setq jh1 (instance rotational-joint :init :parent-link (car head) :child-link (cadr head) :name :head-neck-p :axis :y))
- (setq jal0 (instance rotational-joint :init :parent-link (cadr torso) :child-link (elt larm 0) :name :larm-shoulder-p :axis :y))
- (setq jal1 (instance rotational-joint :init :parent-link (elt larm 0) :child-link (elt larm 1) :name :larm-shoulder-r :axis :x :min -30 :max 180))
- (setq jal2 (instance rotational-joint :init :parent-link (elt larm 1) :child-link (elt larm 2) :name :larm-shoulder-y :axis :z))
- (setq jal3 (instance rotational-joint :init :parent-link (elt larm 2) :child-link (elt larm 3) :name :larm-elbow-p :axis :y :min -180 :max 0))
- (setq jal4 (instance rotational-joint :init :parent-link (elt larm 3) :child-link (elt larm 4) :name :larm-wrist-y :axis :z))
- (setq jal5 (instance rotational-joint :init :parent-link (elt larm 4) :child-link (elt larm 5) :name :larm-wrist-r :axis :x))
- (setq jal6 (instance rotational-joint :init :parent-link (elt larm 5) :child-link (elt larm 6) :name :larm-wrist-p :axis :y))
+ (setq jal0 (instance rotational-joint :init :parent-link (cadr torso) :child-link (elt larm 0) :name :larm-shoulder-p :axis :y))
+ (setq jal1 (instance rotational-joint :init :parent-link (elt larm 0) :child-link (elt larm 1) :name :larm-shoulder-r :axis :x :min -30 :max 180))
+ (setq jal2 (instance rotational-joint :init :parent-link (elt larm 1) :child-link (elt larm 2) :name :larm-shoulder-y :axis :z))
+ (setq jal3 (instance rotational-joint :init :parent-link (elt larm 2) :child-link (elt larm 3) :name :larm-elbow-p :axis :y :min -180 :max 0))
+ (setq jal4 (instance rotational-joint :init :parent-link (elt larm 3) :child-link (elt larm 4) :name :larm-wrist-y :axis :z))
+ (setq jal5 (instance rotational-joint :init :parent-link (elt larm 4) :child-link (elt larm 5) :name :larm-wrist-r :axis :x))
+ (setq jal6 (instance rotational-joint :init :parent-link (elt larm 5) :child-link (elt larm 6) :name :larm-wrist-p :axis :y))
- (setq jar0 (instance rotational-joint :init :parent-link (cadr torso) :child-link (elt rarm 0) :name :rarm-shoulder-p :axis :y))
- (setq jar1 (instance rotational-joint :init :parent-link (elt rarm 0) :child-link (elt rarm 1) :name :rarm-shoulder-r :axis :-x :min -30 :max 180))
- (setq jar2 (instance rotational-joint :init :parent-link (elt rarm 1) :child-link (elt rarm 2) :name :rarm-shoulder-y :axis :-z))
- (setq jar3 (instance rotational-joint :init :parent-link (elt rarm 2) :child-link (elt rarm 3) :name :rarm-elbow-p :axis :y :min -180 :max 0))
- (setq jar4 (instance rotational-joint :init :parent-link (elt rarm 3) :child-link (elt rarm 4) :name :rarm-wrist-y :axis :-z))
- (setq jar5 (instance rotational-joint :init :parent-link (elt rarm 4) :child-link (elt rarm 5) :name :rarm-wrist-r :axis :-x))
- (setq jar6 (instance rotational-joint :init :parent-link (elt rarm 5) :child-link (elt rarm 6) :name :rarm-wrist-p :axis :y))
+ (setq jar0 (instance rotational-joint :init :parent-link (cadr torso) :child-link (elt rarm 0) :name :rarm-shoulder-p :axis :y))
+ (setq jar1 (instance rotational-joint :init :parent-link (elt rarm 0) :child-link (elt rarm 1) :name :rarm-shoulder-r :axis :-x :min -30 :max 180))
+ (setq jar2 (instance rotational-joint :init :parent-link (elt rarm 1) :child-link (elt rarm 2) :name :rarm-shoulder-y :axis :-z))
+ (setq jar3 (instance rotational-joint :init :parent-link (elt rarm 2) :child-link (elt rarm 3) :name :rarm-elbow-p :axis :y :min -180 :max 0))
+ (setq jar4 (instance rotational-joint :init :parent-link (elt rarm 3) :child-link (elt rarm 4) :name :rarm-wrist-y :axis :-z))
+ (setq jar5 (instance rotational-joint :init :parent-link (elt rarm 4) :child-link (elt rarm 5) :name :rarm-wrist-r :axis :-x))
+ (setq jar6 (instance rotational-joint :init :parent-link (elt rarm 5) :child-link (elt rarm 6) :name :rarm-wrist-p :axis :y))
- (setq jll0 (instance rotational-joint :init :parent-link aroot-link :child-link (elt lleg 0) :name :lleg-crotch-y :axis :z))
- (setq jll1 (instance rotational-joint :init :parent-link (elt lleg 0) :child-link (elt lleg 1) :name :lleg-crotch-r :axis :x))
- (setq jll2 (instance rotational-joint :init :parent-link (elt lleg 1) :child-link (elt lleg 2) :name :lleg-crotch-p :axis :y))
- (setq jll3 (instance rotational-joint :init :parent-link (elt lleg 2) :child-link (elt lleg 3) :name :lleg-knee-p :axis :y :min 0))
- (setq jll4 (instance rotational-joint :init :parent-link (elt lleg 3) :child-link (elt lleg 4) :name :lleg-ankle-p :axis :y))
- (setq jll5 (instance rotational-joint :init :parent-link (elt lleg 4) :child-link (elt lleg 5) :name :lleg-ankle-r :axis :x))
+ (setq jll0 (instance rotational-joint :init :parent-link aroot-link :child-link (elt lleg 0) :name :lleg-crotch-y :axis :z))
+ (setq jll1 (instance rotational-joint :init :parent-link (elt lleg 0) :child-link (elt lleg 1) :name :lleg-crotch-r :axis :x))
+ (setq jll2 (instance rotational-joint :init :parent-link (elt lleg 1) :child-link (elt lleg 2) :name :lleg-crotch-p :axis :y))
+ (setq jll3 (instance rotational-joint :init :parent-link (elt lleg 2) :child-link (elt lleg 3) :name :lleg-knee-p :axis :y :min 0))
+ (setq jll4 (instance rotational-joint :init :parent-link (elt lleg 3) :child-link (elt lleg 4) :name :lleg-ankle-p :axis :y))
+ (setq jll5 (instance rotational-joint :init :parent-link (elt lleg 4) :child-link (elt lleg 5) :name :lleg-ankle-r :axis :x))
- (setq jlr0 (instance rotational-joint :init :parent-link aroot-link :child-link (elt rleg 0) :name :rleg-crotch-y :axis :-z))
- (setq jlr1 (instance rotational-joint :init :parent-link (elt rleg 0) :child-link (elt rleg 1) :name :rleg-crotch-r :axis :-x))
- (setq jlr2 (instance rotational-joint :init :parent-link (elt rleg 1) :child-link (elt rleg 2) :name :rleg-crotch-p :axis :y))
- (setq jlr3 (instance rotational-joint :init :parent-link (elt rleg 2) :child-link (elt rleg 3) :name :rleg-knee-p :axis :y :min 0))
- (setq jlr4 (instance rotational-joint :init :parent-link (elt rleg 3) :child-link (elt rleg 4) :name :rleg-ankle-p :axis :y))
- (setq jlr5 (instance rotational-joint :init :parent-link (elt rleg 4) :child-link (elt rleg 5) :name :rleg-ankle-r :axis :-x))
+ (setq jlr0 (instance rotational-joint :init :parent-link aroot-link :child-link (elt rleg 0) :name :rleg-crotch-y :axis :-z))
+ (setq jlr1 (instance rotational-joint :init :parent-link (elt rleg 0) :child-link (elt rleg 1) :name :rleg-crotch-r :axis :-x))
+ (setq jlr2 (instance rotational-joint :init :parent-link (elt rleg 1) :child-link (elt rleg 2) :name :rleg-crotch-p :axis :y))
+ (setq jlr3 (instance rotational-joint :init :parent-link (elt rleg 2) :child-link (elt rleg 3) :name :rleg-knee-p :axis :y :min 0))
+ (setq jlr4 (instance rotational-joint :init :parent-link (elt rleg 3) :child-link (elt rleg 4) :name :rleg-ankle-p :axis :y))
+ (setq jlr5 (instance rotational-joint :init :parent-link (elt rleg 4) :child-link (elt rleg 5) :name :rleg-ankle-r :axis :-x))
- ;; define other parameters
- (setq larm-root-link (car larm) rarm-root-link (car rarm)
- lleg-root-link (car lleg) rleg-root-link (car rleg)
- torso-root-link (car torso) head-root-link (car head))
- (setq links (append (list aroot-link) torso head larm rarm lleg rleg))
- (setq joint-list (list jc0 jc1 jh0 jh1
- jal0 jal1 jal2 jal3 jal4 jal5 jal6
- jar0 jar1 jar2 jar3 jar4 jar5 jar6
- jll0 jll1 jll2 jll3 jll4 jll5
- 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]
+ ;; define other parameters
+ (setq larm-root-link (car larm) rarm-root-link (car rarm)
+ lleg-root-link (car lleg) rleg-root-link (car rleg)
+ torso-root-link (car torso) head-root-link (car head))
+ (setq links (append (list aroot-link) torso head larm rarm lleg rleg))
+ (setq joint-list (list jc0 jc1 jh0 jh1
+ jal0 jal1 jal2 jal3 jal4 jal5 jal6
+ jar0 jar1 jar2 jar3 jar4 jar5 jar6
+ jll0 jll1 jll2 jll3 jll4 jll5
+ 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))
- (send l :weight 100.0))
- (dolist (l (append larm rarm lleg rleg head))
- (send l :weight 7.0))
- (dolist (l (list aroot-link (elt torso 1) (elt head 1)))
- (let* ((valid-bodies (remove-if #'(lambda (x)
- (and (> (send x :volume) 0) (< (send x :volume) 0))) ;; nan check
- (send l :bodies))))
- (send l :centroid
- (if (= (length valid-bodies) 1)
- (send (car valid-bodies) :centroid)
- (scale (/ 1.0 (reduce #'+ (mapcar #'(lambda (x) (send x :volume)) valid-bodies)))
- (reduce #'v+ (mapcar #'(lambda (x) (scale (send x :volume) (send x :centroid))) valid-bodies)))))
- ))
+ ;; set mass properties
+ (dolist (l (append (list aroot-link) torso))
+ (send l :weight 100.0))
+ (dolist (l (append larm rarm lleg rleg head))
+ (send l :weight 7.0))
+ (dolist (l (list aroot-link (elt torso 1) (elt head 1)))
+ (let* ((valid-bodies (remove-if #'(lambda (x)
+ (and (> (send x :volume) 0) (< (send x :volume) 0))) ;; nan check
+ (send l :bodies))))
+ (send l :centroid
+ (if (= (length valid-bodies) 1)
+ (send (car valid-bodies) :centroid)
+ (scale (/ 1.0 (reduce #'+ (mapcar #'(lambda (x) (send x :volume)) valid-bodies)))
+ (reduce #'v+ (mapcar #'(lambda (x) (scale (send x :volume) (send x :centroid))) valid-bodies)))))
+ ))
- (send self :init-ending)
- self)))
+ (send self :init-ending)
+ self))
(:make-root-link
()
(let ((bc0 (make-cube 100 200 100)))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2012-05-17 03:36:47
|
Revision: 809
http://jskeus.svn.sourceforge.net/jskeus/?rev=809&view=rev
Author: snozawa
Date: 2012-05-17 03:36:40 +0000 (Thu, 17 May 2012)
Log Message:
-----------
separate limb generating function to class method
Modified Paths:
--------------
trunk/irteus/demo/sample-robot-model.l
Modified: trunk/irteus/demo/sample-robot-model.l
===================================================================
--- trunk/irteus/demo/sample-robot-model.l 2012-05-13 05:39:24 UTC (rev 808)
+++ trunk/irteus/demo/sample-robot-model.l 2012-05-17 03:36:40 UTC (rev 809)
@@ -15,110 +15,14 @@
(arm-radius 50) (upper-arm-length 275) (lower-arm-length 195) (shoulder-width 150) (hand-length 50))
(send-super* :init args)
;; define local functions to generate limb links
- (labels ((make-root-link
- ()
- (let ((bc0 (make-cube 100 200 100)))
- (send bc0 :locate #f(0 0 -75))
- (send bc0 :set-color :green)
- (instance bodyset-link :init (make-cascoords) :bodies (list bc0) :name :waist)))
- (make-torso-links
- ()
- (let ((bc1 (make-default-robot-link 0 50 :y :torso-link0))
- (bc2 (make-cube 100 200 200)))
- (send bc1 :locate #f(0 0 -12.5))
- (send bc2 :locate #f(0 0 100))
- (send bc2 :set-color :green)
- (setq bc2 (instance bodyset-link :init (make-cascoords :pos #f(0 0 -12.5)) :bodies (list bc2) :name :torso-link1))
- (send bc1 :assoc bc2)
- (list bc1 bc2)))
- (make-head-links
- ()
- (let ((bh0 (make-default-robot-link 0 50 :y :head-link0))
- (bh2 (make-cube 120 100 150))
- (bh2e (make-cylinder 10 30))
- (bh1))
- (send bh2 :locate #f(0 0 80))
- (send bh2 :set-color :green)
- (send bh2e :rotate pi/2 :y)
- (send bh2e :locate #f(60 0 70) :world)
- (send bh2e :set-color :green)
- (send bh2 :assoc bh2e)
- (setq bh1 (instance bodyset-link :init (make-cascoords) :bodies (list bh2 bh2e) :name :head-link1))
- (setq head-end-coords (make-cascoords :pos #f(60 0 100) :rpy (float-vector 0 pi/2 0)))
- (send bh1 :assoc head-end-coords)
- (send bh0 :assoc bh1)
- (send bh0 :locate #f(0 0 225))
- (list bh0 bh1)))
- (make-arm-links
- (l/r)
- (let ((ba1 (make-default-robot-link 0 arm-radius :y (read-from-string (format nil "~A-link0" l/r))))
- (ba2 (make-default-robot-link 0 arm-radius :x (read-from-string (format nil "~A-link1" l/r))))
- (ba3 (make-default-robot-link upper-arm-length arm-radius :z (read-from-string (format nil "~A-link2" l/r))))
- (ba4 (make-default-robot-link lower-arm-length arm-radius :y (read-from-string (format nil "~A-link3" l/r))))
- (ba5 (make-default-robot-link 0 arm-radius :z (read-from-string (format nil "~A-link4" l/r))))
- (ba6 (make-default-robot-link 0 arm-radius :x (read-from-string (format nil "~A-link5" l/r))))
- (ba7 (make-default-robot-link hand-length arm-radius :y (read-from-string (format nil "~A-link6" l/r)))))
- (case l/r
- (:rarm
- (setq rarm-end-coords (make-cascoords))
- (send rarm-end-coords :locate (float-vector 0 0 (- hand-length)))
- (send rarm-end-coords :rotate pi/2 :y)
- (send ba7 :assoc rarm-end-coords))
- (:larm
- (setq larm-end-coords (make-cascoords))
- (send larm-end-coords :locate (float-vector 0 0 (- hand-length)))
- (send larm-end-coords :rotate pi/2 :y)
- (send ba7 :assoc larm-end-coords)))
- (send ba6 :assoc ba7)
- (send ba5 :assoc ba6)
- (send ba5 :translate (float-vector 0 0 (- lower-arm-length)) :world)
- (send ba4 :assoc ba5)
- (send ba4 :translate (float-vector 0 0 (- upper-arm-length)) :world)
- (send ba3 :assoc ba4)
- (send ba2 :assoc ba3)
- (send ba1 :assoc ba2)
- (case l/r
- (:rarm (send ba1 :translate (float-vector 0 (- shoulder-width) 175) :world))
- (:larm (send ba1 :translate (float-vector 0 shoulder-width 175) :world)))
- (list ba1 ba2 ba3 ba4 ba5 ba6 ba7)))
- (make-leg-links
- (l/r)
- (let* ((bl1 (make-default-robot-link 0 leg-radius :y (read-from-string (format nil "~A-link0" l/r))))
- (bl2 (make-default-robot-link 0 leg-radius :x (read-from-string (format nil "~A-link1" l/r))))
- (bl3 (make-default-robot-link (- upper-leg-length (/ leg-radius 2.0)) leg-radius :z (read-from-string (format nil "~A-link2" l/r))))
- (bl4 (make-default-robot-link (- lower-leg-length (/ leg-radius 2.0)) leg-radius :y (read-from-string (format nil "~A-link3" l/r))))
- (bl5 (make-default-robot-link 0 leg-radius :x (read-from-string (format nil "~A-link4" l/r))))
- (bl6b (make-cube foot-depth foot-width foot-thickness))
- (bl6))
- (send bl6b :locate (float-vector foot-offset 0 (- ankle-length)))
- (send bl6b :set-color :green)
- (setq bl6 (make-default-robot-link ankle-length leg-radius :y (read-from-string (format nil "~A-link5" l/r)) (list bl6b)))
- (case l/r
- (:rleg
- (setq rleg-end-coords (make-cascoords))
- (send rleg-end-coords :locate (float-vector 0 0 (- (+ ankle-length (/ foot-thickness 2.0)))))
- (send bl6 :assoc rleg-end-coords))
- (:lleg
- (setq lleg-end-coords (make-cascoords))
- (send lleg-end-coords :locate (float-vector 0 0 (- (+ ankle-length (/ foot-thickness 2.0)))))
- (send bl6 :assoc lleg-end-coords)))
- (send bl5 :assoc bl6)
- (send bl5 :translate (float-vector 0 0 (- lower-leg-length)) :world)
- (send bl4 :assoc bl5)
- (send bl4 :translate (float-vector 0 0 (- upper-leg-length)) :world)
- (send bl3 :assoc bl4)
- (send bl2 :assoc bl3)
- (send bl1 :assoc bl2)
- (case l/r
- (:rleg (send bl1 :translate (float-vector 0 (- crotch-width) -150) :world))
- (:lleg (send bl1 :translate (float-vector 0 crotch-width -150) :world)))
- (list bl1 bl2 bl3 bl4 bl5 bl6)))
- )
+ (labels ()
;; generate links and assoc all links
- (let ((aroot-link (make-root-link)))
- (setq torso (make-torso-links) head (make-head-links)
- rarm (make-arm-links :rarm) larm (make-arm-links :larm)
- rleg (make-leg-links :rleg) lleg (make-leg-links :lleg))
+ (let ((aroot-link (send self :make-root-link)))
+ (setq torso (send self :make-torso-links) head (send self :make-head-links)
+ rarm (send self :make-arm-links :rarm arm-radius upper-arm-length lower-arm-length shoulder-width hand-length)
+ larm (send self :make-arm-links :larm arm-radius upper-arm-length lower-arm-length shoulder-width hand-length)
+ rleg (send self :make-leg-links :rleg leg-radius upper-leg-length lower-leg-length ankle-length crotch-width foot-depth foot-width foot-thickness foot-offset)
+ lleg (send self :make-leg-links :lleg leg-radius upper-leg-length lower-leg-length ankle-length crotch-width foot-depth foot-width foot-thickness foot-offset))
(send self :assoc aroot-link)
(send aroot-link :assoc (car torso))
(send (cadr torso) :assoc (car head))
@@ -196,6 +100,104 @@
(send self :init-ending)
self)))
+ (:make-root-link
+ ()
+ (let ((bc0 (make-cube 100 200 100)))
+ (send bc0 :locate #f(0 0 -75))
+ (send bc0 :set-color :green)
+ (instance bodyset-link :init (make-cascoords) :bodies (list bc0) :name :waist)))
+ (:make-torso-links
+ ()
+ (let ((bc1 (make-default-robot-link 0 50 :y :torso-link0))
+ (bc2 (make-cube 100 200 200)))
+ (send bc1 :locate #f(0 0 -12.5))
+ (send bc2 :locate #f(0 0 100))
+ (send bc2 :set-color :green)
+ (setq bc2 (instance bodyset-link :init (make-cascoords :pos #f(0 0 -12.5)) :bodies (list bc2) :name :torso-link1))
+ (send bc1 :assoc bc2)
+ (list bc1 bc2)))
+ (:make-head-links
+ ()
+ (let ((bh0 (make-default-robot-link 0 50 :y :head-link0))
+ (bh2 (make-cube 120 100 150))
+ (bh2e (make-cylinder 10 30))
+ (bh1))
+ (send bh2 :locate #f(0 0 80))
+ (send bh2 :set-color :green)
+ (send bh2e :rotate pi/2 :y)
+ (send bh2e :locate #f(60 0 70) :world)
+ (send bh2e :set-color :green)
+ (send bh2 :assoc bh2e)
+ (setq bh1 (instance bodyset-link :init (make-cascoords) :bodies (list bh2 bh2e) :name :head-link1))
+ (setq head-end-coords (make-cascoords :pos #f(60 0 100) :rpy (float-vector 0 pi/2 0)))
+ (send bh1 :assoc head-end-coords)
+ (send bh0 :assoc bh1)
+ (send bh0 :locate #f(0 0 225))
+ (list bh0 bh1)))
+ (:make-arm-links
+ (l/r arm-radius upper-arm-length lower-arm-length shoulder-width hand-length)
+ (let ((ba1 (make-default-robot-link 0 arm-radius :y (read-from-string (format nil "~A-link0" l/r))))
+ (ba2 (make-default-robot-link 0 arm-radius :x (read-from-string (format nil "~A-link1" l/r))))
+ (ba3 (make-default-robot-link upper-arm-length arm-radius :z (read-from-string (format nil "~A-link2" l/r))))
+ (ba4 (make-default-robot-link lower-arm-length arm-radius :y (read-from-string (format nil "~A-link3" l/r))))
+ (ba5 (make-default-robot-link 0 arm-radius :z (read-from-string (format nil "~A-link4" l/r))))
+ (ba6 (make-default-robot-link 0 arm-radius :x (read-from-string (format nil "~A-link5" l/r))))
+ (ba7 (make-default-robot-link hand-length arm-radius :y (read-from-string (format nil "~A-link6" l/r)))))
+ (case l/r
+ (:rarm
+ (setq rarm-end-coords (make-cascoords))
+ (send rarm-end-coords :locate (float-vector 0 0 (- hand-length)))
+ (send rarm-end-coords :rotate pi/2 :y)
+ (send ba7 :assoc rarm-end-coords))
+ (:larm
+ (setq larm-end-coords (make-cascoords))
+ (send larm-end-coords :locate (float-vector 0 0 (- hand-length)))
+ (send larm-end-coords :rotate pi/2 :y)
+ (send ba7 :assoc larm-end-coords)))
+ (send ba6 :assoc ba7)
+ (send ba5 :assoc ba6)
+ (send ba5 :translate (float-vector 0 0 (- lower-arm-length)) :world)
+ (send ba4 :assoc ba5)
+ (send ba4 :translate (float-vector 0 0 (- upper-arm-length)) :world)
+ (send ba3 :assoc ba4)
+ (send ba2 :assoc ba3)
+ (send ba1 :assoc ba2)
+ (case l/r
+ (:rarm (send ba1 :translate (float-vector 0 (- shoulder-width) 175) :world))
+ (:larm (send ba1 :translate (float-vector 0 shoulder-width 175) :world)))
+ (list ba1 ba2 ba3 ba4 ba5 ba6 ba7)))
+ (:make-leg-links
+ (l/r leg-radius upper-leg-length lower-leg-length ankle-length crotch-width foot-depth foot-width foot-thickness foot-offset)
+ (let* ((bl1 (make-default-robot-link 0 leg-radius :y (read-from-string (format nil "~A-link0" l/r))))
+ (bl2 (make-default-robot-link 0 leg-radius :x (read-from-string (format nil "~A-link1" l/r))))
+ (bl3 (make-default-robot-link (- upper-leg-length (/ leg-radius 2.0)) leg-radius :z (read-from-string (format nil "~A-link2" l/r))))
+ (bl4 (make-default-robot-link (- lower-leg-length (/ leg-radius 2.0)) leg-radius :y (read-from-string (format nil "~A-link3" l/r))))
+ (bl5 (make-default-robot-link 0 leg-radius :x (read-from-string (format nil "~A-link4" l/r))))
+ (bl6b (make-cube foot-depth foot-width foot-thickness))
+ (bl6))
+ (send bl6b :locate (float-vector foot-offset 0 (- ankle-length)))
+ (send bl6b :set-color :green)
+ (setq bl6 (make-default-robot-link ankle-length leg-radius :y (read-from-string (format nil "~A-link5" l/r)) (list bl6b)))
+ (case l/r
+ (:rleg
+ (setq rleg-end-coords (make-cascoords))
+ (send rleg-end-coords :locate (float-vector 0 0 (- (+ ankle-length (/ foot-thickness 2.0)))))
+ (send bl6 :assoc rleg-end-coords))
+ (:lleg
+ (setq lleg-end-coords (make-cascoords))
+ (send lleg-end-coords :locate (float-vector 0 0 (- (+ ankle-length (/ foot-thickness 2.0)))))
+ (send bl6 :assoc lleg-end-coords)))
+ (send bl5 :assoc bl6)
+ (send bl5 :translate (float-vector 0 0 (- lower-leg-length)) :world)
+ (send bl4 :assoc bl5)
+ (send bl4 :translate (float-vector 0 0 (- upper-leg-length)) :world)
+ (send bl3 :assoc bl4)
+ (send bl2 :assoc bl3)
+ (send bl1 :assoc bl2)
+ (case l/r
+ (:rleg (send bl1 :translate (float-vector 0 (- crotch-width) -150) :world))
+ (:lleg (send bl1 :translate (float-vector 0 crotch-width -150) :world)))
+ (list bl1 bl2 bl3 bl4 bl5 bl6)))
(:reset-pose ()
(send self :angle-vector #f(0.0 0.0 0.0 0.0 10.0 20.0 0.0 -20.0 10.0 0.0 0.0 10.0 20.0 0.0 -20.0 10.0 0.0 0.0 0.0 0.0 -15.0 30.0 -15.0 0.0 0.0 0.0 -15.0 30.0 -15.0 0.0)))
)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2012-05-13 05:39:31
|
Revision: 808
http://jskeus.svn.sourceforge.net/jskeus/?rev=808&view=rev
Author: snozawa
Date: 2012-05-13 05:39:24 +0000 (Sun, 13 May 2012)
Log Message:
-----------
add ;; to warning message in :joint-angle method
Modified Paths:
--------------
trunk/irteus/irtmodel.l
Modified: trunk/irteus/irtmodel.l
===================================================================
--- trunk/irteus/irtmodel.l 2012-05-11 10:10:06 UTC (rev 807)
+++ trunk/irteus/irtmodel.l 2012-05-13 05:39:24 UTC (rev 808)
@@ -159,13 +159,13 @@
(setq v (mod v 360))
(if (> v 180.0) (setq v (- v 360.0))))
((> v max-angle)
- (unless relative (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)
- (unless relative (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)
@@ -212,10 +212,10 @@
(when v
(if relative (setq v (+ v joint-angle)))
(when (> v max-angle)
- (unless relative (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)
- (unless relative (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: <ky...@us...> - 2012-05-11 10:10:17
|
Revision: 807
http://jskeus.svn.sourceforge.net/jskeus/?rev=807&view=rev
Author: kyouhei
Date: 2012-05-11 10:10:06 +0000 (Fri, 11 May 2012)
Log Message:
-----------
fix typo
Modified Paths:
--------------
trunk/irteus/irtpointcloud.l
Modified: trunk/irteus/irtpointcloud.l
===================================================================
--- trunk/irteus/irtpointcloud.l 2012-05-09 12:11:37 UTC (rev 806)
+++ trunk/irteus/irtpointcloud.l 2012-05-11 10:10:06 UTC (rev 807)
@@ -243,7 +243,7 @@
(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 n (c-matrix-row narray i n))
(if (and (or (null key) (funcall key p))
(or (null ckey) (or (null c) (funcall ckey c)))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sn...@us...> - 2012-05-09 12:11:46
|
Revision: 806
http://jskeus.svn.sourceforge.net/jskeus/?rev=806&view=rev
Author: snozawa
Date: 2012-05-09 12:11:37 +0000 (Wed, 09 May 2012)
Log Message:
-----------
use :arrow-scale argument in :draw-circle to configure scaling of tip of arrow
Modified Paths:
--------------
trunk/irteus/irtviewer.l
Modified: trunk/irteus/irtviewer.l
===================================================================
--- trunk/irteus/irtviewer.l 2012-05-06 07:19:32 UTC (rev 805)
+++ trunk/irteus/irtviewer.l 2012-05-09 12:11:37 UTC (rev 806)
@@ -31,7 +31,7 @@
c))
(defmethod geo::viewer
- (:draw-circle (c &key (radius 50) (flush nil) (arrow nil) (arc 2pi))
+ (:draw-circle (c &key (radius 50) (flush nil) (arrow nil) (arc 2pi) (arrow-scale #f(1 1)))
(let* ((s 16) (sr (/ arc s)) p0 p1)
(dotimes (i s)
(setq p1
@@ -44,7 +44,7 @@
(float-vector (* radius (sin (* s sr)))
(* radius (cos (* s sr))) 0)))
(if arrow
- (send self :draw-arrow p0 p1)
+ (send self :draw-arrow p0 p1 t nil :arrow-scale arrow-scale)
(send self :draw-line p0 p1))
(if flush (send self :flush))
))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
