ros-o support by mqcmd196 · Pull Request #794 · jsk-ros-pkg/jsk_control

mqcmd196 · 2025-06-05T12:01:53Z

d344b4c Ensure the return value is propagated correctly
2b6bb0d [ci] enable ros-o test
007a981 [jsk_footstep_planner] fix pcl related compilation issues in noble
efe1ae3 eusqp: add CATKIN_ENABLE_TESTING for add_rostest
7889cb8 jsk_footstep_planner/CMakeLists.txt: add CATKIN_ENABLE_TESTING for add_rostest
08df911 [eus_teleop] skip some keys when ROS_DISTRO==one. ref: https://github.com/tork-a/jsk_control-release/blob/debian/noetic/eus_teleop/package.xml
31f05a7 qpoasis: use patch --forward for fakeroot debian/rules
3682aff eus_qpoases/CMakeLists.txt: add CATKIN_ENABLE_TESTING for add_rostest

mqcmd196 · 2025-06-05T12:03:20Z

jammy-one-x64 was tested here:

https://github.com/mqcmd196/ros-builder-action/actions/runs/15460873107

mqcmd196 · 2025-07-30T03:18:29Z

Segfault in

❯ roslaunch eus_qp test_contact_wrench_opt.test

❯ rosrun eus_qp test_contact_wrench_opt.l
configuring by "/opt/ros/one/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 gnuplotlib ___time ___pgsql irtgeo euspqp pqp irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada irtstl irtwrl irtpointcloud eusbullet bullet irtcollision irtx eusjpeg euspng png irtimage irtglrgb
;; extending gcstack 0x58f21769db40[16374] --> 0x58f217b2a680[32748] top=3d4b
irtgl irtglc irtviewer
EusLisp 9.29( 1.2.5) for Linux64 created on fv-az1759-403(Sat May 31 20:45:06 UTC 2025)
roseus ;; loading roseus("1.7.5-117-gf131801") on euslisp((9.29 fv-az1759-403 Sat May 31 20:45:06 UTC 2025  1.2.5))
eustf roseus_c_util   [0] target="/opt/ros/one/share/euslisp/jskeus/eus//Linux64/lib/libeus_qp.so"
  [1] target="/home/obinata/ros/review_ws/devel/lib/libeus_qp.so"
