Commit 865d3de0 authored by Florent Lamiraux's avatar Florent Lamiraux Committed by Florent Lamiraux florent@laas.fr
Browse files

Update to devel branch.

parent ba858c9f
from math import sqrt
from hpp import Transform
from manipulation import robot, vf, ConstraintGraph, ps, Ground, Box, Pokeball, PathPlayer, gripperName, ballName
from hpp.corbaserver.manipulation import ConstraintGraph
from manipulation import robot, vf, ps, Ground, Box, Pokeball, PathPlayer, gripperName, ballName
vf.loadEnvironmentModel (Ground, 'ground')
vf.loadObjectModel (Pokeball, 'pokeball')
......@@ -15,14 +17,19 @@ graph = ConstraintGraph (robot, 'graph')
## Create constraint of relative position of the ball in the gripper when ball
## is grasped
graph.createGrasp ('grasp', 'ur5/gripper', 'pokeball/handle')
ballInGripper = [0, 0, 0.055, 0, -sqrt (2)/2, -sqrt (2)/2, 0]
ps.createTransformationConstraint ('grasp', gripperName, ballName,
ballInGripper, 6*[True,])
## Create nodes and edges
# Warning the order of the nodes may be important. When checking in which node
# a configuration lies, node constraints will be checked in the order of node
# creation.
graph.createNode (['grasp', 'placement'])
graph.createEdge ('placement', 'placement', 'transit', 1, 'placement')
graph.createEdge ('grasp', 'grasp', 'transfer', 1, 'grasp')
graph.createEdge ('placement', 'grasp', 'grasp-ball', 1, 'placement')
graph.createEdge ('grasp', 'placement', 'release-ball', 1, 'placement')
graph.createEdge ('grasp', 'placement', 'release-ball', 1, 'grasp')
## Create transformation constraint : ball is in horizontal plane with free
## rotation around z
......@@ -37,10 +44,12 @@ ps.setConstantRightHandSide ('placement/complement', False)
graph.setConstraints (node='placement', numConstraints = ['placement'])
graph.setConstraints (node='grasp', numConstraints = ['grasp'])
graph.setConstraints (edge='transit',
numConstraints = ['placement', 'placement/complement'])
graph.setConstraints (edge='transfer', numConstraints = ['grasp'])
numConstraints = ['placement/complement'])
graph.setConstraints (edge='grasp-ball',
numConstraints = ['placement', 'placement/complement'])
numConstraints = ['placement/complement'])
# These edges are in node 'grasp'
graph.setConstraints (edge='transfer', numConstraints = [])
graph.setConstraints (edge='release-ball', numConstraints = [])
## Project initial configuration on state 'placement'
res, q_init, error = ps.client.manipulation.problem.applyConstraints \
......@@ -56,6 +65,7 @@ res, q_goal, error = ps.client.manipulation.problem.applyConstraints \
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.selectPathValidation ("Dichotomy", 0)
ps.selectPathProjector ("Progressive", 0.1)
pp = PathPlayer (ps.client.basic, r)
......@@ -67,7 +77,8 @@ for i in range (100):
if res:
robot.setCurrentConfig (q3)
gripperPose = Transform (robot.getLinkPosition (gripperName))
ballPose = Transform (robot.getLinkPosition (ballName))
gripperAboveBall = gripperPose.inverse () * ballPose
gripperPose = Transform (robot.getJointPosition (gripperName))
ballPose = Transform (robot.getJointPosition (ballName))
gripperGraspsBall = gripperPose.inverse () * ballPose
gripperAboveBall = Transform (gripperGraspsBall)
gripperAboveBall.translation [2] += .1
from common import robot, vf, ConstraintGraph, ps, Ground, Box, Pokeball, PathPlayer, gripperName, ballName
from common import robot, vf, ps, Ground, Box, Pokeball, PathPlayer, gripperName, ballName
from hpp.corbaserver.manipulation.ur5 import Robot
from hpp.corbaserver.manipulation import ConstraintGraph as Parent
from hpp.corbaserver.manipulation import ProblemSolver
from hpp.gepetto.manipulation import ViewerFactory
from hpp.gepetto import PathPlayer
......@@ -32,91 +31,6 @@ class Box (object):
urdfSuffix = ""
srdfSuffix = ""
class ConstraintGraph (Parent) :
def __init__ (self, robot, graphName, makeGraph = True) :
Parent.__init__ (self, robot, graphName, makeGraph)
## Apply constaints to a configuration
#
# \param node name of the node the constraints of which to apply
# \param input input configuration,
# \retval output output configuration,
# \retval error norm of the residual error.
def applyNodeConstraints (self, node, input) :
return self.client.problem.applyConstraints (self.nodes [node],
input)
## Apply edge constaints to a configuration
#
# \param edge name of the edge
# \param qfrom configuration defining the right hand side of the edge
# constraint,
# \param input input configuration,
# \retval output output configuration,
# \retval error norm of the residual error.
#
# Compute a configuration in the destination node of the edge,
# reachable from qFrom.
def generateTargetConfig (self, edge, qfrom, input) :
return self.client.problem.applyConstraintsWithOffset \
(self.edges [edge], qfrom, input)
## Build a path from qb to qe using the Edge::build.
# \param edge name of the edge to use.
# \param qb configuration at the beginning of the path
# \param qe configuration at the end of the path
# \retval return true if the path is built and fully projected.
# \retval indexNotProj -1 is the path could not be built. The index
# of the built path (before projection) in the
# in the ProblemSolver path vector.
# \retval indexProj -1 is the path could not be fully projected. The
# index of the built path (before projection) in the
# in the ProblemSolver path vector.
# No path validation is made. The paths can be retrieved using
# corbaserver::Problem::configAtParam
def buildAndProjectPath (self, edge, qb, qe) :
return self.client.problem.buildAndProjectPath \
(self.edges [edge], qb, qe)
## Get error of a config with respect to a node constraint
#
# \param config Configuration,
# \param node name of the node.
# \retval error the error of the node constraint for the
# configuration
# \return whether the configuration belongs to the node.
# Call method core::ConstraintSet::isSatisfied for the node
# constraints.
def getConfigErrorForNode (self, config, nodeId) :
return self.client.graph.getConfigErrorForNode \
(config, self.nodes [nodeId])
## Add a configuration to the roadmap.
# \param config to be added to the roadmap.
def addConfigToRoadmap (self, config):
return self.clientBasic.problem.addConfigToRoadmap(config)
## Add an edge to roadmap. If
# \param config1, config2 the ends of the path,
# \param pathId the index if the path in the vector of path,
# \param bothEdges if FALSE, only add config1 to config2, otherwise, If TRUE. add edges config1->config2 AND config2->config1.
def addEdgeToRoadmap (self, config1, config2, pathId, bothEdges):
return self.clientBasic.problem.addEdgeToRoadmap \
(config1, config2, pathId, bothEdges)
## Set that an edge is short
# \param edge name of the edge
# \param True or False
#
# When an edge is tagged as short, extension along this edge is
# done differently in RRT-like algorithms. Instead of projecting
# a random configuration in the destination node, the
# configuration to extend itself is projected in the destination
# node. This makes the rate of success higher.
def setShort (self, edge, isShort) :
return self.client.graph.setShort (self.edges [edge], isShort)
robot = Robot ('ur5-pokeball', 'ur5')
robot.client.basic.problem.setErrorThreshold (1e-4)
robot.client.basic.problem.setMaxIterations (40)
......
from math import sqrt
from hpp import Transform
from manipulation import robot, vf, ConstraintGraph, ps, Ground, Box, Pokeball, PathPlayer, gripperName, ballName
from hpp.corbaserver.manipulation import ConstraintGraph
from manipulation import robot, vf, ps, Ground, Box, Pokeball, PathPlayer, gripperName, ballName
vf.loadEnvironmentModel (Ground, 'ground')
vf.loadEnvironmentModel (Box, 'box')
......@@ -31,6 +32,7 @@ res, q_goal, error = graph.applyNodeConstraints ('placement', q2)
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.selectPathValidation ("Dichotomy", 0)
ps.selectPathProjector ("Progressive", 0.1)
pp = PathPlayer (ps.client.basic, r)
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment