Skip to content

Commit 7f0b733

Browse files
k-okadak-okada
authored
Merge branch 'master' into png_transparent
2 parents d1afb80 + 8f6bcba commit 7f0b733

File tree

12 files changed

+117
-27
lines changed

12 files changed

+117
-27
lines changed

‎.github/workflows/config.yml

Lines changed: 29 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -23,14 +23,14 @@ jobs:
2323
- DOCKER_IMAGE: debian:stretch
2424
- DOCKER_IMAGE: osrf/ubuntu_armhf:trusty
2525
- DOCKER_IMAGE: osrf/ubuntu_armhf:xenial
26-
- DOCKER_IMAGE: osrf/ubuntu_arm64:trusty
26+
# - DOCKER_IMAGE: osrf/ubuntu_arm64:trusty # deprecated
2727
- DOCKER_IMAGE: osrf/ubuntu_arm64:xenial
2828
- DOCKER_IMAGE: arm64v8/ubuntu:bionic
2929
- DOCKER_IMAGE: arm64v8/ubuntu:focal
3030
- DOCKER_IMAGE: osrf/debian_arm64:stretch
3131
fail-fast: false
3232

33-
runs-on: ubuntu-18.04
33+
runs-on: ubuntu-latest
3434
timeout-minutes: 90
3535

3636
name: linux
@@ -48,29 +48,42 @@ jobs:
4848
export TRAVIS_OS_NAME=linux
4949
export DOCKER_IMAGE=${{matrix.DOCKER_IMAGE}}
5050
if [[ "$DOCKER_IMAGE" == *"arm"* ]]; then sudo apt-get install -y -qq qemu-user-static git; fi
51-
if [[ "$DOCKER_IMAGE" == *"arm64v8"* ]]; then export QEMU_VOLUME="-v /usr/bin/qemu-aarch64-static:/usr/bin/qemu-aarch64-static"; fi #
51+
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 #
5252
echo "Testing branch $TRAVIS_BRANCH of $REPOSITORY_NAME"
53-
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"
53+
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"
5454
5555
catkin:
56-
runs-on: ubuntu-18.04
56+
runs-on: ubuntu-latest
57+
container:
58+
image: ubuntu:18.04
59+
volumes:
60+
- /tmp/node20:/__e/node20
5761
timeout-minutes: 60
5862

5963
steps:
64+
- name: Try to replace `node` with an glibc 2.17
65+
shell: bash
66+
run: |
67+
ls -lar /__e/node20 &&
68+
apt update && apt-get install -y curl &&
69+
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 &&
70+
cd /__e/node20 &&
71+
tar -x --strip-components=1 -f /tmp/node.tar.gz &&
72+
ls -lar /__e/node20/bin/
6073
- name: Checkout
6174
uses: actions/checkout@v2
6275
- name: Setup Apt
6376
run: |
64-
echo 'debconf debconf/frontend select Noninteractive' | sudo debconf-set-selections
65-
sudo apt-get update -qq
66-
sudo apt-get install -qq -y lsb-release wget gnupg2 # necessary for catkin-pkg to be installable
67-
sudo apt-get install -qq -y dpkg # necessary for catkin-pkg to be installable
77+
echo 'debconf debconf/frontend select Noninteractive' | debconf-set-selections
78+
apt-get update -qq
79+
apt-get install -qq -y lsb-release wget gnupg2 # necessary for catkin-pkg to be installable
80+
apt-get install -qq -y dpkg # necessary for catkin-pkg to be installable
6881
echo "Testing branch $GITHUB_REF of $GITHUB_REPOSITORY"
69-
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'
70-
wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
71-
sudo apt-get update -qq
72-
sudo apt-get install -qq -y python-catkin-tools python-rosdep
73-
sudo apt-get install -qq -y build-essential git ros-melodic-rosbash ros-melodic-rospack
82+
sh -c 'echo "deb http://packages.ros.org/ros-shadow-fixed/ubuntu `lsb_release -cs` main" > /etc/apt/sources.list.d/ros-latest.list'
83+
wget http://packages.ros.org/ros.key -O - | apt-key add -
84+
apt-get update -qq
85+
apt-get install -qq -y python-catkin-tools python-rosdep
86+
apt-get install -qq -y build-essential git ros-melodic-rosbash ros-melodic-rospack
7487
- name: Setup Src
7588
shell: bash
7689
run: |
@@ -89,7 +102,7 @@ jobs:
89102
wget https://raw.githubusercontent.com/tork-a/jskeus-release/master/patches/CMakeLists.txt -O jskeus/CMakeLists.txt
90103
# rosdep install
91104
cd ..
92-
sudo rosdep init
105+
rosdep init
93106
rosdep update
94107
rosdep install -y -r --rosdistro melodic --from-paths src --ignore-src
95108
- name: Build
@@ -140,7 +153,7 @@ jobs:
140153
141154
142155
osx:
143-
runs-on: macos-latest
156+
runs-on: macos-13 # https://github.com/actions/runner-images/blob/main/images/macos/macos-13-Readme.md
144157
timeout-minutes: 60
145158
steps:
146159
- name: Checkout

