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
05e12ede
Commit
05e12ede
authored
Oct 22, 2019
by
Guilhem Saurel
Browse files
[Python] Format
parent
85b2ae88
Changes
19
Expand all
Hide whitespace changes
Inline
Side-by-side
.yapfignore
0 → 100644
View file @
05e12ede
script
profile
affordance-tests
setup.cfg
0 → 100644
View file @
05e12ede
[flake8]
max-line-length = 119
exclude = cmake,script,profile,affordance-tests
[yapf]
column_limit = 119
[isort]
line_length = 119
skip = cmake
src/hpp/corbaserver/rbprm/__init__.py
View file @
05e12ede
from
.client
import
Client
from
.client
import
Client
# noqa
src/hpp/corbaserver/rbprm/client.py
View file @
05e12ede
...
...
@@ -20,26 +20,27 @@
from
hpp.corbaserver.client
import
Client
as
_Parent
from
hpp_idl.hpp.corbaserver.rbprm
import
RbprmBuilder
class
Client
(
_Parent
):
"""
class
Client
(
_Parent
):
"""
Connect and create clients for hpp-rbprm library.
"""
defaultClients
=
{
'rbprmbuilder'
:
RbprmBuilder
,
}
defaultClients
=
{
'rbprmbuilder'
:
RbprmBuilder
,
}
def
__init__
(
self
,
url
=
None
,
context
=
"corbaserver"
):
"""
def
__init__
(
self
,
url
=
None
,
context
=
"corbaserver"
):
"""
Initialize CORBA and create default clients.
:param url: URL in the IOR, corbaloc, corbalocs, and corbanames formats.
For a remote corba server, use
url = "corbaloc:iiop:<host>:<port>/NameService"
"""
self
.
_initOrb
(
url
)
self
.
_makeClients
(
"rbprm"
,
self
.
defaultClients
,
context
)
self
.
_initOrb
(
url
)
self
.
_makeClients
(
"rbprm"
,
self
.
defaultClients
,
context
)
# self.rbprmbuilder is created by self._makeClients
# The old code stored the object as self.rbprm
# Make it backward compatible.
self
.
rbprm
=
self
.
rbprmbuilder
# self.rbprmbuilder is created by self._makeClients
# The old code stored the object as self.rbprm
# Make it backward compatible.
self
.
rbprm
=
self
.
rbprmbuilder
src/hpp/corbaserver/rbprm/fewstepsplanner.py
View file @
05e12ede
...
...
@@ -16,100 +16,133 @@
# hpp-manipulation-corba. If not, see
# <http://www.gnu.org/licenses/>.
from
hpp.corbaserver.rbprm
import
Client
as
RbprmClient
from
hpp.corbaserver
import
Client
as
BasicClient
#from hpp.corbaserver.rbprm.tools.com_constraints import *
from
numpy
import
array
from
hpp.corbaserver.rbprm
import
rbprmstate
from
hpp.corbaserver.rbprm.rbprmstate
import
State
def
_interpolateState
(
ps
,
fullBody
,
stepsize
,
pathId
,
robustnessTreshold
=
0
,
filterStates
=
False
,
testReachability
=
True
,
quasiStatic
=
False
,
erasePreviousStates
=
True
):
if
(
filterStates
):
# from hpp.corbaserver.rbprm.tools.com_constraints import *
def
_interpolateState
(
ps
,
fullBody
,
stepsize
,
pathId
,
robustnessTreshold
=
0
,
filterStates
=
False
,
testReachability
=
True
,
quasiStatic
=
False
,
erasePreviousStates
=
True
):
if
(
filterStates
):
filt
=
1
else
:
else
:
filt
=
0
#discretize path
totalLength
=
ps
.
pathLength
(
pathId
)
configsPlan
=
[];
step
=
0.
configSize
=
fullBody
.
getConfigSize
()
-
len
(
ps
.
configAtParam
(
pathId
,
0.
))
z
=
[
0.
for
_
in
range
(
configSize
)
]
while
step
<
totalLength
:
configsPlan
+=
[
ps
.
configAtParam
(
pathId
,
step
)
+
z
[:]]
step
+=
stepsize
configsPlan
+=
[
ps
.
configAtParam
(
pathId
,
totalLength
)
+
z
[:]]
configs
=
fullBody
.
clientRbprm
.
rbprm
.
interpolateConfigs
(
configsPlan
,
robustnessTreshold
,
filt
,
testReachability
,
quasiStatic
,
erasePreviousStates
)
firstStateId
=
fullBody
.
clientRbprm
.
rbprm
.
getNumStates
()
-
len
(
configs
)
return
[
State
(
fullBody
,
i
)
for
i
in
range
(
firstStateId
,
firstStateId
+
len
(
configs
))
],
configs
# discretize path
totalLength
=
ps
.
pathLength
(
pathId
)
configsPlan
=
[]
step
=
0.
configSize
=
fullBody
.
getConfigSize
()
-
len
(
ps
.
configAtParam
(
pathId
,
0.
))
z
=
[
0.
for
_
in
range
(
configSize
)]
while
step
<
totalLength
:
configsPlan
+=
[
ps
.
configAtParam
(
pathId
,
step
)
+
z
[:]]
step
+=
stepsize
configsPlan
+=
[
ps
.
configAtParam
(
pathId
,
totalLength
)
+
z
[:]]
configs
=
fullBody
.
clientRbprm
.
rbprm
.
interpolateConfigs
(
configsPlan
,
robustnessTreshold
,
filt
,
testReachability
,
quasiStatic
,
erasePreviousStates
)
firstStateId
=
fullBody
.
clientRbprm
.
rbprm
.
getNumStates
()
-
len
(
configs
)
return
[
State
(
fullBody
,
i
)
for
i
in
range
(
firstStateId
,
firstStateId
+
len
(
configs
))],
configs
def
_guidePath
(
problemSolver
,
fromPos
,
toPos
):
ps
=
problemSolver
ps
.
setInitialConfig
(
fromPos
)
ps
.
resetGoalConfigs
()
ps
.
addGoalConfig
(
toPos
)
ps
.
solve
()
return
ps
.
numberPaths
()
-
1
ps
=
problemSolver
ps
.
setInitialConfig
(
fromPos
)
ps
.
resetGoalConfigs
()
ps
.
addGoalConfig
(
toPos
)
ps
.
solve
()
return
ps
.
numberPaths
()
-
1
class
FewStepPlanner
(
object
):
def
__init__
(
self
,
client
,
problemSolver
,
rbprmBuilder
,
fullBody
,
planContext
=
"rbprm_path"
,
fullBodyContext
=
"default"
,
pathPlayer
=
None
,
viewer
=
None
):
self
.
fullBody
=
fullBody
self
.
rbprmBuilder
=
rbprmBuilder
self
.
client
=
client
self
.
planContext
=
planContext
self
.
fullBodyContext
=
fullBodyContext
self
.
problemSolver
=
problemSolver
self
.
pathPlayer
=
pathPlayer
self
.
viewer
=
viewer
def
setPlanningContext
(
self
):
self
.
client
.
problem
.
selectProblem
(
self
.
planContext
)
def
setFullBodyContext
(
self
):
self
.
client
.
problem
.
selectProblem
(
self
.
fullBodyContext
)
def
setCurrentContext
(
self
,
context
):
return
self
.
client
.
problem
.
selectProblem
(
context
)
def
currentContext
(
self
):
return
self
.
client
.
problem
.
getSelected
(
"problem"
)[
0
]
def
_actInContext
(
self
,
context
,
f
,
*
args
):
oldContext
=
self
.
currentContext
()
self
.
setCurrentContext
(
context
)
res
=
f
(
*
args
)
self
.
setCurrentContext
(
oldContext
)
return
res
def
guidePath
(
self
,
fromPos
,
toPos
,
displayPath
=
False
):
pId
=
self
.
_actInContext
(
self
.
planContext
,
_guidePath
,
self
.
problemSolver
,
fromPos
,
toPos
)
self
.
setPlanningContext
()
names
=
self
.
rbprmBuilder
.
getAllJointNames
()[
1
:]
if
displayPath
:
if
self
.
pathPlayer
is
None
:
print
"can't display path, no path player given"
else
:
self
.
pathPlayer
(
pId
)
self
.
client
.
problem
.
movePathToProblem
(
pId
,
self
.
fullBodyContext
,
names
)
self
.
setFullBodyContext
()
return
pId
def
interpolateStates
(
self
,
stepsize
,
pathId
=
1
,
robustnessTreshold
=
0
,
filterStates
=
False
,
testReachability
=
True
,
quasiStatic
=
False
,
erasePreviousStates
=
True
):
return
_interpolateState
(
self
.
problemSolver
,
self
.
fullBody
,
stepsize
,
pathId
,
robustnessTreshold
,
filterStates
,
testReachability
,
quasiStatic
,
erasePreviousStates
)
def
goToQuasiStatic
(
self
,
currentState
,
toPos
,
stepsize
=
0.002
,
goalLimbsInContact
=
None
,
goalNormals
=
None
,
displayGuidePath
=
False
,
erasePreviousStates
=
False
):
pId
=
self
.
guidePath
(
currentState
.
q
()[:
7
],
toPos
,
displayPath
=
displayGuidePath
)
self
.
fullBody
.
setStartStateId
(
currentState
.
sId
)
targetState
=
currentState
.
q
()[:];
targetState
[:
7
]
=
toPos
if
goalLimbsInContact
is
None
:
goalLimbsInContact
=
currentState
.
getLimbsInContact
()
if
goalNormals
is
None
:
goalNormals
=
[[
0.
,
0.
,
1.
]
for
_
in
range
(
len
(
goalLimbsInContact
))]
self
.
fullBody
.
setEndState
(
targetState
,
goalLimbsInContact
,
goalNormals
)
return
self
.
interpolateStates
(
stepsize
,
pathId
=
pId
,
robustnessTreshold
=
1
,
filterStates
=
True
,
quasiStatic
=
True
,
erasePreviousStates
=
erasePreviousStates
)
def
__init__
(
self
,
client
,
problemSolver
,
rbprmBuilder
,
fullBody
,
planContext
=
"rbprm_path"
,
fullBodyContext
=
"default"
,
pathPlayer
=
None
,
viewer
=
None
):
self
.
fullBody
=
fullBody
self
.
rbprmBuilder
=
rbprmBuilder
self
.
client
=
client
self
.
planContext
=
planContext
self
.
fullBodyContext
=
fullBodyContext
self
.
problemSolver
=
problemSolver
self
.
pathPlayer
=
pathPlayer
self
.
viewer
=
viewer
def
setPlanningContext
(
self
):
self
.
client
.
problem
.
selectProblem
(
self
.
planContext
)
def
setFullBodyContext
(
self
):
self
.
client
.
problem
.
selectProblem
(
self
.
fullBodyContext
)
def
setCurrentContext
(
self
,
context
):
return
self
.
client
.
problem
.
selectProblem
(
context
)
def
currentContext
(
self
):
return
self
.
client
.
problem
.
getSelected
(
"problem"
)[
0
]
def
_actInContext
(
self
,
context
,
f
,
*
args
):
oldContext
=
self
.
currentContext
()
self
.
setCurrentContext
(
context
)
res
=
f
(
*
args
)
self
.
setCurrentContext
(
oldContext
)
return
res
def
guidePath
(
self
,
fromPos
,
toPos
,
displayPath
=
False
):
pId
=
self
.
_actInContext
(
self
.
planContext
,
_guidePath
,
self
.
problemSolver
,
fromPos
,
toPos
)
self
.
setPlanningContext
()
names
=
self
.
rbprmBuilder
.
getAllJointNames
()[
1
:]
if
displayPath
:
if
self
.
pathPlayer
is
None
:
print
(
"can't display path, no path player given"
)
else
:
self
.
pathPlayer
(
pId
)
self
.
client
.
problem
.
movePathToProblem
(
pId
,
self
.
fullBodyContext
,
names
)
self
.
setFullBodyContext
()
return
pId
def
interpolateStates
(
self
,
stepsize
,
pathId
=
1
,
robustnessTreshold
=
0
,
filterStates
=
False
,
testReachability
=
True
,
quasiStatic
=
False
,
erasePreviousStates
=
True
):
return
_interpolateState
(
self
.
problemSolver
,
self
.
fullBody
,
stepsize
,
pathId
,
robustnessTreshold
,
filterStates
,
testReachability
,
quasiStatic
,
erasePreviousStates
)
def
goToQuasiStatic
(
self
,
currentState
,
toPos
,
stepsize
=
0.002
,
goalLimbsInContact
=
None
,
goalNormals
=
None
,
displayGuidePath
=
False
,
erasePreviousStates
=
False
):
pId
=
self
.
guidePath
(
currentState
.
q
()[:
7
],
toPos
,
displayPath
=
displayGuidePath
)
self
.
fullBody
.
setStartStateId
(
currentState
.
sId
)
targetState
=
currentState
.
q
()[:]
targetState
[:
7
]
=
toPos
if
goalLimbsInContact
is
None
:
goalLimbsInContact
=
currentState
.
getLimbsInContact
()
if
goalNormals
is
None
:
goalNormals
=
[[
0.
,
0.
,
1.
]
for
_
in
range
(
len
(
goalLimbsInContact
))]
self
.
fullBody
.
setEndState
(
targetState
,
goalLimbsInContact
,
goalNormals
)
return
self
.
interpolateStates
(
stepsize
,
pathId
=
pId
,
robustnessTreshold
=
1
,
filterStates
=
True
,
quasiStatic
=
True
,
erasePreviousStates
=
erasePreviousStates
)
src/hpp/corbaserver/rbprm/rbprmbuilder.py
View file @
05e12ede
...
...
@@ -19,98 +19,102 @@
from
hpp.corbaserver.rbprm
import
Client
as
RbprmClient
from
hpp.corbaserver.robot
import
Robot
## Load and handle a RbprmDevice robot for rbprm planning
# # Load and handle a RbprmDevice robot for rbprm planning
#
# A RbprmDevice robot is a dual representation of a robots. One robot describes the
# A RbprmDevice robot is a dual representation of a robots. One robot describes the
# trunk of the robot, and a set of robots describe the range of motion of each limb of the robot.
class
Builder
(
Robot
):
## Constructor
def
__init__
(
self
,
load
=
True
):
self
.
tf_root
=
"base_link"
self
.
clientRbprm
=
RbprmClient
()
self
.
load
=
load
## Virtual function to load the robot model.
#
# \param urdfName urdf description of the robot trunk,
# \param urdfNameroms either a string, or an array of strings, indicating the urdf of the different roms to add.
# \param rootJointType type of root joint among ("freeflyer", "planar",
# "anchor"),
# \param meshPackageName name of the meshpackage from where the robot mesh will be loaded
# \param packageName name of the package from where the robot will be loaded
# \param urdfSuffix optional suffix for the urdf of the robot package
# \param srdfSuffix optional suffix for the srdf of the robot package
def
loadModel
(
self
,
urdfName
,
urdfNameroms
,
rootJointType
,
meshPackageName
,
packageName
,
urdfSuffix
,
srdfSuffix
):
Robot
.
__init__
(
self
,
urdfName
,
rootJointType
,
False
)
if
(
isinstance
(
urdfNameroms
,
list
)):
for
urdfNamerom
in
urdfNameroms
:
self
.
clientRbprm
.
rbprm
.
loadRobotRomModel
(
urdfNamerom
,
rootJointType
,
packageName
,
urdfNamerom
,
urdfSuffix
,
srdfSuffix
)
else
:
self
.
clientRbprm
.
rbprm
.
loadRobotRomModel
(
urdfNameroms
,
rootJointType
,
packageName
,
urdfNameroms
,
urdfSuffix
,
srdfSuffix
)
self
.
clientRbprm
.
rbprm
.
initNewProblemSolver
()
self
.
clientRbprm
.
rbprm
.
loadRobotCompleteModel
(
urdfName
,
rootJointType
,
packageName
,
urdfName
,
urdfSuffix
,
srdfSuffix
)
self
.
client
.
robot
.
meshPackageName
=
meshPackageName
self
.
meshPackageName
=
meshPackageName
self
.
packageName
=
packageName
self
.
urdfName
=
urdfName
self
.
urdfSuffix
=
urdfSuffix
self
.
srdfSuffix
=
srdfSuffix
class
Builder
(
Robot
):
# # Constructor
def
__init__
(
self
,
load
=
True
):
self
.
tf_root
=
"base_link"
self
.
clientRbprm
=
RbprmClient
()
self
.
load
=
load
# # Virtual function to load the robot model.
#
# \param urdfName urdf description of the robot trunk,
# \param urdfNameroms either a string, or an array of strings, indicating the urdf of the different roms to add.
# \param rootJointType type of root joint among ("freeflyer", "planar",
# "anchor"),
# \param meshPackageName name of the meshpackage from where the robot mesh will be loaded
# \param packageName name of the package from where the robot will be loaded
# \param urdfSuffix optional suffix for the urdf of the robot package
# \param srdfSuffix optional suffix for the srdf of the robot package
def
loadModel
(
self
,
urdfName
,
urdfNameroms
,
rootJointType
,
meshPackageName
,
packageName
,
urdfSuffix
,
srdfSuffix
):
Robot
.
__init__
(
self
,
urdfName
,
rootJointType
,
False
)
if
(
isinstance
(
urdfNameroms
,
list
)):
for
urdfNamerom
in
urdfNameroms
:
self
.
clientRbprm
.
rbprm
.
loadRobotRomModel
(
urdfNamerom
,
rootJointType
,
packageName
,
urdfNamerom
,
urdfSuffix
,
srdfSuffix
)
else
:
self
.
clientRbprm
.
rbprm
.
loadRobotRomModel
(
urdfNameroms
,
rootJointType
,
packageName
,
urdfNameroms
,
urdfSuffix
,
srdfSuffix
)
self
.
clientRbprm
.
rbprm
.
initNewProblemSolver
()
self
.
clientRbprm
.
rbprm
.
loadRobotCompleteModel
(
urdfName
,
rootJointType
,
packageName
,
urdfName
,
urdfSuffix
,
srdfSuffix
)
self
.
client
.
robot
.
meshPackageName
=
meshPackageName
self
.
meshPackageName
=
meshPackageName
self
.
packageName
=
packageName
self
.
urdfName
=
urdfName
self
.
urdfSuffix
=
urdfSuffix
self
.
srdfSuffix
=
srdfSuffix
## Init RbprmShooter
#
def
initshooter
(
self
):
return
self
.
clientRbprm
.
rbprm
.
initshooter
()
#
# Init RbprmShooter
#
def
initshooter
(
self
):
return
self
.
clientRbprm
.
rbprm
.
initshooter
()
## Sets limits on robot orientation, described according to Euler's ZYX rotation order
#
# \param bounds 6D vector with the lower and upperBound for each rotation axis in sequence
def
boundSO3
(
self
,
bounds
):
return
self
.
clientRbprm
.
rbprm
.
boundSO3
(
bounds
)
#
# Sets limits on robot orientation, described according to Euler's ZYX rotation order
#
# \param bounds 6D vector with the lower and upperBound for each rotation axis in sequence
def
boundSO3
(
self
,
bounds
):
return
self
.
clientRbprm
.
rbprm
.
boundSO3
(
bounds
)
## Specifies a preferred affordance for a given rom.
# This constrains the planner to accept a rom configuration only if
# it collides with a surface the normal of which has these properties.
#
# \param rom name of the rome,
# \param affordances list of affordance names
def
setAffordanceFilter
(
self
,
rom
,
affordances
):
return
self
.
clientRbprm
.
rbprm
.
setAffordanceFilter
(
rom
,
affordances
)
#
# Specifies a preferred affordance for a given rom.
# This constrains the planner to accept a rom configuration only if
# it collides with a surface the normal of which has these properties.
#
# \param rom name of the rome,
# \param affordances list of affordance names
def
setAffordanceFilter
(
self
,
rom
,
affordances
):
return
self
.
clientRbprm
.
rbprm
.
setAffordanceFilter
(
rom
,
affordances
)
## Specifies a rom constraint for the planner.
# A configuration will be valid if and only if the considered rom collides
# with the environment.
#
# \param romFilter array of roms indicated by name, which determine the constraint.
def
setFilter
(
self
,
romFilter
):
return
self
.
clientRbprm
.
rbprm
.
setFilter
(
romFilter
)
#
# Specifies a rom constraint for the planner.
# A configuration will be valid if and only if the considered rom collides
# with the environment.
#
# \param romFilter array of roms indicated by name, which determine the constraint.
def
setFilter
(
self
,
romFilter
):
return
self
.
clientRbprm
.
rbprm
.
setFilter
(
romFilter
)
## Export a computed path for blender
#
# Export a computed path for blender
#
# \param problem the problem associated with the path computed for the robot
# \param stepsize increment along the path
# \param pathId if of the considered path
# \param filename name of the output file where to save the output
def
exportPath
(
self
,
viewer
,
problem
,
pathId
,
stepsize
,
filename
):
import
hpp.gepetto.blender.exportmotion
as
em
em
.
exportPath
(
viewer
,
self
.
client
.
robot
,
problem
,
pathId
,
stepsize
,
filename
)
# \param problem the problem associated with the path computed for the robot
# \param stepsize increment along the path
# \param pathId if of the considered path
# \param filename name of the output file where to save the output
def
exportPath
(
self
,
viewer
,
problem
,
pathId
,
stepsize
,
filename
):
import
hpp.gepetto.blender.exportmotion
as
em
em
.
exportPath
(
viewer
,
self
.
client
.
robot
,
problem
,
pathId
,
stepsize
,
filename
)
## set a reference position of the end effector for the given ROM
# This reference will be used in the heuristic that choose the "best" contact surface
# and approximate the contact points in the kinodynamic planner
# \param romName the name of the rom
# \param ref the 3D reference position of the end effector, expressed in the root frame
def
setReferenceEndEffector
(
self
,
romName
,
ref
):
return
self
.
clientRbprm
.
rbprm
.
setReferenceEndEffector
(
romName
,
ref
)
#
# set a reference position of the end effector for the given ROM
# This reference will be used in the heuristic that choose the "best" contact surface
# and approximate the contact points in the kinodynamic planner
# \param romName the name of the rom
# \param ref the 3D reference position of the end effector, expressed in the root frame
def
setReferenceEndEffector
(
self
,
romName
,
ref
):
return
self
.
clientRbprm
.
rbprm
.
setReferenceEndEffector
(
romName
,
ref
)
## For a given limb, return all the intersections between the limb reachable workspace and a contact surface
# \param configuration the root configuration
# \param limbName name of the considered limb
# \return a 3D list : first id is the different surfaces, second id is the different vertex of a surface, last id is the 3 coordinate of each vertex
def
getContactSurfacesAtConfig
(
self
,
configuration
,
limbName
):
surfaces
=
self
.
clientRbprm
.
rbprm
.
getContactSurfacesAtConfig
(
configuration
,
limbName
)
res
=
[]
for
surface
in
surfaces
:
res
+=
[[
surface
[
i
:
i
+
3
]
for
i
in
range
(
0
,
len
(
surface
),
3
)]]
# split list of coordinate every 3 points (3D points)
return
res
# # For a given limb, return all the intersections between the limb reachable workspace and a contact surface
# \param configuration the root configuration
# \param limbName name of the considered limb
# \return a 3D list : first id is the different surfaces, second id is the different vertex of a surface, last id
# is the 3 coordinate of each vertex
def
getContactSurfacesAtConfig
(
self
,
configuration
,
limbName
):
surfaces
=
self
.
clientRbprm
.
rbprm
.
getContactSurfacesAtConfig
(
configuration
,
limbName
)
res
=
[]
for
surface
in
surfaces
:
res
+=
[[
surface
[
i
:
i
+
3
]
for
i
in
range
(
0
,
len
(
surface
),
3
)]]
# split list of coordinate every 3 points (3D points)
return
res
src/hpp/corbaserver/rbprm/rbprmfullbody.py
View file @
05e12ede
This diff is collapsed.
Click to expand it.
src/hpp/corbaserver/rbprm/rbprmstate.py
View file @
05e12ede
...
...
@@ -16,152 +16,158 @@
# hpp-manipulation-corba. If not, see
# <http://www.gnu.org/licenses/>.
from
hpp.corbaserver.rbprm
import
Client
as
RbprmClient
from
hpp.corbaserver
import
Client
as
BasicClient
#from hpp.corbaserver.rbprm.tools.com_constraints import *
from
hpp.corbaserver.rbprm.tools.com_constraints
import
get_com_constraint
from
numpy
import
array
## Creates a state given an Id pointing to an existing c++ state
#
# Creates a state given an Id pointing to an existing c++ state
#
# A RbprmDevice robot is a set of two robots. One for the
# A RbprmDevice robot is a set of two robots. One for the
# trunk of the robot, one for the range of motion
class
State
(
object
):
## Constructor
def
__init__
(
self
,
fullBody
,
sId
=-
1
,
isIntermediate
=
False
,
q
=
None
,
limbsIncontact
=
[]):
assert
((
sId
>
-
1
and
len
(
limbsIncontact
)
==
0
)
or
sId
==
-
1
),
"state created from existing id can't specify limbs in contact"
class
State
(
object
):
# # Constructor
def
__init__
(
self
,
fullBody
,
sId
=-
1
,
isIntermediate
=
False
,
q
=
None
,
limbsIncontact
=
[]):
assert
((
sId
>
-
1
and
len
(
limbsIncontact
)
==
0
)
or
sId
==
-
1
),
"state created from existing id can't specify limbs in contact"
self
.
cl
=
fullBody
.
clientRbprm
.
rbprm
if
(
sId
==
-
1
):
if
(
sId
==
-
1
):
self
.
sId
=
self
.
cl
.
createState
(
q
,
limbsIncontact
)
self
.
isIntermediate
=
False
self
.
isIntermediate
=
False
else
:
self
.
sId
=
sId
self
.
isIntermediate
=
isIntermediate
self
.
isIntermediate
=
isIntermediate
self
.
fullBody
=
fullBody
self
.
H_h
=
None
self
.
__last_used_friction
=
None
## assert for case where functions can't be used with intermediate state
#
# assert for case where functions can't be used with intermediate state
def
_cni
(
self
):
assert
not
self
.
isIntermediate
,
"method can't be called with intermediate state"
## Get the state configuration
#
# Get the state configuration
def
q
(
self
):
self
.
_cni
()
return
self
.
cl
.
getConfigAtState
(
self
.
sId
)
## Set the state configuration
#
# Set the state configuration
# \param q configuration of the robot
# \return whether or not the configuration was successfully set
def
setQ
(
self
,
q
):
self
.
_cni
()
return
self
.
cl
.
setConfigAtState
(
self
.
sId
,
q
)
>
0
return
self
.
cl
.
setConfigAtState
(
self
.
sId
,
q
)
>
0
# \param state1 current state considered
# \param limb name of the limb for which the request aplies
# \return whether the limb is in contact at this state
def
isLimbInContact
(
self
,
limbname
):
if
(
self
.
isIntermediate
):
return
self
.
cl
.
isLimbInContactIntermediary
(
limbname
,
self
.
sId
)
>
0
else
:
return
self
.
cl
.
isLimbInContact
(
limbname
,
self
.
sId
)
>
0
#
if
(
self
.
isIntermediate
):
return
self
.
cl
.
isLimbInContactIntermediary
(
limbname
,
self
.
sId
)
>
0
else
:
return
self
.
cl
.
isLimbInContact
(
limbname
,
self
.
sId
)
>
0
#
# \param state1 current state considered
# \param limb name of the limb for which the request aplies
# \return all limbs in contact at this state
def
getLimbsInContact
(
self
):
return
[
limbName
for
limbName
in
self
.
fullBody
.
limbNames
if
self
.
isLimbInContact
(
limbName
)]
## Get the end effector position for a given configuration, assuming z normal
#
# Get the end effector position for a given configuration, assuming z normal
# \param limbName name of the limb from which to retrieve a sample
# \param configuration configuration of the robot
# \return world position of the limb end effector given the current robot configuration.
# array of size 4, where each entry is the position of a corner of the contact surface
def
getEffectorPosition
(
self
,
limbName
):
self
.
_cni
()
return
self
.
cl
.
getEffectorPosition
(
limbName
,
self
.
q
())
## Get the end effector position for a given configuration, assuming z normal
return
self
.
cl
.
getEffectorPosition
(
limbName
,
self
.
q
())
#
# Get the end effector position for a given configuration, assuming z normal
# \param limbName name of the limb from which to retrieve a sample
# \param configuration configuration of the robot
# \return world position of the limb end effector given the current robot configuration.
# array of size 4, where each entry is the position of a corner of the contact surface
def
getContactPosAndNormals
(
self
):
if
(
self
.
isIntermediate
):
if
(
self
.
isIntermediate
):
rawdata
=
self
.
cl
.
computeContactPointsAtState
(
self
.
sId
,
1
)
else
:
rawdata
=
self
.
cl
.
computeContactPointsAtState
(
self
.
sId
,
0
)