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
c5fc6077
Commit
c5fc6077
authored
Nov 10, 2016
by
Steve Tonneau
Browse files
generation ok
parent
ecab1d26
Changes
3
Hide whitespace changes
Inline
Side-by-side
script/tools/tro_capture_point/gen_hrp2_statically_balanced_positions_2d.py
View file @
c5fc6077
...
...
@@ -128,29 +128,36 @@ def draw_cp(cid, limb, config):
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
draw_com
(
config
):
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"
]
=
[]
def
fill_contact_points
(
limbs
,
fullbody
,
data
):
data
[
"contact_points"
]
=
{}
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
P
,
N
=
fullBody
.
computeContactForConfig
(
fullbody
.
getCurrentConfig
(),
limb
)
data
[
"contact_points"
][
effector
]
=
{}
data
[
"contact_points"
][
effector
][
"P"
]
=
P
data
[
"contact_points"
][
effector
][
"N"
]
=
N
return
data
from
random
import
randint
...
...
@@ -166,7 +173,6 @@ 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
)
...
...
@@ -191,15 +197,19 @@ def removeLimb(limb, limbs):
#~ res.append(limb)
#~ return res
def
predict_com_for_limb_candidate
(
c
,
limb
,
limbs
,
res
,
data
,
config_gepetto
):
def
predict_com_for_limb_candidate
(
c
,
limb
,
limbs
,
res
,
data
,
config_gepetto
,
orig_contact_points
):
effector
=
limbsCOMConstraints
[
limb
][
'effector'
]
contact_points
=
{}
maintained_limbs
=
[
l
for
l
in
limbs
if
limb
!=
l
]
for
k
,
v
in
res
[
"
contact_points
"
]
.
iteritems
():
for
k
,
v
in
orig_
contact_points
.
iteritems
():
if
k
!=
effector
:
contact_points
[
k
]
=
v
success
,
dc
,
c_final
,
v0
=
scv
.
com_pos_after_t
(
c
,
res
[
"q"
],
contact_points
)
#~ success, dc, c_final, v0 = scv.com_pos_after_t(c, res["q"], contact_points, data["v0"])
success
,
dc
,
c_final
,
v0
=
scv
.
com_pos_after_t
(
c
,
res
[
"q"
],
contact_points
,
data
[
"v0"
])
print
"c for limb "
,
c_final
,
":"
,
limb
print
"success "
,
success
effector_data
=
{}
print
"maintained_limbs "
,
maintained_limbs
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
...
...
@@ -209,14 +219,15 @@ def predict_com_for_limb_candidate(c, limb, limbs, res, data, config_gepetto):
effector_data
[
"projected_config"
]
=
proj_config
#DEBUG
data
[
"candidates_per_effector"
][
effector
]
=
effector_data
return
True
print
"projection failed for limb "
,
limb
return
False
def
gen_contact_candidates
(
limbs
,
config_gepetto
,
res
):
def
gen_contact_candidates
(
limbs
,
config_gepetto
,
res
,
contact_points
):
#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
"
]
)
success
,
v0
,
dc
=
scv
.
gen_com_vel
(
res
[
"q"
],
contact_points
)
if
(
success
):
data
=
{}
data
[
"v0"
]
=
v0
...
...
@@ -225,9 +236,12 @@ def gen_contact_candidates(limbs, config_gepetto, res):
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
):
if
(
predict_com_for_limb_candidate
(
c
,
limb
,
limbs
,
res
,
data
,
config_gepetto
,
contact_points
)):
#all good, all contacts kinematically maintained
configs_candidates
.
append
(
gen_contact_candidates_one_limb
(
limb
,
data
[
"candidates_per_effector"
],
10
))
#DEBUG
if
(
len
(
data
[
"candidates_per_effector"
].
keys
())
>
0
):
#~ if((data["candidates_per_effector"].has_key('RARM_JOINT5') and not data["candidates_per_effector"].has_key('LARM_JOINT5')) or
#~ (data["candidates_per_effector"].has_key('LARM_JOINT5') and not data["candidates_per_effector"].has_key('RARM_JOINT5'))):
#~ raise ValueError("RARM AND LARM candidates not coherent (if there is candidates for one there should be for the other)");
data
[
"candidates_per_effector"
][
"config_candidates"
]
=
configs_candidates
#DEBUG
if
(
not
res
.
has_key
(
"candidates"
)):
res
[
"candidates"
]
=
[]
...
...
@@ -301,7 +315,9 @@ all_states = []
def
gen
(
limbs
,
num_samples
=
1000
,
coplanar
=
True
):
q_0
=
fullBody
.
getCurrentConfig
();
#~ fullBody.getSampleConfig()
qs
=
[];
qs_gepetto
=
[];
states
=
[]
qs
=
[];
qs_gepetto
=
[];
states
=
[]
data
=
{}
fill_contact_points
(
limbs
,
fullBody
,
data
)
for
_
in
range
(
num_samples
):
if
(
coplanar
):
q
=
fullBody
.
generateGroundContact
(
limbs
)
...
...
@@ -311,9 +327,10 @@ def gen(limbs, num_samples = 1000, coplanar = True):
quat_end
=
q
[
4
:
7
]
q
[
6
]
=
q
[
3
]
q
[
3
:
6
]
=
quat_end
res
=
fill_contact_points
(
limbs
,
q_gep
,
q
)
res
=
{}
res
[
"q"
]
=
q
[:]
for
_
in
range
(
1
):
gen_contact_candidates
(
limbs
,
q_gep
,
res
)
gen_contact_candidates
(
limbs
,
q_gep
,
res
,
data
[
"contact_points"
]
)
if
(
res
.
has_key
(
"candidates"
)):
#contact candidates found
states
.
append
(
res
)
qs
.
append
(
q
)
...
...
@@ -326,10 +343,11 @@ def gen(limbs, num_samples = 1000, coplanar = True):
fname
+=
"configs"
if
(
coplanar
):
fname
+=
"_coplanar"
data
[
"samples"
]
=
states
from
pickle
import
dump
#~ f1=open("configs_feet_on_ground_static_eq", 'w+')
f1
=
open
(
fname
,
'w+'
)
dump
(
states
,
f1
)
dump
(
data
,
f1
)
f1
.
close
()
all_states
.
append
(
states
)
...
...
@@ -348,10 +366,10 @@ limbs = [[lLegId,rLegId],[lLegId,rLegId, rarmId], [lLegId,rLegId, larmId], [lLeg
#~ limbs = [[lLegId,rLegId, rarmId]]
#~ limbs = [[larmId, rarmId]]
gen
(
limbs
[
0
],
10
)
for
ls
in
limbs
:
gen
(
ls
,
10
,
False
)
#~ gen(limbs[0], 10)
#~ for ls in limbs:
#~ gen(ls, 10, False)
gen
(
limbs
[
0
],
1
)
i
=
0
...
...
@@ -385,4 +403,12 @@ def c3():
def
c4
():
r
(
b
[
'config_candidates'
][
3
][
0
])
def
inc
():
global
i
global
a
global
b
i
+=
1
a
=
all_states
[
0
][
i
][
'candidates'
][
0
]
b
=
a
[
'candidates_per_effector'
]
script/tools/tro_capture_point/sample_com_vel.py
View file @
c5fc6077
...
...
@@ -9,7 +9,7 @@ if cwd+'/data/config' not in sys.path:
sys
.
path
+=
[
cwd
+
'/data/config'
,];
import
conf_hpp
as
conf
from
utils
import
compute_initial_joint_velocities_multi_contact
from
utils
import
compute_initial_joint_velocities_multi_contact
,
compute_contact_points_from_contact_dictionary
from
pinocchio_inv_dyn.multi_contact.stability_criterion
import
StabilityCriterion
import
pinocchio
as
se3
...
...
@@ -30,47 +30,47 @@ from numpy.linalg import norm
EPS
=
1e-4
;
def
createInvDynFormUtil
(
q
,
v
):
invDynForm
=
InvDynFormulation
(
'inv_dyn'
,
q
,
v
,
conf
.
dt
,
conf
.
model_path
,
conf
.
urdfFileName
,
conf
.
freeFlyer
);
invDynForm
.
enableCapturePointLimits
(
conf
.
ENABLE_CAPTURE_POINT_LIMITS
);
invDynForm
.
enableTorqueLimits
(
conf
.
ENABLE_TORQUE_LIMITS
);
invDynForm
.
enableForceLimits
(
conf
.
ENABLE_FORCE_LIMITS
);
invDynForm
.
enableJointLimits
(
conf
.
ENABLE_JOINT_LIMITS
,
conf
.
IMPOSE_POSITION_BOUNDS
,
conf
.
IMPOSE_VELOCITY_BOUNDS
,
conf
.
IMPOSE_VIABILITY_BOUNDS
,
conf
.
IMPOSE_ACCELERATION_BOUNDS
);
invDynForm
.
JOINT_POS_PREVIEW
=
conf
.
JOINT_POS_PREVIEW
;
invDynForm
.
JOINT_VEL_PREVIEW
=
conf
.
JOINT_VEL_PREVIEW
;
invDynForm
.
MAX_JOINT_ACC
=
conf
.
MAX_JOINT_ACC
;
invDynForm
.
MAX_MIN_JOINT_ACC
=
conf
.
MAX_MIN_JOINT_ACC
;
invDynForm
.
USE_JOINT_VELOCITY_ESTIMATOR
=
conf
.
USE_JOINT_VELOCITY_ESTIMATOR
;
invDynForm
.
ACCOUNT_FOR_ROTOR_INERTIAS
=
conf
.
ACCOUNT_FOR_ROTOR_INERTIAS
;
return
invDynForm
;
#~
def createInvDynFormUtil(q, v):
#~
invDynForm = InvDynFormulation('inv_dyn', q, v, conf.dt, conf.model_path, conf.urdfFileName, conf.freeFlyer);
#~
invDynForm.enableCapturePointLimits(conf.ENABLE_CAPTURE_POINT_LIMITS);
#~
invDynForm.enableTorqueLimits(conf.ENABLE_TORQUE_LIMITS);
#~
invDynForm.enableForceLimits(conf.ENABLE_FORCE_LIMITS);
#~
invDynForm.enableJointLimits(conf.ENABLE_JOINT_LIMITS, conf.IMPOSE_POSITION_BOUNDS, conf.IMPOSE_VELOCITY_BOUNDS,
#~
conf.IMPOSE_VIABILITY_BOUNDS, conf.IMPOSE_ACCELERATION_BOUNDS);
#~
invDynForm.JOINT_POS_PREVIEW = conf.JOINT_POS_PREVIEW;
#~
invDynForm.JOINT_VEL_PREVIEW = conf.JOINT_VEL_PREVIEW;
#~
invDynForm.MAX_JOINT_ACC = conf.MAX_JOINT_ACC;
#~
invDynForm.MAX_MIN_JOINT_ACC = conf.MAX_MIN_JOINT_ACC;
#~
invDynForm.USE_JOINT_VELOCITY_ESTIMATOR = conf.USE_JOINT_VELOCITY_ESTIMATOR;
#~
invDynForm.ACCOUNT_FOR_ROTOR_INERTIAS = conf.ACCOUNT_FOR_ROTOR_INERTIAS;
#~
return invDynForm;
def
updateConstraints
(
t
,
q
,
v
,
invDynForm
,
contacts
):
contact_changed
=
False
;
for
(
name
,
PN
)
in
contacts
.
iteritems
():
if
(
invDynForm
.
existUnilateralContactConstraint
(
name
)):
continue
;
contact_changed
=
True
;
invDynForm
.
r
.
forwardKinematics
(
q
,
v
,
0
*
v
);
invDynForm
.
r
.
framesKinematics
(
q
);
fid
=
invDynForm
.
getFrameId
(
name
);
oMi
=
invDynForm
.
r
.
framePosition
(
fid
);
ref_traj
=
ConstantSE3Trajectory
(
name
,
oMi
);
constr
=
SE3Task
(
invDynForm
.
r
,
fid
,
ref_traj
,
name
);
constr
.
kp
=
conf
.
kp_constr
;
constr
.
kv
=
conf
.
kd_constr
;
constr
.
mask
(
conf
.
constraint_mask
);
Pi
=
np
.
matrix
(
PN
[
'P'
]).
T
;
Ni
=
np
.
matrix
(
PN
[
'N'
]).
T
;
for
j
in
range
(
Pi
.
shape
[
1
]):
print
" contact point %d in world frame:"
%
j
,
oMi
.
act
(
Pi
[:,
j
]).
T
,
(
oMi
.
rotation
*
Ni
[:,
j
]).
T
;
invDynForm
.
addUnilateralContactConstraint
(
constr
,
Pi
,
Ni
,
conf
.
fMin
,
conf
.
mu
);
return
contact_changed
;
#~
def updateConstraints(t, q, v, invDynForm, contacts):
#~
contact_changed = False;
#~
#~
for (name, PN) in contacts.iteritems():
#~
if(invDynForm.existUnilateralContactConstraint(name)):
#~
continue;
#~
#~
contact_changed =True;
#~
invDynForm.r.forwardKinematics(q, v, 0 * v);
#~
invDynForm.r.framesKinematics(q);
#~
#~
fid = invDynForm.getFrameId(name);
#~
oMi = invDynForm.r.framePosition(fid);
#~
ref_traj = ConstantSE3Trajectory(name, oMi);
#~
constr = SE3Task(invDynForm.r, fid, ref_traj, name);
#~
constr.kp = conf.kp_constr;
#~
constr.kv = conf.kd_constr;
#~
constr.mask(conf.constraint_mask);
#~
#~
Pi = np.matrix(PN['P']).T;
#~
Ni = np.matrix(PN['N']).T;
#~
for j in range(Pi.shape[1]):
#~
print " contact point %d in world frame:"%j, oMi.act(Pi[:,j]).T, (oMi.rotation * Ni[:,j]).T;
#~
invDynForm.addUnilateralContactConstraint(constr, Pi, Ni, conf.fMin, conf.mu);
#~
#~
return contact_changed;
def
createSimulator
(
q0
,
v0
):
...
...
@@ -86,26 +86,13 @@ def createSimulator(q0, v0):
simulator
.
CONTACT_FORCE_ARROW_SCALE
=
2e-3
;
simulator
.
verb
=
0
;
return
simulator
;
#~
def
draw_q
(
q0
):
p
=
np
.
matrix
.
copy
(
invDynForm
.
contact_points
);
simulator
.
viewer
.
updateRobotConfig
(
q0
)
;
simulator
.
updateComPositionInViewer
(
np
.
matrix
([
invDynForm
.
x_com
[
0
,
0
],
invDynForm
.
x_com
[
1
,
0
],
0.
]).
T
);
#~
p = np.matrix.copy(invDynForm.contact_points);
x_com
=
robot
.
com
(
q0
)
simulator
.
updateComPositionInViewer
(
np
.
matrix
([
x_com
[
0
,
0
],
x_com
[
1
,
0
],
0.
]).
T
);
q0
[
2
]
-=
0.005
simulator
.
viewer
.
updateRobotConfig
(
q0
);
p
[
2
,:]
-=
0.005
;
for
j
in
range
(
p
.
shape
[
1
]):
simulator
.
viewer
.
addSphere
(
"contact_point"
+
str
(
j
),
0.005
,
p
[:,
j
],
(
0.
,
0.
,
0.
),
(
1
,
1
,
1
,
1
));
simulator
.
viewer
.
updateObjectConfigRpy
(
"contact_point"
+
str
(
j
),
p
[:,
j
]);
f
=
open
(
conf
.
INITIAL_CONFIG_FILENAME
,
'rb'
);
res
=
pickle
.
load
(
f
);
f
.
close
();
p_steve
=
np
.
matrix
(
res
[
conf
.
INITIAL_CONFIG_ID
][
'P'
]);
p_steve
[:,
2
]
-=
0.005
;
for
j
in
range
(
p_steve
.
shape
[
0
]):
simulator
.
viewer
.
addSphere
(
"contact_point_steve"
+
str
(
j
),
0.005
,
p_steve
[
j
,:].
T
,
color
=
(
1
,
0
,
0
,
1
));
simulator
.
viewer
.
updateObjectConfigRpy
(
"contact_point_steve"
+
str
(
j
),
p_steve
[
j
,:].
T
);
def
q_pin
(
q_hpp
):
return
np
.
matrix
(
q_hpp
).
T
...
...
@@ -114,41 +101,43 @@ def gen_com_vel(q_hpp, contacts):
q0
=
q_pin
(
q_hpp
)
init
(
q0
);
v0
=
mat_zeros
(
nv
);
invDynForm
.
setNewSensorData
(
0
,
q0
,
v0
);
updateConstraints
(
0
,
q0
,
v0
,
invDynForm
,
contacts
);
#~ draw_q(q0);
#~ invDynForm.setNewSensorData(0, q0, v0);
#~ updateConstraints(0, q0, v0, invDynForm, contacts);
print
'Gonna compute initial joint velocities that satisfy contact constraints'
;
print
'conf.MAX_INITIAL_COM_VEL'
,
conf
.
MAX_INITIAL_COM_VEL
(
success
,
v
)
=
compute_initial_joint_velocities_multi_contact
(
q0
,
invDynForm
,
conf
.
mu
[
0
],
(
success
,
v
)
=
compute_initial_joint_velocities_multi_contact
(
q0
,
robot
,
contacts
,
np
.
array
([
True
]
*
6
)
,
conf
.
mu
[
0
],
conf
.
ZERO_INITIAL_ANGULAR_MOMENTUM
,
conf
.
ZERO_INITIAL_VERTICAL_COM_VEL
,
False
,
#conf.ENSURE_STABILITY,
True
,
#conf.ENSURE_UNSTABILITY,
conf
.
MAX_INITIAL_COM_VEL
,
100
);
True
,
#conf.ENSURE_UNSTABILITY,
conf
.
MAX_INITIAL_COM_VEL
,
conf
.
MAX_MIN_JOINT_ACC
,
100
);
#~ r(q_hpp)
draw_q
(
q0
);
if
success
:
print
"Initial velocities found"
return
(
success
,
v
[:],
invDynForm
.
J_com
*
v
);
print
"Initial velocities found"
J_com
=
robot
.
Jcom
(
q0
,
update_kinematics
=
False
);
return
(
success
,
v
[:],
J_com
*
v
);
print
"Could not find initial velocities"
return
(
success
,
v
[:],
invDynForm
.
J_com
*
v
);
return
(
success
,
v
[:],
v
);
def
com_pos_after_t
(
c
,
q_hpp
,
contacts
,
v
=
None
,
t
=
0.7
):
q0
=
q_pin
(
q_hpp
)
init
(
q0
);
v0
=
mat_zeros
(
nv
);
invDynForm
.
setNewSensorData
(
0
,
q0
,
v0
);
updateConstraints
(
0
,
q0
,
v0
,
invDynForm
,
contacts
);
#~
invDynForm.setNewSensorData(0, q0, v0);
#~
updateConstraints(0, q0, v0, invDynForm, contacts);
#~ draw_q(q0);
print
'Gonna compute initial joint velocities that satisfy contact constraints'
;
print
'conf.MAX_INITIAL_COM_VEL'
,
conf
.
MAX_INITIAL_COM_VEL
if
(
v
==
None
):
(
success
,
v
)
=
compute_initial_joint_velocities_multi_contact
(
q0
,
invDynForm
,
conf
.
mu
[
0
],
(
success
,
v
)
=
compute_initial_joint_velocities_multi_contact
(
q0
,
robot
,
contacts
,
[
True
]
*
6
,
conf
.
mu
[
0
],
conf
.
ZERO_INITIAL_ANGULAR_MOMENTUM
,
conf
.
ZERO_INITIAL_VERTICAL_COM_VEL
,
False
,
#conf.ENSURE_STABILITY,
True
,
#conf.ENSURE_UNSTABILITY,
conf
.
MAX_INITIAL_COM_VEL
,
100
);
conf
.
MAX_INITIAL_COM_VEL
,
conf
.
MAX_MIN_JOINT_ACC
,
100
);
else
:
success
=
True
if
(
not
success
):
...
...
@@ -157,10 +146,19 @@ def com_pos_after_t(c, q_hpp, contacts, v = None, t = 0.7):
c_init
=
np
.
matrix
(
c
)
dx_com_des
=
mat_zeros
(
3
);
P
=
np
.
matlib
.
copy
(
invDynForm
.
contact_points
);
N
=
np
.
matlib
.
copy
(
invDynForm
.
contact_normals
);
stab_criterion
=
StabilityCriterion
(
"default"
,
invDynForm
.
x_com
,
dx_com_des
,
P
.
T
,
N
.
T
,
conf
.
mu
[
0
],
np
.
array
([
0
,
0
,
-
9.81
]),
invDynForm
.
M
[
0
,
0
])
res
=
stab_criterion
.
predict_future_state
(
t
,
c_init
,
invDynForm
.
J_com
*
v
)
#~ P = np.matlib.copy(invDynForm.contact_points);
#~ N = np.matlib.copy(invDynForm.contact_normals);
robot
.
computeAllTerms
(
q0
,
mat_zeros
(
nv
));
robot
.
framesKinematics
(
q0
);
(
P
,
N
)
=
compute_contact_points_from_contact_dictionary
(
robot
,
contacts
)
print
"contacts "
,
len
(
contacts
)
print
"contacts "
,
contacts
print
"normals "
,
N
M
=
robot
.
mass
(
q0
,
update_kinematics
=
True
);
J_com
=
robot
.
Jcom
(
q0
,
update_kinematics
=
False
);
print
"Mas "
,
M
stab_criterion
=
StabilityCriterion
(
"default"
,
np
.
matrix
(
c
),
dx_com_des
,
P
.
T
,
N
.
T
,
conf
.
mu
[
0
],
np
.
array
([
0
,
0
,
-
9.81
]),
M
[
0
,
0
])
res
=
stab_criterion
.
predict_future_state
(
t
,
c_init
,
J_com
*
v
)
#TODO : res.t != 0.7
print
"c "
,
res
.
c
print
"dc "
,
res
.
dc
...
...
@@ -178,18 +176,16 @@ nv = robot.nv;
v0
=
mat_zeros
(
nv
);
invDynForm
=
None
;
robot
=
None
;
na
=
None
;
# number of joints
simulator
=
None
;
def
init
(
q0
):
''' CREATE CONTROLLER AND SIMULATOR '''
global
invDynForm
#~
global invDynForm
global
robot
global
na
global
simulator
print
"reset invdyn"
invDynForm
=
createInvDynFormUtil
(
q0
,
v0
);
robot
=
invDynForm
.
r
;
na
=
invDynForm
.
na
;
# number of joints
#~ invDynForm = createInvDynFormUtil(q0, v0);
robot
=
RobotWrapper
(
conf
.
urdfFileName
,
conf
.
model_path
,
root_joint
=
se3
.
JointModelFreeFlyer
());
simulator
=
createSimulator
(
q0
,
v0
);
#~ gen_com_vel(q0, config_test['contact_points'])
script/tools/tro_capture_point/utils.py
View file @
c5fc6077
...
...
@@ -12,28 +12,40 @@ from pinocchio.utils import zero as mat_zeros
from
pinocchio.utils
import
rand
as
mat_rand
from
pinocchio_inv_dyn.acc_bounds_util
import
computeVelLimits
from
pinocchio_inv_dyn.sot_utils
import
solveLeastSquare
from
pinocchio_inv_dyn.multi_contact.stability_criterion
import
StabilityCriterion
from
pinocchio_inv_dyn.multi_contact.stability_criterion
import
StabilityCriterion
,
Bunch
EPS
=
1e-5
class
Bunch
:
def
__init__
(
self
,
**
kwds
):
self
.
__dict__
.
update
(
kwds
);
def
compute_contact_points_from_contact_dictionary
(
robot
,
contacts
):
''' Compute the contact points and the contact normals in world reference frame
starting from a dictionary of contact points and contact normals in local frame.
robot -- An instance of RobotWrapper
contacts -- A dictionary containing the contact points and normals in local frame
and using the joint names as keys
Output a tuple (P, N) where:
P -- A 3xk numpy matrix containing all the 3d contact points in world frame
N -- A 3xk numpy matrix containing all the 3d contact normals in world frame
'''
ncp
=
np
.
sum
([
np
.
matrix
(
c
[
'P'
]).
shape
[
0
]
for
c
in
contacts
.
itervalues
()]);
P
=
mat_zeros
((
3
,
ncp
));
N
=
mat_zeros
((
3
,
ncp
));
i
=
0
;
for
(
contact_name
,
PN
)
in
contacts
.
iteritems
():
print
"key "
,
contact_name
,
robot
.
model
.
getFrameId
(
contact_name
)
print
"existe "
,
contact_name
,
robot
.
model
.
existFrame
(
contact_name
)
oMi
=
robot
.
framePosition
(
robot
.
model
.
getFrameId
(
contact_name
));
print
"oMi "
,
oMi
Pi
=
np
.
matrix
(
PN
[
'P'
]).
T
;
Ni
=
np
.
matrix
(
PN
[
'N'
]).
T
;
for
j
in
range
(
Pi
.
shape
[
1
]):
P
[:,
i
]
=
oMi
.
act
(
Pi
[:,
j
]);
N
[:,
i
]
=
oMi
.
rotation
*
Ni
[:,
j
];
i
+=
1
;
return
(
P
,
N
);
def
__str__
(
self
):
res
=
""
;
for
(
key
,
value
)
in
self
.
__dict__
.
iteritems
():
if
(
isinstance
(
value
,
np
.
ndarray
)
and
len
(
value
.
shape
)
==
2
and
value
.
shape
[
0
]
>
value
.
shape
[
1
]):
res
+=
" - "
+
key
+
": "
+
str
(
value
.
T
)
+
"
\n
"
;
else
:
res
+=
" - "
+
key
+
": "
+
str
(
value
)
+
"
\n
"
;
return
res
[:
-
1
];
def
select_contact_to_make
(
x_com
,
dx_com
,
robot
,
name_contact_to_break
,
contacts
,
contact_candidates
,
mu
,
gravity
,
mass
,
viewer
=
None
,
verb
=
0
):
ncp
=
np
.
sum
([
np
.
matrix
(
c
[
'P'
]).
shape
[
0
]
for
c
in
contacts
.
itervalues
()]);
p
=
mat_zeros
((
3
,
ncp
));
# contact points in world frame
N
=
mat_zeros
((
3
,
ncp
));
# contact normals in world frame
N
[
2
,:]
=
1.0
;
(
p
,
N
)
=
compute_contact_points_from_contact_dictionary
(
robot
,
contacts
);
stabilityCriterion
=
StabilityCriterion
(
"Stab_crit"
,
x_com
,
dx_com
,
p
.
T
,
N
.
T
,
mu
,
gravity
,
mass
,
verb
=
verb
);
c_pred
=
mat_zeros
((
3
,
len
(
contact_candidates
)));
dc_pred
=
mat_zeros
((
3
,
len
(
contact_candidates
)));
...
...
@@ -41,27 +53,23 @@ def select_contact_to_make(x_com, dx_com, robot, name_contact_to_break, contacts
t_pred
=
mat_zeros
(
len
(
contact_candidates
));
# compute contact points for all contacts except the one to move
i
=
0
;
for
(
contact_name
,
PN
)
in
contacts
.
iteritems
():
if
(
contact_name
==
name_contact_to_break
):
continue
;
oMi
=
robot
.
framePosition
(
robot
.
model
.
getFrameId
(
contact_name
));
Pi
=
np
.
matrix
(
PN
[
'P'
]).
T
;
Ni
=
np
.
matrix
(
PN
[
'N'
]).
T
;
for
j
in
range
(
Pi
.
shape
[
1
]):
p
[:,
i
]
=
oMi
.
act
(
Pi
[:,
j
]);
N
[:,
i
]
=
oMi
.
rotation
*
Ni
[:,
j
];
i
+=
1
;
contacts_minus_one
=
dict
(
contacts
);
del
contacts_minus_one
[
name_contact_to_break
];
(
P_minus_one
,
N_minus_one
)
=
compute_contact_points_from_contact_dictionary
(
robot
,
contacts_minus_one
);
i
=
P_minus_one
.
shape
[
1
];
p
[:,:
i
]
=
P_minus_one
;
N
[:,:
i
]
=
N_minus_one
;
c_id
=
0
;
P_cand
=
np
.
matrix
(
contacts
[
name_contact_to_break
][
'P'
]).
T
;
# contact points of contact to break in local frame
N_cand
=
np
.
matrix
(
contacts
[
name_contact_to_break
][
'N'
]).
T
;
# contact normals of contact to break in local frame
for
(
c_id
,
oMi
)
in
enumerate
(
contact_candidates
):
# compute new position of contact points
for
j
in
range
(
P
i
.
shape
[
1
]):
for
j
in
range
(
P
_cand
.
shape
[
1
]):
p
[:,
i
+
j
]
=
oMi
.
act
(
P_cand
[:,
j
]);
N
[:,
i
+
j
]
=
oMi
.
rotation
*
N_cand
[:,
j
];
if
(
viewer
is
not
None
):
if
(
verb
>
0
and
viewer
is
not
None
):
# update contact points in viewer
for
j
in
range
(
p
.
shape
[
1
]):
viewer
.
addSphere
(
"contact_point"
+
str
(
j
),
0.005
,
p
[:,
j
],
(
0.
,
0.
,
0.
),
(
1
,
1
,
1
,
1
));
...
...
@@ -93,16 +101,13 @@ def select_contact_to_make(x_com, dx_com, robot, name_contact_to_break, contacts
dc
=
dc_pred
[:,
best_candidate_id
],
t
=
t_pred
[
best_candidate_id
,
0
]);
def
select_contact_to_break
(
x_com
,
dx_com
,
robot
,
mass
,
contacts
,
mu
,
gravity
,
step_time
):
def
select_contact_to_break
(
x_com
,
dx_com
,
robot
,
mass
,
contacts
,
mu
,
gravity
,
step_time
,
verb
=
0
):
''' Select which contact to break
'''
ncp
=
np
.
sum
([
np
.
matrix
(
c
[
'P'
]).
shape
[
0
]
for
c
in
contacts
.
itervalues
()]);
# assume all contacts have the same number of contact points
ncp
-=
np
.
matrix
(
contacts
.
itervalues
().
next
()[
'P'
]).
shape
[
0
];
p
=
mat_zeros
((
3
,
ncp
));
N
=
mat_zeros
((
3
,
ncp
));
p
=
mat_zeros
((
3
,
1
));
N
=
mat_zeros
((
3
,
1
));
N
[
2
,:]
=
1.0
;
stabilityCriterion
=
StabilityCriterion
(
"Stab_crit"
,
x_com
,
dx_com
,
p
.
T
,
N
.
T
,
mu
,
gravity
,
mass
);
stabilityCriterion
=
StabilityCriterion
(
"Stab_crit"
,
x_com
,
dx_com
,
p
.
T
,
N
.
T
,
mu
,
gravity
,
mass
,
verb
=
verb
);
t_pred
=
mat_zeros
(
len
(
contacts
));
t_min
=
mat_zeros
(
len
(
contacts
));
v_pred
=
mat_zeros
(
len
(
contacts
));
...
...
@@ -111,18 +116,10 @@ def select_contact_to_break(x_com, dx_com, robot, mass, contacts, mu, gravity, s
dc_min
=
mat_zeros
((
3
,
len
(
contacts
)));
for
(
contactId
,
name_of_contact_to_break
)
in
enumerate
(
contacts
):
# compute the contact points without name_of_contact_to_break
i
=
0
;
for
(
contact_name
,
PN
)
in
contacts
.
iteritems
():
if
(
contact_name
==
name_of_contact_to_break
):
continue
;
fid
=
robot
.
model
.
getFrameId
(
contact_name
);
oMi
=
robot
.
framePosition
(
fid
);
Pi
=
np
.
matrix
(
PN
[
'P'
]).
T
;
Ni
=
np
.
matrix
(
PN
[
'N'
]).
T
;
for
j
in
range
(
Pi
.
shape
[
1
]):
p
[:,
i
]
=
oMi
.
act
(
Pi
[:,
j
]);
N
[:,
i
]
=
oMi
.
rotation
*
Ni
[:,
j
];
i
+=
1
;
contacts_minus_one
=
dict
(
contacts
);
del
contacts_minus_one
[
name_of_contact_to_break
];
(
p
,
N
)
=
compute_contact_points_from_contact_dictionary
(
robot
,
contacts_minus_one
);
# predict future com state supposing to break name_of_contact_to_break
stabilityCriterion
.
set_contacts
(
p
.
T
,
N
.
T
,
mu
);
try
:
...
...
@@ -133,8 +130,9 @@ def select_contact_to_break(x_com, dx_com, robot, mass, contacts, mu, gravity, s
dc_pred
[:,
contactId
]
=
np
.
asmatrix
(
res
.
dc
).
T
;
dc_min
[:,
contactId
]
=
np
.
asmatrix
(
res
.
dc_min
).
T
;
v_pred
[
contactId
]
=
norm
(
res
.
dc
);
# print "Without contact %s robot will be able to maintain the contact for %.3f s"%(name_of_contact_to_break,t);
# print " Predicted com state = (", c_t.T, dc_t.T, "), norm(dc)=%.3f"%norm(dc_t);
if
(
verb
>
0
):
print
"[select_contact_to_break] Without contact %s contacts can be kept for %.3f s"
%
(
name_of_contact_to_break
,
res
.
t
);
print
" Predicted com state = ("
,
res
.
c
.
T
,
res
.
dc
.
T
,
"), norm(dc)=%.3f"
%
norm
(
res
.
dc
);
except
Exception
as
e
:
print
"ERROR while computing stability criterion:"
,
e
;
...
...
@@ -147,6 +145,22 @@ def select_contact_to_break(x_com, dx_com, robot, mass, contacts, mu, gravity, s
return
Bunch
(
name
=
name_of_contact_to_break
,
c
=
c_pred
[:,
id_contact_to_break
],
dc
=
dc_pred
[:,
id_contact_to_break
],
t
=
t_pred
[
id_contact_to_break
,
0
],
t_min
=
t_min
[
id_contact_to_break
,
0
],
dc_min
=
dc_min
[:,
id_contact_to_break
]);
def
predict_future_com_state
(
x_com
,
dx_com
,
robot
,
mass
,
contacts
,
mu
,
gravity
,
time
,
verb
=
0
):
''' Predict future com state
'''
(
P
,
N
)
=
compute_contact_points_from_contact_dictionary
(
robot
,
contacts
);
stabilityCriterion
=
StabilityCriterion
(
"Stab_crit"
,
x_com
,
dx_com
,
P
.
T
,
N
.
T
,
mu
,
gravity
,
mass
,
verb
=
verb
);
try
: