Skip to content
Snippets Groups Projects
relativeFootPositionQuasiFlat.py 13.1 KiB
Newer Older
Guilhem Saurel's avatar
Guilhem Saurel committed
# 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
Steve T's avatar
Steve T committed
from hpp.gepetto import ViewerFactory
Guilhem Saurel's avatar
Guilhem Saurel committed
from hpp.corbaserver.rbprm import rbprmstate, state_alg
Steve T's avatar
Steve T committed
from hpp.corbaserver.rbprm.anymal import Robot
Guilhem Saurel's avatar
Guilhem Saurel committed
from hpp.corbaserver import ProblemSolver
from hpp.corbaserver.rbprm.tools.display_tools import hull_to_obj, plt, plot_hull
Guilhem Saurel's avatar
Guilhem Saurel committed
# from plot_polytopes import *
# from pinocchio import Quaternion
Steve T's avatar
Steve T committed

NUM_SAMPLES = 18000
IT_DISPLAY_PROGRESS = NUM_SAMPLES / 10
MIN_DIST_BETWEEN_FEET_Y = 0.10
MIN_DIST_BETWEEN_FEET_X = 0.10
Steve T's avatar
Steve T committed
MAX_DIST_BETWEEN_FEET_X = 0.35
MAX_DIST_BETWEEN_FEET_Z = 0.35
Steve T's avatar
Steve T committed
MIN_HEIGHT_COM = 0.3
Guilhem Saurel's avatar
Guilhem Saurel committed
# 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
Steve T's avatar
Steve T committed
# for more than this margin, we reject this sample:
MARGIN_FEET_SIDE = 0.05

Guilhem Saurel's avatar
Guilhem Saurel committed
fullBody = Robot()
Steve T's avatar
Steve T committed

fullBody.setConstrainedJointsBounds()
Guilhem Saurel's avatar
Guilhem Saurel committed
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])
Guilhem Saurel's avatar
Guilhem Saurel committed
fullBody.setJointBounds("root_joint", [-20, 20, -20, 20, -20, 20])
dict_heuristic = {
    fullBody.rLegId: "static",
    fullBody.lLegId: "static",
    fullBody.rArmId: "fixedStep04",
Guilhem Saurel's avatar
Guilhem Saurel committed
    fullBody.lArmId: "fixedStep04",
Guilhem Saurel's avatar
Guilhem Saurel committed
}
fullBody.loadAllLimbs(dict_heuristic, "ReferenceConfiguration", nbSamples=12)

Steve T's avatar
Steve T committed
nbSamples = 1

Guilhem Saurel's avatar
Guilhem Saurel committed
ps = ProblemSolver(fullBody)
vf = ViewerFactory(ps)
Steve T's avatar
Steve T committed
v = vf.createViewer()
Guilhem Saurel's avatar
Guilhem Saurel committed
rootName = "root_joint"
Guilhem Saurel's avatar
Guilhem Saurel committed
zero = [0.0, 0.0, 0.0]
Steve T's avatar
Steve T committed
rLegId = fullBody.rLegId
rLeg = fullBody.rleg
rfoot = fullBody.rfoot
rLegOffset = fullBody.offset[:]
lLegOffset = fullBody.offset[:]
rArmOffset = fullBody.offset[:]
lArmOffset = fullBody.offset[:]

lLegId = fullBody.lLegId
Guilhem Saurel's avatar
Guilhem Saurel committed
lLeg = fullBody.lleg
lfoot = fullBody.lfoot
Guilhem Saurel's avatar
Guilhem Saurel committed
# make sure this is 0
q_0 = fullBody.getCurrentConfig()
Guilhem Saurel's avatar
Guilhem Saurel committed
zeroConf = [0, 0, 0, 0, 0, 0, 1.0]
Steve T's avatar
Steve T committed
q_0[0:7] = zeroConf
Guilhem Saurel's avatar
Guilhem Saurel committed
fullBody.setCurrentConfig(q_0)

