generate random config in contact updated

script/scenarios/ground_crouch_hyq_interp.py

@@ -126,15 +126,125 @@ limbsCOMConstraints = { rLegId : {'file': "hyq/"+rLegId+"_com.ineq", 'effector'
 
 
 def act(i, numOptim = 0, use_window = 0, friction = 0.5, optim_effectors = True, time_scale = 20,  verbose = False, draw = False, trackedEffectors = []):
-	return step(fullBody, configs, i, numOptim, pp, limbsCOMConstraints, friction, optim_effectors = optim_effectors, time_scale = time_scale, useCOMConstraints = True, use_window = use_window,
+	return step(fullBody, configs, i, numOptim, pp, limbsCOMConstraints, friction, optim_effectors = optim_effectors, time_scale = time_scale, useCOMConstraints = False, use_window = use_window,
 	verbose = verbose, draw = draw, trackedEffectors = trackedEffectors)
 
 def play(frame_rate = 1./24.):
 	play_traj(fullBody,pp,frame_rate)
 	
 def saveAll(name):
-	saveAllData(fullBody, r, name)
+	return saveAllData(fullBody, r, name)
 #~ saveAll ('hole_hyq_t_var_04f_andrea');
 #~ fullBody.exportAll(r, configs, 'hole_hyq_t_var_04f_andrea_contact_planning');
 #~ saveToPinocchio('obstacle_hyq_t_var_04f_andrea')
 
+
+
+from numpy import array, zeros
+from numpy import matrix
+from numpy import matlib
+
+#~ for i in range(6,25):
+	#~ act(i, 60, optim_effectors = True)
+
+#~ pid = act(8,0,optim_effectors = False)
+from hpp.corbaserver.rbprm.tools.cwc_trajectory_helper import res
+pid = res[0]-2 
+dt = 0.001
+
+#~ qs = [ pp.client.problem.configAtParam (pid, i*dt) for i in range(600,700)]
+#~ cs = []
+#~ res = zeros((3, 100))
+#~ for i in range(len(qs)):
+	#~ q = qs[i]
+	#~ fullBody.setCurrentConfig(q[:-1]) 
+	#~ c = fullBody.getCenterOfMass()
+	#~ cs += [c[0]]
+	#~ res[:,i] = c
+
+#~ for i in range(8,9): act(i,0,optim_effectors=False, draw=False)
+#~ ddc = [2 *(cs[i-1] - c[i])  for i in range(1, len(c))]
+#~ ddcx = [ddci[0] for ddci in ddc]
+
+
+#~ res = zeros((3, 100))
+
+#~ for i,ci in enumerate(c):
+	#~ res[:,i] = ci
+
+#~ m = matrix(res)
+
+
+from derivative_filters import *
+
+#~ res = computeSecondOrderPolynomialFitting(m, dt, 11)
+#~ (x, dx, ddx) = res
+
+import matplotlib.pyplot as plt
+import plot_utils 
+
+#~ plt.plot(ddx[0,:].A.squeeze())
+#~ plt.show()
+
+from hpp.corbaserver.rbprm.tools.cwc_trajectory_helper import trajec_mil
+from hpp.corbaserver.rbprm.tools.cwc_trajectory_helper import trajec
+
+#~ plt.plot(x[0,:].A.squeeze())
+#~ plt.plot(dx[0,:].A.squeeze())
+#~ plt.plot(ddx[2,:].A.squeeze())
+#~ plt.plot(dx[0,:].A.squeeze())
+#~ plt.plot(x[0,:].A.squeeze())
+
+#~ plt.show()
+
+#~ testc= []
+#~ trajec_mil = trajec
+sample = len(trajec_mil)
+
+dt_me = 1./24.
+dt = 1./1000.
+
+res = zeros((3, sample))
+
+for i,q in enumerate(trajec_mil[:sample]):
+	fullBody.setCurrentConfig(q) 
+	c = fullBody.getCenterOfMass()
+	res[:,i] = c
+	
+#~ for i,q in enumerate(trajec):
+	#~ fullBody.setCurrentConfig(q) 
+	#~ c = fullBody.getCenterOfMass()
+	#~ testc += [array(c)]
+	
+
+#~ testddc = [2 *(testc[i-1] - testc[i])  for i in range(1, len(testc))]
+#~ testddcx = [ddc[0] for ddc in testddc]
+#~ testcx = [c[0] for c in testc]
+
+#~ cs = res
+#~ res = zeros((3, 100))
+#~ for c in cs:
+	#~ res[:,i] = c	
+m = matrix(res)
+
+
+from derivative_filters import *
+
+res = computeSecondOrderPolynomialFitting(m, dt, 51)
+(x, dx, ddx) = res
+
+
+import matplotlib.pyplot as plt
+import plot_utils 
+
+#~ if(conf.SHOW_FIGURES and conf.PLOT_REF_COM_TRAJ):
+#~ plt.plot(x[0,:].A.squeeze())
+#~ plt.plot(dx[0,:].A.squeeze())
+plt.plot(ddx[0,:].A.squeeze())
+#~ plt.plot(x[0,:].A.squeeze())
+#~ plt.plot(testddcx)
+#~ plt.plot(testcx)
+#~ plt.show()
+
+
+	

script/tools/4Larm_3Rarm_configs

+(lp0
+(dp1
+S'q'
+p2
+(lp3
+I0
+aI0
+aF0.5
+acnumpy.core.multiarray
+scalar
+p4
+(cnumpy
+dtype
+p5
+(S'f8'
+p6
+I0
+I1
+tp7
+Rp8
+(I3
+S'<'
+p9
+NNNI-1
+I-1
+I0
+tp10
+bS'\x07y[\x0f\xfa\xb6\xbe?'
+p11
+tp12
+Rp13
+ag4
+(g8
+S'\x162\x946\x02\xf9\xad?'
+p14
+tp15
+Rp16
+ag4
+(g8
+S'\x8d\x87\xce\xf5\x89\x07}\xbf'
+p17
+tp18
+Rp19
+ag4
+(g8
+S'\x82\xde\x02\x0ew\xb6\xef?'
+p20
+tp21
+Rp22
+aF0.15138907645889144
+aF0.3211587405900267
+aF0.28800158509873386
+aF-0.39862030071022797
+aF-1.142594297144275
+aF0.8461975130542807
+aF-0.716933232118391
+aF-1.3752892242201837
+aF1.460833427947356
+aF-0.17173318529149206
+aF0.17
+aF-1.7307415230151877
+aF-0.5012923214099216
+aF-0.13113843087220478
+aF-1.0648664713505007
+aF-0.37543613542408516
+aF-0.12425520351825048
+aF0.1683436040782107
+aF0.0715783510439375
+aF0.10958815454083037
+aF-1.3491425861398283
+aF0.8777987481470152
+aF-0.38690251132945375
+aF-0.48568610580169924
+aF-0.622389873622945
+aF0.32797132591569067
+aF-0.40233126109996586
+aF0.23739109433926298
+aF0.5381614663937087
+aF0.0025525110049371413
+asS'contact_points'
+p23
+(dp24
+S'RARM_JOINT5'
+p25
+(dp26
+S'P'
+p27
+(lp28
+(lp29
+F0.004999999999999982
+aF-0.045
+aF0.06499999999999996
+aa(lp30
+F0.004999999999999992
+aF-0.04499999999999999
+aF0.10499999999999995
+aa(lp31
+F-0.02500000000000001
+aF-0.044999999999999984
+aF0.10499999999999997
+aa(lp32
+F-0.025000000000000015
+aF-0.04499999999999999
+aF0.06499999999999997
+aasS'N'
+p33
+(lp34
+(lp35
+F-1.7763568394002505e-15
+aF0.9999999999999973
+aF1.27675647831893e-15
+aa(lp36
+F-1.7763568394002505e-15
+aF0.9999999999999973
+aF1.27675647831893e-15
+aa(lp37
+F-1.7763568394002505e-15
+aF0.9999999999999973
+aF1.27675647831893e-15
+aa(lp38
+F-1.7763568394002505e-15
+aF0.9999999999999973
+aF1.27675647831893e-15
+aassS'LARM_JOINT5'
+p39
+(dp40
+g27
+(lp41
+(lp42
+F0.02499999999999998
+aF-0.045
+aF0.06499999999999996
+aa(lp43
+F0.024999999999999988
+aF-0.04499999999999999
+aF0.10499999999999995
+aa(lp44
+F-0.00500000000000001
+aF-0.044999999999999984
+aF0.10499999999999997
+aa(lp45
+F-0.00500000000000002
+aF-0.04499999999999999
+aF0.06499999999999997
+aasg33
+(lp46
+(lp47
+F-4.440892098500626e-16
+aF0.9999999999999978
+aF2.609024107869118e-15
+aa(lp48
+F-4.440892098500626e-16
+aF0.9999999999999978
+aF2.609024107869118e-15
+aa(lp49
+F-4.440892098500626e-16
+aF0.9999999999999978
+aF2.609024107869118e-15
+aa(lp50
+F-4.440892098500626e-16
+aF0.9999999999999978
+aF2.609024107869118e-15
+aasssg33
+(lp51
+g47
+ag48
+ag49
+ag50
+ag35
+ag36
+ag37
+ag38
+asg27
+(lp52
+g42
+ag43
+ag44
+ag45
+ag29
+ag30
+ag31
+ag32
+asa(dp53
+g2
+(lp54
+I0
+aI0
+aF0.5
+ag4
+(g8
+S'x\x1a\x1b\xab\xdb\xab\xd4?'
+p55
+tp56
+Rp57
+ag4
+(g8
+S'\x1e\xa9\xee\xb4UT\xd7?'
+p58
+tp59
+Rp60
+ag4
+(g8
+S'\x00\xda\x16\x94=x\xc1\xbf'
+p61
+tp62
+Rp63
+ag4
+(g8
+S'\xe2~\x8f\n\xdf\x9a\xeb?'
+p64
+tp65
+Rp66
+aF0.7013245155444567
+aF0.5897292099742067
+aF-0.4269297653034999
+aF0.22084241201193522
+aF-0.77231972919818
+aF-0.06684907163937485
+aF1.2229776793457001
+aF-0.3322704670906287
+aF0.643049029757431
+aF-1.3997071449684944
+aF0.17
+aF-2.665095598467181
+aF-0.9963845172218264
+aF-0.3791688783736289
+aF-0.7641181167736735
+aF1.1719583286027697
+aF0.490728166554695
+aF0.17238502242014977
+aF0.3198769833082753
+aF0.41091346094917347
+aF-1.0356137123246527
+aF1.436752476120596
+aF-0.8702891748497827
+aF-0.05183788899786157
+aF-0.6405721907023728
+aF-0.5928399922812343
+aF0.3786274548139339
+aF1.7722062589346617
+aF-0.4286325616973725
+aF0.5557594246224912
+asg23
+(dp67
+g25
+(dp68
+g27
+(lp69
+(lp70
+F0.004999999999999982
+aF-0.045
+aF0.06499999999999996
+aa(lp71
+F0.004999999999999992
+aF-0.04499999999999999
+aF0.10499999999999995
+aa(lp72
+F-0.02500000000000001
+aF-0.044999999999999984
+aF0.10499999999999997
+aa(lp73
+F-0.025000000000000015
+aF-0.04499999999999999
+aF0.06499999999999997
+aasg33
+(lp74
+(lp75
+F5.551115123125783e-16
+aF0.9999999999999988
+aF-1.1102230246251565e-16
+aa(lp76
+F5.551115123125783e-16
+aF0.9999999999999988
+aF-1.1102230246251565e-16
+aa(lp77
+F5.551115123125783e-16
+aF0.9999999999999988
+aF-1.1102230246251565e-16
+aa(lp78
+F5.551115123125783e-16
+aF0.9999999999999988
+aF-1.1102230246251565e-16
+aassg39
+(dp79
+g27
+(lp80
+(lp81
+F0.02499999999999998
+aF-0.045
+aF0.06499999999999996
+aa(lp82
+F0.024999999999999988
+aF-0.04499999999999999
+aF0.10499999999999995
+aa(lp83
+F-0.00500000000000001
+aF-0.044999999999999984
+aF0.10499999999999997
+aa(lp84
+F-0.00500000000000002
+aF-0.04499999999999999
+aF0.06499999999999997
+aasg33
+(lp85
+(lp86
+F5.551115123125783e-17
+aF0.9999999999999968
+aF8.881784197001252e-16
+aa(lp87
+F5.551115123125783e-17
+aF0.9999999999999968
+aF8.881784197001252e-16
+aa(lp88
+F5.551115123125783e-17
+aF0.9999999999999968
+aF8.881784197001252e-16
+aa(lp89
+F5.551115123125783e-17
+aF0.9999999999999968
+aF8.881784197001252e-16
+aasssg33
+(lp90
+g86
+ag87
+ag88
+ag89
+ag75
+ag76
+ag77
+ag78
+asg27
+(lp91
+g81
+ag82
+ag83
+ag84
+ag70
+ag71
+ag72
+ag73
+asa.
\ No newline at end of file

script/tools/gen_hrp2_statically_balanced_positions_2d.py

@@ -6,7 +6,7 @@ from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
 from hpp.corbaserver.rbprm.tools.cwc_trajectory import *
 
 from hpp import Error as hpperr
-from numpy import array
+from numpy import array, matrix
 
 
 packageName = "hrp2_14_description"
@@ -91,7 +91,17 @@ def _getTransform(qEffector):
 		rm[m,3] = qEffector[m]
 	rm[3,3] = 1
 	return rm
-				
+			
+from numpy.linalg import norm
+def __loosely_z_aligned(limb, config):
+	fullBody.setCurrentConfig(config)
+	effectorName = limbsCOMConstraints[limb]['effector']
+	m = _getTransform(fullBody.getJointPosition(effectorName))
+	P, N = fullBody.computeContactForConfig(config, limb)
+	#~ N_world = m.dot(array(N[0]+[1]))[:3]
+	N_world = m[:3,:3].dot(array(N[0]))
+	N_world = N_world / np.linalg.norm(N_world)
+	return N_world.dot(array([0,0,1])) > 0.7
 
 def draw_cp(cid, limb, config):
 	global r
@@ -137,26 +147,75 @@ def fill_contact_points(limbs, config, config_pinocchio):
 		#~ res["N"] += [n for i, n in enumerate (posetc) if (i%2 == 1)]
 		res["N"] += N
 	return res
+	
+from numpy import cos, sin, pi
+def __eulerToQuat(pitch, roll, yaw):
+	t0 = cos(yaw * 0.5);
+	t1 = sin(yaw * 0.5);
+	t2 = cos(roll * 0.5);
+	t3 = sin(roll * 0.5);
+	t4 = cos(pitch * 0.5);
+	t5 = sin(pitch * 0.5);
+	w= t0 * t2 * t4 + t1 * t3 * t5;
+	x= t0 * t3 * t4 - t1 * t2 * t5;
+	y= t0 * t2 * t5 + t1 * t3 * t4;
+	z= t1 * t2 * t4 - t0 * t3 * t5;
+	return [w, x, y, z]
+	#~ 
+#~ void SampleRotation(model::DevicePtr_t so3, ConfigurationPtr_t config, JointVector_t& jv)
+#~ {
+	#~ std::size_t id = 1;
+	#~ if(so3->rootJoint())
+	#~ {
+		#~ Eigen::Matrix <value_type, 3, 1> confso3;
+		#~ id+=1;
+		#~ model::JointPtr_t joint = so3->rootJoint();
+		#~ for(int i =0; i <3; ++i)
+		#~ {
+			#~ joint->configuration()->uniformlySample (i, confso3);
+		#~ }
+		#~ Eigen::Quaterniond qt = Eigen::AngleAxisd(confso3(0), Eigen::Vector3d::UnitZ())
+		  #~ * Eigen::AngleAxisd(confso3(1), Eigen::Vector3d::UnitY())
+		  #~ * Eigen::AngleAxisd(confso3(2), Eigen::Vector3d::UnitX());
+		#~ std::size_t rank = 3;
+		#~ (*config)(rank+0) = qt.w();
+		#~ (*config)(rank+1) = qt.x();
+		#~ (*config)(rank+2) = qt.y();
+		#~ (*config)(rank+3) = qt.z();
+	#~ }
+	#~ if(id < jv.size())
+		#~ SampleRotationRec(config,jv,id);
+#~ }
+	
+from random import uniform
+def _boundSO3(q, num_limbs):
+	q[:3] = [0,0,0.5]
+	limb_weight = float(4 - num_limbs)
+	#generate random angle 
+	rot_y = uniform(-pi/(4+limb_weight), pi/(4+limb_weight))
+	rot_x = uniform(-pi/8, pi/(3+limb_weight))
+	
+	rot_z = 0;
+	q[3:7] = __eulerToQuat(rot_x, rot_y, rot_z)
+	return q
+	
 
 def _genbalance(limbs):
 	for i in range(10000):
 		q = fullBody.client.basic.robot.shootRandomConfig()
-		q[:2] = [0,0]
-		if fullBody.isConfigValid and fullBody.isConfigBalanced(q, limbs, 5):
-			#check normals
-			_, N = fullBody.computeContactForConfig(config, limbs[0])
-			_, N1 = fullBody.computeContactForConfig(config, limbs[1])
-			if (array(N[0]).dot(array([0,0,1])) > 0.5 and array(N1[0]).dot(array([0,0,1])) > 0.5):
-				return q
+		q = _boundSO3(q, len(limbs))
+		if fullBody.isConfigValid(q)[0] and fullBody.isConfigBalanced(q, limbs, 5) and __loosely_z_aligned(limbs[0], q) and __loosely_z_aligned(limbs[1], q):
+		#~ if fullBody.isConfigValid(q)[0] and  __loosely_z_aligned(limbs[0], q) and __loosely_z_aligned(limbs[1], q):
+			return q
 	print "can't generate equilibrium config"
 
 all_qs = []
-def gen(limbs):
+def gen(limbs, num_samples = 1000, coplanar = True):
 	q_0 = fullBody.getCurrentConfig(); 
 	#~ fullBody.getSampleConfig()
 	qs = []; qs_gepetto = []; states = []
-	for _ in range(10):
-		if(len(limbs) == 2):
+	for _ in range(num_samples):
+		if(coplanar):
 			q = fullBody.generateGroundContact(limbs)
 		else:
 			q = _genbalance(limbs)
@@ -173,6 +232,8 @@ def gen(limbs):
 	for lname in limbs:
 		fname += lname + "_"
 	fname += "configs"
+	if(coplanar):
+		fname += "_coplanar"
 	from pickle import dump
 	#~ f1=open("configs_feet_on_ground_static_eq", 'w+')
 	f1=open(fname, 'w+')
@@ -191,6 +252,12 @@ q_init =  [
         ]; r (q_init)
         
 limbs = [[lLegId,rLegId],[lLegId,rLegId, rarmId], [lLegId,rLegId, larmId], [lLegId,rLegId, rarmId, larmId] ]
+#~ limbs = [[lLegId,rLegId, rarmId]]
+#~ limbs = [[larmId, rarmId]]
 
+#~ gen(limbs[0], 10)
 for ls in limbs:
-	gen(ls)
+	gen(ls, 1000, False)
+gen(limbs[0], 1000)
+	
+i = 0

src/rbprmbuilder.impl.cc

@@ -234,6 +234,8 @@ namespace hpp {
     std::vector<fcl::Vec3f> computeRectangleContact(const rbprm::RbPrmFullBodyPtr_t device,
                                                     const rbprm::State& state)
     {
+        device->device_->currentConfiguration(state.configuration_);
+        device->device_->computeForwardKinematics();
         std::vector<fcl::Vec3f> res;
         const rbprm::T_Limb& limbs = device->GetLimbs();
         rbprm::RbPrmLimbPtr_t limb;
@@ -244,6 +246,11 @@ namespace hpp {