;; (setup)
;;   (demo-robot-contact-wrench-distribution-1) ;; Demo for robot contact wrench distribution by pseudo-inverse.
;;   (demo-robot-contact-wrench-distribution-2) ;; Demo for robot contact wrench distribution by QP.
;;   (demo-robot-contact-wrench-distribution-3) ;; Demo for robot contact wrench and joint torque distribution by QP.
;;   (demo-robot-contact-wrench-distribution-4) ;; Demo for robot contact wrench and joint torque distribution by QP. Infeasible solution.
;;   (demo-robot-contact-wrench-distribution-5) ;; Demo for robot contact wrench and joint torque distribution by QP. Originally infeasible solution, but obtain solution by allowing min inequality boundary violation.
;;   (demo-robot-contact-wrench-distribution-6) ;; Demo for robot contact wrench distribution with no-contact constraint. Only the second example successes.
;;   (demo-robot-contact-wrench-distribution-7) ;; Demo for robot contact wrench distribution with mu/cop-margin-ratio.
;;   (demo-robot-contact-wrench-distribution-8) ;; Demo for robot contact wrench distribution with hand gripper contacts.
;; (demo-robot-contact-wrench-distribution-all)
;; (demo-robot-contact-wrench-distribution-all :force-use-equality-error-weight t)
;; (test-compare-wrench-torque-calc)
;; (demo-robot-wrench-distribute-from-total-wrench-and-given-wrench)
;; cbox demo functions
;;   (demo-cbox-wrench-calc-1) ;; Demo for cbox wrench calculation by default-contact-constraint. cbox is neutral pos rot.
;;   (demo-cbox-wrench-calc-2) ;; Demo for cbox wrench calculation by norm-contact-constraint. cbox is neutral pos rot.
;;   (demo-cbox-wrench-calc-3) ;; Demo for cbox wrench calculation by 2D-translational-friction-contact-constraint. cbox is neutral pos rot.
;;   (demo-cbox-wrench-calc-4) ;; Demo for cbox wrench calculation by rotational-friction-contact-constraint. cbox is neutral pos rot.
;;   (demo-cbox-wrench-calc-5) ;; Demo for cbox wrench calculation by 2D-cop-contact-constraint. cbox is neutral pos rot.
;;   (demo-cbox-wrench-calc-6) ;; Demo for cbox wrench calculation by default-contact-constraint. cbox is tilting.
;;   (demo-cbox-wrench-calc-7) ;; Demo for cbox wrench calculation by norm-contact-constraint. cbox is tilting.
;;   (demo-cbox-wrench-calc-8) ;; Demo for cbox wrench calculation by 2D-translational-friction-contact-constraint. cbox is tilting.
;;   (demo-cbox-wrench-calc-9) ;; Demo for cbox wrench calculation by rotational-friction-contact-constraint. cbox is tilting.
;;   (demo-cbox-wrench-calc-10) ;; Demo for cbox wrench calculation by 2D-cop-contact-constraint. cbox is tilting.
;;   (demo-cbox-wrench-calc-11) ;; Demo for cbox wrench calculation by default-contact-constraint. cbox is tilting. Infeasible solution.
;;   (demo-cbox-wrench-calc-12) ;; Demo for cbox wrench calculation by default-contact-constraint. cbox is tilting. Originally infeasible solution, but obtain solution by allowing min inequality boundary violation.
;;   (demo-cbox-wrench-calc-13) ;; Demo for cbox wrench calculation by min-max-contact-constraint. cbox is tilting.
;;   (demo-cbox-wrench-calc-14) ;; Demo for cbox wrench calculation by 2D-translational-sliding-contact-constraint. cbox is neutral pos rot. This sample is infeasible but it's correct.
;;   (demo-cbox-wrench-calc-15) ;; Demo for cbox wrench calculation by 2D-translational-sliding-contact-constraint. cbox is neutral pos rot. This sample is originally infeasible, but obtain solution by allowing min inequality boundary violation.
;;   (demo-cbox-wrench-calc-16) ;; Demo for cbox wrench calculation by default-contact-constraint. cbox is neutral pos rot. cbox is sliding in the -y direction.
;;   (demo-cbox-wrench-calc-17) ;; Demo for cbox wrench calculation by 6d-min-max-contact-constraint. cbox is neutral pos rot.
;;   (demo-cbox-wrench-calc-18) ;; Demo for cbox wrench calculation by polygon-cop-contact-constraint. cbox is neutral pos rot.
;;   (demo-cbox-wrench-calc-19) ;; Demo for cbox wrench calculation by symmetric-friction-polycone-contact-constraint. cbox is neutral pos rot.
;;   (demo-cbox-wrench-calc-20) ;; Demo for cbox wrench calculation by rotational-sliding-contact-constraint. cbox is neutral pos rot.
;;   (demo-cbox-wrench-calc-21) ;; Demo for cbox wrench calculation by default-contact-constraint. cbox is neutral pos rot. cbox is sliding in the -y direction.
;;   (demo-cbox-wrench-calc-all) ;; for all tests
;; (test-cop-polygon-constraint)
;; (test-friction-polycone-constraint)
TEST-NAME: test-samplerobot-distribute
  now testing...
start testing [test-samplerobot-distribute]
;; demo-robot-contact-wrench-distribution-1
;; demo-robot-contact-wrench-distribution-2
zsh: illegal hardware instruction (core dumped)  rosrun eus_qp test_contact_wrench_opt.l

Editing like

diff --git a/eus_qp/euslisp/contact-optimization.l b/eus_qp/euslisp/contact-optimization.l
index d166e56..6a6da85 100644
--- a/eus_qp/euslisp/contact-optimization.l
+++ b/eus_qp/euslisp/contact-optimization.l
@@ -269,12 +269,14 @@
          (state-min-vector)
          (use-min-inequality-violation-p (or min-inequality-violation-weight min-inequality-violation-weight-vector))
          (use-equality-error-p (or equality-error-weight equality-error-weight-vector)))
+    (print "000")
     (when use-min-inequality-violation-p
       (let* ((ineq-dim (length inequality-min-vector))
              (violation-state-weight-matrix
               (if min-inequality-violation-weight-vector
                   (diagonal min-inequality-violation-weight-vector)
-                (scale-matrix min-inequality-violation-weight (unit-matrix ineq-dim)))))
+                  (scale-matrix min-inequality-violation-weight (unit-matrix ineq-dim)))))
+        (print "001")
         (setq equality-matrix (concatenate-matrix-row equality-matrix (make-matrix (length equality-vector) ineq-dim))
               inequality-matrix (concatenate-matrix-row inequality-matrix (unit-matrix ineq-dim))
               eval-weight-matrix (concatenate-matrix-diagonal eval-weight-matrix violation-state-weight-matrix)
@@ -284,9 +286,11 @@
               state-min-vector (concatenate float-vector
                                             (fill (instantiate float-vector state-dim) -1e35)
                                             (instantiate float-vector ineq-dim)))))
