Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Guilhem Saurel
hpp-rbprm-corba
Commits
49f5ca89
Commit
49f5ca89
authored
Oct 06, 2016
by
Steve Tonneau
Browse files
generate random config in contact updated
parent
076b4f0f
Changes
9
Expand all
Hide whitespace changes
Inline
Side-by-side
script/scenarios/ground_crouch_hyq_interp.py
View file @
49f5ca89
...
...
@@ -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
=
Tru
e
,
use_window
=
use_window
,
return
step
(
fullBody
,
configs
,
i
,
numOptim
,
pp
,
limbsCOMConstraints
,
friction
,
optim_effectors
=
optim_effectors
,
time_scale
=
time_scale
,
useCOMConstraints
=
Fals
e
,
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/1lLeg_0rLeg_3Rarm_4Larm_configs
View file @
49f5ca89
This diff is collapsed.
Click to expand it.
script/tools/1lLeg_0rLeg_3Rarm_configs
View file @
49f5ca89
This diff is collapsed.
Click to expand it.
script/tools/1lLeg_0rLeg_4Larm_configs
View file @
49f5ca89
This diff is collapsed.
Click to expand it.
script/tools/1lLeg_0rLeg_configs
View file @
49f5ca89
This diff is collapsed.
Click to expand it.
script/tools/1lLeg_0rLeg_configs_coplanar
0 → 100644
View file @
49f5ca89
This diff is collapsed.
Click to expand it.
script/tools/4Larm_3Rarm_configs
0 → 100644
View file @
49f5ca89
(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
View file @
49f5ca89
...
...
@@ -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
View file @
49f5ca89
...
...
@@ -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 {