Skip to content
GitLab
Explore
Sign in
Register
Primary navigation
Search or go to…
Project
H
hpp-rbprm-corba
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Container Registry
Model registry
Operate
Environments
Monitor
Incidents
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
Guilhem Saurel
hpp-rbprm-corba
Commits
2d4f94bb
Commit
2d4f94bb
authored
9 years ago
by
Steve Tonneau
Browse files
Options
Downloads
Patches
Plain Diff
managed python bindings, can plan with a rbprmdevice
parent
428beda7
No related branches found
Branches containing commit
No related tags found
No related merge requests found
Changes
3
Hide whitespace changes
Inline
Side-by-side
Showing
3 changed files
src/CMakeLists.txt
+1
-0
1 addition, 0 deletions
src/CMakeLists.txt
src/hpp/corbaserver/rbprm/problem_solver.py
+376
-0
376 additions, 0 deletions
src/hpp/corbaserver/rbprm/problem_solver.py
src/hpp/corbaserver/rbprm/rbprmbuilder.py
+25
-7
25 additions, 7 deletions
src/hpp/corbaserver/rbprm/rbprmbuilder.py
with
402 additions
and
7 deletions
src/CMakeLists.txt
+
1
−
0
View file @
2d4f94bb
...
@@ -119,6 +119,7 @@ INSTALL(
...
@@ -119,6 +119,7 @@ INSTALL(
FILES
FILES
${
CMAKE_CURRENT_SOURCE_DIR
}
/hpp/corbaserver/rbprm/client.py
${
CMAKE_CURRENT_SOURCE_DIR
}
/hpp/corbaserver/rbprm/client.py
${
CMAKE_CURRENT_SOURCE_DIR
}
/hpp/corbaserver/rbprm/rbprmbuilder.py
${
CMAKE_CURRENT_SOURCE_DIR
}
/hpp/corbaserver/rbprm/rbprmbuilder.py
${
CMAKE_CURRENT_SOURCE_DIR
}
/hpp/corbaserver/rbprm/problem_solver.py
DESTINATION
${
PYTHON_SITELIB
}
/hpp/corbaserver/rbprm
DESTINATION
${
PYTHON_SITELIB
}
/hpp/corbaserver/rbprm
)
)
...
...
This diff is collapsed.
Click to expand it.
src/hpp/corbaserver/rbprm/problem_solver.py
0 → 100644
+
376
−
0
View file @
2d4f94bb
#!/usr/bin/env python
# Copyright (c) 2014 CNRS
# Author: Florent Lamiraux
#
# This file is part of hpp-corbaserver.
# hpp-corbaserver is free software: you can redistribute it
# and/or modify it under the terms of the GNU Lesser General Public
# License as published by the Free Software Foundation, either version
# 3 of the License, or (at your option) any later version.
#
# hpp-corbaserver is distributed in the hope that it will be
# useful, but WITHOUT ANY WARRANTY; without even the implied warranty
# of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
# General Lesser Public License for more details. You should have
# received a copy of the GNU Lesser General Public License along with
# hpp-corbaserver. If not, see
# <http://www.gnu.org/licenses/>.
## Definition of a path planning problem
#
# This class wraps the Corba client to the server implemented by
# libhpp-corbaserver.so
#
# Some method implemented by the server can be considered as private. The
# goal of this class is to hide them and to expose those that can be
# considered as public.
class
ProblemSolver
(
object
):
def
__init__
(
self
,
robot
):
self
.
client
=
robot
.
client
.
basic
self
.
robot
=
robot
## \name Initial and goal configurations
# \{
## Set initial configuration of specified problem.
# \param dofArray Array of degrees of freedom
# \throw Error.
def
setInitialConfig
(
self
,
dofArray
):
return
self
.
client
.
problem
.
setInitialConfig
(
dofArray
)
## Get initial configuration of specified problem.
# \return Array of degrees of freedom
def
getInitialConfig
(
self
):
return
self
.
client
.
problem
.
getInitialConfig
()
## Add goal configuration to specified problem.
# \param dofArray Array of degrees of freedom
# \throw Error.
def
addGoalConfig
(
self
,
dofArray
):
return
self
.
client
.
problem
.
addGoalConfig
(
dofArray
)
## Get goal configurations of specified problem.
# \return Array of degrees of freedom
def
getGoalConfigs
(
self
):
return
self
.
client
.
problem
.
getGoalConfigs
()
## Reset goal configurations
def
resetGoalConfigs
(
self
):
return
self
.
client
.
problem
.
resetGoalConfigs
()
## \}
## \name Obstacles
# \{
## Load obstacle from urdf file
# \param package Name of the package containing the model,
# \param filename name of the urdf file in the package
# (without suffix .urdf)
# \param prefix prefix added to object names in case the same file is
# loaded several times
#
# The ros url is built as follows:
# "package://${package}/urdf/${filename}.urdf"
#
# The kinematic structure of the urdf file is ignored. Only the geometric
# objects are loaded as obstacles.
def
loadObstacleFromUrdf
(
self
,
package
,
filename
,
prefix
):
return
self
.
client
.
obstacle
.
loadObstacleModel
(
package
,
filename
,
prefix
)
## Remove an obstacle from outer objects of a joint body
#
# \param objectName name of the object to remove,
# \param jointName name of the joint owning the body,
# \param collision whether collision with object should be computed,
# \param distance whether distance to object should be computed.
# \throw Error.
def
removeObstacleFromJoint
(
self
,
objectName
,
jointName
,
collision
,
distance
):
return
self
.
client
.
obstacle
.
removeObstacleFromJoint
\
(
objectName
,
jointName
,
collision
,
distance
)
## Move an obstacle to a given configuration.
# \param objectName name of the polyhedron.
# \param cfg the configuration of the obstacle.
# \throw Error.
#
# \note The obstacle is not added to local map
# impl::Obstacle::collisionListMap.
#
# \note Build the collision entity of polyhedron for KCD.
def
moveObstacle
(
self
,
objectName
,
cfg
):
return
self
.
client
.
obstacle
.
moveObstacle
(
objectName
,
cfg
)
## Get the position of an obstacle
#
# \param objectName name of the polyhedron.
# \retval cfg Position of the obstacle.
# \throw Error.
def
getObstaclePosition
(
self
,
objectName
):
return
self
.
client
.
obstacle
.
getObstaclePosition
(
objectName
)
## Get list of obstacles
#
# \param collision whether to return obstacle for collision,
# \param distance whether to return obstacles for distance computation
# \return list of obstacles
def
getObstacleNames
(
self
,
collision
,
distance
):
return
self
.
client
.
obstacle
.
getObstacleNames
(
collision
,
distance
)
##\}
## \name Constraints
# \{
## Create orientation constraint between two joints
#
# \param constraintName name of the constraint created,
# \param joint1Name name of first joint
# \param joint2Name name of second joint
# \param p quaternion representing the desired orientation
# of joint2 in the frame of joint1.
# \param mask Select which axis to be constrained.
# If joint1 or joint2 is "", the corresponding joint is replaced by
# the global frame.
# constraints are stored in ProblemSolver object
def
createOrientationConstraint
(
self
,
constraintName
,
joint1Name
,
joint2Name
,
p
,
mask
):
return
self
.
client
.
problem
.
createOrientationConstraint
\
(
constraintName
,
joint1Name
,
joint2Name
,
p
,
mask
)
## Create RelativeCom constraint between two joints
#
# \param constraintName name of the constraint created,
# \param comName name of CenterOfMassComputation
# \param jointName name of joint
# \param point point in local frame of joint.
# \param mask Select axis to be constrained.
# If jointName is "", the robot root joint is used.
# Constraints are stored in ProblemSolver object
def
createRelativeComConstraint
(
self
,
constraintName
,
comName
,
jointLName
,
point
,
mask
):
return
self
.
client
.
problem
.
createRelativeComConstraint
\
(
constraintName
,
comName
,
jointLName
,
point
,
mask
)
## Create ComBeetweenFeet constraint between two joints
#
# \param constraintName name of the constraint created,
# \param comName name of CenterOfMassComputation
# \param jointLName name of first joint
# \param jointRName name of second joint
# \param pointL point in local frame of jointL.
# \param pointR point in local frame of jointR.
# \param jointRefName name of second joint
# \param mask Select axis to be constrained.
# If jointRef is "", the robot root joint is used.
# Constraints are stored in ProblemSolver object
def
createComBeetweenFeet
(
self
,
constraintName
,
comName
,
jointLName
,
jointRName
,
pointL
,
pointR
,
jointRefName
,
mask
):
return
self
.
client
.
problem
.
createComBeetweenFeet
\
(
constraintName
,
comName
,
jointLName
,
jointRName
,
pointL
,
pointR
,
jointRefName
,
mask
)
## Add an object to compute a partial COM of the robot.
# \param name of the partial com
# \param jointNames list of joint name of each tree ROOT to consider.
# \note Joints are added recursively, it is not possible so far to add a
# joint without addind all its children.
def
addPartialCom
(
self
,
comName
,
jointNames
):
return
self
.
client
.
robot
.
addPartialCom
(
comName
,
jointNames
);
## Create position constraint between two joints
#
# \param constraintName name of the constraint created,
# \param joint1Name name of first joint
# \param joint2Name name of second joint
# \param point1 point in local frame of joint1,
# \param point2 point in local frame of joint2.
# \param mask Select which axis to be constrained.
# If joint1 of joint2 is "", the corresponding joint is replaced by
# the global frame.
# constraints are stored in ProblemSolver object
def
createPositionConstraint
(
self
,
constraintName
,
joint1Name
,
joint2Name
,
point1
,
point2
,
mask
):
return
self
.
client
.
problem
.
createPositionConstraint
\
(
constraintName
,
joint1Name
,
joint2Name
,
point1
,
point2
,
mask
)
## Reset Constraints
#
# Reset all constraints, including numerical constraints and locked
# joints
def
resetConstraints
(
self
):
return
self
.
client
.
problem
.
resetConstraints
()
## Set numerical constraints in ConfigProjector
#
# \param name name of the resulting numerical constraint obtained
# by stacking elementary numerical constraints,
# \param names list of names of the numerical constraints as
# inserted by method hpp::core::ProblemSolver::addNumericalConstraint.
def
setNumericalConstraints
(
self
,
name
,
names
):
return
self
.
client
.
problem
.
setNumericalConstraints
(
name
,
names
)
## Apply constraints
#
# \param q initial configuration
# \return configuration projected in success,
# \throw Error if projection failed.
def
applyConstraints
(
self
,
q
):
return
self
.
client
.
problem
.
applyConstraints
(
q
)
## Create a vector of passive dofs.
#
# \param name name of the vector in the ProblemSolver map.
# \param dofNames list of names of DOF that may
def
addPassiveDofs
(
self
,
name
,
dofNames
):
return
self
.
client
.
problem
.
addPassiveDofs
(
name
,
dofNames
)
## Generate a configuration satisfying the constraints
#
# \param maxIter maximum number of tries,
# \return configuration projected in success,
# \throw Error if projection failed.
def
generateValidConfig
(
self
,
maxIter
):
return
self
.
client
.
problem
.
generateValidConfig
(
maxIter
)
## Lock joint with given joint configuration
# \param jointName name of the joint
# \param value value of the joint configuration
def
lockJoint
(
self
,
jointName
,
value
):
return
self
.
client
.
problem
.
lockJoint
(
jointName
,
value
)
## error threshold in numerical constraint resolution
def
setErrorThreshold
(
self
,
threshold
):
return
self
.
client
.
problem
.
setErrorThreshold
(
threshold
)
## Set the maximal number of iterations
def
setMaxIterations
(
self
,
iterations
):
return
self
.
client
.
problem
.
setMaxIterations
(
iterations
)
## \}
## \name Solve problem and get paths
# \{
## Select path planner type
# \param Name of the path planner type, either "DiffusingPlanner",
# "VisibilityPrmPlanner", or any type added by method
# core::ProblemSolver::addPathPlannerType
def
selectPathPlanner
(
self
,
pathPlannerType
):
return
self
.
client
.
problem
.
selectPathPlanner
(
pathPlannerType
)
## Add a path optimizer
# \param Name of the path optimizer type, either "RandomShortcut" or
# any type added by core::ProblemSolver::addPathOptimizerType
def
addPathOptimizer
(
self
,
pathOptimizerType
):
return
self
.
client
.
problem
.
addPathOptimizer
(
pathOptimizerType
)
## Clear sequence of path optimizers
#
def
clearPathOptimizers
(
self
):
return
self
.
client
.
problem
.
clearPathOptimizers
()
## Select path validation method
# \param Name of the path validation method, either "Discretized"
# "Progressive", "Dichotomy", or any type added by
# core::ProblemSolver::addPathValidationType,
# \param tolerance maximal acceptable penetration.
def
selectPathValidation
(
self
,
pathValidationType
,
tolerance
):
return
self
.
client
.
problem
.
selectPathValidation
(
pathValidationType
,
tolerance
)
## Select path projector method
# \param Name of the path projector method, either "Discretized"
# "Progressive", "Dichotomy", or any type added by
# core::ProblemSolver::addPathProjectorType,
# \param tolerance maximal acceptable penetration.
def
selectPathProjector
(
self
,
pathProjectorType
,
tolerance
):
return
self
.
client
.
problem
.
selectPathProjector
(
pathProjectorType
,
tolerance
)
## Solve the problem of corresponding ChppPlanner object
def
solve
(
self
):
return
self
.
client
.
problem
.
solve
()
## Make direct connection between two configurations
# \param startConfig, endConfig: the configurations to link.
# \throw Error if steering method fails to create a direct path of if
# direct path is not valid
def
directPath
(
self
,
startConfig
,
endConfig
):
return
self
.
client
.
problem
.
directPath
(
startConfig
,
endConfig
)
## Get Number of paths
def
numberPaths
(
self
):
return
self
.
client
.
problem
.
numberPaths
()
## Optimize a given path
# \param inPathId Id of the path in this problem.
# \throw Error.
def
optimizePath
(
self
,
inPathId
):
return
self
.
client
.
problem
.
optimizePath
(
inPathId
)
## Get length of path
# \param inPathId rank of the path in the problem
# \return length of path if path exists.
def
pathLength
(
self
,
inPathId
):
return
self
.
client
.
problem
.
pathLength
(
inPathId
)
## Get the robot's config at param on the a path
# \param inPathId rank of the path in the problem
# \param atDistance : the user parameter choice
# \return dofseq : the config at param
def
configAtParam
(
self
,
inPathId
,
atDistance
):
return
self
.
client
.
problem
.
configAtParam
(
inPathId
,
atDistance
)
## Get way points of a path
# \param pathId rank of the path in the problem
def
getWaypoints
(
self
,
pathId
):
return
self
.
client
.
problem
.
getWaypoints
(
pathId
)
## \name Interruption of a path planning request
# \{
## \brief Interrupt path planning activity
# \note this method is effective only when multi-thread policy is used
# by CORBA server.
# See constructor of class Server for details.
def
interruptPathPlanning
(
self
):
return
self
.
client
.
problem
.
interruptPathPlanning
()
# \}
## \name exploring the roadmap
# \{
## Get nodes of the roadmap.
def
nodes
(
self
):
return
self
.
client
.
problem
.
nodes
()
# the configuration of the node nodeId
def
node
(
self
,
nodeId
):
return
self
.
client
.
problem
.
node
(
nodeId
)
# the number of nodes in the roadmap
def
numberNodes
(
self
):
return
self
.
client
.
problem
.
numberNodes
()
## Number of edges
def
numberEdges
(
self
):
return
self
.
client
.
problem
.
numberEdges
()
## Edge at given rank
def
edge
(
self
,
edgeId
):
return
self
.
client
.
problem
.
edge
(
edgeId
)
## Number of connected components
def
numberConnectedComponents
(
self
):
return
self
.
client
.
problem
.
numberConnectedComponents
()
## Nodes of a connected component
# \param connectedComponentId index of connected component in roadmap
# \return list of nodes of the connected component.
def
nodesConnectedComponent
(
self
,
ccId
):
return
self
.
client
.
problem
.
nodesConnectedComponent
(
ccId
)
## Clear the roadmap
def
clearRoadmap
(
self
):
return
self
.
client
.
problem
.
clearRoadmap
()
## \}
This diff is collapsed.
Click to expand it.
src/hpp/corbaserver/rbprm/rbprmbuilder.py
+
25
−
7
View file @
2d4f94bb
...
@@ -42,18 +42,36 @@ class Builder (object):
...
@@ -42,18 +42,36 @@ class Builder (object):
# \param load whether to actually load urdf files. Set to no if you only
# \param load whether to actually load urdf files. Set to no if you only
# want to initialize a corba client to an already initialized
# want to initialize a corba client to an already initialized
# problem.
# problem.
def
__init__
(
self
,
trunkName
,
romName
,
rootJointType
,
load
=
True
):
def
__init__
(
self
,
load
=
True
):
self
.
tf_root
=
"
base_link
"
self
.
tf_root
=
"
base_link
"
self
.
rootJointType
=
dict
()
self
.
rootJointType
=
dict
()
self
.
name
=
trunkName
self
.
displayName
=
trunkName
self
.
client
=
CorbaClient
()
self
.
client
=
CorbaClient
()
self
.
load
=
load
self
.
load
=
load
self
.
loadModel
(
trunkName
,
romName
,
rootJointType
)
## Virtual function to load the robot model
## Virtual function to load the robot model
def
loadModel
(
self
,
trunkName
,
romName
,
rootJointType
):
def
loadModel
(
self
,
urdfName
,
rootJointType
,
packageName
,
meshPackageName
,
urdfSuffix
,
srdfSuffix
):
self
.
client
.
rbprm
.
robot
.
create
(
trunkName
,
romName
)
self
.
client
.
rbprm
.
rbprm
.
loadRobotRomModel
(
urdfName
,
rootJointType
,
packageName
,
urdfName
,
urdfSuffix
,
srdfSuffix
)
self
.
client
.
rbprm
.
rbprm
.
loadRobotCompleteModel
(
urdfName
,
rootJointType
,
packageName
,
urdfName
,
urdfSuffix
,
srdfSuffix
)
self
.
name
=
urdfName
self
.
displayName
=
urdfName
self
.
tf_root
=
"
base_link
"
self
.
rootJointType
=
rootJointType
self
.
jointNames
=
self
.
client
.
basic
.
robot
.
getJointNames
()
self
.
allJointNames
=
self
.
client
.
basic
.
robot
.
getAllJointNames
()
self
.
client
.
basic
.
robot
.
meshPackageName
=
meshPackageName
self
.
meshPackageName
=
meshPackageName
self
.
rankInConfiguration
=
dict
()
self
.
rankInVelocity
=
dict
()
self
.
packageName
=
packageName
self
.
urdfName
=
urdfName
self
.
urdfSuffix
=
urdfSuffix
self
.
srdfSuffix
=
srdfSuffix
rankInConfiguration
=
rankInVelocity
=
0
for
j
in
self
.
jointNames
:
self
.
rankInConfiguration
[
j
]
=
rankInConfiguration
rankInConfiguration
+=
self
.
client
.
basic
.
robot
.
getJointConfigSize
(
j
)
self
.
rankInVelocity
[
j
]
=
rankInVelocity
rankInVelocity
+=
self
.
client
.
basic
.
robot
.
getJointNumberDof
(
j
)
## \name Degrees of freedom
## \name Degrees of freedom
# \{
# \{
...
...
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
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!
Save comment
Cancel
Please
register
or
sign in
to comment