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_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
