Commit af3ae857 authored by Steve T's avatar Steve T
Browse files

recomputed relative positions to avoid foot overlapping

parent 600a07f6
#!/bin/bash
cd /media/data/dev/linux/hpp/src/anymal-rbprm/script/relative_foot_positions;
./run.sh relativeFootPositionQuasiFlat.py
/media/data/blender-2.82-linux64/blender --background --python reduce.py
for f in ./output/*red.obj; do mv "$f" "${f%.obj}uced.obj"; done
cp ./output/* /media/data/dev/linux/hpp/src/sl1m/stand_alone_scenarios/constraints_files/anymal/
cd -
......@@ -19,7 +19,8 @@ def decimate(obj):
return
def load_obj(file):
bpy.ops.import_scene.obj(filepath=FOLDER_PATH+file, axis_forward='X', axis_up='Z')
# ~ bpy.ops.import_scene.obj(filepath=FOLDER_PATH+file, axis_forward='X', axis_up='Z')
bpy.ops.import_scene.obj(filepath=FOLDER_PATH+file)
obj = bpy.data.objects[-1]
......@@ -41,10 +42,11 @@ def load_obj(file):
use_mesh_modifiers=True, use_edges=True,
use_smooth_groups=False, use_smooth_groups_bitflags=False,
use_normals=True, use_uvs=True, use_materials=False,
use_triangles=True, use_nurbs=False,
use_triangles=False, use_nurbs=False,
use_vertex_groups=False, use_blen_objects=True,
group_by_object=False, group_by_material=False, keep_vertex_order=False,
global_scale=1.0, path_mode='AUTO', axis_forward='X', axis_up='Z')
# ~ global_scale=1.0, path_mode='AUTO', axis_forward='X', axis_up='Z')
global_scale=1.0, path_mode='AUTO')
#delete all objects
bpy.ops.object.delete()
......@@ -62,4 +64,4 @@ if not os.path.exists(directory):
#clear the scene !
for file in glob.glob("*.obj"):
load_obj(file)
\ No newline at end of file
load_obj(file)
......@@ -14,7 +14,8 @@ from pinocchio import Quaternion
NUM_SAMPLES = 18000
IT_DISPLAY_PROGRESS = NUM_SAMPLES / 10
MIN_DIST_BETWEEN_FEET_Y = 0.18
MIN_DIST_BETWEEN_FEET_Y = 0.10
MIN_DIST_BETWEEN_FEET_X = 0.10
MAX_DIST_BETWEEN_FEET_X = 0.35
MAX_DIST_BETWEEN_FEET_Z = 0.35
MIN_HEIGHT_COM = 0.3
......@@ -94,8 +95,11 @@ def staticEq(positions, com):
E[:2,i] = pos[:2]
e = array([com[0], com[1], 1.])
E[2,:] = ones(sizeX)
res = linprog(ones(sizeX), A_ub=None, b_ub=None, A_eq=E, b_eq=e, bounds=[(0.,1.) for _ in range(sizeX)], method='interior-point', callback=None, options={'presolve': True})
return res['success']
try:
res = linprog(ones(sizeX), A_ub=None, b_ub=None, A_eq=E, b_eq=e, bounds=[(0.,1.) for _ in range(sizeX)], method='interior-point', callback=None, options={'presolve': True})
return res['success']
except:
return False
#returns true of one of the point is inside the convex hulls of the others. We do not want that
......@@ -106,8 +110,10 @@ def pointInsideHull(positions):
return True
return False
def genFlat():
def genFlat(init = False):
q = fullBody.shootRandomConfig()
if init:
q = fullBody.referenceConfig[::]
q[0:7] = zeroConf
fullBody.setCurrentConfig(q)
#~ v(q)
......@@ -153,7 +159,7 @@ def printFootPositionRelativeToOther(nbConfigs):
for i in range(0, nbConfigs):
if i > 0 and not i % IT_DISPLAY_PROGRESS:
print(int((i * 100) / nbConfigs), " % done")
q, succ, s, pos = genFlat()
q, succ, s, pos = genFlat(i==0)
if succ:
global success
success += 1
......@@ -182,11 +188,17 @@ def printFootPositionRelativeToOther(nbConfigs):
# ~ rm[m, 3] = qEffector[m]
# ~ rm[3, 3] = 1
# ~ invrm = np.linalg.inv(rm)
# ~ p = invrm.dot([0, 0, 0., 1])
if (MAX_DIST_BETWEEN_FEET_Z > abs(p[2])):
points[j][oeffectorName].append(p[:3])
# ~ p = invrm.dot([0, 0, 0., 1])
if (MAX_DIST_BETWEEN_FEET_Z > abs(p[2])):
if (MIN_DIST_BETWEEN_FEET_Y <= abs(p[1])):
if (MIN_DIST_BETWEEN_FEET_X <= abs(p[0])): #this is not what we want to do in theory but it works well in fact
points[j][oeffectorName].append(p[:3])
else:
addCom = False
else:
addCom = False
else:
# ~ print ('rejecting ',effectorName, ' ', oeffectorName , p, abs(p[2]))
print ('rejecting ',effectorName, ' ', oeffectorName , p, abs(p[2]))
# ~ print ('pos_other', pos_other)
# ~ print ('old_pos', old_pos)
addCom = False
......@@ -201,6 +213,7 @@ def printFootPositionRelativeToOther(nbConfigs):
fullBody.setCurrentConfig(q)
com = array(fullBody.getCenterOfMass())
print ('com ', com)
# ~ for x in range(0, 3):
# ~ q[x] = -com[x]
for j, effectorName in enumerate(effectors):
......@@ -224,6 +237,7 @@ def printFootPositionRelativeToOther(nbConfigs):
if (rp[2] < MIN_HEIGHT_COM):
addCom = False
print ("reject min heught")
if addCom:
compoints[j].append(rp)
# ~ if j == 1:
......
Markdown is supported
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