‎.travis.sh

Lines changed: 14 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,11 @@ function travis_time_end {
2121
}
2222

2323
travis_time_start setup.apt-get_update
24+
# Use archive repository for Debian Stretch
25+
if [[ "$DOCKER_IMAGE" == *"stretch" ]]; then
26+
sed -i 's/[[:alpha:]]*.debian.org/archive.debian.org/' /etc/apt/sources.list
27+
sed -i '/stretch-updates/ s/^#*/#/' /etc/apt/sources.list
28+
fi
2429
if [ "$(which sudo)" == "" ]; then apt-get update && apt-get install -y sudo; else sudo apt-get update; fi
2530
travis_time_end
2631

@@ -89,20 +94,27 @@ for test_l in irteus/test/*.l; do
8994
irteusgl $test_l;
9095
export TMP_EXIT_STATUS=$?
9196

92-
travis_time_end `expr 32 - $TMP_EXIT_STATUS`
97+
export CONTINUE=0
98+
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
99+
if [[ $CONTINUE != 0 ]]; then export TMP_EXIT_STATUS=0; fi
93100

94101
export EXIT_STATUS=`expr $TMP_EXIT_STATUS + $EXIT_STATUS`;
95102

103+
travis_time_end `expr 32 - $TMP_EXIT_STATUS`
96104

97105
travis_time_start jskeus.compiled.${test_l##*/}.test
98106

99107
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))))"
100108
export TMP_EXIT_STATUS=$?
101109

102-
travis_time_end `expr 32 - $TMP_EXIT_STATUS`
110+
export CONTINUE=0
111+
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
112+
if [[ $CONTINUE != 0 ]]; then export TMP_EXIT_STATUS=0; fi
103113

104114
export EXIT_STATUS=`expr $TMP_EXIT_STATUS + $EXIT_STATUS`;
105115

116+
travis_time_end `expr 32 - $TMP_EXIT_STATUS`
117+
106118
done;
107119
echo "Exit status : $EXIT_STATUS";
108120

‎Makefile

Lines changed: 7 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,12 @@ ifneq ($(GCC_MACHINE),)
1414
ifeq ($(GCC_MACHINE),i686-linux-gnu)
1515
MACHINE=x86
1616
endif
17+
ifeq ($(GCC_MACHINE),aarch64-linux-gnu)
18+
MACHINE=aarch64
19+
endif
20+
ifeq ($(GCC_MACHINE),arm-linux-gnueabihf)
21+
MACHINE=arm
22+
endif
1723
endif
1824
ifeq ($(MACHINE),)
1925
MACHINE=$(shell uname -m)
@@ -26,7 +32,7 @@ ifeq ($(OS),Linux)
2632
export ARCHDIR=Linux64
2733
export MAKEFILE=Makefile.Linux64
2834
else
29-
ifneq (, $(findstring armv,$(MACHINE)))
35+
ifneq (, $(findstring arm,$(MACHINE)))
3036
export ARCHDIR=LinuxARM
3137
export MAKEFILE=Makefile.LinuxARM
3238
else ifneq (, $(findstring aarch,$(MACHINE)))

