Commit 0d95bbf2 authored by ggory15's avatar ggory15
Browse files

locomote test update

parent 57440ba2
......@@ -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("")
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment