obstacle-ik-test-01.l

(load "package://eus_qp/optmotiongen/euslisp/inverse-kinematics-wrapper.l")
(load "package://eus_qp/optmotiongen/euslisp/discrete-kinematics-configuration-task.l")
(load "package://eus_qp/optmotiongen/euslisp/sqp-msc-optimization.l")
(require "irteus/demo/sample-arm-model.l")
(require "package://pr2eus/pr2.l")
(require "models/arrow-object.l")

(setq irteus-ik? nil)

(setq *robot* (instance pr2-robot :init))
(setq *arrow* (instance arrow-object :init))
(send *arrow* :newcoords
      (make-coords :pos (float-vector  550 0 500) :rpy (list (deg2rad 60) 0 0)))
(setq *obstacle* (make-cube 250 250 250))
(send *obstacle* :newcoords
      (make-coords :pos (float-vector 500 -200 600)))
(send *obstacle* :set-color (float-vector 1 0 0) 0.5)
(objects (list *robot* *arrow* *obstacle*))


(warn "please run (normal-ik) or (obstacle-ik)~%")

(defun normal-ik ()
  (send *robot* :reset-pose)
  (send *robot* :inverse-kinematics
	(send *arrow* :copy-worldcoords)
	:link-list (send *robot* :link-list (send (send *robot* :rarm :end-coords) :parent))
	:move-target (send *robot* :rarm :end-coords)
	:rotation-axis nil
	:debug-view t
	)
  )
(defun obstacle-ik ()
  (send *robot* :reset-pose)
  (send *robot*
	:inverse-kinematics-optmotiongen
	(send *arrow* :copy-worldcoords)
	:link-list (send *robot* :link-list (send (send *robot* :rarm :end-coords) :parent))
	:move-target (send *robot* :rarm :end-coords)
	:translation-axis t
	:rotation-axis nil
	:debug-view t
	:obstacles (list *obstacle*)
	)
  )

obstacle-ik-test-02-log

$ wget https://gist.githubusercontent.com/Kanazawanaoaki/1810958f83c44c037b9cb286a047b385/raw/94178ce637dd6921d62948aecd39662ce0f9543b/obstacle-ik-test-02.l 
$ roseus obstacle-ik-test-02.l
configuring by "/opt/ros/melodic/share/euslisp/jskeus/eus//lib/eusrt.l"
;; readmacro ;; object ;; packsym ;; common ;; constants ;; stream ;; string ;; loader ;; pprint ;; process ;; hashtab ;; array ;; mathtran ;; eusdebug ;; eusforeign ;; extnum ;; coordinates ;; tty ;; history ;; toplevel ;; trans ;; comp ;; builtins ;; par ;; intersection ;; geoclasses ;; geopack ;; geobody ;; primt ;; compose ;; polygon ;; viewing ;; viewport ;; viewsurface ;; hid ;; shadow ;; bodyrel ;; dda ;; helpsub ;; eushelp ;; xforeign ;; Xdecl ;; Xgraphics ;; Xcolor ;; Xeus ;; Xevent ;; Xpanel ;; Xitem ;; Xtext ;; Xmenu ;; Xscroll ;; Xcanvas ;; Xtop ;; Xapplwin 
connected to Xserver DISPLAY=:0
X events are being asynchronously monitored.
;; pixword ;; RGBHLS ;; convolve ;; piximage ;; pbmfile ;; image_correlation ;; oglforeign ;; gldecl ;; glconst ;; glforeign ;; gluconst ;; gluforeign ;; glxconst ;; glxforeign ;; eglforeign ;; eglfunc ;; glutil ;; gltexture ;; glprim ;; gleus ;; glview ;; toiv-undefined ;; fstringdouble irtmath irtutil irtc irtgeoc irtgraph ___time ___pgsql irtgeo euspqp pqp irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada irtpointcloud irtx eusjpeg euspng png irtimage irtglrgb 
;; extending gcstack 0x558d136c0690[16374] --> 0x558d13b4f240[32748] top=3d66
irtgl irtglc irtviewer 
EusLisp 9.27( 1.2.1) for Linux64 created on ip-172-30-1-167(Wed Mar 4 01:33:18 UTC 2020)
roseus ;; loading roseus("1.7.4") on euslisp((9.27 ip-172-30-1-167 Wed Mar 4 01:33:18 UTC 2020  1.2.1))
eustf roseus_c_util ;; (make-irtviewer) executed
please run (normal-ik) or (obstacle-ik)
1.irteusgl$ (obstacle-ik)
;; #<rotational-joint #X558d154fbb78 r_wrist_flex_joint> :joint-angle(-5.72958) violate max-angle(-5.72958)
;; #<rotational-joint #X558d154fbb78 r_wrist_flex_joint> :joint-angle(-5.72958) violate max-angle(-5.72958)
;; #<linear-joint #X558d1576f640 torso_lift_joint> :joint-angle(11.5) violate min-angle(11.5)
;; #<rotational-joint #X558d154fbb78 r_wrist_flex_joint> :joint-angle(-5.72958) violate max-angle(-5.72958)
;; #<rotational-joint #X558d154fbb78 r_wrist_flex_joint> :joint-angle(-5.72958) violate max-angle(-5.72958)
;; #<rotational-joint #X558d154fbb78 r_wrist_flex_joint> :joint-angle(-5.72958) violate max-angle(-5.72958)
;; #<rotational-joint #X558d154fbb78 r_wrist_flex_joint> :joint-angle(-5.72958) violate max-angle(-5.72958)
;; #<rotational-joint #X558d154fbb78 r_wrist_flex_joint> :joint-angle(-5.72958) violate max-angle(-5.72958)
;; #<rotational-joint #X558d154fbb78 r_wrist_flex_joint> :joint-angle(-5.72958) violate max-angle(-5.72958)
=== Optimization converged with 9 iteration ! ===
=== iteration 9/50 ===
|task|=4.434679e-07
|kin-task|=4.434679e-07:
  |kin-task-i|=4.434679e-07