effectors = [
    fullBody.rfoot,
    fullBody.lfoot,
    fullBody.rhand,
    fullBody.lhand,
]
Steve T's avatar
Steve T committed
limbIds = [fullBody.rLegId, fullBody.lLegId, fullBody.rArmId, fullBody.lArmId]
offsets = [array(rLegOffset), array(lLegOffset), array(rArmOffset), array(lArmOffset)]

compoints = [[] for _ in effectors]
Guilhem Saurel's avatar
Guilhem Saurel committed
# compoints = [[[0.012471792486262121, 0.0015769611415203033, 0.8127583093263778]],
# [[0.012471792486262121, 0.0015769611415203033, 0.8127583093263778]]]
points = [{} for _ in effectors]
for i, eff in enumerate(effectors):
Steve T's avatar
Steve T committed
    for j, otherEff in enumerate(effectors):
        if i != j:
            points[i][otherEff] = []

success = 0
fails = 0


Guilhem Saurel's avatar
Guilhem Saurel committed
# static eq is com is convex combination of pos (projected)
Steve T's avatar
Steve T committed
def staticEq(positions, com):
    sizeX = len(positions)
Guilhem Saurel's avatar
Guilhem Saurel committed
    E = zeros((3, sizeX))
Steve T's avatar
Steve T committed
    for i, pos in enumerate(positions):
Guilhem Saurel's avatar
Guilhem Saurel committed
        E[:2, i] = pos[:2]
Guilhem Saurel's avatar
Guilhem Saurel committed
    e = array([com[0], com[1], 1.0])
Guilhem Saurel's avatar
Guilhem Saurel committed
    E[2, :] = ones(sizeX)
Guilhem Saurel's avatar
Guilhem Saurel committed
    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"]


Steve T's avatar
Steve T committed
def pointInsideHull(positions):
Guilhem Saurel's avatar
Guilhem Saurel committed
    """
    returns true of one of the point is inside the convex hulls of the others.
    We do not want that
    """
Steve T's avatar
Steve T committed
    for i, pos in enumerate(positions):
Guilhem Saurel's avatar
Guilhem Saurel committed
        others = positions[:i] + positions[i + 1 :]
Steve T's avatar
Steve T committed
        if staticEq(others, pos):
            return True
    return False

Guilhem Saurel's avatar
Guilhem Saurel committed

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):
Guilhem Saurel's avatar
Guilhem Saurel committed
        s, succ = state_alg.addNewContact(
            s, effId, pos, [0.0, 0.0, 1.0], num_max_sample=0
        )
Guilhem Saurel's avatar
Guilhem Saurel committed
        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)
Guilhem Saurel's avatar
Guilhem Saurel committed
    # s, succ = state_alg.addNewContact(s, rLegId, posrf, [0.,0.,1.], num_max_sample= 0)
Guilhem Saurel's avatar
Guilhem Saurel committed
    # if succ:
Guilhem Saurel's avatar
Guilhem Saurel committed
    # s, succ = state_alg.addNewContact(s, lLegId, poslf, [0.,0.,1.], num_max_sample= 0)
Guilhem Saurel's avatar
Guilhem Saurel committed
    if succ:
Guilhem Saurel's avatar
Guilhem Saurel committed
        # ~ succ = fullBody.isConfigValid(q)[0]
        #          and norm (array(posrf[:2]) - array(poslf[:2]) ) >= 0.3
Guilhem Saurel's avatar
Guilhem Saurel committed
        succ = fullBody.isConfigValid(q)[0]

    # assert that in static equilibrium
    if succ:
Steve T's avatar
Steve T committed
        fullBody.setCurrentConfig(q)
Guilhem Saurel's avatar
Guilhem Saurel committed
        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


Steve T's avatar
Steve T committed
def printFootPositionRelativeToOther(nbConfigs):
    for i in range(0, nbConfigs):
        if i > 0 and not i % IT_DISPLAY_PROGRESS:
            print(int((i * 100) / nbConfigs), " % done")
Guilhem Saurel's avatar
Guilhem Saurel committed
        q, succ, s, pos = genFlat(i == 0)
Steve T's avatar
Steve T committed
        if succ:
            global success
            success += 1
            addCom = True
            for j, effectorName in enumerate(effectors):
