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
Jason Chemin
hpp-rbprm-corba
Commits
9dc2c37f
Commit
9dc2c37f
authored
Nov 08, 2016
by
Steve Tonneau
Browse files
gen_com_pos_per_effector
parent
4022d15f
Changes
2
Hide whitespace changes
Inline
Side-by-side
script/tools/tro_capture_point/gen_hrp2_statically_balanced_positions_2d.py
View file @
9dc2c37f
...
...
@@ -68,12 +68,12 @@ lArmx = 0.015; lArmy = 0.02
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'
},
larmId
:
{
'file'
:
"hrp2/LA_com.ineq"
,
'effector'
:
lHand
},
rarmId
:
{
'file'
:
"hrp2/RA_com.ineq"
,
'effector'
:
rHand
}
}
lLegId
:
{
'file'
:
"hrp2/LL_com.ineq"
,
'effector'
:
'LLEG_JOINT5'
},
larmId
:
{
'file'
:
"hrp2/LA_com.ineq"
,
'effector'
:
lHand
},
rarmId
:
{
'file'
:
"hrp2/RA_com.ineq"
,
'effector'
:
rHand
}
}
#~ limbsCOMConstraints = { rLegId : {'file': "hrp2/RL_com.ineq", 'effector' : 'RLEG_JOINT5'},
#~ lLegId : {'file': "hrp2/LL_com.ineq", 'effector' : 'LLEG_JOINT5'},
#~ rarmId : {'file': "hrp2/RA_com.ineq", 'effector' : rHand} }
#~ lLegId : {'file': "hrp2/LL_com.ineq", 'effector' : 'LLEG_JOINT5'},
#~ rarmId : {'file': "hrp2/RA_com.ineq", 'effector' : rHand} }
#~
...
...
@@ -84,249 +84,262 @@ import quaternion as quat
def
_getTransform
(
qEffector
):
q0
=
quat
.
Quaternion
(
qEffector
[
3
:
7
])
rot
=
q0
.
toRotationMatrix
()
#compute rotation matrix local -> world
p
=
qEffector
[
0
:
3
]
#(0,0,0) coordinate expressed in effector fram
rm
=
np
.
zeros
((
4
,
4
))
for
k
in
range
(
0
,
3
):
for
l
in
range
(
0
,
3
):
rm
[
k
,
l
]
=
rot
[
k
,
l
]
for
m
in
range
(
0
,
3
):
rm
[
m
,
3
]
=
qEffector
[
m
]
rm
[
3
,
3
]
=
1
return
rm
q0
=
quat
.
Quaternion
(
qEffector
[
3
:
7
])
rot
=
q0
.
toRotationMatrix
()
#compute rotation matrix local -> world
p
=
qEffector
[
0
:
3
]
#(0,0,0) coordinate expressed in effector fram
rm
=
np
.
zeros
((
4
,
4
))
for
k
in
range
(
0
,
3
):
for
l
in
range
(
0
,
3
):
rm
[
k
,
l
]
=
rot
[
k
,
l
]
for
m
in
range
(
0
,
3
):
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
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
#~ posetc = fullBody.getEffectorPosition(limb, config)
P
,
N
=
fullBody
.
computeContactForConfig
(
config
,
limb
)
fullBody
.
setCurrentConfig
(
config
)
effectorName
=
limbsCOMConstraints
[
limb
][
'effector'
]
limbId
=
limb
m
=
_getTransform
(
fullBody
.
getJointPosition
(
effectorName
))
scene
=
"qds"
+
limb
+
str
(
cid
)
r
.
client
.
gui
.
createScene
(
scene
)
for
i
in
range
(
4
):
#~ pos = posetc[2*i]
print
"P"
,
array
(
P
[
i
]
+
[
1
])
print
"N"
,
array
(
N
[
i
]
+
[
1
])
print
m
.
dot
(
array
(
P
[
i
]
+
[
1
]))
pos
=
m
.
dot
(
array
(
P
[
i
]
+
[
1
]))[:
3
]
print
"pos"
,
pos
r
.
client
.
gui
.
addBox
(
scene
+
"/b"
+
str
(
i
),
0.01
,
0.01
,
0.01
,
[
1
,
0
,
0
,
1
])
r
.
client
.
gui
.
applyConfiguration
(
scene
+
"/b"
+
str
(
i
),
pos
.
tolist
()
+
[
1
,
0
,
0
,
0
])
r
.
client
.
gui
.
refresh
()
r
.
client
.
gui
.
addSceneToWindow
(
scene
,
0
)
global
r
#~ posetc = fullBody.getEffectorPosition(limb, config)
P
,
N
=
fullBody
.
computeContactForConfig
(
config
,
limb
)
fullBody
.
setCurrentConfig
(
config
)
effectorName
=
limbsCOMConstraints
[
limb
][
'effector'
]
limbId
=
limb
m
=
_getTransform
(
fullBody
.
getJointPosition
(
effectorName
))
scene
=
"qds"
+
limb
+
str
(
cid
)
r
.
client
.
gui
.
createScene
(
scene
)
for
i
in
range
(
4
):
#~ pos = posetc[2*i]
print
"P"
,
array
(
P
[
i
]
+
[
1
])
print
"N"
,
array
(
N
[
i
]
+
[
1
])
print
m
.
dot
(
array
(
P
[
i
]
+
[
1
]))
pos
=
m
.
dot
(
array
(
P
[
i
]
+
[
1
]))[:
3
]
print
"pos"
,
pos
r
.
client
.
gui
.
addBox
(
scene
+
"/b"
+
str
(
i
),
0.01
,
0.01
,
0.01
,
[
1
,
0
,
0
,
1
])
r
.
client
.
gui
.
applyConfiguration
(
scene
+
"/b"
+
str
(
i
),
pos
.
tolist
()
+
[
1
,
0
,
0
,
0
])
r
.
client
.
gui
.
refresh
()
r
.
client
.
gui
.
addSceneToWindow
(
scene
,
0
)
def
fill_contact_points
(
limbs
,
config
,
config_pinocchio
):
res
=
{}
res
[
"q"
]
=
config_pinocchio
[:]
res
[
"contact_points"
]
=
{}
res
[
"P"
]
=
[]
res
[
"N"
]
=
[]
for
limb
in
limbs
:
effector
=
limbsCOMConstraints
[
limb
][
'effector'
]
#~ posetc = fullBody.getEffectorPosition(limb, config)
P
,
N
=
fullBody
.
computeContactForConfig
(
config
,
limb
)
#~ posetc = fullBody.getEffectorPosition(limb, config)
res
[
"contact_points"
][
effector
]
=
{}
#~ res["contact_points"][effector]["P"] = [p for i, p in enumerate (posetc) if (i%2 == 0)]
res
[
"contact_points"
][
effector
][
"P"
]
=
P
#~ res["P"] += [p for i, p in enumerate (posetc) if (i%2 == 0)]
res
[
"P"
]
+=
P
#~ res["contact_points"][effector]["N"] = [n for i, n in enumerate (posetc) if (i%2 == 1)]
res
[
"contact_points"
][
effector
][
"N"
]
=
N
#~ res["N"] += [n for i, n in enumerate (posetc) if (i%2 == 1)]
res
[
"N"
]
+=
N
return
res
res
=
{}
res
[
"q"
]
=
config_pinocchio
[:]
res
[
"contact_points"
]
=
{}
res
[
"P"
]
=
[]
res
[
"N"
]
=
[]
for
limb
in
limbs
:
effector
=
limbsCOMConstraints
[
limb
][
'effector'
]
#~ posetc = fullBody.getEffectorPosition(limb, config)
P
,
N
=
fullBody
.
computeContactForConfig
(
config
,
limb
)
#~ posetc = fullBody.getEffectorPosition(limb, config)
res
[
"contact_points"
][
effector
]
=
{}
#~ res["contact_points"][effector]["P"] = [p for i, p in enumerate (posetc) if (i%2 == 0)]
res
[
"contact_points"
][
effector
][
"P"
]
=
P
#~ res["P"] += [p for i, p in enumerate (posetc) if (i%2 == 0)]
res
[
"P"
]
+=
P
#~ res["contact_points"][effector]["N"] = [n for i, n in enumerate (posetc) if (i%2 == 1)]
res
[
"contact_points"
][
effector
][
"N"
]
=
N
#~ res["N"] += [n for i, n in enumerate (posetc) if (i%2 == 1)]
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
,
res
,
num_candidates
=
10
):
effectorName
=
limbsCOMConstraints
[
limb
][
'effector'
]
candidates
=
[]
print
"res"
,
res
for
i
in
range
(
num_candidates
):
q_contact
=
fullBody
.
getSample
(
limb
,
randint
(
0
,
n_samples
-
1
))
fullBody
.
setCurrentConfig
(
q_contact
)
m
=
_getTransform
(
fullBody
.
getJointPosition
(
effectorName
))
candidates
.
append
(
m
)
#DEBUG
res
[
"config_candidates"
].
append
(
q_contact
)
#~ candidates.append(pos_quat_to_pinocchio(fullBody.getJointPosition(effectorName)))
res
[
effectorName
]
=
candidates
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
,
data
,
num_candidates
=
10
):
effectorName
=
limbsCOMConstraints
[
limb
][
'effector'
]
candidates
=
[]
config_candidates
=
[]
#DEBUG
print
"data"
,
data
for
i
in
range
(
num_candidates
):
q_contact
=
fullBody
.
getSample
(
limb
,
randint
(
0
,
n_samples
-
1
))
fullBody
.
setCurrentConfig
(
q_contact
)
m
=
_getTransform
(
fullBody
.
getJointPosition
(
effectorName
))
candidates
.
append
(
m
)
config_candidates
.
append
(
q_contact
)
#DEBUG
#~ candidates.append(pos_quat_to_pinocchio(fullBody.getJointPosition(effectorName)))
data
[
effectorName
][
"transforms"
]
=
candidates
return
config_candidates
#DEBUG
def
removeLimb
(
limb
,
limbs
):
return
[
l
for
l
in
limbs
if
l
!=
limb
]
return
[
l
for
l
in
limbs
if
l
!=
limb
]
#~
#~ def find_limbs_broken(target_c, config, limbs):
#~ res = []
#~ for limb in limbs:
#~ nLimbs = removeLimb(limb, limbs)
#~ state_id = fullBody.createState(config, nLimbs)
#~ if (fullBody.projectStateToCOM(state_id,target_c)):
#~ res.append(limb)
#~ return res
#~ res = []
#~ for limb in limbs:
#~ nLimbs = removeLimb(limb, limbs)
#~ state_id = fullBody.createState(config, nLimbs)
#~ if (fullBody.projectStateToCOM(state_id,target_c)):
#~ res.append(limb)
#~ return res
def
predict_com_for_limb_candidate
(
c
,
limb
,
limbs
,
res
,
data
,
config_gepetto
):
effector
=
limbsCOMConstraints
[
limb
][
'effector'
]
contact_points
=
{}
maintained_limbs
=
[
l
for
l
in
limbs
if
limb
!=
l
]
for
k
,
v
in
res
[
"contact_points"
].
iteritems
():
if
k
!=
effector
:
contact_points
[
k
]
=
v
success
,
dc
,
c_final
,
v0
=
scv
.
comPosAfter07s
(
c
,
res
[
"q"
],
contact_points
)
effector_data
=
{}
state_id
=
fullBody
.
createState
(
config_gepetto
,
maintained_limbs
)
if
(
success
and
fullBody
.
projectStateToCOM
(
state_id
,
c_final
.
tolist
())):
#all good, all contacts kinematically maintained):
effector_data
[
"dc"
]
=
dc
effector_data
[
"c_final"
]
=
c_final
proj_config
=
fullBody
.
getConfigAtState
(
state_id
)
fullBody
.
setCurrentConfig
(
proj_config
)
effector_data
[
"projected_config"
]
=
proj_config
#DEBUG
data
[
"candidates_per_effector"
][
effector
]
=
effector_data
return
True
return
False
def
gen_contact_candidates
(
limbs
,
config_gepetto
,
res
):
#first generate a com translation current configuration
fullBody
.
setCurrentConfig
(
config_gepetto
)
success
,
dc
,
c_final
,
v0
=
scv
.
comPosAfter07s
(
matrix
(
fullBody
.
getCenterOfMass
()),
res
[
"q"
],
res
[
"contact_points"
])
if
(
success
):
data
=
{}
#set it as new com objective for projection
target_c
=
c_final
.
tolist
()
state_id
=
fullBody
.
createState
(
config_gepetto
,
limbs
)
#~ res["candidates"].append({})
data
[
"v0"
]
=
v0
data
[
"dc"
]
=
dc
data
[
"c_final"
]
=
c_final
data
[
"candidateEffector"
]
=
{}
#DEBUG
data
[
"candidateEffector"
][
"config_candidates"
]
=
[]
if
(
fullBody
.
projectStateToCOM
(
state_id
,
target_c
)):
#all good, all contacts kinematically maintained
#~ res["candidates"][-1]["brokenContacts"] = find_limbs_broken(target_c, config_gepetto, limbs)
#~ for limb in limbs:
proj_config
=
fullBody
.
getConfigAtState
(
state_id
)
#DEBUG
data
[
"init_config"
]
=
config_gepetto
#DEBUG
data
[
"projected_config"
]
=
proj_config
fullBody
.
setCurrentConfig
(
proj_config
)
for
limb
in
limbsCOMConstraints
.
keys
():
gen_contact_candidates_one_limb
(
limb
,
data
[
"candidateEffector"
],
1
)
if
(
not
res
.
has_key
(
"candidates"
)):
res
[
"candidates"
]
=
[]
res
[
"candidates"
].
append
(
data
)
#first generate a com translation current configuration
fullBody
.
setCurrentConfig
(
config_gepetto
)
c
=
matrix
(
fullBody
.
getCenterOfMass
())
success
,
dc
,
v0
=
scv
.
gen_com_vel
(
res
[
"q"
],
res
[
"contact_points"
])
if
(
success
):
data
=
{}
data
[
"v0"
]
=
v0
data
[
"candidates_per_effector"
]
=
{}
configs_candidates
=
[]
#DEBUG
data
[
"init_config"
]
=
config_gepetto
#DEBUG
for
limb
in
limbsCOMConstraints
.
keys
():
print
"limb "
,
limb
if
(
predict_com_for_limb_candidate
(
c
,
limb
,
limbs
,
res
,
data
,
config_gepetto
)):
#all good, all contacts kinematically maintained
configs_candidates
.
append
(
gen_contact_candidates_one_limb
(
limb
,
data
[
"candidates_per_effector"
],
1
))
#DEBUG
if
(
len
(
data
[
"candidates_per_effector"
].
keys
())
>
0
):
data
[
"candidates_per_effector"
][
"config_candidates"
]
=
configs_candidates
#DEBUG
if
(
not
res
.
has_key
(
"candidates"
)):
res
[
"candidates"
]
=
[]
res
[
"candidates"
].
append
(
data
)
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
]
#~
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);
#~ 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
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
=
_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"
for
i
in
range
(
10000
):
q
=
fullBody
.
client
.
basic
.
robot
.
shootRandomConfig
()
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
=
[]
all_states
=
[]
def
gen
(
limbs
,
num_samples
=
1000
,
coplanar
=
True
):
q_0
=
fullBody
.
getCurrentConfig
();
#~ fullBody.getSampleConfig()
qs
=
[];
qs_gepetto
=
[];
states
=
[]
for
_
in
range
(
num_samples
):
if
(
coplanar
):
q
=
fullBody
.
generateGroundContact
(
limbs
)
else
:
q
=
_genbalance
(
limbs
)
q_gep
=
q
[:]
quat_end
=
q
[
4
:
7
]
q
[
6
]
=
q
[
3
]
q
[
3
:
6
]
=
quat_end
qs
.
append
(
q
)
qs_gepetto
.
append
(
q_gep
)
res
=
fill_contact_points
(
limbs
,
q_gep
,
q
)
for
_
in
range
(
10
):
gen_contact_candidates
(
limbs
,
q_gep
,
res
)
states
.
append
(
res
)
global
all_qs
all_qs
+=
[
qs_gepetto
]
fname
=
""
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+'
)
dump
(
states
,
f1
)
f1
.
close
()
all_states
.
append
(
states
)
q_0
=
fullBody
.
getCurrentConfig
();
#~ fullBody.getSampleConfig()
qs
=
[];
qs_gepetto
=
[];
states
=
[]
for
_
in
range
(
num_samples
):
if
(
coplanar
):
q
=
fullBody
.
generateGroundContact
(
limbs
)
else
:
q
=
_genbalance
(
limbs
)
q_gep
=
q
[:]
quat_end
=
q
[
4
:
7
]
q
[
6
]
=
q
[
3
]
q
[
3
:
6
]
=
quat_end
res
=
fill_contact_points
(
limbs
,
q_gep
,
q
)
for
_
in
range
(
1
):
gen_contact_candidates
(
limbs
,
q_gep
,
res
)
if
(
res
.
has_key
(
"candidates"
)):
#contact candidates found
states
.
append
(
res
)
qs
.
append
(
q
)
qs_gepetto
.
append
(
q_gep
)
global
all_qs
all_qs
+=
[
qs_gepetto
]
fname
=
""
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+'
)
dump
(
states
,
f1
)
f1
.
close
()
all_states
.
append
(
states
)
j
=
0
q_init
=
[
0.1
,
-
0.82
,
0.648702
,
1.0
,
0.0
,
0.0
,
0.0
,
# Free flyer 0-6
0.1
,
-
0.82
,
0.648702
,
1.0
,
0.0
,
0.0
,
0.0
,
# Free flyer 0-6
0.0
,
0.0
,
0.0
,
0.0
,
# CHEST HEAD 7-10
0.261799388
,
0.174532925
,
0.0
,
-
0.523598776
,
0.0
,
0.0
,
0.17
,
# LARM 11-17
0.261799388
,
-
0.174532925
,
0.0
,
-
0.523598776
,
0.0
,
0.0
,
0.17
,
# RARM 18-24
0.261799388
,
0.174532925
,
0.0
,
-
0.523598776
,
0.0
,
0.0
,
0.17
,
# LARM 11-17
0.261799388
,
-
0.174532925
,
0.0
,
-
0.523598776
,
0.0
,
0.0
,
0.17
,
# RARM 18-24
0.0
,
0.0
,
-
0.453785606
,
0.872664626
,
-
0.41887902
,
0.0
,
# LLEG 25-30
0.0
,
0.0
,
-
0.453785606
,
0.872664626
,
-
0.41887902
,
0.0
,
# RLEG 31-36
];
r
(
q_init
)
...
...
@@ -337,8 +350,39 @@ limbs = [[lLegId,rLegId],[lLegId,rLegId, rarmId], [lLegId,rLegId, larmId], [lLeg
#~ gen(limbs[0], 10)
#~ for ls in limbs:
#~ gen(ls, 10, False)
gen
(
limbs
[
0
],
1
0
)
#~ gen(ls, 10, False)
gen
(
limbs
[
0
],
1
)
i
=
0
a
=
all_states
[
0
][
0
][
'candidates'
][
0
]
b
=
a
[
'candidates_per_effector'
]
def
init
():
r
(
a
[
'init_config'
])
b
=
a
[
'candidates_per_effector'
]
def
rleg
():
r
(
b
[
'RLEG_JOINT5'
][
'projected_config'
])
def
lleg
():
r
(
b
[
'LLEG_JOINT5'
][
'projected_config'
])
def
larm
():
r
(
b
[
'LARM_JOINT5'
][
'projected_config'
])
def
rarm
():
r
(
b
[
'RARM_JOINT5'
][
'projected_config'
])
def
c1
():
r
(
b
[
'config_candidates'
][
0
][
0
])
def
c2
():
r
(
b
[
'config_candidates'
][
1
][
0
])
def
c3
():
r
(
b
[
'config_candidates'
][
2
][
0
])
def
c4
():
r
(
b
[
'config_candidates'
][
3
][
0
])
script/tools/tro_capture_point/sample_com_vel.py
View file @
9dc2c37f
...
...
@@ -110,7 +110,8 @@ def draw_q(q0):
def
q_pin
(
q_hpp
):
return
np
.
matrix
(
q_hpp
).
T
def
gen_com_vel
(
q0
,
contacts
):
def
gen_com_vel
(
q_hpp
,
contacts
):
q0
=
q_pin
(
q_hpp
)
init
(
q0
);
v0
=
mat_zeros
(
nv
);
invDynForm
.
setNewSensorData
(
0
,
q0
,
v0
);
...
...
@@ -127,12 +128,12 @@ def gen_com_vel(q0, contacts):
conf
.
MAX_INITIAL_COM_VEL
,
100
);
if
success
:
print
"Initial velocities found"
return
(
success
,
v
,
invDynForm
.
J_com
*
v
);
return
(
success
,
v
[:]
,
invDynForm
.
J_com
*
v
);
print
"Could not find initial velocities"
return
(
success
,
v
[:]);
return
(
success
,
v
[:]
,
invDynForm
.
J_com
*
v
);
def
comPosAfter07s
(
c
,
q_hpp
,
contacts
):
def
comPosAfter07s
(
c
,