|posture-task|=0.0
please run (normal-ik) or (obstacle-ik)
nil
2.irteusgl$ 

obstacle-ik-test-02.l

(load "package://eus_qp/optmotiongen/euslisp/inverse-kinematics-wrapper.l")
(load "package://eus_qp/optmotiongen/euslisp/discrete-kinematics-configuration-task.l")
(load "package://eus_qp/optmotiongen/euslisp/sqp-msc-optimization.l")
(require "irteus/demo/sample-arm-model.l")
(require "package://pr2eus/pr2.l")
(require "models/arrow-object.l")

(setq irteus-ik? nil)

(setq *robot* (instance pr2-robot :init))
(setq *arrow* (instance arrow-object :init))
(send *arrow* :newcoords
      (make-coords :pos (float-vector  550 50 600) :rpy (list (deg2rad 60) 0 0)))
(setq *obstacle* (make-cube 250 250 250))
(send *obstacle* :newcoords
      (make-coords :pos (float-vector 500 -200 700)))
(send *obstacle* :set-color (float-vector 1 0 0) 0.5)
(objects (list *robot* *arrow* *obstacle*))


(warn "please run (normal-ik) or (obstacle-ik)~%")

(defun normal-ik ()
  (send *robot* :reset-pose)
  (send *robot* :inverse-kinematics
	(send *arrow* :copy-worldcoords)
	:link-list (send *robot* :link-list (send (send *robot* :rarm :end-coords) :parent))
	:move-target (send *robot* :rarm :end-coords)
	:rotation-axis nil
	:debug-view t
	)
  (warn "please run (normal-ik) or (obstacle-ik)~%")
  )
(defun obstacle-ik ()
  (send *robot* :reset-pose)
  (send *robot*
	:inverse-kinematics-optmotiongen
	(send *arrow* :copy-worldcoords)
	:link-list (send *robot* :link-list (send (send *robot* :rarm :end-coords) :parent))
	:move-target (send *robot* :rarm :end-coords)
	:translation-axis t
	:rotation-axis nil
	:debug-view t
	:collision-avoidance-link-pair (mapcar #'(lambda (l) (list l *obstacle*)) (send *robot* :links))
	)
  (warn "please run (normal-ik) or (obstacle-ik)~%")
  )