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
011b4a8e
Commit
011b4a8e
authored
Nov 07, 2016
by
Steve Tonneau
Browse files
cf previous
parent
269fe450
Changes
3
Hide whitespace changes
Inline
Side-by-side
script/tools/tro_capture_point/gen_hrp2_statically_balanced_positions_2d.py
View file @
011b4a8e
...
...
@@ -8,6 +8,8 @@ from hpp.corbaserver.rbprm.tools.cwc_trajectory import *
from
hpp
import
Error
as
hpperr
from
numpy
import
array
,
matrix
import
sample_com_vel
as
scv
packageName
=
"hrp2_14_description"
meshPackageName
=
"hrp2_14_description"
...
...
@@ -27,20 +29,22 @@ fullBody.setJointBounds ("base_joint_xyz", [-0.135,2, -1, 1, 0, 2.2])
ps
=
ProblemSolver
(
fullBody
)
r
=
Viewer
(
ps
)
n_samples
=
10000
#~ AFTER loading obstacles
rLegId
=
'0rLeg'
rLeg
=
'RLEG_JOINT0'
rLegOffset
=
[
0
,
-
0.105
,
0
,]
rLegNormal
=
[
0
,
1
,
0
]
rLegx
=
0.09
;
rLegy
=
0.05
fullBody
.
addLimb
(
rLegId
,
rLeg
,
''
,
rLegOffset
,
rLegNormal
,
rLegx
,
rLegy
,
10000
,
"manipulability"
,
0.1
)
fullBody
.
addLimb
(
rLegId
,
rLeg
,
''
,
rLegOffset
,
rLegNormal
,
rLegx
,
rLegy
,
n_samples
,
"manipulability"
,
0.1
)
lLegId
=
'1lLeg'
lLeg
=
'LLEG_JOINT0'
lLegOffset
=
[
0
,
-
0.105
,
0
]
lLegNormal
=
[
0
,
1
,
0
]
lLegx
=
0.09
;
lLegy
=
0.05
fullBody
.
addLimb
(
lLegId
,
lLeg
,
''
,
lLegOffset
,
rLegNormal
,
lLegx
,
lLegy
,
10000
,
"manipulability"
,
0.1
)
fullBody
.
addLimb
(
lLegId
,
lLeg
,
''
,
lLegOffset
,
rLegNormal
,
lLegx
,
lLegy
,
n_samples
,
"manipulability"
,
0.1
)
rarmId
=
'3Rarm'
rarm
=
'RARM_JOINT0'
...
...
@@ -51,7 +55,7 @@ rArmOffset = [-0.045,-0.01,-0.085]
rArmNormal
=
[
1
,
0
,
0
]
rArmx
=
0.015
;
rArmy
=
0.02
#disabling collision for hook
fullBody
.
addLimb
(
rarmId
,
rarm
,
rHand
,
rArmOffset
,
rArmNormal
,
rArmx
,
rArmy
,
10000
,
"manipulability"
,
0.05
,
"_6_DOF"
,
True
)
fullBody
.
addLimb
(
rarmId
,
rarm
,
rHand
,
rArmOffset
,
rArmNormal
,
rArmx
,
rArmy
,
n_samples
,
"manipulability"
,
0.05
,
"_6_DOF"
,
True
)
#~ AFTER loading obstacles
larmId
=
'4Larm'
...
...
@@ -61,7 +65,7 @@ lHand = 'LARM_JOINT5'
lArmOffset
=
[
-
0.045
,
0.01
,
-
0.085
]
lArmNormal
=
[
1
,
0
,
0
]
lArmx
=
0.015
;
lArmy
=
0.02
fullBody
.
addLimb
(
larmId
,
larm
,
lHand
,
lArmOffset
,
lArmNormal
,
lArmx
,
lArmy
,
10000
,
"manipulability"
,
0.05
,
"_6_DOF"
,
True
)
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'
},
...
...
@@ -148,6 +152,44 @@ def fill_contact_points(limbs, config, config_pinocchio):
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
,
config_gepetto
,
res
,
num_candidates
=
10
):
effectorName
=
limbsCOMConstraints
[
limb
][
'effector'
]
candidates
=
[]
for
i
in
range
(
num_candidates
):
fullBody
.
setCurrentConfig
(
fullBody
.
getSample
(
limb
,
randint
(
0
,
n_samples
-
1
)))
#~ m = _getTransform(fullBody.getJointPosition(effectorName))
candidates
.
append
(
pos_quat_to_pinocchio
(
fullBody
.
getJointPosition
(
effectorName
)))
res
[
"candidates"
][
effectorName
]
=
candidates
def
find_limbs_broken
(
target_c
,
config
,
limbs
):
def
gen_contact_candidates
(
limbs
,
config_gepetto
,
res
):
#first generate a com translation current configuration
success
,
dc
,
delta_c
=
scv
.
comTranslationAfter07s
(
res
[
"q"
],
res
[
"contact_points"
])
#set it as new com objective for projection
fullBody
.
setCurrentConfig
(
config_gepetto
)
c
=
array
(
fullBody
.
getCenterOfMass
())
target_c
=
c
+
delta_c
state_id
=
fullBody
.
createState
(
config
,
limbs
)
res
[
"candidates"
]
=
{}
brokenContacts
=
[]
candidateLimbs
=
[]
if
(
fullBody
.
projectStateToCOM
(
state_id
,
target_c
)):
#all good, all contacts kinematically maintained
for
limb
in
limbs
:
gen_contact_candidates_one_limb
(
limb
,
config_gepetto
,
res
,
10
)
from
numpy
import
cos
,
sin
,
pi
def
__eulerToQuat
(
pitch
,
roll
,
yaw
):
t0
=
cos
(
yaw
*
0.5
);
...
...
@@ -264,4 +306,3 @@ gen(limbs[0], 10)
i
=
0
import
sample_com_vel
as
scv
script/tools/tro_capture_point/sample_com_vel.py
View file @
011b4a8e
...
...
@@ -133,7 +133,7 @@ def gen_com_vel(q0, contacts):
def
comTranslationAfter07s
(
q_hpp
,
contacts
):
(
success
,
dc0
)
=
gen_com_vel
(
q_pin
(
q_hpp
),
contacts
)
return
success
,
dc0
*
0.7
return
success
,
dc0
,
dc0
*
0.7
np
.
set_printoptions
(
precision
=
2
,
suppress
=
True
);
...
...
src/rbprmbuilder.impl.cc
View file @
011b4a8e
...
...
@@ -638,6 +638,7 @@ namespace hpp {
}
lastStatesComputed_
.
push_back
(
state
);
lastStatesComputedTime_
.
push_back
(
std
::
make_pair
(
-
1.
,
state
));
return
lastStatesComputed_
.
size
()
-
1
;
}
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment