relativeFootPositionQuasiFlat.py

# 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.gepetto import ViewerFactory
from hpp.corbaserver.rbprm import rbprmstate, state_alg
from hpp.corbaserver.rbprm.anymal import Robot
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_X = 0.35
MAX_DIST_BETWEEN_FEET_Z = 0.35
MIN_HEIGHT_COM = 0.3
# 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 = Robot()

fullBody.setConstrainedJointsBounds()
fullBody.setJointBounds("LF_KFE", [-1.4, 0.])
fullBody.setJointBounds("RF_KFE", [-1.4, 0.])
fullBody.setJointBounds("LH_KFE", [0., 1.4])
fullBody.setJointBounds("RH_KFE", [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.lArmId: "fixedStep04"
}
fullBody.loadAllLimbs(dict_heuristic, "ReferenceConfiguration", nbSamples=12)

nbSamples = 1

ps = ProblemSolver(fullBody)
vf = ViewerFactory(ps)
v = vf.createViewer()
rootName = 'root_joint'

zero = [0., 0., 0.]
rLegId = fullBody.rLegId
rLeg = fullBody.rleg
rfoot = fullBody.rfoot
rLegOffset = fullBody.offset[:]
lLegOffset = fullBody.offset[:]
rArmOffset = fullBody.offset[:]
lArmOffset = fullBody.offset[:]

lLegId = fullBody.lLegId
lLeg = fullBody.lleg
lfoot = fullBody.lfoot

# make sure this is 0
q_0 = fullBody.getCurrentConfig()
zeroConf = [0, 0, 0, 0, 0, 0, 1.]
q_0[0:7] = zeroConf
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


# static eq is com is convex combination of pos (projected)
def staticEq(positions, com):
    sizeX = len(positions)
    E = zeros((3, sizeX))
    for i, pos in enumerate(positions):
        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']


# returns true of one of the point is inside the convex hulls of the others. We do not want that
def pointInsideHull(positions):
    for i, pos in enumerate(positions):
        others = positions[:i] + positions[i + 1:]
        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., 1.], 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)
    # if succ:
    # s, succ = state_alg.addNewContact(s, lLegId, poslf, [0.,0.,1.], num_max_sample = 0)
    if succ:
        # ~ 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:
        fullBody.setCurrentConfig(q)
        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")
        q, succ, s, pos = genFlat(i == 0)
        if succ:
            global success
            success += 1
            addCom = True
            for j, effectorName in enumerate(effectors):
                for otheridx, (oeffectorName, limbId) in enumerate(zip(effectors, limbIds)):
                    if otheridx != j:
                        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)

                        # check current joint pos is now zero
                        # ~ 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):
                        # ~ for l in range(0, 3):
                        # ~ rm[k, l] = rot[k, l]
                        # ~ for m in range(0, 3):
                        # ~ 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])):
                            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 ('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):
                        # ~ points[j].append(p[:3])
                        # ~ elif (j == 1 and p[1] < -MIN_DIST_BETWEEN_FEET_Y and abs(p[0]) < MAX_DIST_BETWEEN_FEET_X):
                        # ~ points[j].append(p[:3])
                        # ~ else:
                        # ~ addCom =
            # now compute coms

            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):
                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):
                # ~ for l in range(0, 3):
                # ~ rm[k, l] = rot[k, l]
                # ~ for m in range(0, 3):
                # ~ rm[m, 3] = qEffector[m]
                # ~ 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 (rp[2] < MIN_HEIGHT_COM):
                    addCom = False
                    print("reject min heught")
                if addCom:
                    compoints[j].append(rp)
                    # ~ if j == 1:
                    # ~ if rp[1] < MARGIN_FEET_SIDE:
                    # ~ compoints[j].append(rp)
                    # ~ else:
                    # ~ 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):
    hcom = ConvexHull(comData)
    hull_to_obj(hcom, comData, "anymal_COM_constraints_in_" + str(limbId) + "_effector_frame_quasi_static.obj")
    fig = plt.figure()
    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 = plt.figure()
    fig.suptitle(str(limbId), fontsize=16)
    # ~ axes = [221,222,223,224]
    ax = None
    # ~ 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):
# ~ hcom = ConvexHull(compoints[k])
# ~ plot_hull(hcom, compoints[k], array(compoints[k]))

# ~ hpts = ConvexHull(points[k])
# ~ plot_hull(hpts, points[k], array(points[k]), color = "b", plot = k == 1 and True)