diff --git a/hpp-rbprm-robot-data/Makefile b/hpp-rbprm-robot-data/Makefile index 3a8698702b14358a770ff636ae0218d39dd413a2..1a2763963338437d8323706f486a8c536d1e6b2c 100644 --- a/hpp-rbprm-robot-data/Makefile +++ b/hpp-rbprm-robot-data/Makefile @@ -4,6 +4,7 @@ HPP_PACKAGE= hpp-rbprm-robot-data HPP_COMMENT= file database +PKGREVISION= 1 CATEGORIES= wip diff --git a/hpp-rbprm-robot-data/distinfo b/hpp-rbprm-robot-data/distinfo index 214122495952cf4b36a1e42f64e5065c311e9d29..6c4d950111b9844ede39f8d1c61a17f21dd616f7 100644 --- a/hpp-rbprm-robot-data/distinfo +++ b/hpp-rbprm-robot-data/distinfo @@ -1,3 +1,4 @@ SHA1 (hpp-rbprm-robot-data-4.7.0.tar.gz) = 9234a2d94927349b35866213dd9b28e7e77f7569 RMD160 (hpp-rbprm-robot-data-4.7.0.tar.gz) = 1c010b33843edbf6bba0d7ca77d474202638e115 Size (hpp-rbprm-robot-data-4.7.0.tar.gz) = 21669530 bytes +SHA1 (patch-aa) = fdbd10d448de7369f7ce3bbfb1298753662e0d82 diff --git a/hpp-rbprm-robot-data/patches/patch-aa b/hpp-rbprm-robot-data/patches/patch-aa new file mode 100644 index 0000000000000000000000000000000000000000..75b5a7e3b49c5afa762d926286721a0b292f37d2 --- /dev/null +++ b/hpp-rbprm-robot-data/patches/patch-aa @@ -0,0 +1,115 @@ +--- data/hyq/com_inequalities/obj_to_constraints.py.orig 2019-09-25 18:40:13.000000000 +0200 ++++ data/hyq/com_inequalities/obj_to_constraints.py 2019-10-05 08:12:48.223720636 +0200 +@@ -1,15 +1,20 @@ + #do the loading of the obj file +-import numpy as np + from collections import namedtuple ++from pickle import dump, load ++ ++import numpy as np ++ + ObjectData = namedtuple("ObjectData", "V T N F") + Inequalities = namedtuple("Inequality", "A b N V") + ++ + def toFloat(stringArray): + res= np.zeros(len(stringArray)) + for i in range(0,len(stringArray)): + res[i] = float(stringArray[i]) + return res + ++ + def load_obj(filename) : + V = [] #vertex + T = [] #texcoords +@@ -40,12 +45,14 @@ + + return ObjectData(V, T, N, F) + ++ + def inequality(v, n): + #the plan has for equation ax + by + cz = d, with a b c coordinates of the normal + #inequality is then ax + by +cz -d <= 0 + # last var is v because we need it + return [n[0], n[1], n[2], np.array(v).dot(np.array(n))] + ++ + def as_inequalities(obj): + #for each face, find first three points and deduce plane + #inequality is given by normal +@@ -65,11 +72,14 @@ + N[f,:] = n + return Inequalities(A,b, N, V) + ++ + def is_inside(inequalities, pt): + return ((inequalities.A.dot(pt) - inequalities.b) < 0).all() + ++ + #~ def rotate_inequalities_q(): + ++ + # TODO this is naive, should be a way to simply update d + def rotate_inequalities(ineq, transform): + #for each face, find first three points and deduce plane +@@ -88,19 +98,20 @@ + N[i,:] = n + return Inequalities(A,b, N, V) + +-from pickle import dump ++ + def ineq_to_file(ineq, filename): + f1=open(filename, 'w+') + res = { 'A' : ineq.A, 'b' : ineq.b, 'N' : ineq.N, 'V' : ineq.V} + dump(res, f1) + f1.close() + +-from pickle import load ++ + def ineq_from_file(filename): + f1=open(filename, 'r') + res = load(f1) + return Inequalities(res['A'], res['b'],res['N'],res['V']) + ++ + def test_inequality(): + n = np.array([0,-1,0]) + v = np.array([0,1,1]) +@@ -109,6 +120,7 @@ + else: + print("test_inequality successful") + ++ + def __gen_data(): + obj = load_obj('./hrp2/RL_com._reduced.obj') + ineq = as_inequalities(obj) +@@ -116,6 +128,7 @@ + not_ok_points = [[-0.3399, 0.2478, -0.722],[-0.1385,-0.4401,-0.1071]] + return obj, ineq, ok_points, not_ok_points + ++ + def test_belonging(): + data = __gen_data() + ineq = data[1] +@@ -127,11 +140,10 @@ + assert (not is_inside(ineq, np.array(p))), "point " + str(p) + " should NOT be inside object" + print("test_belonging successful") + ++ + def test_rotate_inequalities(): + +- tr = np.array([[ 1. , 0. , 0. , 0. ], +- [ 0. , 0.98006658, -0.19866933, 2. ], +- [ 0. , 0.19866933, 0.98006658, 0. ], ++ tr = np.array([[1., 0., 0., 0.], [0., 0.98006658, -0.19866933, 2.], [0., 0.19866933, 0.98006658, 0.], + [ 0. , 0. , 0. , 1. ]]) + + data = __gen_data() +@@ -150,6 +162,7 @@ + ineq = as_inequalities(obj) + ineq_to_file (ineq, out_name) + ++ + load_obj_and_save_ineq('./lfleg_com_reduced.obj','./lfleg_com.ineq') + load_obj_and_save_ineq('./lhleg_com_reduced.obj','./lhleg_com.ineq') + load_obj_and_save_ineq('./rhleg_com_reduced.obj','./rhleg_com.ineq')