locomote test update

script/locomote_test.py

@@ -12,7 +12,6 @@ cs = ContactSequenceHumanoid(0)
 CONTACT_SEQUENCE_XML_TAG = "ContactSequence"
 
 cs.loadFromXML("contact_sequence.xml", CONTACT_SEQUENCE_XML_TAG)
-
 q_init = cs.contact_phases[0].reference_configurations[0].copy()
 num_phases = cs.size()
 
@@ -38,7 +37,6 @@ isInit = True
 isFinal = True
 
 for k in range(num_phases):
-    print k, "th"
     contact_phase = cs.contact_phases[k]
     init_guess_provided = (len(contact_phase.state_trajectory) > 0)
 
@@ -154,8 +152,9 @@ for i in range(len(RF)):
 for i in range(len(LF)):
     tp.setPhase(i + len(RF), timeopt.phase(timeopt.EndeffectorID.LF, LF_time[i][0, 0], LF_time[i][0, 1], LF[i].translation, LF[i].rotation))
 
+print "set configuration file for time-optimization"
 import os
-cfg_path=str(os.path.dirname(os.path.abspath(__file__))) + '/../../../../config/' + 'cfg_momSc_demo03.yaml'
+cfg_path=str(os.path.dirname(os.path.abspath(__file__))) + '/../config/' + 'cfg_momSc_demo03.yaml'
 tp.setConfigurationFile(cfg_path)
 tp.setTimeoptSolver(cfg_path)
 tp.solve()
@@ -199,5 +198,6 @@ for k in range(tp.getTrajectorySize()):
   x[6:9] = (((tp.getLMOM(k)/MASS) - (tp.getLMOM(k-1)/MASS)) / (tp.getTime(k)-tp.getTime(k-1))).tolist()
   cs.contact_phases[cnt].state_trajectory.append(np.matrix(x))
 
+print "save resultant trajectories by time optimization"
 cs.saveAsXML("Result.xml", "ContactSequence")
 print("")