optimization working

src/hpp/corbaserver/rbprm/rbprmfullbody.py

@@ -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]
 		

src/hpp/corbaserver/rbprm/tools/cwc_trajectory.py

@@ -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()

src/hpp/corbaserver/rbprm/tools/cwc_trajectory_helper.py

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