Merge branch 'master' of https://github.com/euslisp/jskeus into clear-open-list

Author: nakane11

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.

doc/jmanual.dvi

@@ -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

doc/jmanual.pdf

@@ -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/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/Makefile.LinuxARM

@@ -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/PQP/Makefile.Linux

@@ -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~%")