+    (print "002")
     (setq eval-coeff-vector (array-entity eval-coeff-vector)) ;; array->float-vector
     ;; Use equality error.
     ;;   For ex, Ax = b => (Ax - b)^T W (Ax - b) in evaluation function
+    (print "003")
     (when use-equality-error-p
       (let* ((equality-error-weight-matrix
               (if equality-error-weight-vector
@@ -300,6 +304,7 @@
                                  (scale -1 (transform G^TW equality-vector))
                                  eval-coeff-vector
                                  ))))
+    (print "004")
     (let* ((ret (apply
                  qp-solver
                  :initial-state initial-state
@@ -317,11 +322,14 @@
                     (list :equality-matrix equality-matrix
                           :equality-vector equality-vector)))
                  )))
+      (print "005")
       (when (and use-equality-error-p ret (> (distance (transform equality-matrix ret) equality-vector) 1e-4)) ;; [N], [Nm]
         (when debug (warn ";; too large equality error in wrench torque opt ~A~%" (distance (transform equality-matrix ret) equality-vector)))
+        (print "006")
         (setq ret nil))
       (if ret
           (let ((ret-wrench (if use-min-inequality-violation-p (subseq ret 0 state-dim) ret)))
+            (print "007")
             (list :wrench-list (send robot :wrench-vector->wrench-list ret-wrench)
                   :torque-vector (v- inertial-torque (transform jacobi^T ret-wrench)))))
       )))
diff --git a/eus_qp/euslisp/test-contact-wrench-opt.l b/eus_qp/euslisp/test-contact-wrench-opt.l
index b369c51..54c9023 100644
--- a/eus_qp/euslisp/test-contact-wrench-opt.l
+++ b/eus_qp/euslisp/test-contact-wrench-opt.l
@@ -117,24 +117,31 @@
         (contact-constraint-matrix-list)
         (min-inequality-violation-weight)
         (equality-error-weight)
-        (func #'wrench-torque-distribute-from-total-wrench-inertial-torque))
+     (func #'wrench-torque-distribute-from-total-wrench-inertial-torque))
+  (print "1")
   (unless contact-constraint-matrix-list
     (setq contact-constraint-matrix-list
           (mapcar #'(lambda (c-constraint c-coords)
                       (send c-constraint :calc-constraint-matrix c-coords))
                   (send *robot* :get :contact-constraint-list) contact-coords-list)))
+  (print "2")
   (send *robot* :calc-torque :calc-statics-p t)
+  (print "3")
   (let* ((link-list
           (mapcar #'(lambda (x) (send *robot* :link-list (send x :parent)))
                   contact-coords-list))
          (union-link-list (send *robot* :calc-union-link-list link-list))
-         (ret (funcall func
-                       contact-coords-list contact-constraint-matrix-list
-                       :robot *robot*
-                       :contact-constraint-vector-list (send-all (send *robot* :get :contact-constraint-list) :get-constraint-vector)
-                       :min-inequality-violation-weight min-inequality-violation-weight
-                       :equality-error-weight equality-error-weight
-                       )))
+         )
+    (print "4")
+    (setq ret (wrench-torque-distribute-from-total-wrench-inertial-torque                            
+               contact-coords-list contact-constraint-matrix-list
+               :robot *robot*
+               :contact-constraint-vector-list (send-all (send *robot* :get :contact-constraint-list) :get-constraint-vector)
+               :min-inequality-violation-weight min-inequality-violation-weight
+               :equality-error-weight equality-error-weight
+               ))
+
+    (print "99")
     (draw-force-value ret :union-link-list union-link-list :limbs limbs)
     (send *irtviewer* :viewer :viewsurface :flush)
     ret))
diff --git a/eus_qp/test/test_contact_wrench_opt.l b/eus_qp/test/test_contact_wrench_opt.l
index 23fe823..1fbb2f7 100755
--- a/eus_qp/test/test_contact_wrench_opt.l
+++ b/eus_qp/test/test_contact_wrench_opt.l
@@ -6,17 +6,23 @@
 (require "package://eus_qp/euslisp/test-contact-wrench-opt.l")
 (setup)
 
