def setupHrp2():
switch_context(0)
q_init = [
1.465951314830228, -0.9176187491454572, 0.556996332024304,
0.8949373222781749, 0.004991393277543945, 0.09328595707567328,
0.43630265344046976, -0.03547067445226664, 0.6709217094864276,
0.2612513403690041, 0.018790826902884042, 1.0472, -0.12716727062306263,
0.014121758070711707, 0.0349066, -0.005336299638361007,
0.09555844824139348, -0.15100592820137693, 1.0472, -0.6564341232652954,
-0.14124478581057368, 0.0349066, 0.006442792051943824,
0.0976416359755596, 0.048557325328327426, 0.025298798438044116,
0.5088603837394758, 0.09277155854142789, 0.06056704285485918,
-0.40397983120647746, -0.3442592958713712, -0.5709648671805531,
-0.5813136986745363, -0.8406045693532259, 0.5508411880503161,
0.06566162355557537, 0.5817029063967701
]
r(q_init)
#~ q_init[1] = -1.05
s1 = State(fullBody, q=q_init, limbsIncontact=[rLegId, lLegId])
#~ s1 = State(fullBody,q=q_init, limbsIncontact = [])
q0 = s1.q()[:]
r(q0)
fullBody.setRefConfig(q0)
return s1
def setupSpidey():
switch_context(1)
q_0 = fullBody.getCurrentConfig()
r(q_0)
q_init = [
1.8644820749752133, -0.05907835662004786, 0.36244227429714687,
0.796622257271051, -0.01123057286487888, -0.04124810131291858,
0.6029638858104035, -0.03547067445226691, 0.6709217094864277,
0.26125134036900377, 0.018790826902884056, 1.0472,
-0.12716727062306263, -0.4194108294469009, -0.5282737768672232,
0.6483375025026266, -0.36743071075802436, -1.3354344044074755,
0.8731280507393882, -0.6564341232652954, -0.14124478581057368,
0.0349066, 1.0782285414916801, -1.2054219129402555, 0.6375403038065977
]
#~ q_init[0:7] = tp.q_init[0:7]
#~ q_init[1] += 1.05
#~ q_0[2] = 1.1
s1 = State(fullBody, q=q_init, limbsIncontact=['r1', 'r3', 'l3'])
#~ q_goal = fullBody.generateContacts(s1.q(), [0,0,1])
#~ s1.setQ(q_goal)
q0 = s1.q()[:]
r(q0)
return s1
def setupHrp2():
switch_context(0)
q_init = [
0.8728886120451924, -0.8101285717720692, 0.9831109373101212,
0.9907480839443233, 0.04566101822960869, 6.080932806073498e-05,
-0.12780180701818292, 0.0, 0.0, 0.700898687394426, 0.2643416012373336,
1.0472, -0.04797431922382627, -0.013276945338995495,
-0.1474914458541713, -0.002729397895803521, 0.0417439495415805,
-0.0959524864077709, 1.0472, -0.7688894346658527, -0.12266497430814384,
-0.12242722827560858, 0.0031594766930237773, 0.0443584738891719,
0.015217658052039982, 0.6567108816211705, 0.3519940697164601,
-0.3096847583571286, 0.01661099687285411, 0.6843606576274044,
-0.33788734650437907, 0.3649203666768749, -0.3603776588496829,
0.1551833621146152, -0.001045294421723865, 0.23087743645539907,
0.28930745214380776
]
r(q_init)
#~ q_init[1] = -1.05
s1 = State(fullBody, q=q_init, limbsIncontact=[rLegId, lLegId])
#~ s1 = State(fullBody,q=q_init, limbsIncontact = [])
q0 = s1.q()[:]
r(q0)
fullBody.setRefConfig(q0)
return s1
def setupSpidey():
switch_context(1)
q_0 = fullBody.getCurrentConfig()
r(q_0)
q_init = [
1.5118243482119733, 0.1515578829873285, 0.48976931477040697,
0.9915264175846067, -0.018363722321990766, -0.068500987098437,
-0.10883819045195667, 0.5302771431871597, 0.5441203380575321,
0.2782134865083353, -0.06010765345021368, -0.8900451738361533,
0.276589550853067, -0.34163470046205424, -0.2710174858398316,
0.06526434276718414, -0.43672572993852576, -0.9562437680837305,
0.9084315210259988, -0.6088478466994447, -0.13563212555983598,
0.0349066, 0.9658270566778959, -0.6362233126122913, 1.5
]
#~ q_init[0:7] = tp.q_init[0:7]
#~ q_init[1] += 1.05
#~ q_0[2] = 1.1
s1 = State(fullBody, q=q_init, limbsIncontact=['r1', 'r3', 'l3', 'l2'])
#~ q_goal = fullBody.generateContacts(s1.q(), [0,0,1])
#~ s1.setQ(q_goal)
q0 = s1.q()[:]
r(q0)
return s1
def setupSpidey():
switch_context(1)
q_0 = fullBody.getCurrentConfig(); r(q_0)
q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
q_init[1] += 1.05
#~ q_0[2] = 1.1
s1 = State(fullBody,q=q_init, limbsIncontact = [rLegId, lLegId])
q0 = s1.q()[:]
r(q0)
return s1
def setupHrp2():
switch_context(0)
q_init = fullBody.getCurrentConfig()
r(q_init)
#~ q_init [3:7] = [ 0.98877108, 0. , 0.14943813, 0. ]
#~ q_init [0:3] = [-0.05, -0.82, 0.50];
q_init = [
-0.05,
-0.82,
0.65,
1.0,
0.0,
0.0,
0.0, # Free flyer 0-6
0.0,
0.0,
0.0,
0.0, # CHEST HEAD 7-10
0.261799388,
0.174532925,
0.0,
-0.523598776,
0.0,
0.0,
0.17, # LARM 11-17
0.261799388,
-0.174532925,
0.0,
-0.523598776,
0.0,
0.0,
0.17, # RARM 18-24
0.0,
0.0,
-0.453785606,
0.872664626,
-0.41887902,
0.0, # LLEG 25-30
0.0,
0.0,
-0.453785606,
0.872664626,
-0.41887902,
0.0, # RLEG 31-36
]
r(q_init)
#~ q_init[1] = -1.05
s1 = State(fullBody, q=q_init, limbsIncontact=[rLegId, lLegId])
#~ s1 = State(fullBody,q=q_init, limbsIncontact = [])
q0 = s1.q()[:]
r(q0)
fullBody.setRefConfig(q0)
return s1
def projectMidFeet(fullBody, q, limbs):
fullBody.setCurrentConfig(q)
s = State(fullBody, q=q, limbsIncontact=limbs)
com = np.array(fullBody.getJointPosition(rfoot)[0:3])
com += np.array(fullBody.getJointPosition(lfoot)[0:3])
com /= 2.
com[2] = fullBody.getCenterOfMass()[2]
successProj = s.projectToCOM(com.tolist())
if successProj and fullBody.isConfigValid(
s.q())[0] and fullBody.isConfigBalanced(s.q(), limbs, 5):
return s
else:
return None
def setupHrp2():
switch_context(0)
q_init = [
0.1, -0.82, 0.648702, 1.0, 0.0 , 0.0, 0.0, # Free flyer 0-6
0.0, 0.0, 0.0, 0.0, # CHEST HEAD 7-10
0.261799388, 0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17, # LARM 11-17
0.261799388, -0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17, # RARM 18-24
0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # LLEG 25-30
0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # RLEG 31-36
]; r (q_init)
#~ q_init[1] = -1.05
s1 = State(fullBody,q=q_init, limbsIncontact = [rLegId, lLegId])
q0 = s1.q()[:]
r(q0)
return s1
def _interpolateState(ps,
fullBody,
stepsize,
pathId,
robustnessTreshold=0,
filterStates=False,
testReachability=True,
quasiStatic=False,
erasePreviousStates=True):
if (filterStates):
filt = 1
else:
filt = 0
#discretize path
totalLength = ps.pathLength(pathId)
configsPlan = []
step = 0.
configSize = fullBody.getConfigSize() - len(ps.configAtParam(pathId, 0.))
z = [0. for _ in range(configSize)]
while step < totalLength:
configsPlan += [ps.configAtParam(pathId, step) + z[:]]
step += stepsize
configsPlan += [ps.configAtParam(pathId, totalLength) + z[:]]
configs = fullBody.clientRbprm.rbprm.interpolateConfigs(
configsPlan, robustnessTreshold, filt, testReachability, quasiStatic,
erasePreviousStates)
firstStateId = fullBody.clientRbprm.rbprm.getNumStates() - len(configs)
return [
State(fullBody, i)
for i in range(firstStateId, firstStateId + len(configs))
], configs
def generateConfigFromPhase(fb, phase, projectCOM=False):
fb.usePosturalTaskContactCreation(False)
effectorsInContact = phase.effectorsInContact()
contacts = [
] # contacts should contains the limb names, not the effector names
list_effector = list(fb.dict_limb_joint.values())
for eeName in effectorsInContact:
contacts += [
list(fb.dict_limb_joint.keys())[list_effector.index(eeName)]
]
#q = phase.q_init.tolist() # should be the correct config for the previous phase, if used only from high level helper methods
q = fb.referenceConfig[::] + [0] * 6 # FIXME : more generic !
root = computeCenterOfSupportPolygonFromPhase(
phase, fb.DEFAULT_COM_HEIGHT).tolist()
q[0:2] = root[0:2]
q[2] += root[2] - fb.DEFAULT_COM_HEIGHT
quat = Quaternion(phase.root_t.evaluateAsSE3(phase.timeInitial).rotation)
q[3:7] = [quat.x, quat.y, quat.z, quat.w]
# create state in fullBody :
state = State(fb, q=q, limbsIncontact=contacts)
# check if q is consistent with the contact placement in the phase :
fb.setCurrentConfig(q)
for limbId in contacts:
eeName = fb.dict_limb_joint[limbId]
placement_fb = SE3FromConfig(fb.getJointPosition(eeName))
placement_phase = phase.contactPatch(eeName).placement
if placement_fb != placement_phase: # add a threshold instead of 0 ? how ?
# need to project the new contact :
placement = phase.contactPatch(eeName).placement
p = placement.translation.tolist()
n = computeContactNormal(placement).tolist()
state, success = StateHelper.addNewContact(state, limbId, p, n,
1000)
if not success:
print(
"Cannot project the configuration to contact, for effector : ",
eeName)
return state.q()
if projectCOM:
success = projectCoMInSupportPolygon(state)
if not success:
print(
"cannot project com to the middle of the support polygon."
)
phase.q_init = np.array(state.q())
return state.q()
def perturbateContactNormal(fb, state_id, epsilon=1e-2):
"""
Add a small variation (+- epsilon) to the contact normals of the given state
:param fb:
:param state_id:
:param epsilon:
:return: the new state ID, -1 if fail
"""
state = State(fb, state_id)
for name in state.getLimbsInContact():
p, n = state.getCenterOfContactForLimb(name)
n[2] += uniform(-epsilon, epsilon)
n = np.array(n)
state, success = StateHelper.addNewContact(state, name, p, n.tolist())
if not success:
return -1
return state.sId
def sampleRandomTransitionFlatFloor(fullBody, limbsInContact, movingLimb, z=0):
random.seed()
success = False
it_tot = 0
while not success and it_tot < 1000:
it_tot += 1
s0 = sampleRandomStateFlatFloor(fullBody, limbsInContact, z)
success, s1 = sampleRandomTranstionFromState(fullBody, s0,
limbsInContact,
movingLimb, z)
if not success:
print "Timeout for generation of feasible transition"
sys.exit(1)
# recreate the states to assure the continuity of the index in fullBody :
state0 = State(fullBody, q=s0.q(), limbsIncontact=s0.getLimbsInContact())
state1 = State(fullBody, q=s1.q(), limbsIncontact=s1.getLimbsInContact())
return state0, state1
def check_projection_path(s0, s1, c, dc, ddc, t_per_phase):
kin_valid = True
stab_valid = True
moving_limb = s0.contactsVariations(s1)
s0_orig = State(s0.fullBody,
q=s0.q(),
limbsIncontact=s0.getLimbsInContact())
s1_orig = State(s1.fullBody,
q=s1.q(),
limbsIncontact=s1.getLimbsInContact())
if len(moving_limb) > 1:
print("Too many contact variation between adjacent states")
return False, False
if len(moving_limb) == 0:
print("No contact between adjacent states")
return True, True
#print "test for phase 0 :"
for t in t_per_phase[0]:
if kin_valid and not check_projection(s0, c, t):
kin_valid = False
if stab_valid and not check_stability(s0_orig, c, dc, ddc, t):
stab_valid = False
smid, success = StateHelper.removeContact(s0, moving_limb[0])
smid_orig, success_orig = StateHelper.removeContact(
s0_orig, moving_limb[0])
if not (success and success_orig):
print("Error in creation of intermediate state")
return False, False
#print "test for phase 1 :"
for t in t_per_phase[1]:
if kin_valid and not check_projection(smid, c, t):
kin_valid = False
if stab_valid and not check_stability(smid_orig, c, dc, ddc, t):
stab_valid = False
#print "test for phase 2"
# go in reverse order for p2, it's easier for the projection :
for i in range(1, len(t_per_phase[2]) + 1):
t = t_per_phase[2][-i]
if kin_valid and not check_projection(s1, c, t):
kin_valid = False
if stab_valid and not check_stability(s1_orig, c, dc, ddc, t):
stab_valid = False
return kin_valid, stab_valid
def sampleRandomTranstionFromState(fullBody, s0, limbsInContact, movingLimb,
z):
it = 0
success = False
n = [0, 0, 1]
vz = np.matrix(n).T
while not success and it < 10000:
it += 1
# sample a random position for movingLimb and try to project s0 to this position
qr = fullBody.shootRandomConfig()
q1 = s0.q()[::]
q1[limb_ids[movingLimb][0]:limb_ids[movingLimb][1]] = qr[
limb_ids[movingLimb][0]:limb_ids[movingLimb][1]]
s1 = State(fullBody, q=q1, limbsIncontact=limbsInContact)
fullBody.setCurrentConfig(s1.q())
p = fullBody.getJointPosition(
fullBody.dict_limb_joint[movingLimb])[0:3]
p[0] = random.uniform(eff_x_range[0], eff_x_range[1])
p[1] = random.uniform(eff_y_range[0], eff_y_range[1])
p[2] = z
s1, success = StateHelper.addNewContact(s1, movingLimb, p, n)
if success:
"""
# force root orientation : (align current z axis with vertical)
quat_1 = Quaternion(s1.q()[6],s1.q()[3],s1.q()[4],s1.q()[5])
v_1 = quat_1.matrix() * vz
align = Quaternion.FromTwoVectors(v_1,vz)
rot = align * quat_1
q_root = s1.q()[0:7]
q_root[3:7] = rot.coeffs().T.tolist()[0]
"""
root = SE3.Identity()
root.translation = np.matrix(s1.q()[0:3]).T
root = sampleRotationAlongZ(root)
success = s1.projectToRoot(se3ToXYZQUATtuple(root))
# check if new state is in static equilibrium
if success:
# check stability
success = fullBody.isStateBalanced(s1.sId, 3)
if success:
success = projectInKinConstraints(fullBody, s1)
# check if transition is feasible according to CROC
if success:
#success = fullBody.isReachableFromState(s0.sId,s1.sId) or (len(fullBody.isDynamicallyReachableFromState(s0.sId,s1.sId, numPointsPerPhases=0)) > 0)
success = fullBody.isReachableFromState(s0.sId, s1.sId)
return success, s1
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 = State(fullBody, q=q, limbsIncontact=limbIds)
succ = True
for effId, pos in zip(limbIds, positions):
s, succ = StateHelper.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 = State(fullBody, q = q, limbsIncontact = limbIds)
# ~ s, succ = StateHelper.addNewContact(s, rLegId, posrf, [0.,0.,1.], num_max_sample = 0)
# ~ if succ:
# ~ s, succ = StateHelper.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 check_traj_valid(ps, fullBody, s0_, s1_, pIds, dt=0.001):
# create copy of original states :
s0 = State(fullBody, q=s0_.q(), limbsIncontact=s0_.getLimbsInContact())
s1 = State(fullBody, q=s1_.q(), limbsIncontact=s1_.getLimbsInContact())
fullBody.setStaticStability(False)
Ts = [
ps.pathLength(int(pIds[1])),
ps.pathLength(int(pIds[2])),
ps.pathLength(int(pIds[3]))
]
t_array = genTimeArray(Ts, dt)
#print "Time array : ",t_array
c_qp = fullBody.getPathAsBezier(int(pIds[0]))
dc_qp = c_qp.compute_derivate(1)
ddc_qp = dc_qp.compute_derivate(1)
return check_projection_path(s0, s1, c_qp, dc_qp, ddc_qp, t_array)
def removeContact(state, limbName, projectToCOM = False, friction = 0.6):
sId = state.cl.removeContact(state.sId, limbName)
if(sId != -1):
s = State(state.fullBody, sId = sId)
if projectToCOM:
return s, projectToFeasibleCom(s, ddc =[0.,0.,0.], max_num_samples = 10, friction = friction)
else:
return s, True
return state, False
def check_muscod_traj(fullBody, cs, s0_, s1_):
if cs.size() != 3:
print("Contact sequence must be of size 3 (one step)")
return False, False
kin_valid = True
stab_valid = True
stab_valid_discretized = True
# create copy of original states :
s0 = State(fullBody, q=s0_.q(), limbsIncontact=s0_.getLimbsInContact())
s1 = State(fullBody, q=s1_.q(), limbsIncontact=s1_.getLimbsInContact())
moving_limb = s0.contactsVariations(s1)
smid, success = StateHelper.removeContact(s0, moving_limb[0])
if not success:
print("Error in creation of intermediate state")
return False, False
phases = []
c_array = []
dc_array = []
ddc_array = []
t_array = []
states = [s0, smid, s1]
states_copy = [
State(fullBody, q=s0_.q(), limbsIncontact=s0_.getLimbsInContact()),
State(fullBody, q=smid.q(), limbsIncontact=smid.getLimbsInContact()),
State(fullBody, q=s1_.q(), limbsIncontact=s1_.getLimbsInContact())
]
for k in range(3):
phases += [cs.contact_phases[k]]
state_traj = stdVecToMatrix(phases[k].state_trajectory).transpose()
control_traj = stdVecToMatrix(phases[k].control_trajectory).transpose()
c_array = state_traj[:, :3]
dc_array = state_traj[:, 3:6]
ddc_array = control_traj[:, :3]
t_array = stdVecToMatrix(phases[k].time_trajectory).transpose()
for i in range(len(c_array)):
#if kin_valid and not states[k].projectToCOM(c_array[i].tolist()[0],500) :
# kin_valid = False
q = states_copy[k].q()
q[-6:-3] = dc_array[i].tolist()[0]
q[-3:] = ddc_array[i].tolist()[0]
if stab_valid_discretized and not states_copy[
k].fullBody.isConfigBalanced(
q,
states_copy[k].getLimbsInContact(),
CoM=c_array[i].tolist()[0]):
stab_valid_discretized = False
for i in range(int(len(c_array) / 100)):
q = states_copy[k].q()
q[-6:-3] = dc_array[i * 100].tolist()[0]
q[-3:] = ddc_array[i * 100].tolist()[0]
if stab_valid and not states_copy[k].fullBody.isConfigBalanced(
q,
states_copy[k].getLimbsInContact(),
CoM=c_array[i * 100].tolist()[0]):
stab_valid = False
return kin_valid, stab_valid, stab_valid_discretized
def addNewContact(state,
limbName,
p,
n,
num_max_sample=0,
lockOtherJoints=False):
sId = state.cl.addNewContact(state.sId, limbName, p, n, num_max_sample,
lockOtherJoints)
if (sId != -1):
return State(state.fullBody, sId=sId), True
return state, False
def check_contact_plan(ps, r, pp, fullBody, idBegin, idEnd):
fullBody.client.basic.robot.setExtraConfigSpaceBounds(
[-0, 0, -0, 0, -0, 0, 0, 0, 0, 0, 0, 0])
fullBody.setStaticStability(False)
validPlan = True
for id_state in range(idBegin, idEnd - 1):
print("#### check for transition between state " + str(id_state) +
" and " + str(id_state + 1))
s0 = State(fullBody, sId=id_state)
s1 = State(fullBody, sId=id_state + 1)
# make a copy of each state because they are modified by the projection
s0_ = State(fullBody, q=s0.q(), limbsIncontact=s0.getLimbsInContact())
s1_ = State(fullBody, q=s1.q(), limbsIncontact=s1.getLimbsInContact())
valid = check_one_transition(ps, fullBody, s0_, s1_, r, pp)
validPlan = validPlan and valid
global curves_initGuess
global timings_initGuess
return validPlan, curves_initGuess, timings_initGuess
def load_save(fname):
f = open(fname, "r+")
all_data = load(f)
f.close()
sc(0)
global states
states = []
#~ for i in range(0,len(all_data[0]),2):
#~ print "q",all_data[0][i]
#~ print "lic",all_data[0][i+1]
#~ states+=[State(fullBody,q=all_data[0][i], limbsIncontact = all_data[0][i+1]) ]
for _, s in enumerate(all_data[0]):
states += [State(fullBody, q=s[0], limbsIncontact=s[1])]
r(states[0].q())
sc(1)
global states
states = []
#~ for i in range(0,len(all_data[1]),2):
#~ states+=[State(fullBody,q=all_data[1][i], limbsIncontact = all_data[1][i+1]) ]
for _, s in enumerate(all_data[1]):
states += [State(fullBody, q=s[0], limbsIncontact=s[1])]
r(states[0].q())
def generateConfigFromPhase(fb, phase, projectCOM=False):
fb.usePosturalTaskContactCreation(False)
contacts = getActiveContactLimbs(phase, fb)
#q = phase.reference_configurations[0].T.tolist()[0] # should be the correct config for the previous phase, if used only from high level helper methods
q = fb.referenceConfig[::] + [0] * 6 # FIXME : more generic !
root = computeCenterOfSupportPolygonFromPhase(phase, fb).T.tolist()[0]
q[0:2] = root[0:2]
q[2] += root[2] - fb.DEFAULT_COM_HEIGHT
quat = Quaternion(
rootOrientationFromFeetPlacement(phase, None)[0].rotation)
q[3:7] = [quat.x, quat.y, quat.z, quat.w]
# create state in fullBody :
state = State(fb, q=q, limbsIncontact=contacts)
# check if q is consistent with the contact placement in the phase :
fb.setCurrentConfig(q)
for limbId in contacts:
eeName = fb.dict_limb_joint[limbId]
placement_fb = SE3FromConfig(fb.getJointPosition(eeName))
placement_phase = JointPlacementForEffector(phase, eeName, fb)
if placement_fb != placement_phase: # add a threshold instead of 0 ? how ?
# need to project the new contact :
placement = getContactPlacement(phase, eeName, fb)
p = placement.translation.T.tolist()[0]
n = computeContactNormal(placement).T.tolist()[0]
state, success = StateHelper.addNewContact(state, limbId, p, n,
1000)
if not success:
print "Cannot project the configuration to contact, for effector : ", eeName
return state.q()
if projectCOM:
success = projectCoMInSupportPolygon(state)
if not success:
print "cannot project com to the middle of the support polygon."
phase.reference_configurations[0] = np.matrix(state.q()).T
return state.q()
def test_contacts_3d(self):
with ServerManager('hpp-rbprm-server'):
fullbody = Robot()
fullbody.client.robot.setDimensionExtraConfigSpace(6)
fullbody.setJointBounds("root_joint", [-10, 10, -10, 10, -10, 10])
fullbody.client.robot.setExtraConfigSpaceBounds([-10, 10, -10, 10, -10, 10, -10, 10, -10, 10, -10, 10])
fullbody.loadAllLimbs("static", nbSamples=10000)
q = fullbody.referenceConfig[::] + [0] * 6
fullbody.setCurrentConfig(q)
com = fullbody.getCenterOfMass()
contacts = [fullbody.rLegId, fullbody.lLegId, fullbody.rArmId, fullbody.lArmId]
state = State(fullbody, q=q, limbsIncontact=contacts)
self.assertTrue(state.isBalanced())
self.assertTrue(state.isValid()[0])
self.assertTrue(state.isLimbInContact(fullbody.rLegId))
self.assertTrue(state.isLimbInContact(fullbody.lLegId))
self.assertTrue(state.isLimbInContact(fullbody.rArmId))
self.assertTrue(state.isLimbInContact(fullbody.lArmId))
self.assertEqual(com, state.getCenterOfMass())
# remove rf contact :
state1, success = StateHelper.removeContact(state, fullbody.rLegId)
self.assertTrue(success)
self.assertFalse(state1.isLimbInContact(fullbody.rLegId))
self.assertTrue(state1.isLimbInContact(fullbody.lLegId))
self.assertTrue(state.isLimbInContact(fullbody.rArmId))
self.assertTrue(state.isLimbInContact(fullbody.lArmId))
self.assertEqual(com, state1.getCenterOfMass())
# create a new contact :
n = [0, 0, 1]
p = [0.45, -0.2, 0.002]
state2, success = StateHelper.addNewContact(state1, fullbody.rLegId, p, n)
self.assertTrue(success)
self.assertTrue(state2.isLimbInContact(fullbody.rLegId))
self.assertTrue(state2.isLimbInContact(fullbody.lLegId))
self.assertTrue(state.isLimbInContact(fullbody.rArmId))
self.assertTrue(state.isLimbInContact(fullbody.lArmId))
self.assertTrue(state2.isBalanced())
p_real, n_real = state2.getCenterOfContactForLimb(fullbody.rLegId)
self.assertEqual(n, n_real)
def genStates(fullbody, limbs, num_samples=1000, coplanar=True):
q_0 = fullBody.getCurrentConfig()
#~ fullBody.getSampleConfig()
qs = []
states = []
for _ in range(num_samples):
if (coplanar):
q = fullBody.generateGroundContact(limbs)
else:
q = _genbalance(fullBody, limbs)
if len(q) > 1:
qs.append(q)
s = State(fullbody, q=q, limbsIncontact=limbs)
states.append(s)
return states
def createRandomState(fullBody, limbsInContact):
extraDof = int(fullBody.client.robot.getDimensionExtraConfigSpace())
q0 = fullBody.referenceConfig[::]
if extraDof > 0:
q0 += [0] * extraDof
qr = fullBody.shootRandomConfig()
root = SE3.Identity()
root.translation = np.matrix(qr[0:3]).T
# sample random orientation along z :
root = sampleRotationAlongZ(root)
q0[0:7] = se3ToXYZQUATtuple(root)
# apply random config to legs (FIXME : ID hardcoded for Talos)
q0[7:19] = qr[7:19]
fullBody.setCurrentConfig(q0)
s0 = State(fullBody, q=q0, limbsIncontact=limbsInContact)
return s0
def computeIntermediateState(sfrom, sto):
sid = sfrom.cl.computeIntermediary(sfrom.sId, sto.sId)
return State(sfrom.fullBody, sid, False)
def planToStates(fullBody, configs):
states = []
for i, _ in enumerate(configs):
states += [State(fullBody, i)]
return states
"""
tStart = time.time()
configsFull = fullBody.interpolate(0.001,pathId=pId,robustnessTreshold = 1, filterStates = False)
tInterpolateConfigs = time.time() - tStart
print "number of configs :", len(configsFull)
"""
q_init[0] += 0.05
createSphere('s',r)
n = [0,0,1]
p = [0,0.1,0]
q_init[-6:-3] = [0.2,0,0]
q_goal[-6:-3] = [0.1,0,0]
sf = State(fullBody,q=q_goal,limbsIncontact=[lLegId,rLegId])
si = State(fullBody,q=q_init,limbsIncontact=[lLegId,rLegId])
n = [0.0, -0.42261828000211843, 0.9063077785212101]
p = [0.775, 0.22, -0.019]
moveSphere('s',r,p)
smid,success = StateHelper.addNewContact(si,lLegId,p,n,100)
assert(success)
smid2,success = StateHelper.addNewContact(sf,lLegId,p,n,100)
assert(success)
r(smid.q())
sf2 = State(fullBody,q=q_goal,limbsIncontact=[lLegId,rLegId])
0.0,
0.0
] + [0] * 6
q_ref[0:2] = [-3., -1.7575]
v(q_ref) # move root position
q_ref[2] -= 0.015
fb.setReferenceConfig(q_ref)
fb.setPostureWeights(fb.postureWeights_straff[::] + [0] * 6)
# create hand contacts :
pRH = SE3.Identity()
pRH = rotatePlacement(pRH, 'y', -0.8115781021773631)
n = computeContactNormal(pRH).T.tolist()[0]
state = State(fb, q=q_ref, limbsIncontact=[fb.rLegId, fb.lLegId])
pLHand = [-2.75, -1.235, 1.02]
pRHand = [-2.75, -2.28, 1.02]
"""
from tools.display_tools import *
createSphere('s',v)
moveSphere('s',v,pLHand)
"""
state, success = StateHelper.addNewContact(state,
fb.rArmId,
pRHand,
n,
lockOtherJoints=True)
assert success, "Unable to create contact for right hand"
state, success = StateHelper.addNewContact(state,
if not successCOM:
# is it really an issue ?
print("Unable to project CoM in the center of the support polygone")
v(sres.q())
return sres
import tools.display_tools as disp
disp.createSphere("c",v,color = v.color.green)
v.addLandmark("c",0.1)
q = fullBody.getCurrentConfig()
v(q)
s = State(fullBody, q = q, limbsIncontact = [fullBody.lLegId, fullBody.rLegId])
idfirst = 2
#coms[0] = array(s.getCenterOfMass())
coms[0][2] -= 0.02
def normal(phase):
s = phase["S"][0]
i = 0
n = np.zeros(3)
while i < s.shape[1]-3 and norm(n) < 1e-4 :
n = cross(s[:,i+1] - s[:,i+0], s[:,i+2] - s[:,i+0])
i+=1
if norm(n) < 1e-4:
return Z_AXIS
