Skip to content
Snippets Groups Projects
relativeFootPositionQuasiFlat.py 12.8 KiB
Newer Older
Steve T's avatar
Steve T committed
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
from hpp.gepetto import ViewerFactory
from numpy import array
from numpy.linalg import norm


from hpp.corbaserver.rbprm.anymal import Robot
from hpp.corbaserver.rbprm.tools.display_tools import *

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

#~ from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
from hpp.corbaserver import ProblemSolver
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)]

import numpy as np

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
from hpp.corbaserver.rbprm import  rbprmstate, state_alg
from scipy.spatial import ConvexHull

from scipy.optimize import linprog

#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)
    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
Steve T's avatar
Steve T committed
        

#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):
Steve T's avatar
Steve T committed
        q = fullBody.shootRandomConfig()
        if init:
            q =  fullBody.referenceConfig[::]
Steve T's avatar
Steve T committed
        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)
Steve T's avatar
Steve T committed
        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)
Steve T's avatar
Steve T committed

                        # 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
                            print ('rejecting ',effectorName, ' ', oeffectorName , p,  abs(p[2]))
                            # ~ print ('pos_other', pos_other)
                            # ~ print ('old_pos', old_pos)
                            addCom = False
                            v(q)
Steve T's avatar
Steve T committed
                        # ~ 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())
            # ~ for x in range(0, 3):
                # ~ 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)
                # ~ 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()
Steve T's avatar
Steve T committed

                if (rp[2] < MIN_HEIGHT_COM):
                    addCom = False
                    print ("reject min heught")
Steve T's avatar
Steve T committed
                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)
Steve T's avatar
Steve T committed
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):
Steve T's avatar
Steve T committed
    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)
    
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)
        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)
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):
    # ~ 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)