Merge branch 'master' into png_transparent

.github/workflows/config.yml

@@ -23,14 +23,14 @@ jobs:
           - DOCKER_IMAGE: debian:stretch
           - DOCKER_IMAGE: osrf/ubuntu_armhf:trusty
           - DOCKER_IMAGE: osrf/ubuntu_armhf:xenial
-          - DOCKER_IMAGE: osrf/ubuntu_arm64:trusty
+          # - DOCKER_IMAGE: osrf/ubuntu_arm64:trusty  # deprecated
           - DOCKER_IMAGE: osrf/ubuntu_arm64:xenial
           - DOCKER_IMAGE: arm64v8/ubuntu:bionic
           - DOCKER_IMAGE: arm64v8/ubuntu:focal
           - DOCKER_IMAGE: osrf/debian_arm64:stretch
       fail-fast: false
 
-    runs-on: ubuntu-18.04
+    runs-on: ubuntu-latest
     timeout-minutes: 90
 
     name: linux
@@ -48,29 +48,42 @@ jobs:
           export TRAVIS_OS_NAME=linux
           export DOCKER_IMAGE=${{matrix.DOCKER_IMAGE}}
           if [[ "$DOCKER_IMAGE" == *"arm"* ]]; then sudo apt-get install -y -qq qemu-user-static git; fi
-          if [[ "$DOCKER_IMAGE" == *"arm64v8"* ]]; then export QEMU_VOLUME="-v /usr/bin/qemu-aarch64-static:/usr/bin/qemu-aarch64-static"; fi #
+          if [[ "$DOCKER_IMAGE" == *"arm64v8"* ]]; then export QEMU_VOLUME="-v /usr/bin/qemu-aarch64-static:/usr/bin/qemu-aarch64-static"; export PLATFORM_OPTION="--platform linux/aarch64"; fi #
           echo "Testing branch $TRAVIS_BRANCH of $REPOSITORY_NAME"
-          docker run --rm $QEMU_VOLUME -v $CI_SOURCE_PATH:$CI_SOURCE_PATH -e "DOCKER_IMAGE=$DOCKER_IMAGE" -e "COLLISION_LIB=$COLLISION_LIB" -e "CI_SOURCE_PATH=$CI_SOURCE_PATH" -e "HOME=$HOME" -t $DOCKER_IMAGE sh -c "cd $CI_SOURCE_PATH; ./.travis.sh"
+          docker run $PLATFORM_OPTION --rm $QEMU_VOLUME -v $CI_SOURCE_PATH:$CI_SOURCE_PATH -e "DOCKER_IMAGE=$DOCKER_IMAGE" -e "COLLISION_LIB=$COLLISION_LIB" -e "CI_SOURCE_PATH=$CI_SOURCE_PATH" -e "HOME=$HOME" -t $DOCKER_IMAGE sh -c "cd $CI_SOURCE_PATH; ./.travis.sh"
 
   catkin:
-    runs-on: ubuntu-18.04
+    runs-on: ubuntu-latest
+    container:
+      image: ubuntu:18.04
+      volumes:
+        - /tmp/node20:/__e/node20
     timeout-minutes: 60
 
     steps:
+      - name: Try to replace `node` with an glibc 2.17
+        shell: bash
+        run: |
+          ls -lar /__e/node20 &&
+          apt update && apt-get install -y curl &&
+          curl -Lo /tmp/node.tar.gz https://unofficial-builds.nodejs.org/download/release/v20.17.0/node-v20.17.0-linux-x64-glibc-217.tar.gz &&
+          cd /__e/node20 &&
+          tar -x --strip-components=1 -f /tmp/node.tar.gz &&
+          ls -lar /__e/node20/bin/
       - name: Checkout
         uses: actions/checkout@v2
       - name: Setup Apt
         run: |
