Skip to content
Snippets Groups Projects
relativeFootPositionQuasiFlat.py 12.2 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.18
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)
    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():
        q = fullBody.shootRandomConfig()
        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()
        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])):                            
                            points[j][oeffectorName].append(p[:3])
                        else:
                            # ~ 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
                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)