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
d72e7618
Commit
d72e7618
authored
May 12, 2017
by
t steve
Browse files
grasping
parent
197024a7
Changes
14
Hide whitespace changes
Inline
Side-by-side
CMakeLists.txt
View file @
d72e7618
...
...
@@ -94,6 +94,7 @@ install(FILES
data/urdf/polaris.urdf
data/urdf/siggraph_asia/down.urdf
data/urdf/siggraph_asia/scale.urdf
data/urdf/siggraph_asia/grasp.urdf
#~ data/urdf/scene2.urdf
DESTINATION
${
CATKIN_PACKAGE_SHARE_DESTINATION
}
/urdf
)
...
...
@@ -127,6 +128,7 @@ install(FILES
data/srdf/polaris.srdf
data/srdf/siggraph_asia/down.srdf
data/srdf/siggraph_asia/scale.srdf
data/srdf/siggraph_asia/grasp.srdf
#~ data/srdf/scene2.srdf
DESTINATION
${
CATKIN_PACKAGE_SHARE_DESTINATION
}
/srdf
)
...
...
@@ -166,6 +168,7 @@ install(FILES
data/meshes/polaris_reduced.stl
data/meshes/siggraph_asia/down.stl
data/meshes/siggraph_asia/scale.stl
data/meshes/siggraph_asia/grasp.stl
DESTINATION
${
CATKIN_PACKAGE_SHARE_DESTINATION
}
/meshes
)
install
(
FILES
...
...
data/meshes/siggraph_asia/grasp.stl
0 → 100644
View file @
d72e7618
File added
data/srdf/siggraph_asia/grasp.srdf
0 → 100644
View file @
d72e7618
<?xml version="1.0"?>
<robot
name=
"grasp"
>
</robot>
data/urdf/siggraph_asia/grasp.urdf
0 → 100644
View file @
d72e7618
<robot name="grasp">
<link name="base_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/grasp.stl"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/grasp.stl"/>
</geometry>
</collision>
</link>
</robot>
idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
View file @
d72e7618
...
...
@@ -226,7 +226,8 @@ module hpp
/// \param disableEffectorCollision whether collision detection should be disabled for the end effector bones
void addLimb(in string id, in string limb, in string effector, in floatSeq offset, in floatSeq normal,
in double x, in double y, in unsigned short samples, in string heuristicName,
in double resolution, in string contactType, in double disableEffectorCollision) raises (Error);
in double resolution, in string contactType, in double disableEffectorCollision,
in double grasp) raises (Error);
/// Specifies a subchain of the robot as a limb, which can be used to create contacts.
/// A limb must consist in a simple kinematic chain, where every node has only one child
...
...
@@ -236,7 +237,7 @@ module hpp
/// \param loadValues whether other values computed for the limb database should be loaded
/// \param disableEffectorCollision whether collision detection should be disabled for the end effector bones
void addLimbDatabase(in string databasepath, in string id, in string heuristicName, in double loadValues,
in double disableEffectorCollision) raises (Error);
in double disableEffectorCollision
,in double grasp
) raises (Error);
/// Set the start state of a contact generation problem
/// environment, ordered by their efficiency
...
...
script/scenarios/demos/siggraph_asia/grasp_hrp2_interp.py
0 → 100644
View file @
d72e7618
from
hpp.corbaserver.rbprm.rbprmbuilder
import
Builder
from
hpp.corbaserver.rbprm.rbprmfullbody
import
FullBody
from
hpp.gepetto
import
Viewer
import
grasp_hrp2_path
as
tp
import
time
packageName
=
"hrp2_14_description"
meshPackageName
=
"hrp2_14_description"
rootJointType
=
"freeflyer"
##
# Information to retrieve urdf and srdf files.
urdfName
=
"hrp2_14"
urdfSuffix
=
"_reduced"
srdfSuffix
=
""
fullBody
=
FullBody
()
fullBody
.
loadFullBodyModel
(
urdfName
,
rootJointType
,
meshPackageName
,
packageName
,
urdfSuffix
,
srdfSuffix
)
fullBody
.
setJointBounds
(
"base_joint_xyz"
,
[
-
3
,
3
,
-
2
,
2
,
0
,
1
])
ps
=
tp
.
ProblemSolver
(
fullBody
)
r
=
tp
.
Viewer
(
ps
,
viewerClient
=
tp
.
r
.
client
)
#~ AFTER loading obstacles
rLegId
=
'hrp2_rleg_rom'
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
)
lLegId
=
'hrp2_lleg_rom'
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
)
#~ AFTER loading obstacles
larmId
=
'hrp2_larm_rom'
larm
=
'LARM_JOINT0'
lHand
=
'LARM_JOINT5'
lArmOffset
=
[
-
0.05
,
-
0.050
,
-
0.050
]
lArmNormal
=
[
1
,
0
,
0
]
lArmOffset
=
[
0
,
0
,
-
0.105
]
lArmNormal
=
[
0
,
0
,
1
]
lArmx
=
0.024
;
lArmy
=
0.024
fullBody
.
addLimb
(
larmId
,
larm
,
lHand
,
lArmOffset
,
lArmNormal
,
lArmx
,
lArmy
,
10000
,
"manipulability"
,
0.1
,
"_6_DOF"
,
True
,
grasp
=
True
)
#~ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, "manipulability", 0.1, "_6_DOF", True)
#~ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, 0.05)
rarmId
=
'hrp2_rarm_rom'
rarm
=
'RARM_JOINT0'
rHand
=
'RARM_JOINT5'
rArmOffset
=
[
0
,
0
,
-
0.105
]
rArmNormal
=
[
0
,
0
,
1
]
rArmx
=
0.024
;
rArmy
=
0.024
#disabling collision for hook
fullBody
.
addLimb
(
rarmId
,
rarm
,
rHand
,
rArmOffset
,
rArmNormal
,
rArmx
,
rArmy
,
10000
,
"manipulability"
,
0.1
,
"_6_DOF"
,
True
,
grasp
=
True
)
#~ fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 10000, "manipulability", 0.1, "_6_DOF", True)
rKneeId
=
'0RKnee'
rLeg
=
'RLEG_JOINT0'
rKnee
=
'RLEG_JOINT3'
rLegOffset
=
[
0.105
,
0.055
,
0.017
]
rLegNormal
=
[
-
1
,
0
,
0
]
rLegx
=
0.05
;
rLegy
=
0.05
#~ fullBody.addLimb(rKneeId, rLeg,rKnee,rLegOffset,rLegNormal, rLegx, rLegy, 10000, 0.01)
#~
lKneeId
=
'1LKnee'
lLeg
=
'LLEG_JOINT0'
lKnee
=
'LLEG_JOINT3'
lLegOffset
=
[
0.105
,
0.055
,
0.017
]
lLegNormal
=
[
-
1
,
0
,
0
]
lLegx
=
0.05
;
lLegy
=
0.05
#~ fullBody.addLimb(lKneeId,lLeg,lKnee,lLegOffset,lLegNormal, lLegx, lLegy, 10000, 0.01)
#~
#~
fullBody
.
runLimbSampleAnalysis
(
rLegId
,
"jointLimitsDistance"
,
True
)
fullBody
.
runLimbSampleAnalysis
(
lLegId
,
"jointLimitsDistance"
,
True
)
#~ fullBody.runLimbSampleAnalysis(larmId, "jointLimitsDistance", True)
#~ fullBody.runLimbSampleAnalysis(rarmId, "jointLimitsDistance", True)
#~ fullBody.client.basic.robot.setJointConfig('LARM_JOINT0',[1])
#~ fullBody.client.basic.robot.setJointConfig('RARM_JOINT0',[-1])
q_0
=
fullBody
.
getCurrentConfig
();
#~ fullBody.createOctreeBoxes(r.client.gui, 1, rarmId, q_0,)
q_init
=
fullBody
.
getCurrentConfig
();
q_init
[
0
:
7
]
=
tp
.
q_init
[
0
:
7
]
q_goal
=
fullBody
.
getCurrentConfig
();
q_goal
[
0
:
7
]
=
tp
.
q_goal
[
0
:
7
]
fullBody
.
setCurrentConfig
(
q_init
)
q_init
=
[
-
0.05
,
-
1.12
,
0.5
,
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.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
)
fullBody
.
setCurrentConfig
(
q_goal
)
#~ r(q_goal)
q_goal
=
fullBody
.
generateContacts
(
q_goal
,
[
0
,
0
,
1
])
q_init
=
fullBody
.
generateContacts
(
q_init
,
[
0
,
0
,
1
])
#~ r(q_goal)
fullBody
.
setStartState
(
q_init
,[
rLegId
,
lLegId
,
larmId
])
#,rarmId,larmId])
#~ fullBody.setStartState(q_init,[rLegId,lLegId]) #,rarmId,larmId])
fullBody
.
setEndState
(
q_goal
,[
rLegId
,
lLegId
])
#,rarmId,larmId])
#~
#~ configs = fullBody.interpolate(0.1)
#~ configs = fullBody.interpolate(0.15)
i
=
0
;
configs
=
[]
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
}
,
larmId
:
{
'file'
:
"hrp2/LA_com.ineq"
,
'effector'
:
lHand
}
}
#~ fullBody.limbRRTFromRootPath(0,len(configs)-1,0,2)
from
hpp.corbaserver.rbprm.tools.cwc_trajectory_helper
import
step
,
clean
,
stats
,
saveAllData
,
play_traj
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
fullBody
.
client
.
basic
,
r
)
def
act
(
i
,
numOptim
=
0
,
use_window
=
0
,
friction
=
0.5
,
optim_effectors
=
True
,
verbose
=
False
,
draw
=
False
):
return
step
(
fullBody
,
configs
,
i
,
numOptim
,
pp
,
limbsCOMConstraints
,
0.4
,
optim_effectors
=
optim_effectors
,
time_scale
=
20.
,
useCOMConstraints
=
True
,
use_window
=
use_window
,
verbose
=
verbose
,
draw
=
draw
)
def
play
(
frame_rate
=
1.
/
24.
):
play_traj
(
fullBody
,
pp
,
frame_rate
)
def
saveAll
(
name
):
saveAllData
(
fullBody
,
r
,
name
)
def
initConfig
():
r
.
client
.
gui
.
setVisibility
(
"hrp2_14"
,
"ON"
)
tp
.
cl
.
problem
.
selectProblem
(
"default"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"hrp2_trunk_flexible"
,
"OFF"
)
r
(
q_init
)
def
endConfig
():
r
.
client
.
gui
.
setVisibility
(
"hrp2_14"
,
"ON"
)
tp
.
cl
.
problem
.
selectProblem
(
"default"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"hrp2_trunk_flexible"
,
"OFF"
)
r
(
q_goal
)
def
rootPath
():
tp
.
cl
.
problem
.
selectProblem
(
"rbprm_path"
)
r
.
client
.
gui
.
setVisibility
(
"hrp2_14"
,
"OFF"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
r
.
client
.
gui
.
setVisibility
(
"hyq"
,
"OFF"
)
r
.
client
.
gui
.
setVisibility
(
"hrp2_trunk_flexible"
,
"ON"
)
tp
.
pp
(
0
)
r
.
client
.
gui
.
setVisibility
(
"hrp2_trunk_flexible"
,
"OFF"
)
r
.
client
.
gui
.
setVisibility
(
"hyq"
,
"ON"
)
tp
.
cl
.
problem
.
selectProblem
(
"default"
)
def
genPlan
(
stepsize
=
0.1
):
r
.
client
.
gui
.
setVisibility
(
"hrp2_14"
,
"ON"
)
tp
.
cl
.
problem
.
selectProblem
(
"default"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"hrp2_trunk_flexible"
,
"OFF"
)
global
configs
start
=
time
.
clock
()
print
"BEFORE"
configs
=
fullBody
.
interpolate
(
stepsize
,
0
,
0
,
True
)
print
"AFTER"
end
=
time
.
clock
()
print
"Contact plan generated in "
+
str
(
end
-
start
)
+
"seconds"
def
contactPlan
(
step
=
0.5
):
r
.
client
.
gui
.
setVisibility
(
"hrp2_14"
,
"ON"
)
tp
.
cl
.
problem
.
selectProblem
(
"default"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"hrp2_trunk_flexible"
,
"OFF"
)
for
i
in
range
(
0
,
len
(
configs
)):
r
(
configs
[
i
]);
time
.
sleep
(
step
)
def
a
():
print
"initial configuration"
initConfig
()
def
b
():
print
"end configuration"
endConfig
()
def
c
():
print
"displaying root path"
rootPath
()
def
d
(
step
=
0.1
):
print
"computing contact plan"
genPlan
(
step
)
def
e
(
step
=
0.5
):
print
"displaying contact plan"
contactPlan
(
step
)
print
"Root path WXXSD in "
+
str
(
tp
.
t
)
+
" ms."
d
(
0.005
);
e
()
print
"Root path SDDSD in "
+
str
(
tp
.
t
)
+
" ms."
#~ from gen_data_from_rbprm import *
#~
#~ for config in configs:
#~ r(config)
#~ print(fullBody.client.basic.robot.getComPosition())
#~
#~ gen_and_save(fullBody,configs, "stair_bauzil_contacts_data")
#~ main()
from
gen_data_from_rbprm
import
*
from
hpp.corbaserver.rbprm.tools.com_constraints
import
get_com_constraint
#computing com bounds 0 and 1
def
__get_com
(
robot
,
config
):
save
=
robot
.
getCurrentConfig
()
robot
.
setCurrentConfig
(
config
)
com
=
robot
.
getCenterOfMass
()
robot
.
setCurrentConfig
(
save
)
return
com
from
numpy
import
matrix
,
asarray
from
numpy.linalg
import
norm
from
spline
import
bezier
def
__curveToWps
(
curve
):
return
asarray
(
curve
.
waypoints
().
transpose
()).
tolist
()
def
__Bezier
(
wps
,
init_acc
=
[
0.
,
0.
,
0.
],
end_acc
=
[
0.
,
0.
,
0.
],
init_vel
=
[
0.
,
0.
,
0.
],
end_vel
=
[
0.
,
0.
,
0.
]):
c
=
curve_constraints
();
c
.
init_vel
=
matrix
(
init_vel
);
c
.
end_vel
=
matrix
(
end_vel
);
c
.
init_acc
=
matrix
(
init_acc
);
c
.
end_acc
=
matrix
(
end_acc
);
matrix_bezier
=
matrix
(
wps
).
transpose
()
curve
=
bezier
(
matrix_bezier
,
c
)
return
__curveToWps
(
curve
),
curve
#~ return __curveToWps(bezier(matrix_bezier))
allpaths
=
[]
def
play_all_paths
():
for
_
,
pid
in
enumerate
(
allpaths
):
pp
(
pid
)
def
play_all_paths_smooth
():
for
i
,
pid
in
enumerate
(
allpaths
):
if
i
%
2
==
1
:
pp
(
pid
)
def
play_all_paths_qs
():
for
i
,
pid
in
enumerate
(
allpaths
):
if
i
%
2
==
0
:
pp
(
pid
)
def
test
(
stateid
=
1
,
path
=
False
,
use_rand
=
False
,
just_one_curve
=
False
,
num_optim
=
0
)
:
com_1
=
__get_com
(
fullBody
,
configs
[
stateid
])
com_2
=
__get_com
(
fullBody
,
configs
[
stateid
+
1
])
data
=
gen_sequence_data_from_state
(
fullBody
,
stateid
,
configs
,
mu
=
0.8
)
c_bounds_1
=
get_com_constraint
(
fullBody
,
stateid
,
configs
[
stateid
],
limbsCOMConstraints
,
interm
=
False
)
c_bounds_mid
=
get_com_constraint
(
fullBody
,
stateid
,
configs
[
stateid
],
limbsCOMConstraints
,
interm
=
True
)
c_bounds_2
=
get_com_constraint
(
fullBody
,
stateid
,
configs
[
stateid
+
1
],
limbsCOMConstraints
,
interm
=
False
)
success
,
c_mid_1
,
c_mid_2
=
solve_quasi_static
(
data
,
c_bounds
=
[
c_bounds_1
,
c_bounds_2
,
c_bounds_mid
],
use_rand
=
use_rand
,
mu
=
0.8
)
#~ success, c_mid_1, c_mid_2 = solve_dyn(data, c_bounds = [c_bounds_1, c_bounds_2, c_bounds_mid], use_rand = use_rand)
#~ success, c_mid_1, c_mid_2 = solve_dyn(data, c_bounds = [c_bounds_1, c_bounds_2])
paths_ids
=
[]
if
path
and
success
:
#~ fullBody.straightPath([c_mid_1[0].tolist(),c_mid_2[0].tolist()])
#~ fullBody.straightPath([c_mid_2[0].tolist(),com_2])
if
just_one_curve
:
print
"just one curve"
bezier_0
,
curve
=
__Bezier
([
com_1
,
c_mid_1
[
0
].
tolist
(),
c_mid_2
[
0
].
tolist
(),
com_2
])
partions
=
[
0.
,
0.1
,
0.9
,
1.
]
p0
=
fullBody
.
generateCurveTrajParts
(
bezier_0
,
partions
)
#testing intermediary configurations
print
'wtf'
,
partions
[
1
],
" "
com_interm1
=
curve
(
partions
[
1
])
com_interm2
=
curve
(
partions
[
2
])
print
"com_1"
,
com_1
success_proj1
=
project_com_colfree
(
fullBody
,
stateid
,
asarray
((
com_interm1
).
transpose
()).
tolist
()[
0
])
success_proj2
=
project_com_colfree
(
fullBody
,
stateid
+
1
,
asarray
((
com_interm2
).
transpose
()).
tolist
()[
0
])
if
not
success_proj1
:
print
"proj 1 failed"
return
False
,
c_mid_1
,
c_mid_2
,
paths_ids
if
not
success_proj2
:
print
"proj 2 failed"
return
False
,
c_mid_1
,
c_mid_2
,
paths_ids
print
"p0"
,
p0
#~ pp.displayPath(p0+1)
#~ pp.displayPath(p0+2)
pp
.
displayPath
(
p0
)
paths_ids
=
[
int
(
el
)
for
el
in
fullBody
.
comRRTFromPos
(
stateid
,
p0
+
1
,
p0
+
2
,
p0
+
3
,
num_optim
)]
else
:
print
"just all curve"
bezier_0
,
curve
=
__Bezier
([
com_1
,
c_mid_1
[
0
].
tolist
()]
,
end_acc
=
c_mid_1
[
1
].
tolist
()
,
end_vel
=
[
0.
,
0.
,
0.
])
bezier_1
,
curve
=
__Bezier
([
c_mid_1
[
0
].
tolist
(),
c_mid_2
[
0
].
tolist
()],
end_acc
=
c_mid_2
[
1
].
tolist
(),
init_acc
=
c_mid_1
[
1
].
tolist
(),
init_vel
=
[
0.
,
0.
,
0.
],
end_vel
=
[
0.
,
0.
,
0.
])
bezier_2
,
curve
=
__Bezier
([
c_mid_2
[
0
].
tolist
(),
com_2
]
,
init_acc
=
c_mid_2
[
1
].
tolist
(),
init_vel
=
[
0.
,
0.
,
0.
])
p0
=
fullBody
.
generateCurveTraj
(
bezier_0
)
print
"p0"
,
p0
fullBody
.
generateCurveTraj
(
bezier_1
)
fullBody
.
generateCurveTraj
(
bezier_2
)
pp
.
displayPath
(
p0
)
pp
.
displayPath
(
p0
+
1
)
pp
.
displayPath
(
p0
+
2
)
paths_ids
=
[
int
(
el
)
for
el
in
fullBody
.
comRRTFromPos
(
stateid
,
p0
,
p0
+
1
,
p0
+
2
,
num_optim
)]
#~ paths_ids = []
global
allpaths
allpaths
+=
paths_ids
[:
-
1
]
#~ allpaths += [paths_ids[-1]]
#~ pp(paths_ids[-1])
#~ return success, paths_ids, c_mid_1, c_mid_2
return
success
,
c_mid_1
,
c_mid_2
,
paths_ids
#~ data = gen_sequence_data_from_state(fullBody,3,configs)
def
prepare_whole_interp
(
stateid
,
stateid_end
):
all_points
=
[]
allSuc
=
True
for
i
in
range
(
stateid
,
stateid_end
):
com_1
=
__get_com
(
fullBody
,
configs
[
stateid
])
success
,
c_mid_1
,
c_mid_2
,
paths_ids
=
test
(
i
,
False
,
True
,
False
)
allSuc
=
success
and
allSuc
if
not
success
:
break
all_points
=
all_points
+
[
com_1
,
c_mid_1
[
0
].
tolist
(),
c_mid_2
[
0
].
tolist
()]
all_points
=
all_points
+
[
__get_com
(
fullBody
,
configs
[
stateid_end
])]
if
allSuc
:
bezier_0
=
__Bezier
(
all_points
)
p0
=
fullBody
.
generateCurveTraj
(
bezier_0
)
pp
.
displayPath
(
p0
)
num_paths
=
stateid_end
-
stateid
num_sub_paths
=
num_paths
*
3
increment
=
1.
/
float
(
num_paths
)
partitions
=
[
0.
]
for
i
in
range
(
0
,
num_paths
):
dec
=
increment
*
float
(
i
)
partitions
+=
[
dec
+
0.01
*
increment
,
dec
+
0.99
*
increment
,
dec
+
1.
*
increment
]
print
"partitions"
,
partitions
,
len
(
partitions
)
p0
=
fullBody
.
generateCurveTrajParts
(
bezier_0
,
partitions
)
+
1
paths_ids
=
[]
for
i
in
range
(
0
,
num_paths
):
print
"***************************3i"
,
p0
+
3
*
i
paths_ids
+=
[
int
(
el
)
for
el
in
fullBody
.
comRRTFromPos
(
stateid
+
i
,
p0
+
3
*
i
,
p0
+
3
*
i
+
1
,
p0
+
3
*
i
+
2
)]
#~ paths_ids = []
global
allpaths
allpaths
+=
paths_ids
[:
-
1
]
#~ pp(paths_ids[-1])
#~ success, paths_ids, c_mid_1, c_mid_2 = test(0, True, True, False)
#~ prepare_whole_interp(1, 2)
#~ pp(29),pp(9),pp(17)
from
hpp.corbaserver.rbprm.tools.path_to_trajectory
import
*
def
gen
(
len_con
=
10
,
num_optim
=
0
,
ine_curve
=
True
):
for
i
in
range
(
len_con
):
if
not
test
(
i
,
True
,
False
,
ine_curve
,
num_optim
):
for
j
in
range
(
10
):
found
=
test
(
i
,
True
,
True
,
ine_curve
,
num_optim
)
if
found
:
break
a
=
gen_trajectory_to_play
(
fullBody
,
pp
,
allpaths
,
flatten
([[
0.3
,
0.6
,
0.1
]
for
_
in
range
(
len
(
allpaths
)
/
3
)]))
return
a
#~ pp(29),pp(9),pp(17)
#~ gen(True)
script/scenarios/demos/siggraph_asia/grasp_hrp2_path.py
0 → 100644
View file @
d72e7618
from
hpp.corbaserver.rbprm.rbprmbuilder
import
Builder
from
hpp.gepetto
import
Viewer
from
hpp.corbaserver
import
Client
from
hpp.corbaserver.robot
import
Robot
as
Parent
class
Robot
(
Parent
):
rootJointType
=
'freeflyer'
packageName
=
'hpp-rbprm-corba'
meshPackageName
=
'hpp-rbprm-corba'
# URDF file describing the trunk of the robot HyQ
urdfName
=
'hrp2_trunk_flexible'
urdfSuffix
=
""
srdfSuffix
=
""
def
__init__
(
self
,
robotName
,
load
=
True
):
Parent
.
__init__
(
self
,
robotName
,
self
.
rootJointType
,
load
)
self
.
tf_root
=
"base_footprint"
self
.
client
.
basic
=
Client
()
self
.
load
=
load
rootJointType
=
'freeflyer'
packageName
=
'hpp-rbprm-corba'
meshPackageName
=
'hpp-rbprm-corba'
urdfName
=
'hrp2_trunk_flexible'
urdfNameRoms
=
[
'hrp2_larm_rom'
,
'hrp2_rarm_rom'
,
'hrp2_lleg_rom'
,
'hrp2_rleg_rom'
]
urdfSuffix
=
""
srdfSuffix
=
""
rbprmBuilder
=
Builder
()
rbprmBuilder
.
loadModel
(
urdfName
,
urdfNameRoms
,
rootJointType
,
meshPackageName
,
packageName
,
urdfSuffix
,
srdfSuffix
)
rbprmBuilder
.
setJointBounds
(
"base_joint_xyz"
,
[
-
3
,
3
,
-
2
,
2
,
0
,
1
])
#~ rbprmBuilder.setFilter(['hrp2_larm_rom','hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom'])
#~ rbprmBuilder.setFilter(['hrp2_lleg_rom','hrp2_rleg_rom'])
#~ rbprmBuilder.setAffordanceFilter('hrp2_rarm_rom', ['Support','Lean'])
#~ rbprmBuilder.setAffordanceFilter('hrp2_larm_rom', ['Support','Lean'])
#~ rbprmBuilder.setAffordanceFilter('hrp2_rarm_rom', ['Support'])
#~ rbprmBuilder.setAffordanceFilter('hrp2_larm_rom', ['Support'])
rbprmBuilder
.
setAffordanceFilter
(
'hrp2_rarm_rom'
,
[
'Lean'
])
rbprmBuilder
.
setAffordanceFilter
(
'hrp2_larm_rom'
,
[
'Lean'
])
rbprmBuilder
.
setAffordanceFilter
(
'hrp2_lleg_rom'
,
[
'Support'
,])
rbprmBuilder
.
setAffordanceFilter
(
'hrp2_rleg_rom'
,
[
'Support'
])
#~ rbprmBuilder.setNormalFilter('hrp2_rarm_rom', [0,0,1], 0.5)
#~ rbprmBuilder.setNormalFilter('hrp2_lleg_rom', [0,0,1], 0.9)
#~ rbprmBuilder.setNormalFilter('hrp2_rleg_rom', [0,0,1], 0.9)
#~ rbprmBuilder.setNormalFilter('hyq_rhleg_rom', [0,0,1], 0.9)
rbprmBuilder
.
boundSO3
([
-
0.
,
0
,
-
1
,
1
,
-
1
,
1
])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from
hpp.corbaserver.rbprm.problem_solver
import
ProblemSolver
ps
=
ProblemSolver
(
rbprmBuilder
)
r
=
Viewer
(
ps
)
q_init
=
rbprmBuilder
.
getCurrentConfig
();
#~ q_init [3:7] = [ 0.98877108, 0. , 0.14943813, 0. ]
q_init
[
0
:
3
]
=
[
-
0.05
,
-
1.12
,
0.5
];
rbprmBuilder
.
setCurrentConfig
(
q_init
);
r
(
q_init
)
#~ q_init [0:3] = [0.1, -0.82, 0.648702]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
#~ q_init [0:3] = [0, -0.63, 0.6]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
#~ q_init [3:7] = [ 0.98877108, 0. , 0.14943813, 0. ]
q_goal
=
q_init
[::]
#~ q_goal [3:7] = [ 0.98877108, 0. , 0.14943813, 0. ]
q_goal
[
0
:
3
]
=
[
-
0.05
,
-
0.12
,
0.5
];
r
(
q_goal
)
#~ q_goal [0:3] = [1.2, -0.65, 1.1]; r (q_goal)
#~ ps.addPathOptimizer("GradientBased")
ps
.
addPathOptimizer
(
"RandomShortcut"
)
ps
.
setInitialConfig
(
q_init
)
ps
.
addGoalConfig
(
q_goal
)
from
hpp.corbaserver.affordance.affordance
import
AffordanceTool
afftool
=
AffordanceTool
()
afftool
.
setAffordanceConfig
(
'Support'
,
[
0.000005
,
0.000005
,
0.00005
])
afftool
.
setAffordanceConfig
(
'Lean'
,
[
0.00005
,
0.000005
,
0.00005
])
afftool
.
loadObstacleModel
(
packageName
,
"grasp"
,
"planning"
,
r
)
#~ afftool.analyseAll()
afftool
.
visualiseAffordances
(
'Support'
,
r
,
[
0.25
,
0.5
,
0.5
])
afftool
.
visualiseAffordances
(
'Lean'
,
r
,
[
0
,
0
,
0.9
])
ps
.
client
.
problem
.
selectConFigurationShooter
(
"RbprmShooter"
)
ps
.
client
.
problem
.
selectPathValidation
(
"RbprmPathValidation"
,
0.05
)
#~ ps.solve ()
t
=
ps
.
solve
()
print
t
;
if
isinstance
(
t
,
list
):
t
=
t
[
0
]
*
3600000
+
t
[
1
]
*
60000
+
t
[
2
]
*
1000
+
t
[
3
]
f
=
open
(
'log.txt'
,
'a'
)
f
.
write
(
"path computation "
+
str
(
t
)
+
"
\n
"
)
f
.
close
()
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
rbprmBuilder
.
client
.
basic
,
r
)
#~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
#~
#~ pp (2)
#~ pp (0)
#~ pp (1)
#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
#~ rbprmBuilder.exportPath (r, ps.client.problem, 1, 0.01, "stair_bauzil_hrp2_path.txt")
cl
=
Client
()
cl
.
problem
.
selectProblem
(
"rbprm_path"
)
rbprmBuilder2
=
Robot
(
"toto"
)
ps2
=
ProblemSolver
(
rbprmBuilder2
)
cl
.
problem
.
selectProblem
(
"default"
)