def check_kin_feasibility(s0Id, s1Id):
res = fullBody.isReachableFromState(s0Id, s1Id, True)
if not res[0]:
print "Error : isReachable returned False !"
raise Exception('Error : isReachable returned False !')
r(configs[s0Id])
c0 = np.array(fullBody.getCenterOfMass())
c1 = np.array(res[1])
c2 = np.array(res[2])
r(configs[s1Id])
c3 = np.array(fullBody.getCenterOfMass())
s0 = State(fullBody, sId=s0Id)
s1 = State(fullBody, sId=s1Id)
s0_orig = State(s0.fullBody,
q=s0.q(),
limbsIncontact=s0.getLimbsInContact())
s1_orig = State(s1.fullBody,
q=s1.q(),
limbsIncontact=s1.getLimbsInContact())
moving_limb = s0.contactsVariations(s1)
smid, successMid = StateHelper.removeContact(s0_orig, moving_limb[0])
if not successMid:
return False
successFeasible = True
print "check first part"
successFeasible = successFeasible and check_traj(s0_orig, c0, c1)
print "check mid part"
successFeasible = successFeasible and check_traj(smid, c1, c2)
print "check last part"
successFeasible = successFeasible and check_traj(s1_orig, c2, c3, True)
return successFeasible
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 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
