print"Contact plan generated in : "+str(tInterpolateConfigs)+" s"
print"number of configs :",len(configs)
#raw_input("Press Enter to display the contact sequence ...")
#displayContactSequence(v,configs)
"""
if len(configs) < 2 :
cg_success = False
print "Error during contact generation."
else:
cg_success = True
print "Contact generation Done."
if abs(configs[-1][0] - tp.q_goal[0]) < 0.01 and abs(configs[-1][1]- tp.q_goal[1]) < 0.01 and (len(fullBody.getContactsVariations(len(configs)-2,len(configs)-1))==1):
print "Contact generation successful."
cg_reach_goal = True
else:
print "Contact generation failed to reach the goal."
cg_reach_goal = False
if len(configs) > 5 :
cg_too_many_states = True
cg_success = False
print "Discarded contact sequence because it was too long."