Guilhem Saurel's avatar
Guilhem Saurel committed
                for otheridx, (oeffectorName, limbId) in enumerate(
                    zip(effectors, limbIds)
                ):
Guilhem Saurel's avatar
Guilhem Saurel committed
                    if otheridx != j:
Steve T's avatar
Steve T committed
                        fullBody.setCurrentConfig(q)
                        pos_other = fullBody.getJointPosition(oeffectorName)
                        pos = fullBody.getJointPosition(effectorName)
                        p = array(pos_other[:3]) - array(pos[:3]).tolist()
                        # ~ qtr = q[:]
Guilhem Saurel's avatar
Guilhem Saurel committed
                        # ~ qtr[:3] = [qtr[0] - pos_other[0], qtr[1] - pos_other[1],
                        # ~             qtr[2] - pos_other[2]]
                        # ~ fullBody.setCurrentConfig(qtr)
                        # ~ qEffector = fullBody.getJointPosition(effectorName)
Steve T's avatar
Steve T committed

                        # check current joint pos is now zero
Guilhem Saurel's avatar
Guilhem Saurel committed
                        # ~ q0 = Quaternion(qEffector[6], qEffector[3], qEffector[4],
                        #                   qEffector[5])
                        # ~ rot = q0.matrix()  # compute rotation matrix world -> local
Guilhem Saurel's avatar
Guilhem Saurel committed
                        # (0,0,0) coordinate expressed in effector fram
                        # ~ p = qEffector[0:3]
                        # ~ rm = np.zeros((4, 4))
                        # ~ for k in range(0, 3):
Guilhem Saurel's avatar
Guilhem Saurel committed
                        # ~ for l in range(0, 3):
                        # ~ rm[k, l] = rot[k, l]
                        # ~ for m in range(0, 3):
Guilhem Saurel's avatar
Guilhem Saurel committed
                        # ~ rm[m, 3] = qEffector[m]
                        # ~ rm[3, 3] = 1
                        # ~ invrm = np.linalg.inv(rm)
Guilhem Saurel's avatar
Guilhem Saurel committed
                        # ~ p = invrm.dot([0, 0, 0., 1])
Guilhem Saurel's avatar
Guilhem Saurel committed
                        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
Guilhem Saurel's avatar
Guilhem Saurel committed
                            print(
                                "rejecting ",
                                effectorName,
                                " ",
                                oeffectorName,
                                p,
                                abs(p[2]),
                            )
                            # ~ print ('pos_other', pos_other)
                            # ~ print ('old_pos', old_pos)
                            addCom = False
                            v(q)
Guilhem Saurel's avatar
Guilhem Saurel committed
                        # ~ if (j == 0 and p[1] > MIN_DIST_BETWEEN_FEET_Y
                        #       and abs(p[0]) < MAX_DIST_BETWEEN_FEET_X):
Guilhem Saurel's avatar
Guilhem Saurel committed
                        # ~ points[j].append(p[:3])
Guilhem Saurel's avatar
Guilhem Saurel committed
                        # ~ elif (j == 1 and p[1] < -MIN_DIST_BETWEEN_FEET_Y
                        #         and abs(p[0]) < MAX_DIST_BETWEEN_FEET_X):
Guilhem Saurel's avatar
Guilhem Saurel committed
                        # ~ points[j].append(p[:3])
Steve T's avatar
Steve T committed
                        # ~ else:
Guilhem Saurel's avatar
Guilhem Saurel committed
                        # ~ addCom =
Steve T's avatar
Steve T committed
            # now compute coms

            fullBody.setCurrentConfig(q)
            com = array(fullBody.getCenterOfMass())
Guilhem Saurel's avatar
Guilhem Saurel committed
            print("com ", com)
            # ~ for x in range(0, 3):
Guilhem Saurel's avatar
Guilhem Saurel committed
            # ~ q[x] = -com[x]
Steve T's avatar
Steve T committed
            for j, effectorName in enumerate(effectors):
                pos = fullBody.getJointPosition(effectorName)
                rp = array(com) - array(pos[:3]).tolist()
                # ~ qEffector = fullBody.getJointPosition(effectorName)