-(deftest test-samplerobot-distribute
-  (assert (demo-robot-contact-wrench-distribution-all :press-enter-p nil)))
-(deftest test-samplerobot-distribute-use-equality-error-weight
-  (assert (demo-robot-contact-wrench-distribution-all :press-enter-p nil :force-use-equality-error-weight t)))
-(deftest test-samplerobot-distribute-with-given-wrench
-  (assert (demo-robot-wrench-distribute-from-total-wrench-and-given-wrench)))
-(deftest test-cbox-distribute
-  (assert (demo-cbox-wrench-calc-all :press-enter-p nil)))
-(deftest test-cop-polygon-friction-polycone-constraint
-  (assert
-   (and (test-cop-polygon-constraint) (test-friction-polycone-constraint))))
+(print "Done setup!!")
+
+;; (deftest test-samplerobot-distribute
+;;     (assert (demo-robot-contact-wrench-distribution-all :press-enter-p nil)))
+;; (demo-robot-contact-wrench-distribution-all :press-enter-p nil)
+(demo-robot-contact-wrench-distribution-common :func #'test-wrench-torque-distribute-by-qp :press-enter-p nil))
+
+
+;; (deftest test-samplerobot-distribute-use-equality-error-weight
+;;   (assert (demo-robot-contact-wrench-distribution-all :press-enter-p nil :force-use-equality-error-weight t)))
+;; (deftest test-samplerobot-distribute-with-given-wrench
+;;   (assert (demo-robot-wrench-distribute-from-total-wrench-and-given-wrench)))
+;; (deftest test-cbox-distribute
+;;   (assert (demo-cbox-wrench-calc-all :press-enter-p nil)))
+;; (deftest test-cop-polygon-friction-polycone-constraint
+;;   (assert
+;;    (and (test-cop-polygon-constraint) (test-friction-polycone-constraint))))
 
 (run-all-tests)
 (exit 0)

prints

"Done setup!!"
"1"
"2"
"3"
"4"
"000"
"002"
"003"
"004"

it seems that it failes at

(let* ((ret (apply
                 qp-solver
                 :initial-state initial-state
                 :eval-weight-matrix eval-weight-matrix
                 :eval-coeff-vector eval-coeff-vector
                 :inequality-matrix (if additional-inequality-matrix (concatenate-matrix-column inequality-matrix additional-inequality-matrix) inequality-matrix)
                 :inequality-min-vector (if additional-inequality-min-vector (concatenate float-vector inequality-min-vector additional-inequality-min-vector) inequality-min-vector)
                 :check-status check-status
                 :solve-qp-mode solve-qp-mode
                 :debug debug
                 (append
                  (if use-min-inequality-violation-p
                      (list :state-min-vector state-min-vector))
                  (unless use-equality-error-p
                    (list :equality-matrix equality-matrix
                          :equality-vector equality-vector)))
                 )))

CMake Error at /opt/ros/one/share/catkin/cmake/test/tests.cmake:18 (message): () is not available when tests are not enabled. The CMake code should only use it inside a conditional block which checks that testing is enabled: if(CATKIN_ENABLE_TESTING) (...) endif() Call Stack (most recent call first): CMakeLists.txt:39 (add_rostest) -- Configuring incomplete, errors occurred!

cd /<<BUILDDIR>>package/.obj-x86_64-linux-gnu/qpoases-prefix/src/qpoases && patch -s -p0 < /<<BUILDDIR>>package/patch/UseShareLibrary.patch Reversed (or previously applied) patch detected! Assume -R? [n] Apply anyway? [n] 1 out of 1 hunk ignored -- saving rejects to file CMakeLists.txt.rej make[4]: *** [CMakeFiles/qpoases.dir/build.make:122: qpoases-prefix/src/qpoases-stamp/qpoases-patch] Error 1 make[4]: Leaving directory '/<<BUILDDIR>>package/.obj-x86_64-linux-gnu' make[3]: *** [CMakeFiles/Makefile2:171: CMakeFiles/qpoases.dir/all] Error 2 make[3]: Leaving directory '/<<BUILDDIR>>package/.obj-x86_64-linux-gnu' make[2]: *** [Makefile:139: all] Error 2 make[2]: Leaving directory '/<<BUILDDIR>>package/.obj-x86_64-linux-gnu' dh_auto_install: error: cd .obj-x86_64-linux-gnu && make -j4 install DESTDIR=/<<BUILDDIR>>package/debian/ros-one-eus-qpoases AM_UPDATE_INFO_DIR=no "INSTALL=install --strip-program=true" returned exit code 2 make[1]: *** [debian/rules:66: override_dh_auto_install] Error 25 make[1]: Leaving directory '/<<BUILDDIR>>package' make: *** [debian/rules:27: binary] Error 2 dpkg-buildpackage: error: fakeroot debian/rules binary subprocess returned exit status 2

….com/tork-a/jsk_control-release/blob/debian/noetic/eus_teleop/package.xml

…d_rostest -- jsk_footstep_planner: 0 messages, 4 services CMake Error at /opt/ros/one/share/catkin/cmake/test/tests.cmake:18 (message): () is not available when tests are not enabled. The CMake code should only use it inside a conditional block which checks that testing is enabled: if(CATKIN_ENABLE_TESTING) (...) endif() Call Stack (most recent call first): CMakeLists.txt:109 (add_rostest) -- Configuring incomplete, errors occurred!

CMake Error at /opt/ros/one/share/catkin/cmake/test/tests.cmake:18 (message): () is not available when tests are not enabled. The CMake code should only use it inside a conditional block which checks that testing is enabled: if(CATKIN_ENABLE_TESTING) (...) endif() Call Stack (most recent call first): CMakeLists.txt:23 (add_rostest) -- Configuring incomplete, errors occurred!

fixes ``` [eus_qpoases:make] /home/k-okada/catkin_ws/ws_jsk_control/src/jsk_control/eus_qpoases/src/eus_qpoases.cpp:294:3: warning: no return statement in function returning non-void [-Wreturn-type] [eus_qpoases:make] 294 | }; [eus_qpoases:make] | ^ [eus_qpoases:make] /home/k-okada/catkin_ws/ ```

k-okada · 2025-09-24T03:50:22Z

FYI. d344b4c

Ensure the return value is propagated correctly
fixes

[eus_qpoases:make] /home/k-okada/catkin_ws/ws_jsk_control/src/jsk_control/eus_qpoases/src/eus_qpoases.cpp:294:3: warning: no return statement in function returning non-void [-Wreturn-type]
[eus_qpoases:make]   294 |   };
[eus_qpoases:make]       |   ^
[eus_qpoases:make] /home/k-okada/catkin_ws/

mqcmd196 · 2025-09-24T08:23:57Z

Release ->

Update jsk_control repository URL ubi-agni/ros-builder-action#89

mqcmd196 force-pushed the ros-o branch 2 times, most recently from a16a979 to 0b5c8cf Compare June 5, 2025 12:17

mqcmd196 force-pushed the ros-o branch from 0b5c8cf to cf08051 Compare July 28, 2025 03:39

mqcmd196 and others added 8 commits September 24, 2025 12:40

[eus_teleop] skip some keys when ROS_DISTRO==one. ref: https://github…

08df911

….com/tork-a/jsk_control-release/blob/debian/noetic/eus_teleop/package.xml

[jsk_footstep_planner] fix pcl related compilation issues in noble

007a981

[ci] enable ros-o test

2b6bb0d

k-okada force-pushed the ros-o branch from 01fc15b to d344b4c Compare September 24, 2025 03:49

k-okada merged commit 41a0e3c into jsk-ros-pkg:master Sep 24, 2025
8 checks passed

mqcmd196 deleted the ros-o branch September 24, 2025 07:14

mqcmd196 mentioned this pull request Sep 24, 2025

Update jsk_control repository URL ubi-agni/ros-builder-action#89

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

ros-o support#794

ros-o support#794
k-okada merged 8 commits intojsk-ros-pkg:masterfrom
mqcmd196:ros-o

mqcmd196 commented Jun 5, 2025 •

edited by k-okada

Loading

Uh oh!

mqcmd196 commented Jun 5, 2025

Uh oh!

mqcmd196 commented Jul 30, 2025

Uh oh!

k-okada commented Sep 24, 2025

Uh oh!

Uh oh!

mqcmd196 commented Sep 24, 2025

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

Conversation

mqcmd196 commented Jun 5, 2025 • edited by k-okada Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

mqcmd196 commented Jun 5, 2025

Uh oh!

mqcmd196 commented Jul 30, 2025

Uh oh!

k-okada commented Sep 24, 2025

Uh oh!

Uh oh!

mqcmd196 commented Sep 24, 2025

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

mqcmd196 commented Jun 5, 2025 •

edited by k-okada

Loading