From baaa9a7a8e5f3e541a3a97b54e61db08a4afeffb Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Thu, 30 Jan 2020 00:21:11 +0900 Subject: [PATCH 1/6] [eus_cddlib]add eus_cddlib --- eus_cddlib/CHANGELOG.rst | 3 + eus_cddlib/CMakeLists.txt | 22 +++ eus_cddlib/README.md | 3 + eus_cddlib/euslisp/eus-cddlib.l | 167 ++++++++++++++++++++++ eus_cddlib/euslisp/test-eus-cddlib.l | 76 ++++++++++ eus_cddlib/lib/.placeholder | 0 eus_cddlib/package.xml | 17 +++ eus_cddlib/src/eus_cddlib.c | 201 +++++++++++++++++++++++++++ 8 files changed, 489 insertions(+) create mode 100644 eus_cddlib/CHANGELOG.rst create mode 100644 eus_cddlib/CMakeLists.txt create mode 100644 eus_cddlib/README.md create mode 100644 eus_cddlib/euslisp/eus-cddlib.l create mode 100644 eus_cddlib/euslisp/test-eus-cddlib.l create mode 100644 eus_cddlib/lib/.placeholder create mode 100644 eus_cddlib/package.xml create mode 100644 eus_cddlib/src/eus_cddlib.c diff --git a/eus_cddlib/CHANGELOG.rst b/eus_cddlib/CHANGELOG.rst new file mode 100644 index 000000000..18d04d363 --- /dev/null +++ b/eus_cddlib/CHANGELOG.rst @@ -0,0 +1,3 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package eus_cddlib +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ diff --git a/eus_cddlib/CMakeLists.txt b/eus_cddlib/CMakeLists.txt new file mode 100644 index 000000000..d9d7feb0d --- /dev/null +++ b/eus_cddlib/CMakeLists.txt @@ -0,0 +1,22 @@ +project(eus_cddlib) + +cmake_minimum_required(VERSION 2.4.6) + +find_package(catkin REQUIRED COMPONENTS rostest) + +catkin_package() + +add_library(eus_cddlib SHARED src/eus_cddlib.c) +set_target_properties(eus_cddlib PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib) +target_link_libraries(eus_cddlib cdd) + +install(DIRECTORY lib/ + USE_SOURCE_PERMISSIONS + DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + +install(DIRECTORY euslisp + USE_SOURCE_PERMISSIONS + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +install(TARGETS eus_cddlib # lib/ is added here to install euslisp library + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/lib) diff --git a/eus_cddlib/README.md b/eus_cddlib/README.md new file mode 100644 index 000000000..40ede31c1 --- /dev/null +++ b/eus_cddlib/README.md @@ -0,0 +1,3 @@ +# eus_cddlib + +Use cddlib (https://inf.ethz.ch/personal/fukudak/cdd_home/) from EusLisp for finding vertices of convex polytopes. \ No newline at end of file diff --git a/eus_cddlib/euslisp/eus-cddlib.l b/eus_cddlib/euslisp/eus-cddlib.l new file mode 100644 index 000000000..7d8c363e4 --- /dev/null +++ b/eus_cddlib/euslisp/eus-cddlib.l @@ -0,0 +1,167 @@ +(defvar *libeuscddlib* (load-foreign (format nil "~A/lib/libeus_cddlib.so" (ros::resolve-ros-path "package://eus_cddlib")))) + +(defforeign _cddlib-initialize + *libeuscddlib* + "cddlib_initialize" + () + :integer) + +(defforeign _cddlib-finalize + *libeuscddlib* + "cddlib_finalize" + () + :integer) + +(defforeign _cddlib-h-to-v + *libeuscddlib* + "cddlib_H_to_V" + (:string ;; A_eq + :string ;; b_eq + :string ;; A_ineq + :string ;; b_ineq + :integer ;; d + :integer ;; m_eq + :integer ;; m_ineq + :string ;; out_n + :string ;; ont_s + :integer ;; verbose + ) + :integer) + +(defforeign _cddlib-get-v + *libeuscddlib* + "cddlib_get_V" + (:string ;; V + :string ;; R + :integer ;; d + :integer ;; n + :integer ;; s + ) + :integer) + +(defforeign _cddlib-v-to-h + *libeuscddlib* + "cddlib_V_to_H" + (:string ;; V + :string ;; R + :integer ;; d + :integer ;; n + :integer ;; s + :string ;; m_eq + :string ;; m_ineq + :integer ;; verbose + ) + :integer) + +(defforeign _cddlib-get-h + *libeuscddlib* + "cddlib_get_H" + (:string ;; A_eq + :string ;; b_eq + :string ;; A_ineq + :string ;; b_ineq + :integer ;; d + :integer ;; m_eq + :integer ;; m_ineq + ) + :integer) + +;;input +;; A_eq x + b_eq = 0 +;; A_ineq x + b_ineq >= 0 +;;output +;; (list V R) +;; x = V y + R z (sum of y = 1, y >= 0, z >= 0) +(defun cddlib-H-to-V + (&key + (A_eq) + (b_eq) + (A_ineq) + (b_ineq) + (verbose 0) + ) + (cond + ((and A_eq A_ineq) t) + (A_eq (setq A_ineq (make-matrix 0 (array-dimension A_eq 1))) + (setq b_ineq (instantiate float-vector (array-dimension A_eq 1)))) + (A_ineq (setq A_eq (make-matrix 0 (array-dimension A_ineq 1))) + (setq b_eq (instantiate float-vector (array-dimension A_ineq 1)))) + (t (return-from cddlib-H-to-V (list (make-matrix 0 0) (make-matrix 0 0))))) + (let ((n (instantiate integer-vector 1)) + (s (instantiate integer-vector 1)) + (d (array-dimension A_eq 1)) + ) + (_cddlib-H-to-V + (array-entity A_eq) + b_eq + (array-entity A_ineq) + b_ineq + d + (array-dimension A_eq 0) + (array-dimension A_ineq 0) + n + s + verbose) + (let ((V (make-matrix d (elt n 0))) + (R (make-matrix d (elt s 0))) + ) + (_cddlib-get-V + (array-entity V) + (array-entity R) + d + (elt n 0) + (elt s 0)) + (list V R) + ) + ) + ) + +;;input +;; x = V y + R z (sum of y = 1, y >= 0, z >= 0) +;;output +;; (list A_eq b_eq A_ineq b_ineq) +;; A_eq x + b_eq = 0 +;; A_ineq x + b_ineq >= 0 +(defun cddlib-V-to-H + (&key + (V) + (R) + (verbose 0) + ) + (cond + ((and V R) t) + (V (setq R (make-matrix (array-dimension V 0) 0))) + (R (setq V (make-matrix (array-dimension R 0) 0))) + (t (return-from cddlib-V-to-H (list (make-matrix 0 0) (make-matrix 0 0) (make-matrix 0 0) (make-matrix 0 0))))) + (let ((m_eq (instantiate integer-vector 1)) + (m_ineq (instantiate integer-vector 1)) + (d (array-dimension V 0)) + ) + (_cddlib-V-to-H + (array-entity V) + (array-entity R) + d + (array-dimension V 1) + (array-dimension R 1) + m_eq + m_ineq + verbose) + (let ((A_eq (make-matrix (elt m_eq 0) d)) + (b_eq (instantiate float-vector (elt m_eq 0))) + (A_ineq (make-matrix (elt m_ineq 0) d)) + (b_ineq (instantiate float-vector (elt m_ineq 0))) + ) + (_cddlib-get-H + (array-entity A_eq) + b_eq + (array-entity A_ineq) + b_ineq + d + (elt m_eq 0) + (elt m_ineq 0)) + (list A_eq b_eq A_ineq b_ineq) + ) + ) + ) + +(_cddlib-initialize) diff --git a/eus_cddlib/euslisp/test-eus-cddlib.l b/eus_cddlib/euslisp/test-eus-cddlib.l new file mode 100644 index 000000000..b3d1f723d --- /dev/null +++ b/eus_cddlib/euslisp/test-eus-cddlib.l @@ -0,0 +1,76 @@ +(load (format nil "~A/euslisp/eus-cddlib.l" (ros::resolve-ros-path "package://eus_cddlib"))) + + +;; |0| + | 1 0| x >= 0 +;; |0| | 0 1| +;; |1| |-1 -1| +;; |1| | 0 -1| +;; +;; x =|0 1 0 -1|y + | | z +;; |1 0 -1 0| | | +;; sum y = 1, y >= 0, z >= 0 +(defun test1 () + (let* ((A (matrix #F(1 0) + #F(0 1) + #F(-1 0) + #F(0 -1))) + (b (float-vector 0 + 0 + 1 + 1)) + (ret1 (cddlib-H-to-V + :A_ineq A + :b_ineq b + :verbose 1)) + (ret2 (cddlib-V-to-H + :V (elt ret1 0) + :R (elt ret1 1) + :verbose 1)) + ) + (print ret1) + (print ret2) + ) + ) + + + +(defun test2 () + (let* ((A (matrix #F(0 0 1 0 0 0) + #F(1 0 0.2 0 0 0) + #F(-1 0 0.2 0 0 0) + #F(0 1 0.2 0 0 0) + #F(0 -1 0.2 0 0 0) + #F(0 0 0.09 1 0 0) + #F(0 0 0.09 -1 0 0) + #F(0 0 0.05 0 1 0) + #F(0 0 0.05 0 -1 0) + #F(0 0 0.01 0 0 1) + #F(0 0 0.01 0 0 -1))) + (b (float-vector 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0)) + (ret1 (cddlib-H-to-V + :A_ineq A + :b_ineq b + :verbose 1)) + (ret2 (cddlib-V-to-H + :V (elt ret1 0) + :R (elt ret1 1) + :verbose 1)) + ) + (format-array (elt ret1 0)) + (format-array (elt ret1 1)) + (format-array (elt ret2 0)) + (format-array (elt ret2 1)) + (format-array (elt ret2 2)) + (format-array (elt ret2 3)) + ) + ) diff --git a/eus_cddlib/lib/.placeholder b/eus_cddlib/lib/.placeholder new file mode 100644 index 000000000..e69de29bb diff --git a/eus_cddlib/package.xml b/eus_cddlib/package.xml new file mode 100644 index 000000000..f69951de1 --- /dev/null +++ b/eus_cddlib/package.xml @@ -0,0 +1,17 @@ + + + eus_cddlib + + eus_cddlib + + 0.1.15 + Naoki Hiraoka + Apache License 2.0 + + + catkin + libcddlib-dev + + euslisp + libcddlib-dev + diff --git a/eus_cddlib/src/eus_cddlib.c b/eus_cddlib/src/eus_cddlib.c new file mode 100644 index 000000000..5f1ec4929 --- /dev/null +++ b/eus_cddlib/src/eus_cddlib.c @@ -0,0 +1,201 @@ +#include +#include + +#include +#include + +double* A_eq_cash = NULL; +double* b_eq_cash = NULL; +double* A_ineq_cash = NULL; +double* b_ineq_cash = NULL; +double* V_cash = NULL; +double* R_cash = NULL; + +int cddlib_initialize (void){ + dd_set_global_constants(); /* First, this must be called to use cddlib. */ + return 0; +} + +int cddlib_finalize (void){ + dd_free_global_constants(); /* At the end, this must be called. */ + return 0; +} + +/* + A_eq x + b_eq = 0 + A_ineq x + b_ineq >= 0 + */ +int cddlib_H_to_V (double* A_eq, double* b_eq, double* A_ineq, double* b_ineq, int d, int m_eq, int m_ineq, int* out_n, int* out_s, int verbose) +{ + free(V_cash); + V_cash = NULL; + free(R_cash); + R_cash = NULL; + + dd_PolyhedraPtr poly; + dd_MatrixPtr A, G; + dd_ErrorType err; + + A=dd_CreateMatrix(m_eq + m_ineq,d+1); + for (size_t i = 0; i < m_eq; i++){ + set_addelem(A->linset,i+1); + dd_set_d(A->matrix[i][0],b_eq[i]); + for (size_t j = 0; j < d; j++){ + dd_set_d(A->matrix[i][j+1],A_eq[i*d+j]); + } + } + for (size_t i = 0; i < m_ineq; i++){ + dd_set_d(A->matrix[m_eq+i][0],b_ineq[i]); + for (size_t j = 0; j < d; j++){ + dd_set_d(A->matrix[m_eq+i][j+1],A_ineq[i*d+j]); + } + } + A->representation=dd_Inequality; + poly=dd_DDMatrix2Poly(A, &err); /* compute the second (generator) representation */ + if (err==dd_NoError){ + G=dd_CopyGenerators(poly); + if (verbose){ + printf("\nInput is H-representation:\n"); + dd_WriteMatrix(stdout,A); printf("\n"); + dd_WriteMatrix(stdout,G); + } + int n = 0; + int s = 0; + for (size_t i = 0; i < G->rowsize; i++){ + if (dd_get_d(G->matrix[i][0]) ==0) s++; + else n++; + } + *out_n = n; + *out_s = s; + int _n = 0; + int _s = 0; + V_cash = (double*)malloc(sizeof(double)*n*d); + R_cash = (double*)malloc(sizeof(double)*s*d); + for (size_t i = 0; i < G->rowsize; i++){ + if (dd_get_d(G->matrix[i][0])==0){ + for (size_t j = 0; j < d; j++){ + R_cash[s*j+_s] = dd_get_d(G->matrix[i][1+j]); + } + _s++; + }else{ + for (size_t j = 0; j < d; j++){ + V_cash[n*j+_n] = dd_get_d(G->matrix[i][1+j]); + } + _n++; + } + } + + dd_FreeMatrix(A); + dd_FreeMatrix(G); + dd_FreePolyhedra(poly); + return 0; + }else{ + dd_WriteErrorMessages(stdout,err); + } + return -1; +} + +int cddlib_get_V (double* V, double* R, int d, int n, int s) +{ + for(size_t i=0; i < d*n; i++){ + V[i] = V_cash[i]; + } + for(size_t i=0; i < d*s; i++){ + R[i] = R_cash[i]; + } + return 0; +} + +int cddlib_V_to_H (double* V, double* R, int d, int n, int s, int* out_m_eq, int* out_m_ineq, int verbose) +{ + free(A_eq_cash); + A_eq_cash = NULL; + free(b_eq_cash); + b_eq_cash = NULL; + free(A_ineq_cash); + A_ineq_cash = NULL; + free(b_ineq_cash); + b_ineq_cash = NULL; + + dd_PolyhedraPtr poly; + dd_MatrixPtr A, G; + dd_ErrorType err; + + G=dd_CreateMatrix(n+s,d+1); + for (size_t i = 0; i < n; i++){ + dd_set_d(G->matrix[i][0],1); + for (size_t j = 0; j < d; j++){ + dd_set_d(G->matrix[i][j+1],V[j*n+i]); + } + } + for (size_t i = 0; i < s; i++){ + dd_set_d(G->matrix[n+i][0],0); + for (size_t j = 0; j < d; j++){ + dd_set_d(G->matrix[n+i][j+1],R[j*s+i]); + } + } + G->representation=dd_Generator; + poly=dd_DDMatrix2Poly(G, &err); /* compute the second (generator) representation */ + if (err==dd_NoError){ + A=dd_CopyInequalities(poly); + if (verbose){ + printf("\nInput is V-representation:\n"); + dd_WriteMatrix(stdout,G); printf("\n"); + dd_WriteMatrix(stdout,A); + } + int m_eq = 0; + int m_ineq = 0; + for (size_t i = 0; i < A->rowsize; i++){ + if (set_member(i+1,A->linset)) m_eq++; + else m_ineq++; + } + *out_m_eq = m_eq; + *out_m_ineq = m_ineq; + int _m_eq = 0; + int _m_ineq = 0; + A_eq_cash = (double*)malloc(sizeof(double)*m_eq*d); + b_eq_cash = (double*)malloc(sizeof(double)*m_eq); + A_ineq_cash = (double*)malloc(sizeof(double)*m_ineq*d); + b_ineq_cash = (double*)malloc(sizeof(double)*m_ineq); + for (size_t i = 0; i < A->rowsize; i++){ + if (set_member(i+1,A->linset)){ + for (size_t j = 0; j < d; j++){ + A_eq_cash[d*_m_eq+j] = dd_get_d(A->matrix[i][1+j]); + } + b_eq_cash[_m_eq] = dd_get_d(A->matrix[i][0]); + _m_eq++; + }else{ + for (size_t j = 0; j < d; j++){ + A_ineq_cash[d*_m_ineq+j] = dd_get_d(A->matrix[i][1+j]); + } + b_ineq_cash[_m_ineq] = dd_get_d(A->matrix[i][0]); + _m_ineq++; + } + } + + dd_FreeMatrix(A); + dd_FreeMatrix(G); + dd_FreePolyhedra(poly); + return 0; + }else{ + dd_WriteErrorMessages(stdout,err); + } + return -1; +} + +int cddlib_get_H (double* A_eq, double* b_eq, double* A_ineq, double* b_ineq, int d , int m_eq, int m_ineq) +{ + for(size_t i=0; i < d*m_eq; i++){ + A_eq[i] = A_eq_cash[i]; + } + for(size_t i=0; i < m_eq; i++){ + b_eq[i] = b_eq_cash[i]; + } + for(size_t i=0; i < d*m_ineq; i++){ + A_ineq[i] = A_ineq_cash[i]; + } + for(size_t i=0; i < m_ineq; i++){ + b_ineq[i] = b_ineq_cash[i]; + } + return 0; +} From d8286c66d4506d1bec852613629173911602897c Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Thu, 30 Jan 2020 00:50:47 +0900 Subject: [PATCH 2/6] fix --- eus_cddlib/package.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/eus_cddlib/package.xml b/eus_cddlib/package.xml index f69951de1..2290ea484 100644 --- a/eus_cddlib/package.xml +++ b/eus_cddlib/package.xml @@ -10,8 +10,8 @@ catkin - libcddlib-dev + libcdd-dev euslisp - libcddlib-dev + libcdd-dev From 7555b78e40745a44bf5a644a0a94faad7eed1609 Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Thu, 30 Jan 2020 13:16:28 +0900 Subject: [PATCH 3/6] fix --- eus_cddlib/euslisp/eus-cddlib-compiled.l | 2 ++ eus_cddlib/euslisp/test-eus-cddlib.l | 2 +- eus_cddlib/package.xml | 2 +- 3 files changed, 4 insertions(+), 2 deletions(-) create mode 100644 eus_cddlib/euslisp/eus-cddlib-compiled.l diff --git a/eus_cddlib/euslisp/eus-cddlib-compiled.l b/eus_cddlib/euslisp/eus-cddlib-compiled.l new file mode 100644 index 000000000..e930c39ab --- /dev/null +++ b/eus_cddlib/euslisp/eus-cddlib-compiled.l @@ -0,0 +1,2 @@ +(compiler::compile-file-if-src-newer (format nil "~A/euslisp/eus-cddlib" (ros::resolve-ros-path "package://eus_cddlib"))) +(load (format nil "~A/euslisp/eus-cddlib.so" (ros::resolve-ros-path "package://eus_cddlib"))) diff --git a/eus_cddlib/euslisp/test-eus-cddlib.l b/eus_cddlib/euslisp/test-eus-cddlib.l index b3d1f723d..3f38d368a 100644 --- a/eus_cddlib/euslisp/test-eus-cddlib.l +++ b/eus_cddlib/euslisp/test-eus-cddlib.l @@ -1,4 +1,4 @@ -(load (format nil "~A/euslisp/eus-cddlib.l" (ros::resolve-ros-path "package://eus_cddlib"))) +(load (format nil "~A/euslisp/eus-cddlib-compiled.l" (ros::resolve-ros-path "package://eus_cddlib"))) ;; |0| + | 1 0| x >= 0 diff --git a/eus_cddlib/package.xml b/eus_cddlib/package.xml index 2290ea484..58718e162 100644 --- a/eus_cddlib/package.xml +++ b/eus_cddlib/package.xml @@ -10,7 +10,7 @@ catkin - libcdd-dev + cdd euslisp libcdd-dev From e236443b339fa0572bb2a4013faa67163b434aa3 Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Thu, 30 Jan 2020 19:43:17 +0900 Subject: [PATCH 4/6] add scfr demo --- eus_cddlib/euslisp/eus-cddlib.l | 144 +++++++----- eus_cddlib/euslisp/test-eus-cddlib.l | 330 ++++++++++++++++++++++++++- eus_cddlib/src/eus_cddlib.c | 72 ++++-- 3 files changed, 453 insertions(+), 93 deletions(-) diff --git a/eus_cddlib/euslisp/eus-cddlib.l b/eus_cddlib/euslisp/eus-cddlib.l index 7d8c363e4..51351d05a 100644 --- a/eus_cddlib/euslisp/eus-cddlib.l +++ b/eus_cddlib/euslisp/eus-cddlib.l @@ -22,8 +22,9 @@ :integer ;; d :integer ;; m_eq :integer ;; m_ineq - :string ;; out_n - :string ;; ont_s + :string ;; n + :string ;; s_nonneg + :string ;; s_free :integer ;; verbose ) :integer) @@ -32,10 +33,12 @@ *libeuscddlib* "cddlib_get_V" (:string ;; V - :string ;; R + :string ;; R_nonneg + :string ;; R_free :integer ;; d :integer ;; n - :integer ;; s + :integer ;; s_nonneg + :integer ;; s_free ) :integer) @@ -43,10 +46,12 @@ *libeuscddlib* "cddlib_V_to_H" (:string ;; V - :string ;; R + :string ;; R_nonneg + :string ;; R_free :integer ;; d :integer ;; n - :integer ;; s + :integer ;; s_nonneg + :integer ;; s_free :string ;; m_eq :string ;; m_ineq :integer ;; verbose @@ -70,8 +75,8 @@ ;; A_eq x + b_eq = 0 ;; A_ineq x + b_ineq >= 0 ;;output -;; (list V R) -;; x = V y + R z (sum of y = 1, y >= 0, z >= 0) +;; (list V R_nonneg R_free) +;; x = V y + R_nonneg z + R_free w (sum y = 1, y >= 0, z >= 0) (defun cddlib-H-to-V (&key (A_eq) @@ -81,43 +86,50 @@ (verbose 0) ) (cond - ((and A_eq A_ineq) t) + ((and A_eq A_ineq) (unless (= (array-dimension A_eq 1) (array-dimension A_ineq 1)) + (error "column size mismatch A_eq=~A, A_ineq=~A" (array-dimension A_eq 1) (array-dimension A_ineq 1)))) (A_eq (setq A_ineq (make-matrix 0 (array-dimension A_eq 1))) (setq b_ineq (instantiate float-vector (array-dimension A_eq 1)))) (A_ineq (setq A_eq (make-matrix 0 (array-dimension A_ineq 1))) (setq b_eq (instantiate float-vector (array-dimension A_ineq 1)))) - (t (return-from cddlib-H-to-V (list (make-matrix 0 0) (make-matrix 0 0))))) + (t (return-from cddlib-H-to-V (list (make-matrix 0 0) (make-matrix 0 0) (make-matrix 0 0))))) (let ((n (instantiate integer-vector 1)) - (s (instantiate integer-vector 1)) + (s_nonneg (instantiate integer-vector 1)) + (s_free (instantiate integer-vector 1)) (d (array-dimension A_eq 1)) ) - (_cddlib-H-to-V - (array-entity A_eq) - b_eq - (array-entity A_ineq) - b_ineq - d - (array-dimension A_eq 0) - (array-dimension A_ineq 0) - n - s - verbose) - (let ((V (make-matrix d (elt n 0))) - (R (make-matrix d (elt s 0))) + (if (= 0 (_cddlib-H-to-V + (array-entity A_eq) + b_eq + (array-entity A_ineq) + b_ineq + d + (array-dimension A_eq 0) + (array-dimension A_ineq 0) + n + s_nonneg + s_free + verbose)) + (let ((V (make-matrix d (elt n 0))) + (R_nonneg (make-matrix d (elt s_nonneg 0))) + (R_free (make-matrix d (elt s_free 0))) + ) + (_cddlib-get-V + (array-entity V) + (array-entity R_nonneg) + (array-entity R_free) + d + (elt n 0) + (elt s_nonneg 0) + (elt s_free 0)) + (list V R_nonneg R_free) ) - (_cddlib-get-V - (array-entity V) - (array-entity R) - d - (elt n 0) - (elt s 0)) - (list V R) - ) + nil) ) ) ;;input -;; x = V y + R z (sum of y = 1, y >= 0, z >= 0) +;; x = V y + R_nonneg z + R_free w (sum y = 1, y >= 0, z >= 0) ;;output ;; (list A_eq b_eq A_ineq b_ineq) ;; A_eq x + b_eq = 0 @@ -125,42 +137,54 @@ (defun cddlib-V-to-H (&key (V) - (R) + (R_nonneg) + (R_free) (verbose 0) ) (cond - ((and V R) t) - (V (setq R (make-matrix (array-dimension V 0) 0))) - (R (setq V (make-matrix (array-dimension R 0) 0))) + ((and V R_nonneg R_free) (unless (= (array-dimension V 0) (array-dimension R_nonneg 0) (array-dimension R_free 0)) + (error "row size mismatch V=~A, R_nonneg=~A, R_free~A" (array-dimension V 0) (array-dimension R_nonneg 0) (array-dimension R_free 0)))) + ((and V R_nonneg) (setq R_free (make-matrix (array-dimension V 0) 0))) + ((and V R_free) (setq R_nonneg (make-matrix (array-dimension V 0) 0))) + ((and R_nonneg R_free) (setq V (make-matrix (array-dimension R_nonneg 0) 0))) + (V (progn (setq R_nonneg (make-matrix (array-dimension V 0) 0)) + (setq R_free (make-matrix (array-dimension V 0) 0)))) + (R_nonneg (progn (setq V (make-matrix (array-dimension R_nonneg 0) 0)) + (setq R_free (make-matrix (array-dimension R_nonneg 0) 0)))) + (R_free (progn (setq V (make-matrix (array-dimension R_free 0) 0)) + (setq R_free (make-matrix (array-dimension R_free 0) 0)))) (t (return-from cddlib-V-to-H (list (make-matrix 0 0) (make-matrix 0 0) (make-matrix 0 0) (make-matrix 0 0))))) (let ((m_eq (instantiate integer-vector 1)) (m_ineq (instantiate integer-vector 1)) (d (array-dimension V 0)) ) - (_cddlib-V-to-H - (array-entity V) - (array-entity R) - d - (array-dimension V 1) - (array-dimension R 1) - m_eq - m_ineq - verbose) - (let ((A_eq (make-matrix (elt m_eq 0) d)) - (b_eq (instantiate float-vector (elt m_eq 0))) - (A_ineq (make-matrix (elt m_ineq 0) d)) - (b_ineq (instantiate float-vector (elt m_ineq 0))) + (if (= 0 (_cddlib-V-to-H + (array-entity V) + (array-entity R_nonneg) + (array-entity R_free) + d + (array-dimension V 1) + (array-dimension R_nonneg 1) + (array-dimension R_free 1) + m_eq + m_ineq + verbose)) + (let ((A_eq (make-matrix (elt m_eq 0) d)) + (b_eq (instantiate float-vector (elt m_eq 0))) + (A_ineq (make-matrix (elt m_ineq 0) d)) + (b_ineq (instantiate float-vector (elt m_ineq 0))) + ) + (_cddlib-get-H + (array-entity A_eq) + b_eq + (array-entity A_ineq) + b_ineq + d + (elt m_eq 0) + (elt m_ineq 0)) + (list A_eq b_eq A_ineq b_ineq) ) - (_cddlib-get-H - (array-entity A_eq) - b_eq - (array-entity A_ineq) - b_ineq - d - (elt m_eq 0) - (elt m_ineq 0)) - (list A_eq b_eq A_ineq b_ineq) - ) + nil) ) ) diff --git a/eus_cddlib/euslisp/test-eus-cddlib.l b/eus_cddlib/euslisp/test-eus-cddlib.l index 3f38d368a..71b9abc9e 100644 --- a/eus_cddlib/euslisp/test-eus-cddlib.l +++ b/eus_cddlib/euslisp/test-eus-cddlib.l @@ -1,6 +1,5 @@ (load (format nil "~A/euslisp/eus-cddlib-compiled.l" (ros::resolve-ros-path "package://eus_cddlib"))) - ;; |0| + | 1 0| x >= 0 ;; |0| | 0 1| ;; |1| |-1 -1| @@ -24,11 +23,10 @@ :verbose 1)) (ret2 (cddlib-V-to-H :V (elt ret1 0) - :R (elt ret1 1) + :R_nonneg (elt ret1 1) + :R_free (elt ret1 2) :verbose 1)) ) - (print ret1) - (print ret2) ) ) @@ -63,14 +61,324 @@ :verbose 1)) (ret2 (cddlib-V-to-H :V (elt ret1 0) - :R (elt ret1 1) + :R_nonneg (elt ret1 1) + :R_free (elt ret1 2) :verbose 1)) ) - (format-array (elt ret1 0)) - (format-array (elt ret1 1)) - (format-array (elt ret2 0)) - (format-array (elt ret2 1)) - (format-array (elt ret2 2)) - (format-array (elt ret2 3)) ) ) + +(defun block-matrix + (m &optional (i 0) (j 0) (ii nil) (jj nil)) + ;; i, j : start row and column idx + ;; ii, jj : row and column length of block matrix + (unless ii (setq ii (- (array-dimension m 0) i))) + (unless jj (setq jj (- (array-dimension m 1) j))) + (let ((ret (make-matrix ii jj))) + (dotimes (_i ii) + (dotimes (_j jj) + (setf (aref ret _i _j) (aref m (+ _i i) (+ _j j))))) + ret)) + +(defun draw-region (V R_nonneg R_free &optional (z 0)) + (print V) + (cond + ((= (array-dimension R_free 1) 2) + (let ((pwidth (send *viewer* :viewsurface :line-width)) + (psize (send *viewer* :viewsurface :point-size)) + (pcolor (send *viewer* :viewsurface :color)) + (vec (if (= (array-dimension V 1) 0) + (float-vector 0 0 z) + (float-vector (* 1e3 (aref V 0 0)) + (* 1e3 (aref V 1 0)) + z)))) + (unwind-protect + (progn + (send *viewer* :viewsurface :color #f(0 0 1)) + (send *viewer* :viewsurface :line-width 3) + (send *viewer* :viewsurface :point-size 3) + (send *viewer* :viewsurface :3d-line + (v+ vec (float-vector (* 1e6 (aref R_free 0 0)) + (* 1e6 (aref R_free 1 0)) + 0)) + (v+ vec (float-vector (* -1e6 (aref R_free 0 0)) + (* -1e6 (aref R_free 1 0)) + 0)) + :depth-test t) + (send *viewer* :viewsurface :3d-line + (v+ vec (float-vector (* 1e6 (aref R_free 0 1)) + (* 1e6 (aref R_free 1 1)) + 0)) + (v+ vec (float-vector (* -1e6 (aref R_free 0 1)) + (* -1e6 (aref R_free 1 1)) + 0)) + :depth-test t) + (send *viewer* :flush) + ) + (progn + (send *viewer* :viewsurface :line-width pwidth) + (send *viewer* :viewsurface :point-size psize) + (send *viewer* :viewsurface :color pcolor)))) + (return-from draw-region t)) + ((and (= (array-dimension R_free 1) 1) + (= (array-dimension R_nonneg 1) 1)) + (let ((pwidth (send *viewer* :viewsurface :line-width)) + (psize (send *viewer* :viewsurface :point-size)) + (pcolor (send *viewer* :viewsurface :color)) + (vec (if (= (array-dimension V 1) 0) + (float-vector 0 0 z) + (float-vector (+ 1e3 (aref V 0 0)) + (+ 1e3 (aref V 1 0)) + z)))) + (unwind-protect + (progn + (send *viewer* :viewsurface :color #f(0 0 1)) + (send *viewer* :viewsurface :line-width 3) + (send *viewer* :viewsurface :point-size 3) + (send *viewer* :viewsurface :3d-line + (v+ vec (float-vector (* 1e6 (aref R_free 0 0)) + (* 1e6 (aref R_free 1 0)) + 0)) + (v+ vec (float-vector (* -1e6 (aref R_free 0 0)) + (* -1e6 (aref R_free 1 0)) + 0)) + :depth-test t) + (send *viewer* :viewsurface :3d-line + vec + (v+ vec (float-vector (* 1e6 (aref R_nonneg 0 0)) + (* 1e6 (aref R_nonneg 1 0)) + 0)) + :depth-test t) + (send *viewer* :flush) + ) + (progn + (send *viewer* :viewsurface :line-width pwidth) + (send *viewer* :viewsurface :point-size psize) + (send *viewer* :viewsurface :color pcolor)))) + (return-from draw-region t)) + ((and (= (array-dimension R_free 1) 1) + (= (array-dimension R_nonneg 1) 0)) + (let ((pwidth (send *viewer* :viewsurface :line-width)) + (psize (send *viewer* :viewsurface :point-size)) + (pcolor (send *viewer* :viewsurface :color)) + (vec (if (= (array-dimension V 1) 0) + (list (float-vector 0 0 z)) + (let ((vec)) + (dotimes (i (array-dimension V 1)) + (push (float-vector (+ 1e3 (aref V 0 i)) + (+ 1e3 (aref V 1 i)) + z) vec)) + vec)))) + (unwind-protect + (progn + (send *viewer* :viewsurface :color #f(0 0 1)) + (send *viewer* :viewsurface :line-width 3) + (send *viewer* :viewsurface :point-size 3) + (dolist (vv vec) + (send *viewer* :viewsurface :3d-line + (v+ vv (float-vector (* 1e6 (aref R_free 0 0)) + (* 1e6 (aref R_free 1 0)) + 0)) + (v+ vv (float-vector (* -1e6 (aref R_free 0 0)) + (* -1e6 (aref R_free 1 0)) + 0)) + :depth-test t)) + (send *viewer* :flush) + ) + (progn + (send *viewer* :viewsurface :line-width pwidth) + (send *viewer* :viewsurface :point-size psize) + (send *viewer* :viewsurface :color pcolor)))) + (return-from draw-region t)) + (t nil)) + (let* ((points (let ((points nil)) + (dotimes (i (array-dimension V 1)) + (push (float-vector (* 1e3 (aref V 0 i)) (* 1e3 (aref V 1 i)) z) points)) + points)) + (hull (instance polygon :init :vertices (if points (quickhull points) (list (float-vector 0 0 z))))) + (vec (cond ((= (array-dimension R_nonneg 1) 0) + nil) + ((= (array-dimension R_nonneg 1) 1) + (list (float-vector (aref R_nonneg 0 0) (aref R_nonneg 1 0) 0) + (float-vector (aref R_nonneg 0 0) (aref R_nonneg 1 0) 0))) + (t + (let ((v1 (float-vector (aref R_nonneg 0 0) (aref R_nonneg 1 0) 0)) + (v2 (float-vector (aref R_nonneg 0 1) (aref R_nonneg 1 1) 0))) + (if (> (elt (v* v1 v2) 2) 0) + (list v1 v2) + (list v2 v1)))))) + (ext (if vec + (let ((v1 (v* (elt vec 0) #F(0 0 1))) + (v2 (v* #F(0 0 1) (elt vec 0)))) + (list + (find-extream (send hull :vertices) #'(lambda (v) (v. v v1)) #'>) + (find-extream (send hull :vertices) #'(lambda (v) (v. v v2)) #'>))) + nil)) + ) + (let ((pwidth (send *viewer* :viewsurface :line-width)) + (psize (send *viewer* :viewsurface :point-size)) + (pcolor (send *viewer* :viewsurface :color))) + (unwind-protect + (progn + (send *viewer* :viewsurface :color #f(0 0 1)) + (send *viewer* :viewsurface :line-width 3) + (send *viewer* :viewsurface :point-size 3) + (mapc #'(lambda (e) + (unless (and (find (elt (send e :vertices) 0) vec) (find (elt (send e :vertices) 1) vec)) + (send *viewer* :viewsurface :3d-line + (elt (send e :vertices) 0) + (elt (send e :vertices) 1) + :depth-test t))) + (send hull :edges)) + (mapc #'(lambda (v e) + (send *viewer* :viewsurface :3d-line + e + (v+ e (scale 1e6 v)) + :depth-test t)) + vec + ext) + (send *viewer* :flush) + ) + (progn + (send *viewer* :viewsurface :line-width pwidth) + (send *viewer* :viewsurface :point-size psize) + (send *viewer* :viewsurface :color pcolor))) + ) + ) + ) + +(defun setup () + (unless (boundp '*robot*) + (load "package://eus_qp/euslisp/contact-optimization.l") + (load "irteus/demo/sample-robot-model.l") + (setq *robot* (instance sample-robot :init)) + (send *robot* :make-support-polygons) + (dolist (l (list :rleg :lleg)) + (send (send *robot* l :end-coords) :put :contact-constraint + (instance* default-contact-constraint + :init + :name l + :mu-margin-ratio 1.0 :cop-margin-ratio 1.0 + :mu-trans 0.5 + :mu-rot 0.05 + (let* ((vs (mapcar #'(lambda (v) (send (send *robot* l :end-coords) :inverse-transform-vector v)) (send (send *robot* :support-polygon l) :vertices)))) + (list :l-min-x (elt (find-extream vs #'(lambda (v) (elt v 0)) #'<) 0) + :l-max-x (elt (find-extream vs #'(lambda (v) (elt v 0)) #'>) 0) + :l-min-y (elt (find-extream vs #'(lambda (v) (elt v 1)) #'<) 1) + :l-max-y (elt (find-extream vs #'(lambda (v) (elt v 1)) #'>) 1) + ))) + )) + ) + + (unless (boundp '*irtviewer*) + (make-irtviewer)) + (objects (list *robot*)) + ) + +(defun calc-scfr (&optional (limbs (list :rleg :lleg))) + (let* ((mg (* 1e-6 (send *robot* :weight) (elt *g-vec* 2))) + (A (apply + #'concatenate-matrix-diagonal + (mapcar #'(lambda (l) + (send (send (send *robot* l :end-coords) :get :contact-constraint) :calc-constraint-matrix (send (send *robot* l :end-coords) :worldcoords))) + limbs))) + (b (v- (apply + #'concatenate + float-vector + (mapcar #'(lambda (l) + (send (send (send *robot* l :end-coords) :get :contact-constraint) :get-constraint-vector)) + limbs)))) + (G (send *robot* :calc-grasp-matrix + (mapcar #'(lambda (l) + (send (send *robot* l :end-coords) :worldpos)) + limbs))) + (total-wrench (v- (float-vector 0 + 0 + mg + 0 + 0 + 0))) + (A2 (concatenate-matrix-row A (make-matrix (array-dimension A 0) 2))) + (G2 (concatenate-matrix-row G (matrix (float-vector 0 0) + (float-vector 0 0) + (float-vector 0 0) + (float-vector 0 (- mg)) + (float-vector mg 0) + (float-vector 0 0)))) + (ret (cddlib-H-to-V :A_ineq A2 :b_ineq b :A_eq G2 :b_eq total-wrench)) + (hoge (print "############")) + (Vc (block-matrix (elt ret 0) (- (array-dimension (elt ret 0) 0) 2) 0 2 (array-dimension (elt ret 0) 1))) + (Rc_nonneg (block-matrix (elt ret 1) (- (array-dimension (elt ret 1) 0) 2) 0 2 (array-dimension (elt ret 1) 1))) + (Rc_free (block-matrix (elt ret 2) (- (array-dimension (elt ret 2) 0) 2) 0 2 (array-dimension (elt ret 2) 1))) + (ret2 (cddlib-V-to-H :V Vc :R_nonneg Rc_nonneg)) + (A_eq (elt ret2 0)) + (b_eq (elt ret2 1)) + (A_ineq (elt ret2 2)) + (b_ineq (elt ret2 3)) + (ret3 (cddlib-H-to-V :A_ineq A_ineq :b_ineq b_ineq :A_eq A_eq :b_eq b_eq)) + (V (elt ret3 0)) + (R_nonneg (elt ret3 1)) + (R_free (elt ret3 2)) + ) + (format t "equality: (=0)~%") + (dotimes (i (array-dimension A_eq 0)) + (format-array (float-vector (aref A_eq i 0) (aref A_eq i 1) (elt b_eq i)))) + (format t "inequality: (>=0)~%") + (dotimes (i (array-dimension A_ineq 0)) + (format-array (float-vector (aref A_ineq i 0) (aref A_ineq i 1) (elt b_ineq i)))) + (dolist (l limbs) + (send (send (send *robot* l :end-coords) :get :contact-constraint) :draw-on :flush nil)) + (draw-region V R_nonneg R_free (elt (send *robot* :centroid) 2)) + ) + ) + + +(defun test3 () + (send *robot* :reset-pose) + (send *robot* :fix-leg-to-coords (make-coords)) + (send *irtviewer* :draw-objects) + (calc-scfr (list :rleg :lleg))) + +(defun test4 () + (send *robot* :reset-pose) + (send *robot* :fix-leg-to-coords (make-coords)) + (send *robot* :lleg :move-end-pos #f(-100 0 50) :world) + (send *robot* :rleg :move-end-rot -40 :y) + (send *robot* :lleg :move-end-rot 40 :y) + (send *robot* :rleg :move-end-rot -10 :x) + (send *robot* :lleg :move-end-rot 10 :x) + (send *robot* :rleg :move-end-rot -5 :z) + (send *robot* :lleg :move-end-rot 5 :z) + (send *irtviewer* :draw-objects) + (calc-scfr (list :rleg :lleg))) + +(defun test5 () + (send *robot* :reset-pose) + (send *robot* :fix-leg-to-coords (make-coords)) + (send *robot* :lleg :crotch-p :joint-angle -100) + (send *robot* :lleg :crotch-y :joint-angle -30) + (send *irtviewer* :draw-objects) + (calc-scfr (list :rleg :lleg))) + +(defun test6 () + (send *robot* :reset-pose) + (send *robot* :fix-leg-to-coords (make-coords)) + (send *robot* :lleg :crotch-p :joint-angle -90) + (send *robot* :lleg :knee-p :joint-angle 30) + (send *robot* :lleg :ankle-p :joint-angle 60) + (send *robot* :rleg :crotch-p :joint-angle 90) + (send *robot* :rleg :knee-p :joint-angle 30) + (send *robot* :rleg :ankle-p :joint-angle 60) + (send *irtviewer* :draw-objects) + (calc-scfr (list :rleg :lleg))) + + +(defun test7 () + (send *robot* :reset-pose) + (send *robot* :fix-leg-to-coords (make-coords)) + (send *robot* :lleg :crotch-p :joint-angle -80) + (send *robot* :rleg :crotch-p :joint-angle 80) + (send *irtviewer* :draw-objects) + (calc-scfr (list :rleg :lleg))) + + diff --git a/eus_cddlib/src/eus_cddlib.c b/eus_cddlib/src/eus_cddlib.c index 5f1ec4929..c1f62e68b 100644 --- a/eus_cddlib/src/eus_cddlib.c +++ b/eus_cddlib/src/eus_cddlib.c @@ -9,7 +9,8 @@ double* b_eq_cash = NULL; double* A_ineq_cash = NULL; double* b_ineq_cash = NULL; double* V_cash = NULL; -double* R_cash = NULL; +double* R_nonneg_cash = NULL; +double* R_free_cash = NULL; int cddlib_initialize (void){ dd_set_global_constants(); /* First, this must be called to use cddlib. */ @@ -25,17 +26,18 @@ int cddlib_finalize (void){ A_eq x + b_eq = 0 A_ineq x + b_ineq >= 0 */ -int cddlib_H_to_V (double* A_eq, double* b_eq, double* A_ineq, double* b_ineq, int d, int m_eq, int m_ineq, int* out_n, int* out_s, int verbose) +int cddlib_H_to_V (double* A_eq, double* b_eq, double* A_ineq, double* b_ineq, int d, int m_eq, int m_ineq, int* out_n, int* out_s_nonneq, int* out_s_free, int verbose) { free(V_cash); V_cash = NULL; - free(R_cash); - R_cash = NULL; + free(R_nonneg_cash); + R_nonneg_cash = NULL; + free(R_free_cash); + R_free_cash = NULL; dd_PolyhedraPtr poly; dd_MatrixPtr A, G; dd_ErrorType err; - A=dd_CreateMatrix(m_eq + m_ineq,d+1); for (size_t i = 0; i < m_eq; i++){ set_addelem(A->linset,i+1); @@ -60,23 +62,36 @@ int cddlib_H_to_V (double* A_eq, double* b_eq, double* A_ineq, double* b_ineq, i dd_WriteMatrix(stdout,G); } int n = 0; - int s = 0; + int s_nonneg = 0; + int s_free = 0; for (size_t i = 0; i < G->rowsize; i++){ - if (dd_get_d(G->matrix[i][0]) ==0) s++; - else n++; + if (dd_get_d(G->matrix[i][0]) ==0){ + if (set_member(i+1,G->linset)) s_free++; + else s_nonneg++; + }else n++; } *out_n = n; - *out_s = s; + *out_s_nonneq = s_nonneg; + *out_s_free = s_free; int _n = 0; - int _s = 0; + int _s_nonneg = 0; + int _s_free = 0; V_cash = (double*)malloc(sizeof(double)*n*d); - R_cash = (double*)malloc(sizeof(double)*s*d); + R_nonneg_cash = (double*)malloc(sizeof(double)*s_nonneg*d); + R_free_cash = (double*)malloc(sizeof(double)*s_free*d); for (size_t i = 0; i < G->rowsize; i++){ if (dd_get_d(G->matrix[i][0])==0){ - for (size_t j = 0; j < d; j++){ - R_cash[s*j+_s] = dd_get_d(G->matrix[i][1+j]); + if (set_member(i+1,G->linset)){ + for (size_t j = 0; j < d; j++){ + R_free_cash[s_free*j+_s_free] = dd_get_d(G->matrix[i][1+j]); + } + _s_free++; + }else{ + for (size_t j = 0; j < d; j++){ + R_nonneg_cash[s_nonneg*j+_s_nonneg] = dd_get_d(G->matrix[i][1+j]); + } + _s_nonneg++; } - _s++; }else{ for (size_t j = 0; j < d; j++){ V_cash[n*j+_n] = dd_get_d(G->matrix[i][1+j]); @@ -84,29 +99,33 @@ int cddlib_H_to_V (double* A_eq, double* b_eq, double* A_ineq, double* b_ineq, i _n++; } } - dd_FreeMatrix(A); dd_FreeMatrix(G); dd_FreePolyhedra(poly); return 0; }else{ dd_WriteErrorMessages(stdout,err); + dd_FreeMatrix(A); + dd_FreePolyhedra(poly); } return -1; } -int cddlib_get_V (double* V, double* R, int d, int n, int s) +int cddlib_get_V (double* V, double* R_nonneg, double* R_free, int d, int n, int s_nonneg, int s_free) { for(size_t i=0; i < d*n; i++){ V[i] = V_cash[i]; } - for(size_t i=0; i < d*s; i++){ - R[i] = R_cash[i]; + for(size_t i=0; i < d*s_nonneg; i++){ + R_nonneg[i] = R_nonneg_cash[i]; + } + for(size_t i=0; i < d*s_free; i++){ + R_free[i] = R_free_cash[i]; } return 0; } -int cddlib_V_to_H (double* V, double* R, int d, int n, int s, int* out_m_eq, int* out_m_ineq, int verbose) +int cddlib_V_to_H (double* V, double* R_nonneg, double* R_free, int d, int n, int s_nonneg, int s_free, int* out_m_eq, int* out_m_ineq, int verbose) { free(A_eq_cash); A_eq_cash = NULL; @@ -121,17 +140,24 @@ int cddlib_V_to_H (double* V, double* R, int d, int n, int s, int* out_m_eq, int dd_MatrixPtr A, G; dd_ErrorType err; - G=dd_CreateMatrix(n+s,d+1); + G=dd_CreateMatrix(n+s_nonneg+s_free,d+1); for (size_t i = 0; i < n; i++){ dd_set_d(G->matrix[i][0],1); for (size_t j = 0; j < d; j++){ dd_set_d(G->matrix[i][j+1],V[j*n+i]); } } - for (size_t i = 0; i < s; i++){ + for (size_t i = 0; i < s_nonneg; i++){ dd_set_d(G->matrix[n+i][0],0); for (size_t j = 0; j < d; j++){ - dd_set_d(G->matrix[n+i][j+1],R[j*s+i]); + dd_set_d(G->matrix[n+i][j+1],R_nonneg[j*s_nonneg+i]); + } + } + for (size_t i = 0; i < s_free; i++){ + set_addelem(G->linset,n+s_nonneg+i+1); + dd_set_d(G->matrix[n+s_nonneg+i][0],0); + for (size_t j = 0; j < d; j++){ + dd_set_d(G->matrix[n+s_nonneg+i][j+1],R_free[j*s_free+i]); } } G->representation=dd_Generator; @@ -179,6 +205,8 @@ int cddlib_V_to_H (double* V, double* R, int d, int n, int s, int* out_m_eq, int return 0; }else{ dd_WriteErrorMessages(stdout,err); + dd_FreeMatrix(G); + dd_FreePolyhedra(poly); } return -1; } From 58ca37d6ad103ed48a42d5d23e54e3301820b843 Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Sat, 1 Feb 2020 18:05:11 +0900 Subject: [PATCH 5/6] fix --- eus_cddlib/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/eus_cddlib/package.xml b/eus_cddlib/package.xml index 58718e162..2290ea484 100644 --- a/eus_cddlib/package.xml +++ b/eus_cddlib/package.xml @@ -10,7 +10,7 @@ catkin - cdd + libcdd-dev euslisp libcdd-dev From b07c6e0cec36321729da4eb6495c04bb4b95046e Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Mon, 3 Feb 2020 22:08:43 +0900 Subject: [PATCH 6/6] use gmp rational --- eus_cddlib/CMakeLists.txt | 4 +- eus_cddlib/src/eus_cddlib.c | 176 +++++++++++++++++++++++++++++++++--- 2 files changed, 164 insertions(+), 16 deletions(-) diff --git a/eus_cddlib/CMakeLists.txt b/eus_cddlib/CMakeLists.txt index d9d7feb0d..86e21cf53 100644 --- a/eus_cddlib/CMakeLists.txt +++ b/eus_cddlib/CMakeLists.txt @@ -6,10 +6,10 @@ find_package(catkin REQUIRED COMPONENTS rostest) catkin_package() +add_definitions(-DGMPRATIONAL) add_library(eus_cddlib SHARED src/eus_cddlib.c) set_target_properties(eus_cddlib PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib) -target_link_libraries(eus_cddlib cdd) - +target_link_libraries(eus_cddlib cddgmp) install(DIRECTORY lib/ USE_SOURCE_PERMISSIONS DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) diff --git a/eus_cddlib/src/eus_cddlib.c b/eus_cddlib/src/eus_cddlib.c index c1f62e68b..da219d7f5 100644 --- a/eus_cddlib/src/eus_cddlib.c +++ b/eus_cddlib/src/eus_cddlib.c @@ -3,6 +3,10 @@ #include #include +#include + +#define digits1 5 +#define digits2 3 double* A_eq_cash = NULL; double* b_eq_cash = NULL; @@ -22,6 +26,43 @@ int cddlib_finalize (void){ return 0; } +int max(int a, int b) { + if (a > b) + return a; + else + return b; +} + +int min(int a, int b) { + if (a < b) + return a; + else + return b; +} + +void set_value (mytype x, double value, int num, int m){ + if (value != 0){ + unsigned long int denom = pow(10, m); + int numer = value * denom; + + int over = min(m,max(0,((int)(log10(fabs(numer))) - num))); + denom /= pow(10,over); + numer /= pow(10,over); + + while(denom>1 && numer!=0){ + if((numer / 10)*10 != numer) break; + else{ + denom /= 10; + numer /= 10; + } + } + dd_set_si2(x, numer, denom); + }else{ + dd_set_si2(x, 0, 1); + } + return; +} + /* A_eq x + b_eq = 0 A_ineq x + b_ineq >= 0 @@ -38,22 +79,70 @@ int cddlib_H_to_V (double* A_eq, double* b_eq, double* A_ineq, double* b_ineq, i dd_PolyhedraPtr poly; dd_MatrixPtr A, G; dd_ErrorType err; - A=dd_CreateMatrix(m_eq + m_ineq,d+1); + + + double maxabs = 0; for (size_t i = 0; i < m_eq; i++){ - set_addelem(A->linset,i+1); - dd_set_d(A->matrix[i][0],b_eq[i]); + if (fabs(b_eq[i]) > maxabs) maxabs = fabs(b_eq[i]); for (size_t j = 0; j < d; j++){ - dd_set_d(A->matrix[i][j+1],A_eq[i*d+j]); + if (fabs(A_eq[i*d+j]) > maxabs) maxabs = fabs(A_eq[i*d+j]); } } for (size_t i = 0; i < m_ineq; i++){ - dd_set_d(A->matrix[m_eq+i][0],b_ineq[i]); + if (fabs(b_ineq[i]) > maxabs) maxabs = fabs(b_ineq[i]); for (size_t j = 0; j < d; j++){ - dd_set_d(A->matrix[m_eq+i][j+1],A_ineq[i*d+j]); + if (fabs(A_ineq[i*d+j]) > maxabs) maxabs = fabs(A_ineq[i*d+j]); + } + } + int denom = (maxabs!=0)? max(- ((int)(log10(fabs(maxabs))) - digits1), 0) : 0; + + + ddf_MatrixPtr Af; + Af=ddf_CreateMatrix(m_eq + m_ineq,d+1); + + for (size_t i = 0; i < m_eq; i++){ + set_addelem(Af->linset,i+1); + + ddf_set_d(Af->matrix[i][0], b_eq[i]); + for (size_t j = 0; j < d; j++){ + ddf_set_d(Af->matrix[i][j+1], A_eq[i*d+j]); + } + } + for (size_t i = 0; i < m_ineq; i++){ + ddf_set_d(Af->matrix[m_eq+i][0], b_ineq[i]); + for (size_t j = 0; j < d; j++){ + ddf_set_d(Af->matrix[m_eq+i][j+1], A_ineq[i*d+j]); + } + } + + Af->representation=dd_Inequality; + + ddf_rowindex newpos; + ddf_rowset impl_linset,redset; + ddf_ErrorType errf; + if (verbose){ + printf("\nbefore canonicalize:\n"); + ddf_WriteMatrix(stdout,Af); printf("\n"); + } + + ddf_MatrixCanonicalize(&Af, &impl_linset, &redset, &newpos, &errf); + if (verbose){ + printf("\nafter canonicalize:\n"); + ddf_WriteMatrix(stdout,Af); printf("\n"); + } + + A=dd_CreateMatrix(Af->rowsize,Af->colsize); + for (size_t i = 0; i < Af->rowsize; i++){ + if(set_member(i+1,Af->linset)) set_addelem(A->linset,i+1); + + for (size_t j = 0; j < Af->colsize; j++){ + set_value(A->matrix[i][j], ddf_get_d(Af->matrix[i][j]), digits2,denom); } } A->representation=dd_Inequality; + poly=dd_DDMatrix2Poly(A, &err); /* compute the second (generator) representation */ + if (err==dd_NoError){ G=dd_CopyGenerators(poly); if (verbose){ @@ -104,6 +193,10 @@ int cddlib_H_to_V (double* A_eq, double* b_eq, double* A_ineq, double* b_ineq, i dd_FreePolyhedra(poly); return 0; }else{ + if (verbose){ + printf("\nInput is H-representation:\n"); + dd_WriteMatrix(stdout,A); + } dd_WriteErrorMessages(stdout,err); dd_FreeMatrix(A); dd_FreePolyhedra(poly); @@ -140,28 +233,79 @@ int cddlib_V_to_H (double* V, double* R_nonneg, double* R_free, int d, int n, in dd_MatrixPtr A, G; dd_ErrorType err; - G=dd_CreateMatrix(n+s_nonneg+s_free,d+1); + double maxabs = 0; + for (size_t i = 0; i < n; i++){ + for (size_t j = 0; j < d; j++){ + if (fabs(V[j*n+i]) > maxabs) maxabs = fabs(V[j*n+i]); + } + } + for (size_t i = 0; i < s_nonneg; i++){ + for (size_t j = 0; j < d; j++){ + if (fabs(R_nonneg[j*s_nonneg+i]) > maxabs) maxabs = fabs(R_nonneg[j*s_nonneg+i]); + } + } + for (size_t i = 0; i < s_free; i++){ + for (size_t j = 0; j < d; j++){ + if (fabs(R_free[j*s_free+i]) > maxabs) maxabs = fabs(R_free[j*s_free+i]); + } + } + int denom = (maxabs!=0)? max(- ((int)(log10(fabs(maxabs))) - digits1), 0) : 0; + + ddf_MatrixPtr Gf; + Gf=ddf_CreateMatrix(n+s_nonneg+s_free,d+1); + for (size_t i = 0; i < n; i++){ - dd_set_d(G->matrix[i][0],1); + ddf_set_si(Gf->matrix[i][0],1); for (size_t j = 0; j < d; j++){ - dd_set_d(G->matrix[i][j+1],V[j*n+i]); + ddf_set_d(Gf->matrix[i][j+1],V[j*n+i]); } } for (size_t i = 0; i < s_nonneg; i++){ - dd_set_d(G->matrix[n+i][0],0); + ddf_set_si(Gf->matrix[n+i][0],0); for (size_t j = 0; j < d; j++){ - dd_set_d(G->matrix[n+i][j+1],R_nonneg[j*s_nonneg+i]); + ddf_set_d(Gf->matrix[n+i][j+1],R_nonneg[j*s_nonneg+i]); } } for (size_t i = 0; i < s_free; i++){ - set_addelem(G->linset,n+s_nonneg+i+1); - dd_set_d(G->matrix[n+s_nonneg+i][0],0); + set_addelem(Gf->linset,n+s_nonneg+i+1); + ddf_set_si(Gf->matrix[n+s_nonneg+i][0],0); for (size_t j = 0; j < d; j++){ - dd_set_d(G->matrix[n+s_nonneg+i][j+1],R_free[j*s_free+i]); + ddf_set_d(Gf->matrix[n+s_nonneg+i][j+1],R_free[j*s_free+i]); + } + } + + Gf->representation=dd_Generator; + + ddf_rowindex newpos; + ddf_rowset impl_linset,redset; + ddf_ErrorType errf; + if (verbose){ + printf("\nbefore canonicalize:\n"); + ddf_WriteMatrix(stdout,Gf); printf("\n"); + } + + ddf_MatrixCanonicalize(&Gf, &impl_linset, &redset, &newpos, &errf); + if (verbose){ + printf("\nafter canonicalize:\n"); + ddf_WriteMatrix(stdout,Gf); printf("\n"); + } + + G=dd_CreateMatrix(Gf->rowsize,Gf->colsize); + for (size_t i = 0; i < Gf->rowsize; i++){ + if(set_member(i+1,Gf->linset)) set_addelem(G->linset,i+1); + + dd_set_si(G->matrix[i][0], ddf_get_d(Gf->matrix[i][0])); + for (size_t j = 0; j < Gf->colsize-1; j++){ + double value = ddf_get_d(Gf->matrix[i][j+1]); + set_value(G->matrix[i][j+1], ddf_get_d(Gf->matrix[i][j+1]), digits2, denom); } } + G->representation=dd_Generator; + + poly=dd_DDMatrix2Poly(G, &err); /* compute the second (generator) representation */ + if (err==dd_NoError){ A=dd_CopyInequalities(poly); if (verbose){ @@ -204,6 +348,10 @@ int cddlib_V_to_H (double* V, double* R_nonneg, double* R_free, int d, int n, in dd_FreePolyhedra(poly); return 0; }else{ + if (verbose){ + printf("\nInput is V-representation:\n"); + dd_WriteMatrix(stdout,G); + } dd_WriteErrorMessages(stdout,err); dd_FreeMatrix(G); dd_FreePolyhedra(poly);