Guilhem Saurel's avatar
Guilhem Saurel committed
                # ~ 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):
Guilhem Saurel's avatar
Guilhem Saurel committed
                # ~ for l in range(0, 3):
                # ~ rm[k, l] = rot[k, l]
                # ~ for m in range(0, 3):
Guilhem Saurel's avatar
Guilhem Saurel committed
                # ~ 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()
Guilhem Saurel's avatar
Guilhem Saurel committed
                if rp[2] < MIN_HEIGHT_COM:
Steve T's avatar
Steve T committed
                    addCom = False
Guilhem Saurel's avatar
Guilhem Saurel committed
                    print("reject min heught")
Steve T's avatar
Steve T committed
                if addCom:
                    compoints[j].append(rp)
                    # ~ if j == 1:
Guilhem Saurel's avatar
Guilhem Saurel committed
                    # ~ if rp[1] < MARGIN_FEET_SIDE:
                    # ~ compoints[j].append(rp)
Steve T's avatar
Steve T committed
                    # ~ else:
Guilhem Saurel's avatar
Guilhem Saurel committed
                    # ~ if rp[1] > -MARGIN_FEET_SIDE:
                    # ~ compoints[j].append(rp)
Steve T's avatar
Steve T committed

        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()


Guilhem Saurel's avatar
Guilhem Saurel committed
s = rbprmstate.State(
    fullBody, q=fullBody.getCurrentConfig(), limbsIncontact=[fullBody.limbs_names[0]]
)
Guilhem Saurel's avatar
Guilhem Saurel committed
# printRootPosition(rLegId, rfoot, nbSamples)
# printRootPosition(lLegId, lfoot, nbSamples)
# printRootPosition(rarmId, rHand, nbSamples)
# printRootPosition(larmId, lHand, nbSamples)
printFootPositionRelativeToOther(6000)
print("successes ", success)
print("fails  ", fails)
Steve T's avatar
Steve T committed

# ~ for effector, comData, pointsData in zip(effectors, compoints, points):
Guilhem Saurel's avatar
Guilhem Saurel committed
# ~ for effector, limbId, comData, pointsData in zip(effectors[:1],limbIds[1:],
#                                                    compoints[:1], points[:1]):
Guilhem Saurel's avatar
Guilhem Saurel committed
for effector, limbId, comData, pointsData in zip(effectors, limbIds, compoints, points):
Steve T's avatar
Steve T committed
    hcom = ConvexHull(comData)
Guilhem Saurel's avatar
Guilhem Saurel committed
    hull_to_obj(
        hcom,
        comData,
        "anymal_COM_constraints_in_" + str(limbId) + "_effector_frame_quasi_static.obj",
    )
Guilhem Saurel's avatar
Guilhem Saurel committed
    fig.suptitle(
        "anymal_COM_constraints_in_" + str(limbId) + "_effector_frame_quasi_static.obj",
        fontsize=16,
    )
Guilhem Saurel's avatar
Guilhem Saurel committed
    plot_hull(hcom, comData, array(comData), color="r", plot=False, fig=fig, ax=None)

Steve T's avatar
Steve T committed
    fig = plt.figure()
    fig.suptitle(str(limbId), fontsize=16)
    # ~ axes = [221,222,223,224]
Steve T's avatar
Steve T committed
    ax = None
    # ~ for (oEffector, pts), axId in zip(pointsData.items(), axes):
    for oEffector, pts in pointsData.items():
Steve T's avatar
Steve T committed
        # ~ ax = fig.add_subplot(axId, projection="3d")
        hpts = ConvexHull(pts)
Guilhem Saurel's avatar
Guilhem Saurel committed
        hull_to_obj(
            hpts,
            pts,
            "anymal_" + str(oEffector) + "_constraints_in_" + str(limbId) + ".obj",
        )
Guilhem Saurel's avatar
Guilhem Saurel committed
        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)
Steve T's avatar
Steve T committed

# ~ 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):
Guilhem Saurel's avatar
Guilhem Saurel committed
# ~ 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)