‎doc/irtmodel.tex

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -803,9 +803,9 @@ \subsubsection{関節角度限界回避を含む逆運動学}
803803
\left\{
804804
\begin{array}{l l}
805805
1 + \left|\frac{\partial \bm{H}(\bm{\theta})}{\partial \theta_i}\right|
806-
& if \left|\frac{\partial \bm{H}(\bm{\theta})}{\partial
806+
& if \;\Delta\left|\frac{\partial \bm{H}(\bm{\theta})}{\partial
807807
\theta_i}\right| \geq 0 \\
808-
1 & if \left|\frac{\partial \bm{H}(\bm{\theta})}{\partial
808+
1 & if \;\Delta\left|\frac{\partial \bm{H}(\bm{\theta})}{\partial
809809
\theta_i}\right| < 0
810810
\end{array}
811811
\right.

‎doc/jmanual.dvi

1.88 KB
Binary file not shown.

‎doc/jmanual.pdf

471 Bytes
Binary file not shown.

‎irteus/Makefile.Linux

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -72,7 +72,7 @@ CXXFLAGS=$(CFLAGS)
7272
CFLAGS+= -g -falign-functions=4
7373
CXXFLAGS+=-g -falign-functions=4
7474

75-
ifeq ($(shell /bin/uname -m), x86_64)
75+
ifeq ($(shell gcc -dumpmachine), x86_64-linux-gnu)
7676
CC += -m32
7777
CXX += -m32
7878
LD += -m32

‎irteus/Makefile.LinuxARM

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -63,7 +63,8 @@ FFTW=-L/usr/local/lib -lfftw -lrfftw
6363

6464
SVNVERSION=\"$(shell git rev-parse --short HEAD)\"
6565

66-
MACHINE=$(shell uname -m | sed 's/\(armv[0-9]\).*/\1/')
66+
# gcc -dumpmachine retruns target triplet consists of three fields separated by a hyphen (-).
67+
MACHINE=$(shell gcc -dumpmachine | sed 's/\(.*\)-.*-.*/\1/')
6768
THREAD= -DTHREADED -DPTHREAD
6869

6970
CFLAGS=-O2 -D$(MACHINE) -D$(ARCH) -DLinux -DARM -D_REENTRANT -DGCC -I$(EUSDIR)/include $(THREAD) -DSVNVERSION=$(SVNVERSION) -fPIC

‎irteus/PQP/Makefile.Linux

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -10,7 +10,7 @@ LPFX = lib
1010
LIBS = -L$(ARCHDIR) -lRAPID
1111

1212

13-
ifeq ($(shell /bin/uname -m), x86_64)
13+
ifeq ($(shell gcc -dumpmachine), x86_64-linux-gnu)
1414
CC += -m32
1515
LD += -m32
1616
EXELD += -m32

‎irteus/irtmath.l

Lines changed: 8 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -523,6 +523,7 @@
523523
;; lmeds
524524
;;http://www-pse.cheme.kyoto-u.ac.jp/~kano/document/text-PCA.pdf
525525
(defun lms (point-list)
526+
"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"
526527
(let ((v^ (vector-mean point-list))
527528
(point-length (length point-list))
528529
delx x v eigen-res eigen-val eigen-vec min-lam min-vec)
@@ -543,10 +544,12 @@
543544
))
544545

545546
(defun lms-estimate (res point-)
547+
"returns the signed distance from the fitted line/plane/hyperplane to point-"
546548
(+ (v. point- (car res)) (cadr res))
547549
)
548550

549551
(defun lms-error (result point-list)
552+
"returns the mean of the square of the distance from the fitted line/plane/hyperplane to each point in point-list"
550553
(let ((ret-err 0) tmp-err)
551554
(dolist (l point-list)
552555
(setq tmp-err (lms-estimate result l))
@@ -555,13 +558,14 @@
555558
(/ ret-err (length point-list))
556559
))
557560

558-
;; choose num points randomly and apply lms to find the souliton with smallest errors
559-
;; to use ransac ransac
561+
;; choose num points randomly and apply lms to find the solution with the smallest error
562+
;; to use ransac
560563
;; :lmeds-error-func -> set to ransac-error
561564
;; :ransac-threshold err^2 (square of the distance from the plane)
562565
(defun lmeds (point-list &key (num 5) (err-rate 0.3) (iteration) (ransac-threshold)
563566
(lms-func #'lms) (lmeds-error-func #'lmeds-error)
564567
(lms-estimate-func #'lms-estimate))
568+
"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"
565569
(let (point-num r result result-list error-list iter
566570
comb-index comb-index-list point-list-tmp)
567571
;; initialize variables
@@ -599,6 +603,7 @@
599603
))
600604

601605
(defun lmeds-error (result point-list &key (lms-estimate-func #'lms-estimate))
606+
"returns the median of the square of the distance from the fitted line/plane/hyperplane to each point in point-list"
602607
(let (tmp-err err)
603608
(dolist (l point-list)
604609
(setq tmp-err (funcall lms-estimate-func result l))
@@ -608,6 +613,7 @@
608613
))
609614

610615
(defun lmeds-error-mat (result mat &key (lms-estimate-func #'lms-estimate))
616+
"matrixed version of lmeds-error. mat is the matrixed version of point-list"
611617
(let ((size (array-dimension mat 0))
612618
(p (float-vector 0 0 0))
613619
tmp-err err)

‎irteus/irtrobot.l

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -594,7 +594,7 @@
594594
(find-if #'(lambda (l) (equal fs (car (send self l :force-sensors)))) '(:rleg :lleg)))
595595
(send self :force-sensors)))
596596
'(:rleg :lleg)))
597-
(force-sensors (mapcar #'(lambda (l) (send self :force-sensor l)) limbs))
597+
(force-sensors (mapcar #'(lambda (l) (car (send self l :force-sensors))) limbs))
598598
(cop-coords (mapcar #'(lambda (l) (send self l :end-coords)) limbs))
599599
(fz-thre 1e-3) ;; [N]
600600
(limb-cop-fz-list

‎irteus/test/test-irt-motion.l

Lines changed: 52 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -40,6 +40,57 @@
4040
(every #'identity zmp-cog-p)))
4141
'(:rleg :lleg :both))))
4242

43+
;; calc-zmp / calc-zmp-from-forces-moments
44+
(defun test-zmp-from-forces-moments (robot)
45+
(let (zmp-p1 zmp-p2)
46+
(labels ((calc-zmp-from-forces-moments
47+
()
48+
(let* ((torque-vector (send robot :torque-vector))
49+
(rleg-force-moment (v- (send robot :calc-force-from-joint-torque :rleg torque-vector)))
50+
(lleg-force-moment (v- (send robot :calc-force-from-joint-torque :lleg torque-vector))))
51+
(send robot :calc-zmp-from-forces-moments
52+
(list (subseq rleg-force-moment 0 3)
53+
(subseq lleg-force-moment 0 3))
54+
(list (subseq rleg-force-moment 3)
55+
(subseq lleg-force-moment 3))
56+
))))
57+
(objects (list robot))
58+
(send robot :newcoords (make-coords))
59+
(send robot :reset-pose)
60+
(send robot :arms :shoulder-p :joint-angle -45)
61+
(send robot :arms :elbow-p :joint-angle -45)
62+
(send robot :fix-leg-to-coords (make-coords) '(:rleg :lleg))
63+
(send *irtviewer* :look-all)
64+
;; calc-zmp
65+
(do ((mid -0.0 (+ mid 0.05)))
66+
((>= mid 1.1))
67+
;; calc-zmp
68+
(send robot :move-centroid-on-foot :both '(:rleg :lleg) :mid mid)
69+
(setq zmp-p1 (send robot :calc-zmp))
70+
;; calc-zmp-from-forces-moments
71+
(setq zmp-p2 (calc-zmp-from-forces-moments))
72+
;; draw centroid/zmp
73+
(warning-message 2 "centroid:~A zmp:~A / ~A~%" (send robot :centroid) zmp-p1 zmp-p2)
74+
(send *irtviewer* :draw-objects :flush nil)
75+
(send (send robot :centroid) :draw-on :flush nil :size 80 :color #f(0 0 1) :width 4)
76+
(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)
77+
(send zmp-p1 :draw-on :flush nil :size 150 :color #f(1 0 0) :width 2)
78+
(send zmp-p2 :draw-on :flush t :size 150 :color #f(1 1 0) :width 2)
79+
)
80+
81+
(warning-message 3 "check centroid is not equal to zmp~%")
82+
(warning-message 3 "centroid:~A zmp:~A / ~A~%" (send robot :centroid) zmp-p1 zmp-p2)
83+
(assert (not (eps-v= zmp-p1
84+
(transform #2f((1 0 0)(0 1 0)(0 0 0)) (send robot :centroid)))))
85+
(send robot :calc-zmp) (calc-zmp-from-forces-moments)
86+
(send robot :calc-zmp) (setq zmp-p2 (calc-zmp-from-forces-moments))
87+
(setq zmp-p1 (send robot :get :zmp))
88+
(warning-message 3 "check centroid is not equal to zmp after calling :calc-zmp~%")
89+
(warning-message 3 "centroid:~A zmp:~A / ~A~%" (send robot :centroid) zmp-p1 zmp-p2)
90+
(assert (eps-v= zmp-p1
91+
(transform #2f((1 0 0)(0 1 0)(0 0 0)) (send robot :centroid))))
92+
t)))
93+
4394
(defun init-pose-torque-tests
4495
(robot)
4596
(mapcar #'(lambda (jnt)
@@ -909,6 +960,7 @@
909960
(deftest test-irtdyna-samplerobot
910961
(warning-message 2 ";; run test-zmp-comomn~%")
911962
(assert (test-zmp-comomn *sample-robot*))
963+
(assert (test-zmp-from-forces-moments *sample-robot*))
912964
;; Not sure why, but it fails only COLLISION_LIB=PQP with source compile
913965
(unless (derivedp #'test-torque-from-its-own-weight-common compiled-code)
914966
(warning-message 2 ";; run test-torque-from-its-own-weight-common~%")

0 commit comments

Comments
 (0)