-          echo 'debconf debconf/frontend select Noninteractive' | sudo debconf-set-selections
-          sudo apt-get update -qq
-          sudo apt-get install -qq -y lsb-release wget gnupg2  # necessary for catkin-pkg to be installable
-          sudo apt-get install -qq -y dpkg  # necessary for catkin-pkg to be installable
+          echo 'debconf debconf/frontend select Noninteractive' | debconf-set-selections
+          apt-get update -qq
+          apt-get install -qq -y lsb-release wget gnupg2  # necessary for catkin-pkg to be installable
+          apt-get install -qq -y dpkg  # necessary for catkin-pkg to be installable
           echo "Testing branch $GITHUB_REF of $GITHUB_REPOSITORY"
-          sudo sh -c 'echo "deb http://packages.ros.org/ros-shadow-fixed/ubuntu `lsb_release -cs` main" > /etc/apt/sources.list.d/ros-latest.list'
-          wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
-          sudo apt-get update -qq
-          sudo apt-get install -qq -y python-catkin-tools python-rosdep
-          sudo apt-get install -qq -y build-essential git ros-melodic-rosbash ros-melodic-rospack
+          sh -c 'echo "deb http://packages.ros.org/ros-shadow-fixed/ubuntu `lsb_release -cs` main" > /etc/apt/sources.list.d/ros-latest.list'
+          wget http://packages.ros.org/ros.key -O - | apt-key add -
+          apt-get update -qq
+          apt-get install -qq -y python-catkin-tools python-rosdep
+          apt-get install -qq -y build-essential git ros-melodic-rosbash ros-melodic-rospack
       - name: Setup Src
         shell: bash
         run: |
@@ -89,7 +102,7 @@ jobs:
           wget https://raw.githubusercontent.com/tork-a/jskeus-release/master/patches/CMakeLists.txt -O jskeus/CMakeLists.txt
           # rosdep install
           cd ..
-          sudo rosdep init
+          rosdep init
           rosdep update
           rosdep install -y -r --rosdistro melodic --from-paths src --ignore-src
       - name: Build
@@ -140,7 +153,7 @@ jobs:
 
 
   osx:
-    runs-on: macos-latest
+    runs-on: macos-13  # https://github.com/actions/runner-images/blob/main/images/macos/macos-13-Readme.md
     timeout-minutes: 60
     steps:
       - name: Checkout

.travis.sh

@@ -21,6 +21,11 @@ function travis_time_end {
 }
 
 travis_time_start setup.apt-get_update
+# Use archive repository for Debian Stretch
+if [[ "$DOCKER_IMAGE" == *"stretch" ]]; then
+    sed -i 's/[[:alpha:]]*.debian.org/archive.debian.org/' /etc/apt/sources.list
+    sed -i '/stretch-updates/ s/^#*/#/' /etc/apt/sources.list
+fi
 if [ "$(which sudo)" == "" ]; then apt-get update && apt-get install -y sudo; else sudo apt-get update; fi
 travis_time_end
 
@@ -89,20 +94,27 @@ for test_l in irteus/test/*.l; do
     irteusgl $test_l;
     export TMP_EXIT_STATUS=$?
 
-    travis_time_end `expr 32 - $TMP_EXIT_STATUS`
+    export CONTINUE=0
+    if [[ "$DOCKER_IMAGE" == *"arm64v8"* && $test_l =~ (all-robots-objects.l|coords.l|geo.l|graph.l|interpolator.l|irteus-demo.l|joint.l|mathtest.l|matrix.l|read-img.l|rendering.l|robot-model-usage.l|test-cad.l|test-collada.l|test-collision.l|test-irt-motion.l|test-pointcloud.l|test-triangulation.l|transparent.l) ]]; then export CONTINUE=1; fi ## source
+    if [[ $CONTINUE != 0 ]]; then export TMP_EXIT_STATUS=0; fi
 
     export EXIT_STATUS=`expr $TMP_EXIT_STATUS + $EXIT_STATUS`;
 
+    travis_time_end `expr 32 - $TMP_EXIT_STATUS`
 
     travis_time_start jskeus.compiled.${test_l##*/}.test
 
     irteusgl "(let ((o (namestring (merge-pathnames \".o\" \"$test_l\"))) (so (namestring (merge-pathnames \".so\" \"$test_l\")))) (compile-file \"$test_l\" :o o) (if (probe-file so) (load so) (exit 1))))"
     export TMP_EXIT_STATUS=$?
 
