Commit 49f5ca89 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

generate random config in contact updated

parent 076b4f0f
......@@ -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()
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
(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
......@@ -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
......@@ -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 {