from math import sqrt from hpp import Transform from manipulation import robot, vf, ConstraintGraph, ps, Ground, Box, Pokeball, PathPlayer, gripperName, ballName vf.loadEnvironmentModel (Ground, 'ground') vf.loadEnvironmentModel (Box, 'box') vf.moveObstacle ('box/base_link_0', [0.3+0.04, 0, 0.04, 1, 0, 0, 0]) vf.moveObstacle ('box/base_link_1', [0.3-0.04, 0, 0.04, 1, 0, 0, 0]) vf.moveObstacle ('box/base_link_2', [0.3, 0.04, 0.04, 1, 0, 0, 0]) vf.moveObstacle ('box/base_link_3', [0.3, -0.04, 0.04, 1, 0, 0, 0]) vf.loadObjectModel (Pokeball, 'pokeball') robot.setJointBounds ('pokeball/base_joint_xyz', [-.4,.4,-.4,.4,-.1,1.]) r = vf.createViewer () q1 = [0, -1.57, 1.57, 0, 0, 0, .3, 0, 0.025, 1, 0, 0, 0] r (q1) ## Create graph graph = ConstraintGraph (robot, 'graph') gripperAboveBall = (0, 0, 0.15, 0, -sqrt (2)/2, -sqrt (2)/2, 0) ballAboveGround = [0, 0, 0.1, 1, 0, 0, 0] ## Create your own graph here pp = PathPlayer (ps.client.basic, r)
# importing usefull packages from math import sqrt from hpp import Transform from hpp.corbaserver.manipulation import ConstraintGraph from manipulation import robot, vf, ps, Ground, Box, Pokeball, PathPlayer, gripperName, ballName # Loading & installing our working environment vf.loadEnvironmentModel (Ground, 'ground') vf.loadEnvironmentModel (Box, 'box') vf.moveObstacle ('box/base_link_0', [0.3+0.04, 0, 0.04, 0, 0, 0, 1]) vf.moveObstacle ('box/base_link_1', [0.3-0.04, 0, 0.04, 0, 0, 0, 1]) vf.moveObstacle ('box/base_link_2', [0.3, 0.04, 0.04, 0, 0, 0, 1]) vf.moveObstacle ('box/base_link_3', [0.3, -0.04, 0.04, 0, 0, 0, 1]) # Loading our pokeball which plays our ball's role vf.loadObjectModel (Pokeball, 'pokeball') robot.setJointBounds ('pokeball/root_joint', [-.4,.4,-.4,.4,-.1,1., -1.0001, 1.0001,-1.0001, 1.0001, -1.0001, 1.0001,-1.0001, 1.0001,]) r = vf.createViewer () q1 = [0, -1.57, 1.57, 0, 0, 0, .3, 0, 0.025, 0, 0, 0, 1] r (q1) ## Create graph graph = ConstraintGraph (robot, 'graph') # Contraint of ball relative position while it is located in the gripper ballInGripper = [0, .137, 0, 0.5, 0.5, -0.5, 0.5] ps.createTransformationConstraint ('grasp', gripperName, ballName,
from math import sqrt from hpp import Transform from hpp.corbaserver.manipulation import ConstraintGraph, Constraints from hpp.corbaserver import Client Client ().problem.resetProblem () from manipulation import robot, vf, ps, Ground, Box, Pokeball, PathPlayer, gripperName, ballName vf.loadEnvironmentModel (Ground, 'ground') vf.loadObjectModel (Pokeball, 'pokeball') robot.setJointBounds ('pokeball/root_joint', [-.4,.4,-.4,.4,-.1,2., -1.0001, 1.0001,-1.0001, 1.0001, -1.0001, 1.0001,-1.0001, 1.0001,]) q1 = [0, -1.57, 1.57, 0, 0, 0, .3, 0, 0.025, 0, 0, 0, 1] ## Create constraint graph graph = ConstraintGraph (robot, 'graph') ## Create constraint of relative position of the ball in the gripper when ball ## is grasped ballInGripper = [0, .137, 0, 0.5, 0.5, -0.5, 0.5] ps.createTransformationConstraint ('grasp', gripperName, ballName, ballInGripper, 6*[True,]) ## Create nodes and edges # Warning the order of the nodes is 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')
from hpp import Transform from manipulation import robot, vf, ConstraintGraph, ps, Ground, Box, Pokeball, PathPlayer, gripperName, ballName vf.loadEnvironmentModel (Ground, 'ground') vf.loadObjectModel (Pokeball, 'pokeball') robot.setJointBounds ('pokeball/base_joint_xyz', [-.4,.4,-.4,.4,-.1,1.]) r = vf.createViewer () q1 = [0, -1.57, 1.57, 0, 0, 0, .3, 0, 0.025, 1, 0, 0, 0] r (q1) ## Create constraint graph 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') ## Create nodes and edges 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') ## Create transformation constraint : ball is in horizontal plane with free ## rotation around z ps.createTransformationConstraint ('placement', 'pokeball/base_joint_SO3', '', [0,0,0.025,1,0,0,0], [False, False, True, True, True, False,]) # Create complement constraint ps.createTransformationConstraint ('placement/complement', 'pokeball/base_joint_SO3', '', [0,0,0.025,1,0,0,0], [True, True, False, False, False, True,])
from hpp.corbaserver.manipulation import ConstraintGraph, Constraints from hpp.corbaserver import Client from manipulation import ( robot, vf, ps, Ground, Pokeball, gripperName, ballName, ) from manipulation import PathPlayer # noqa: F401 Client().problem.resetProblem() vf.loadEnvironmentModel(Ground, "ground") vf.loadObjectModel(Pokeball, "pokeball") robot.setJointBounds( "pokeball/root_joint", [ -0.4, 0.4, -0.4, 0.4, -0.1, 2.0, -1.0001, 1.0001, -1.0001, 1.0001, -1.0001,
from hpp.corbaserver.manipulation import ConstraintGraph from hpp.corbaserver import Client from manipulation import ( robot, vf, ps, Ground, Box, Pokeball, ) from manipulation import PathPlayer # noqa: F401 Client().problem.resetProblem() vf.loadEnvironmentModel(Ground, "ground") vf.loadEnvironmentModel(Box, "box") vf.moveObstacle("box/base_link_0", [0.3 + 0.04, 0, 0.04, 0, 0, 0, 1]) vf.moveObstacle("box/base_link_1", [0.3 - 0.04, 0, 0.04, 0, 0, 0, 1]) vf.moveObstacle("box/base_link_2", [0.3, 0.04, 0.04, 0, 0, 0, 1]) vf.moveObstacle("box/base_link_3", [0.3, -0.04, 0.04, 0, 0, 0, 1]) vf.loadObjectModel(Pokeball, "pokeball") robot.setJointBounds( "pokeball/root_joint", [ -0.4, 0.4, -0.4, 0.4, -0.1, 1.0,