Newer
Older
# from numpy.linalg import norm
# import numpy as np
from numpy import array, zeros, ones
from scipy.spatial import ConvexHull
from scipy.optimize import linprog
from hpp.corbaserver import ProblemSolver
from hpp.corbaserver.rbprm.tools.display_tools import hull_to_obj, plt, plot_hull
# from plot_polytopes import *
# from pinocchio import Quaternion
NUM_SAMPLES = 18000
IT_DISPLAY_PROGRESS = NUM_SAMPLES / 10
MIN_DIST_BETWEEN_FEET_Y = 0.10
MIN_DIST_BETWEEN_FEET_X = 0.10
MAX_DIST_BETWEEN_FEET_Z = 0.35
# margin used to constrain the com y position : if it's on the left of the left foot
# or on the right of the right foot
# for more than this margin, we reject this sample:
MARGIN_FEET_SIDE = 0.05
fullBody.setJointBounds("LF_KFE", [-1.4, 0.0])
fullBody.setJointBounds("RF_KFE", [-1.4, 0.0])
fullBody.setJointBounds("LH_KFE", [0.0, 1.4])
fullBody.setJointBounds("RH_KFE", [0.0, 1.4])
fullBody.setJointBounds("root_joint", [-20, 20, -20, 20, -20, 20])
dict_heuristic = {
fullBody.rLegId: "static",
fullBody.lLegId: "static",
fullBody.rArmId: "fixedStep04",
}
fullBody.loadAllLimbs(dict_heuristic, "ReferenceConfiguration", nbSamples=12)
rLegId = fullBody.rLegId
rLeg = fullBody.rleg
rfoot = fullBody.rfoot
rLegOffset = fullBody.offset[:]
lLegOffset = fullBody.offset[:]
rArmOffset = fullBody.offset[:]
lArmOffset = fullBody.offset[:]
lLegId = fullBody.lLegId
fullBody.setCurrentConfig(q_0)
effectors = [
fullBody.rfoot,
fullBody.lfoot,
fullBody.rhand,
fullBody.lhand,
]
limbIds = [fullBody.rLegId, fullBody.lLegId, fullBody.rArmId, fullBody.lArmId]
offsets = [array(rLegOffset), array(lLegOffset), array(rArmOffset), array(lArmOffset)]
compoints = [[] for _ in effectors]
# compoints = [[[0.012471792486262121, 0.0015769611415203033, 0.8127583093263778]],
# [[0.012471792486262121, 0.0015769611415203033, 0.8127583093263778]]]
points = [{} for _ in effectors]
for i, eff in enumerate(effectors):
for j, otherEff in enumerate(effectors):
if i != j:
points[i][otherEff] = []
success = 0
fails = 0
def staticEq(positions, com):
sizeX = len(positions)
res = linprog(
ones(sizeX),
A_ub=None,
b_ub=None,
A_eq=E,
b_eq=e,
bounds=[(0.0, 1.0) for _ in range(sizeX)],
method="interior-point",
callback=None,
options={"presolve": True},
)
return res["success"]
"""
returns true of one of the point is inside the convex hulls of the others.
We do not want that
"""
if staticEq(others, pos):
return True
return False
def genFlat(init=False):
q = fullBody.shootRandomConfig()
if init:
q = fullBody.referenceConfig[::]
q[0:7] = zeroConf
fullBody.setCurrentConfig(q)
# v(q)
positions = [fullBody.getJointPosition(foot)[:3] for foot in effectors]
s = rbprmstate.State(fullBody, q=q, limbsIncontact=limbIds)
succ = True
for effId, pos in zip(limbIds, positions):
s, succ = state_alg.addNewContact(
s, effId, pos, [0.0, 0.0, 1.0], num_max_sample=0
)
if not succ:
break
# posrf = fullBody.getJointPosition(rfoot)[:3]
# poslf = fullBody.getJointPosition(lfoot)[:3]
# print ("limbsIds ", limbIds)
# s = rbprmstate.State(fullBody, q = q, limbsIncontact = limbIds)
# s, succ = state_alg.addNewContact(s, rLegId, posrf, [0.,0.,1.], num_max_sample= 0)
# s, succ = state_alg.addNewContact(s, lLegId, poslf, [0.,0.,1.], num_max_sample= 0)
# ~ succ = fullBody.isConfigValid(q)[0]
# and norm (array(posrf[:2]) - array(poslf[:2]) ) >= 0.3
succ = fullBody.isConfigValid(q)[0]
# assert that in static equilibrium
if succ:
succ = staticEq(positions, fullBody.getCenterOfMass())
if not succ:
v(q)
if succ:
succ = not pointInsideHull(positions)
if not succ:
print("************* contacts crossing", not succ)
v(q)
# if succ and norm (array(posrf[:2]) - array(poslf[:2]) ) <= 0.1:
# if succ and norm (array(posrf) - array(poslf) ) <= 0.1:
v(s.q())
return s.q(), succ, s, positions
def printFootPositionRelativeToOther(nbConfigs):
for i in range(0, nbConfigs):
if i > 0 and not i % IT_DISPLAY_PROGRESS:
print(int((i * 100) / nbConfigs), " % done")
if succ:
global success
success += 1
addCom = True
for j, effectorName in enumerate(effectors):
for otheridx, (oeffectorName, limbId) in enumerate(
zip(effectors, limbIds)
):
fullBody.setCurrentConfig(q)
pos_other = fullBody.getJointPosition(oeffectorName)
pos = fullBody.getJointPosition(effectorName)
p = array(pos_other[:3]) - array(pos[:3]).tolist()
# ~ qtr = q[:]
# ~ qtr[:3] = [qtr[0] - pos_other[0], qtr[1] - pos_other[1],
# ~ qtr[2] - pos_other[2]]
# ~ fullBody.setCurrentConfig(qtr)
# ~ qEffector = fullBody.getJointPosition(effectorName)
# ~ q0 = Quaternion(qEffector[6], qEffector[3], qEffector[4],
# qEffector[5])
# ~ rot = q0.matrix() # compute rotation matrix world -> local
# (0,0,0) coordinate expressed in effector fram
# ~ p = qEffector[0:3]
# ~ rm = np.zeros((4, 4))
# ~ for k in range(0, 3):
# ~ for m in range(0, 3):
# ~ rm[3, 3] = 1
# ~ invrm = np.linalg.inv(rm)
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
print(
"rejecting ",
effectorName,
" ",
oeffectorName,
p,
abs(p[2]),
)
# ~ print ('pos_other', pos_other)
# ~ print ('old_pos', old_pos)
addCom = False
v(q)
# ~ if (j == 0 and p[1] > MIN_DIST_BETWEEN_FEET_Y
# and abs(p[0]) < MAX_DIST_BETWEEN_FEET_X):
# ~ elif (j == 1 and p[1] < -MIN_DIST_BETWEEN_FEET_Y
# and abs(p[0]) < MAX_DIST_BETWEEN_FEET_X):
# now compute coms
fullBody.setCurrentConfig(q)
pos = fullBody.getJointPosition(effectorName)
rp = array(com) - array(pos[:3]).tolist()
# ~ qEffector = fullBody.getJointPosition(effectorName)
# ~ q0 = Quaternion(qEffector[6], qEffector[3], qEffector[4],
# qEffector[5])
# ~ rot = q0.matrix() # compute rotation matrix world -> local
# ~ p = qEffector[0:3] # (0,0,0) coordinate expressed in effector fram
# ~ rm = np.zeros((4, 4))
# ~ for k in range(0, 3):
# ~ rm[3, 3] = 1
# ~ invrm = np.linalg.inv(rm)
# ~ p = invrm.dot([0, 0, 0, 1])
# ~ # add offset
# ~ rp = array(p[:3] - offsets[j]).tolist()
if addCom:
compoints[j].append(rp)
# ~ if j == 1:
# ~ if rp[1] < MARGIN_FEET_SIDE:
# ~ compoints[j].append(rp)
# ~ if rp[1] > -MARGIN_FEET_SIDE:
# ~ compoints[j].append(rp)
else:
global fails
fails += 1
# print(fullBody.isConfigValid(q)[1])
# for j in range(0,len(limbIds)):
# f1=open('./'+str(limbIds[j])+'_com.erom', 'w+')
# for p in points[j]:
# f1.write(str(p[0]) + "," + str(p[1]) + "," + str(p[2]) + "\n")
# f1.close()
s = rbprmstate.State(
fullBody, q=fullBody.getCurrentConfig(), limbsIncontact=[fullBody.limbs_names[0]]
)
# printRootPosition(rLegId, rfoot, nbSamples)
# printRootPosition(lLegId, lfoot, nbSamples)
# printRootPosition(rarmId, rHand, nbSamples)
# printRootPosition(larmId, lHand, nbSamples)
printFootPositionRelativeToOther(6000)
print("successes ", success)
print("fails ", fails)
# ~ for effector, comData, pointsData in zip(effectors, compoints, points):
# ~ for effector, limbId, comData, pointsData in zip(effectors[:1],limbIds[1:],
# compoints[:1], points[:1]):
for effector, limbId, comData, pointsData in zip(effectors, limbIds, compoints, points):
hull_to_obj(
hcom,
comData,
"anymal_COM_constraints_in_" + str(limbId) + "_effector_frame_quasi_static.obj",
)
fig.suptitle(
"anymal_COM_constraints_in_" + str(limbId) + "_effector_frame_quasi_static.obj",
fontsize=16,
)
plot_hull(hcom, comData, array(comData), color="r", plot=False, fig=fig, ax=None)
fig.suptitle(str(limbId), fontsize=16)
# ~ axes = [221,222,223,224]
# ~ for (oEffector, pts), axId in zip(pointsData.items(), axes):
for oEffector, pts in pointsData.items():
# ~ ax = fig.add_subplot(axId, projection="3d")
hpts = ConvexHull(pts)
hull_to_obj(
hpts,
pts,
"anymal_" + str(oEffector) + "_constraints_in_" + str(limbId) + ".obj",
)
print("ax ", ax)
ax = plot_hull(hpts, pts, array(pts), color="b", plot=False, fig=fig, ax=ax)
print(
"effector ",
limbId,
)
print(
"oEffector ",
oEffector,
)
plt.show(block=False)
# ~ hcomRF = ConvexHull(compoints[0])
# ~ hcomLF = ConvexHull(compoints[1])
# ~ hull_to_obj(hcomRF,compoints[0],"anymal_COM_constraints_in_RF_effector_frame.obj")
# ~ hull_to_obj(hcomLF,compoints[1],"anymal_COM_constraints_in_LF_effector_frame.obj")
# ~ hptsRF = ConvexHull(points[0])
# ~ hptsLF = ConvexHull(points[1])
# ~ hull_to_obj(hptsRF,points[0],"anymal_LF_constraints_in_RF.obj")
# ~ hull_to_obj(hptsLF,points[1],"anymal_RF_constraints_in_LF.obj")
# ~ for k in range(2):