-    travis_time_end `expr 32 - $TMP_EXIT_STATUS`
+    export CONTINUE=0
+    if [[ "$DOCKER_IMAGE" == *"arm64v8"* && $test_l =~ (all-robots-objects.l|interpolator.l|irteus-demo.l|joint.l|mathtest.l|matrix.l|read-img.l|robot-model-usage.l|test-cad.l|test-collada.l|test-collision.l|test-file.l|test-irt-motion.l|test-pointcloud.l|test-triangulation.l) ]]; then export CONTINUE=1; fi ## compiled
+    if [[ $CONTINUE != 0 ]]; then export TMP_EXIT_STATUS=0; fi
 
     export EXIT_STATUS=`expr $TMP_EXIT_STATUS + $EXIT_STATUS`;
 
+    travis_time_end `expr 32 - $TMP_EXIT_STATUS`
+
 done;
 echo "Exit status : $EXIT_STATUS";
 

Makefile

@@ -14,6 +14,12 @@ ifneq ($(GCC_MACHINE),)
  ifeq ($(GCC_MACHINE),i686-linux-gnu)
    MACHINE=x86
  endif
+ ifeq ($(GCC_MACHINE),aarch64-linux-gnu)
+   MACHINE=aarch64
+ endif
+ ifeq ($(GCC_MACHINE),arm-linux-gnueabihf)
+   MACHINE=arm
+ endif
 endif
 ifeq ($(MACHINE),)
  MACHINE=$(shell uname -m)
@@ -26,7 +32,7 @@ ifeq ($(OS),Linux)
   export ARCHDIR=Linux64
   export MAKEFILE=Makefile.Linux64
  else
- ifneq (, $(findstring armv,$(MACHINE)))
+ ifneq (, $(findstring arm,$(MACHINE)))
   export ARCHDIR=LinuxARM
   export MAKEFILE=Makefile.LinuxARM
  else ifneq (, $(findstring aarch,$(MACHINE)))

doc/irtmodel.tex

