Commit 1a63e2c0 authored by stevet's avatar stevet
Browse files

add projection method on plane

parent c6ca9ec9
......@@ -42,6 +42,34 @@ def load_obj(filename) :
fh.close()
return ObjectData(V, T, N, F)
#find a point such that ax + by + cz = d that is the closest to the origin
def find_point_on_plane(a,b,c,d):
#project 0 to the plane
m = np.zeros((4,4))
m[:3,:3] = np.identity(3)
m[:,3] = [-a, -b, -c, 0]
m[3,:3] = [a,b,c]
n = np.zeros(4); n[-1] = d
res = np.linalg.inv(m).dot(n)[:-1]
return res
def inequalities_to_Inequalities_object(A,b):
nrows = A.shape[0]
V = np.ones([nrows, 4])
N = np.empty([nrows, 3])
i = 0
for ai, bi in zip(A,b):
N[i,:] = ai
V[i,:3] = find_point_on_plane(ai[0],ai[1],ai[2],bi)
i+=1
return Inequalities(A,b, N, V)
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
......@@ -112,9 +140,11 @@ def test_inequality():
print("test_inequality successful")
def __gen_data():
obj = load_obj('./hrp2/RL_com._reduced.obj')
import os
filepath = os.environ['DEVEL_HPP_DIR'] + '/install/share/hrp2-rbprm/com_inequalities/RLEG_JOINT0_com_constraints.obj'
obj = load_obj(filepath)
ineq = as_inequalities(obj)
ok_points = [[0,0,0], [0.0813, 0.0974, 0.2326], [-0.3387, 0.1271, -0.5354]]
ok_points = [[0,0,0], [0.0813, 0.0974, 0.2326]]
not_ok_points = [[-0.3399, 0.2478, -0.722],[-0.1385,-0.4401,-0.1071]]
return obj, ineq, ok_points, not_ok_points
......@@ -146,6 +176,26 @@ def test_rotate_inequalities():
assert (not is_inside(ineq, p)), "point " + str(p) + " should NOT be inside object"
print("test_rotate_inequalities successful")
def test_inequalities_to_Inequalities_object():
data = __gen_data()
ineq = data[1]
tr = np.array([[ 1. , 0. , 0. , 0. ],
[ 0. , 0.98006658, -0.19866933, 2. ],
[ 0. , 0.19866933, 0.98006658, 0. ],
[ 0. , 0. , 0. , 1. ]])
ok_points = [tr.dot(np.array(el + [1]))[0:3] for el in data[2]]
not_ok_points = [tr.dot(np.array(el + [1]))[0:3] for el in data[3]]
ineq2 = inequalities_to_Inequalities_object(ineq.A, ineq.b)
ineq2 = rotate_inequalities(ineq2, tr)
for p in ok_points:
assert (is_inside(ineq2, p)), "point " + str(p) + " should be inside object"
for p in not_ok_points:
assert (not is_inside(ineq2, p)), "point " + str(p) + " should NOT be inside object"
print("test_inequalities_to_Inequalities_object successful")
def load_obj_and_save_ineq(in_name, out_name):
obj = load_obj(in_name)
......@@ -156,3 +206,9 @@ def load_obj_and_save_ineq(in_name, out_name):
#~ load_obj_and_save_ineq('./spiderman/RA_com_reduced.obj','./spiderman/RA_com.ineq')
#~ load_obj_and_save_ineq('./spiderman/LL_com_reduced.obj','./spiderman/LL_com.ineq')
#~ load_obj_and_save_ineq('./spiderman/RL_com_reduced.obj','./spiderman/RL_com.ineq')
if __name__ == '__main__':
test_inequality()
test_belonging()
test_rotate_inequalities()
test_inequalities_to_Inequalities_object()
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment