Commit c2cdb581 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

optimization working

parent e7ac0877
......@@ -415,6 +415,7 @@ class FullBody (object):
def getContactCone(self, stateId, friction = 0.5):
H_h = array(self.client.rbprm.rbprm.getContactCone(stateId, friction))
print "H_h", H_h.shape
print "norm h", ( H_h[:, -1] != 0).any()
# now decompose cone
return H_h[:,:-1], H_h[:, -1]
......@@ -425,6 +426,7 @@ class FullBody (object):
def getContactIntermediateCone(self, stateId, friction = 0.5):
H_h = array(self.client.rbprm.rbprm.getContactIntermediateCone(stateId, friction))
print "H_h", H_h.shape
print "norm h", ( H_h[:, -1] != 0).any()
# now decompose cone
return H_h[:,:-1], H_h[:, -1]
......
......@@ -96,9 +96,10 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
use_window = max(0, min(use_window, (len(states) - 1) - (state_id + 2))) # can't use preview if last state is reached
assert( len(phase_dt) >= 2 + use_window * 2 ), "phase_dt does not describe all phases"
#~ constraints = ['cones_constraint', 'end_reached_constraint','end_speed_constraint']
constraints = ['cones_constraint', 'end_reached_constraint','end_speed_constraint']
#~ constraints = ['end_reached_constraint']
#~ constraints = ['cones_constraint', 'end_reached_constraint']
constraints = ['end_reached_constraint']
#~ constraints = ['cones_constraint']
#~ constraints = ['cones_constraint', 'end_reached_constraint','end_speed_constraint', 'com_kinematic_constraint']
param_constraints = []
mass = fullBody.getMass()
......
......@@ -161,24 +161,12 @@ trackedEffectors = []):
global trajec
global trajec_mil
frame_rate = 1./24.
#~ frame_rate_andrea = 1./1000.
frame_rate_andrea = 1./1000.
#~ if(len(trajec) > 0):
#~ frame_rate = 1./25.
#~ frame_rate_andrea = 1./1001.
new_traj = gen_trajectory_to_play(fullBody, pp, trajectory, time_per_path, frame_rate)
new_traj_andrea = gen_trajectory_to_play(fullBody, pp, trajectory, time_per_path,frame_rate_andrea)
#~ new_contacts = gencontactsPerFrame(fullBody, i, limbsCOMConstraints, pp, trajectory, times, frame_rate_andrea)
Ps, Ns, freeEffectorsPerPhase, Ks = genPandNandConesperFrame(fullBody, i, limbsCOMConstraints, cones, pp, trajectory, time_per_path, frame_rate_andrea)
NPeffs = genPEffperFrame(fullBody, freeEffectorsPerPhase, new_traj_andrea, pp, time_per_path, frame_rate_andrea)
com = genComPerFrame(final_state, dt, frame_rate_andrea)
#~ if(len(trajec) > 0):
#~ new_traj = new_traj[1:]
#~ new_traj_andrea = new_traj_andrea[1:]
#~ Ps = Ps[1:]
#~ Ns = Ns[1:]
#~ com = com[1:]
#~ NPeffs = NPeffs[1:]
trajec = trajec + new_traj
trajec_mil += new_traj_andrea
#~ global contacts
......
Supports Markdown
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