cf previous

script/tools/tro_capture_point/gen_hrp2_statically_balanced_positions_2d.py

@@ -8,6 +8,8 @@ from hpp.corbaserver.rbprm.tools.cwc_trajectory import *
 from hpp import Error as hpperr
 from numpy import array, matrix
 
+import sample_com_vel as scv
+
 
 packageName = "hrp2_14_description"
 meshPackageName = "hrp2_14_description"
@@ -27,20 +29,22 @@ fullBody.setJointBounds ("base_joint_xyz", [-0.135,2, -1, 1, 0, 2.2])
 ps = ProblemSolver( fullBody )
 r = Viewer (ps)
 
+n_samples = 10000
+
 #~ AFTER loading obstacles
 rLegId = '0rLeg'
 rLeg = 'RLEG_JOINT0'
 rLegOffset = [0,-0.105,0,]
 rLegNormal = [0,1,0]
 rLegx = 0.09; rLegy = 0.05
-fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 10000, "manipulability", 0.1)
+fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, n_samples, "manipulability", 0.1)
 
 lLegId = '1lLeg'
 lLeg = 'LLEG_JOINT0'
 lLegOffset = [0,-0.105,0]
 lLegNormal = [0,1,0]
 lLegx = 0.09; lLegy = 0.05
-fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "manipulability", 0.1)
+fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, n_samples, "manipulability", 0.1)
 
 rarmId = '3Rarm'
 rarm = 'RARM_JOINT0'
@@ -51,7 +55,7 @@ rArmOffset = [-0.045,-0.01,-0.085]
 rArmNormal = [1,0,0]
 rArmx = 0.015; rArmy = 0.02
 #disabling collision for hook
-fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 10000, "manipulability", 0.05, "_6_DOF", True)
+fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, n_samples, "manipulability", 0.05, "_6_DOF", True)
 
 #~ AFTER loading obstacles
 larmId = '4Larm'
@@ -61,7 +65,7 @@ lHand = 'LARM_JOINT5'
 lArmOffset = [-0.045,0.01,-0.085]
 lArmNormal = [1,0,0]
 lArmx = 0.015; lArmy = 0.02
-fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, "manipulability", 0.05, "_6_DOF", True)
+fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, n_samples, "manipulability", 0.05, "_6_DOF", True)
 
 limbsCOMConstraints = { rLegId : {'file': "hrp2/RL_com.ineq", 'effector' : 'RLEG_JOINT5'},  
 						lLegId : {'file': "hrp2/LL_com.ineq", 'effector' : 'LLEG_JOINT5'}, 
@@ -148,6 +152,44 @@ def fill_contact_points(limbs, config, config_pinocchio):
 		res["N"] += N
 	return res
 	
+	
+from random import randint
+
+def pos_quat_to_pinocchio(q):
+	q_res = q[:]
+	quat_end = q[4:7]
+	q_res[6] = q[3]
+	q_res[3:6] = quat_end
+	return q_res
+
+def gen_contact_candidates_one_limb(limb, config_gepetto, res, num_candidates = 10):
+	effectorName = limbsCOMConstraints[limb]['effector']
+	candidates = []
+	for i in range(num_candidates):
+		fullBody.setCurrentConfig(fullBody.getSample(limb,randint(0,n_samples - 1)))
+		#~ m = _getTransform(fullBody.getJointPosition(effectorName))
+		candidates.append(pos_quat_to_pinocchio(fullBody.getJointPosition(effectorName)))
+	res["candidates"][effectorName] = candidates
+		
+		
+def find_limbs_broken(target_c, config, limbs):
+	
+	
+def gen_contact_candidates(limbs, config_gepetto, res):
+	#first generate a com translation current configuration
+	success, dc, delta_c = scv.comTranslationAfter07s(res["q"], res["contact_points"])
+	#set it as new com objective for projection
+	fullBody.setCurrentConfig(config_gepetto)
+	c = array(fullBody.getCenterOfMass())
+	target_c = c + delta_c
+	state_id = fullBody.createState(config, limbs)
+	res["candidates"] = {}
+	brokenContacts = []
+	candidateLimbs = []
+	if(fullBody.projectStateToCOM(state_id,target_c)): #all good, all contacts kinematically maintained
+		for limb in limbs:
+			gen_contact_candidates_one_limb(limb, config_gepetto, res, 10)
+	
 from numpy import cos, sin, pi
 def __eulerToQuat(pitch, roll, yaw):
 	t0 = cos(yaw * 0.5);
@@ -264,4 +306,3 @@ gen(limbs[0], 10)
 	
 i = 0
 
-import sample_com_vel as scv

script/tools/tro_capture_point/sample_com_vel.py

@@ -133,7 +133,7 @@ def gen_com_vel(q0, contacts):
 								
 def comTranslationAfter07s(q_hpp, contacts):
 	(success, dc0)= gen_com_vel(q_pin(q_hpp), contacts)
-	return success, dc0 * 0.7
+	return success, dc0, dc0 * 0.7
 	
 
 np.set_printoptions(precision=2, suppress=True);

src/rbprmbuilder.impl.cc

@@ -638,6 +638,7 @@ namespace hpp {
 
         }
         lastStatesComputed_.push_back(state);
+        lastStatesComputedTime_.push_back(std::make_pair(-1., state));
         return lastStatesComputed_.size()-1;
     }