@@ -803,9 +803,9 @@ \subsubsection{関節角度限界回避を含む逆運動学}
   \left\{
    \begin{array}{l l}
    1 + \left|\frac{\partial \bm{H}(\bm{\theta})}{\partial \theta_i}\right|
-    & if \left|\frac{\partial \bm{H}(\bm{\theta})}{\partial
+    & if \;\Delta\left|\frac{\partial \bm{H}(\bm{\theta})}{\partial
           \theta_i}\right| \geq 0 \\
-    1 & if \left|\frac{\partial \bm{H}(\bm{\theta})}{\partial
+    1 & if \;\Delta\left|\frac{\partial \bm{H}(\bm{\theta})}{\partial
           \theta_i}\right| < 0
     \end{array}
   \right.

irteus/Makefile.Linux

@@ -72,7 +72,7 @@ CXXFLAGS=$(CFLAGS)
 CFLAGS+= -g -falign-functions=4
 CXXFLAGS+=-g -falign-functions=4
 
-ifeq ($(shell /bin/uname -m), x86_64)
+ifeq ($(shell gcc -dumpmachine), x86_64-linux-gnu)
 CC += -m32
 CXX += -m32
 LD += -m32

irteus/Makefile.LinuxARM

@@ -63,7 +63,8 @@ FFTW=-L/usr/local/lib -lfftw -lrfftw
 
 SVNVERSION=\"$(shell git rev-parse --short HEAD)\"
 
-MACHINE=$(shell uname -m | sed 's/\(armv[0-9]\).*/\1/')
+# gcc -dumpmachine retruns target triplet consists of three fields separated by a hyphen (-).
+MACHINE=$(shell gcc -dumpmachine | sed 's/\(.*\)-.*-.*/\1/')
 THREAD= -DTHREADED -DPTHREAD
 
 CFLAGS=-O2 -D$(MACHINE) -D$(ARCH) -DLinux -DARM -D_REENTRANT -DGCC -I$(EUSDIR)/include $(THREAD) -DSVNVERSION=$(SVNVERSION) -fPIC

irteus/PQP/Makefile.Linux

@@ -10,7 +10,7 @@ LPFX = lib
 LIBS = -L$(ARCHDIR) -lRAPID 
 
 
-ifeq ($(shell /bin/uname -m), x86_64)
+ifeq ($(shell gcc -dumpmachine), x86_64-linux-gnu)
 CC += -m32 
 LD += -m32
 EXELD += -m32 

irteus/irtmath.l

@@ -523,6 +523,7 @@
 ;; lmeds
 ;;http://www-pse.cheme.kyoto-u.ac.jp/~kano/document/text-PCA.pdf
 (defun lms (point-list)
+  "returns the result of line/plane/hyperplane fitting (regression) on point-list with least squares. The result consists of the normal vector and the signed distance to the origin"
   (let ((v^ (vector-mean point-list))
         (point-length (length point-list))
 	delx x v eigen-res eigen-val eigen-vec min-lam min-vec)
@@ -543,10 +544,12 @@
     ))
 
 (defun lms-estimate (res point-)
+  "returns the signed distance from the fitted line/plane/hyperplane to point-"
   (+ (v. point- (car res)) (cadr res))
   )
 
 (defun lms-error (result point-list)
+  "returns the mean of the square of the distance from the fitted line/plane/hyperplane to each point in point-list"
   (let ((ret-err 0) tmp-err)
     (dolist (l point-list)
       (setq tmp-err (lms-estimate result l))
@@ -555,13 +558,14 @@
     (/ ret-err (length point-list))
     ))
 
-;; choose num points randomly and apply lms to find the souliton with smallest errors
-;; to use ransac ransac
+;; choose num points randomly and apply lms to find the solution with the smallest error
+;; to use ransac
 ;;   :lmeds-error-func -> set to ransac-error
 ;;   :ransac-threshold err^2 (square of the distance from the plane)
 (defun lmeds (point-list &key (num 5) (err-rate 0.3) (iteration) (ransac-threshold)
 			 (lms-func #'lms) (lmeds-error-func #'lmeds-error)
 			 (lms-estimate-func #'lms-estimate))
+  "returns the result of line/plane/hyperplane fitting (regression) on point-list with LMedS. The result consists of the normal vector and the signed distance to the origin"
   (let (point-num r result result-list error-list iter
 		  comb-index comb-index-list point-list-tmp)
     ;; initialize variables
@@ -599,6 +603,7 @@
     ))
 
 (defun lmeds-error (result point-list &key (lms-estimate-func #'lms-estimate))
+  "returns the median of the square of the distance from the fitted line/plane/hyperplane to each point in point-list"
   (let (tmp-err err)
     (dolist (l point-list)
       (setq tmp-err (funcall lms-estimate-func result l))
@@ -608,6 +613,7 @@
     ))
 
 (defun lmeds-error-mat (result mat &key (lms-estimate-func #'lms-estimate))
+  "matrixed version of lmeds-error. mat is the matrixed version of point-list"
   (let ((size (array-dimension mat 0))
         (p (float-vector 0 0 0))
         tmp-err  err)

irteus/irtrobot.l

@@ -594,7 +594,7 @@
                                       (find-if #'(lambda (l) (equal fs (car (send self l :force-sensors)))) '(:rleg :lleg)))
                                   (send self :force-sensors)))
             '(:rleg :lleg)))
-         (force-sensors (mapcar #'(lambda (l) (send self :force-sensor l)) limbs))
+         (force-sensors (mapcar #'(lambda (l) (car (send self l :force-sensors))) limbs))
          (cop-coords (mapcar #'(lambda (l) (send self l :end-coords)) limbs))
          (fz-thre 1e-3) ;; [N]
          (limb-cop-fz-list

irteus/test/test-irt-motion.l

@@ -40,6 +40,57 @@
                  (every #'identity zmp-cog-p)))
            '(:rleg :lleg :both))))
 
+;; calc-zmp / calc-zmp-from-forces-moments
+(defun test-zmp-from-forces-moments (robot)
+  (let (zmp-p1 zmp-p2)
+    (labels ((calc-zmp-from-forces-moments
+	      ()
+	      (let* ((torque-vector (send robot :torque-vector))
+		     (rleg-force-moment (v- (send robot :calc-force-from-joint-torque :rleg torque-vector)))
+		     (lleg-force-moment (v- (send robot :calc-force-from-joint-torque :lleg torque-vector))))
+		(send robot :calc-zmp-from-forces-moments
+		      (list (subseq rleg-force-moment 0 3)
+			    (subseq lleg-force-moment 0 3))
+		      (list (subseq rleg-force-moment 3)
+			    (subseq lleg-force-moment 3))
+		      ))))
+    (objects (list robot))
+    (send robot :newcoords (make-coords))
+    (send robot :reset-pose)
+    (send robot :arms :shoulder-p :joint-angle -45)
+    (send robot :arms :elbow-p :joint-angle -45)
+    (send robot :fix-leg-to-coords (make-coords) '(:rleg :lleg))
+    (send *irtviewer* :look-all)
+    ;; calc-zmp
+    (do ((mid -0.0 (+ mid 0.05)))
+	((>= mid 1.1))
+	;; calc-zmp
+	(send robot :move-centroid-on-foot :both '(:rleg :lleg) :mid mid)
+	(setq zmp-p1 (send robot :calc-zmp))
+	;; calc-zmp-from-forces-moments
+	(setq zmp-p2 (calc-zmp-from-forces-moments))
+	;; draw centroid/zmp
+	(warning-message 2 "centroid:~A zmp:~A / ~A~%" (send robot :centroid) zmp-p1 zmp-p2)
+	(send *irtviewer* :draw-objects :flush nil)
+	(send (send robot :centroid) :draw-on :flush nil :size 80 :color #f(0 0 1) :width 4)
+	(send (transform #2f((1 0 0)(0 1 0)(0 0 0)) (send robot :centroid)) :draw-on :flush nil :size 80 :color #f(0 0 1) :width 4)
+	(send zmp-p1 :draw-on :flush nil :size 150 :color #f(1 0 0) :width 2)
+	(send zmp-p2 :draw-on :flush t :size 150 :color #f(1 1 0) :width 2)
+	)
+
+    (warning-message 3 "check centroid is not equal to zmp~%")
+    (warning-message 3 "centroid:~A zmp:~A / ~A~%" (send robot :centroid) zmp-p1 zmp-p2)
+    (assert (not (eps-v= zmp-p1
+			 (transform #2f((1 0 0)(0 1 0)(0 0 0)) (send robot :centroid)))))
+    (send robot :calc-zmp) (calc-zmp-from-forces-moments)
+    (send robot :calc-zmp) (setq zmp-p2 (calc-zmp-from-forces-moments))
+    (setq zmp-p1 (send robot :get :zmp))
+    (warning-message 3 "check centroid is not equal to zmp after calling :calc-zmp~%")
+    (warning-message 3 "centroid:~A zmp:~A / ~A~%" (send robot :centroid) zmp-p1 zmp-p2)
+    (assert (eps-v= zmp-p1
+		    (transform #2f((1 0 0)(0 1 0)(0 0 0)) (send robot :centroid))))
+    t)))
+
 (defun init-pose-torque-tests
   (robot)
   (mapcar #'(lambda (jnt)
@@ -909,6 +960,7 @@
 (deftest test-irtdyna-samplerobot
   (warning-message 2 ";; run test-zmp-comomn~%")
   (assert (test-zmp-comomn *sample-robot*))
+  (assert (test-zmp-from-forces-moments *sample-robot*))
   ;; Not sure why, but it fails only COLLISION_LIB=PQP with source compile
   (unless (derivedp #'test-torque-from-its-own-weight-common compiled-code)
     (warning-message 2 ";; run test-torque-from-its-own-weight-common~%")