def makeRobotProblemAndViewerFactory(clients, rolling_table=True): objects = list() robot = HumanoidRobot("talos", "talos", rootJointType="freeflyer", client=clients) robot.leftAnkle = "talos/leg_left_6_joint" robot.rightAnkle = "talos/leg_right_6_joint" robot.setJointBounds("talos/root_joint", [-2, 2, -2, 2, 0, 2]) shrinkJointRange (robot, 0.95) ps = ProblemSolver(robot) ps.setErrorThreshold(1e-3) ps.setMaxIterProjection(40) ps.addPathOptimizer("EnforceTransitionSemantic") ps.addPathOptimizer("SimpleTimeParameterization") vf = ViewerFactory(ps) objects.append(Box(name="box", vf=vf)) robot.setJointBounds("box/root_joint", [-2, 2, -2, 2, 0, 2]) # Loaded as an object to get the visual tags at the right position. # vf.loadEnvironmentModel (Table, 'table') if rolling_table: table = RollingTable(name="table", vf=vf) else: table = Table(name="table", vf=vf) robot.setJointBounds("table/root_joint", [-2, 2, -2, 2, -2, 2]) return robot, ps, vf, table, objects
def initRobot(self): # The first robot is loaded differently by hpp robot_id = len(self.robots) + int(self.robot is not None) robot_name = "talos_" + str(robot_id) if self.robot is None: Robot.packageName = "agimus_demos" Robot.urdfName = "talos" Robot.urdfSuffix = "_calibration_camera" Robot.srdfSuffix = "" self.robot = Robot( "talos", robot_name, rootJointType="freeflyer", client=self.client ) self.ps = ProblemSolver(self.robot) self.ps.setErrorThreshold(0.00000001) self.ps.setMaxIterProjection(100) self.vf = ViewerFactory(self.ps) else: self.robots.append(OtherRobot(name=robot_name, vf=self.vf)) self.robot.setJointBounds(robot_name + "/root_joint", [-1, 1, -1, 1, 0.5, 1.5]) rank, size = self.getRankAndConfigSize(robot_id) self.q[rank : rank + size] = self.robot.getCurrentConfig()[rank : rank + size] return robot_id
'tiago/hand_ring_flex_2_joint', 'tiago/hand_ring_virtual_3_joint', 'tiago/hand_ring_flex_3_joint', 'tiago/hand_thumb_abd_joint', 'tiago/hand_thumb_virtual_1_joint', 'tiago/hand_thumb_flex_1_joint', 'tiago/hand_thumb_virtual_2_joint', 'tiago/hand_thumb_flex_2_joint', ] crobot.removeJoints(removedJoints, qneutral) tiago_fov.reduceModel(removedJoints, qneutral, len_prefix=len("tiago/")) del crobot robot.insertRobotSRDFModel("tiago", "package://tiago_data/srdf/tiago.srdf") robot.insertRobotSRDFModel("tiago", "package://tiago_data/srdf/pal_hey5_gripper.srdf") robot.setJointBounds('tiago/root_joint', [-10, 10, -10, 10]) ps = ProblemSolver(robot) ps.loadPlugin("manipulation-spline-gradient-based.so") vf = ViewerFactory(ps) vf.loadRobotModel (Driller, "driller") robot.insertRobotSRDFModel("driller", "package://gerard_bauzil/srdf/qr_drill.srdf") robot.setJointBounds('driller/root_joint', [-10, 10, -10, 10, 0, 2]) vf.loadRobotModel (PartP72, "part") robot.setJointBounds('part/root_joint', [-5, 5, -5, 5, -5, 5]) srdf_disable_collisions_fmt = """ <disable_collisions link1="{}" link2="{}" reason=""/>\n""" # Disable collision between tiago/hand_safety_box_0 and driller srdf_disable_collisions = """<robot>""" srdf_disable_collisions += srdf_disable_collisions_fmt.format("tiago/hand_safety_box", "driller/base_link") srdf_disable_collisions += srdf_disable_collisions_fmt.format("tiago/hand_safety_box", "driller/tag_support_link_top") srdf_disable_collisions += srdf_disable_collisions_fmt.format("tiago/hand_safety_box", "driller/tag_support_link_back")
rootJointType = 'freeflyer' packageName = 'hpp_tutorial' meshPackageName = 'hpp_tutorial' urdfName = 'box' urdfSuffix = "" srdfSuffix = "" class Environment (object): packageName = 'iai_maps' meshPackageName = 'iai_maps' urdfName = 'kitchen_area' urdfSuffix = "" srdfSuffix = "" robot = Robot ('pr2-box', 'pr2') ps = ProblemSolver (robot) ## ViewerFactory is a class that generates Viewer on the go. It means you can ## restart the server and regenerate a new windows. ## To generate a window: ## vf.createRealClient () vf = ViewerFactory (ps) vf.loadObjectModel (Box, 'box') vf.loadEnvironmentModel (Environment, "kitchen_area") robot.setJointBounds ("pr2/base_joint_xy" , [-5,-2,-5.2,-2.7] ) robot.setJointBounds ("box/base_joint_xyz", [-5.1,-2,-5.2,-2.7,0,1.5]) # 2}}} # Load the Python class ConstraintGraph. {{{2 graph = ConstraintGraph (robot, 'graph')
def makeRobotProblemAndViewerFactory(clients, rolling_table=True, rosParam=None): if rosParam is not None: import rospy, os, hpp HumanoidRobot.urdfString = rospy.get_param(rosParam) srdfFilename = hpp.retrieveRosResource(HumanoidRobot.srdfFilename) with open(srdfFilename, 'r') as f: HumanoidRobot.srdfString = f.read() objects = list() robot = HumanoidRobot("talos", "talos", rootJointType="freeflyer", client=clients) robot.leftAnkle = "talos/leg_left_6_joint" robot.rightAnkle = "talos/leg_right_6_joint" robot.setJointBounds("talos/root_joint", [-2, 2, -2, 2, 0, 2]) shrinkJointRange (robot, 0.95) ps = ProblemSolver(robot) ps.setErrorThreshold(1e-3) ps.setMaxIterProjection(40) ps.addPathOptimizer("EnforceTransitionSemantic") ps.addPathOptimizer("SimpleTimeParameterization") ps.selectPathValidation('Graph-Progressive', 0.01) vf = ViewerFactory(ps) objects.append(Box(name="box", vf=vf)) robot.setJointBounds("box/root_joint", [-2, 2, -2, 2, 0, 2]) # Loaded as an object to get the visual tags at the right position. # vf.loadEnvironmentModel (Table, 'table') if rolling_table: table = RollingTable(name="table", vf=vf) else: table = Table(name="table", vf=vf) robot.setJointBounds("table/root_joint", [-2, 2, -2, 2, -2, 2]) return robot, ps, vf, table, objects
rootJointType = 'freeflyer' packageName = 'hpp_tutorial' meshPackageName = 'hpp_tutorial' urdfName = 'box' urdfSuffix = "" srdfSuffix = "" class Environment (object): packageName = 'iai_maps' meshPackageName = 'iai_maps' urdfName = 'kitchen_area' urdfSuffix = "" srdfSuffix = "" robot = Robot ('pr2-box', 'pr2') ps = ProblemSolver (robot) ## ViewerFactory is a class that generates Viewer on the go. It means you can ## restart the server and regenerate a new windows. ## To generate a window: ## vf.createRealClient () vf = ViewerFactory (ps) vf.loadObjectModel (Box, 'box') vf.loadEnvironmentModel (Environment, "kitchen_area") robot.setJointBounds ("pr2/root_joint" , [-5,-2,-5.2,-2.7,-2,2,-2,2] ) robot.setJointBounds ("box/root_joint", [-5.1,-2,-5.2,-2.7,0,1.5,-2,2,-2,2,-2,2,-2,2]) # 2}}} # 2}}}
from hpp.gepetto.manipulation import Viewer class ScrewGun (object): rootJointType = 'freeflyer' packageName = 'airbus_environment' meshPackageName = 'airbus_environment' urdfName = 'screw_gun' urdfSuffix = "_massless" srdfSuffix = "" Robot.urdfSuffix = '_capsule_mesh' Robot.srdfSuffix = '_manipulation' # Load HRP2 and a screwgun {{{3 robot = Robot ('hrp2-screw', 'hrp2') ps = ProblemSolver (robot) ps.selectPathPlanner ("M-RRT") r = Viewer (ps) r.loadObjectModel (ScrewGun, 'screw_gun') r.buildCompositeRobot (['hrp2', 'screw_gun']) for d in ["hrp2", "screw_gun"]: robot.setJointBounds (d+"/base_joint_xyz", [-4,4,-4,4,-4,4]) ps.selectPathOptimizer ('None') ps.setErrorThreshold (1e-3) ps.setMaxIterations (40) # 3}}} # Define configurations {{{3 half_sitting = q = robot.getInitialConfig () q_init = half_sitting [::]
class Box (object): rootJointType = 'freeflyer' packageName = 'hpp_tutorial' meshPackageName = 'hpp_tutorial' urdfName = 'cup' urdfSuffix = "" srdfSuffix = "" handles = [ "box/top", "box/bottom" ] robot = Robot ('dev', 'talos', rootJointType = "freeflyer") robot. leftAnkle = "talos/leg_left_6_joint" robot.rightAnkle = "talos/leg_right_6_joint" robot.setJointBounds ("talos/root_joint", [-1, 1, -1, 1, 0.5, 1.5]) ps = ProblemSolver (robot) ps.setRandomSeed(123) ps.selectPathProjector("Progressive", 0.2) ps.setErrorThreshold (1e-3) ps.setMaxIterProjection (40) ps.addPathOptimizer("SimpleTimeParameterization") vf = ViewerFactory (ps) Object = Box vf.loadObjectModel (Object, 'box') robot.setJointBounds ("box/root_joint", [-1, 1, -1, 1, 0, 2]) qq = [-0.7671778026566639, 0.0073267002287253635, 1.0035168727631776, -0.003341673452654457, -0.021566597515109524, 0.0002183620894239602, 0.9997618053357284, -0.00020053128587844821, -0.0021365695604276275, -0.4415951773681094, 0.9659230706255528, -0.48119003672520416, 0.007109157982067145, -0.00020095991543181877, -0.002126639473414498, -0.4382848597339842, 0.9589221865248464, -0.4774994711722908, 0.007099218648561522, 0.0, 0.025235347910697536, -0.06985947194357875, 0.0, -0.12446173084176845, -1.5808415926365578, 0.014333078135875619, -0.0806417043955706, -0.37124401660668394, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.25955282922987977, -0.1618313202464181, 0.002447883426630002, -0.5149037074691503, -0.00010703274362664899, 0.0008742582163227642, 0.10168585913285667, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.785398163397, 0.32250041955468695, -0.2569883469655496, 0.18577095561452217, 1.164388709412583, 0.0694401264431558, 0.5475575114527793, -0.11842286843715424, 0.8254301089264399] half_sitting = [
0.8241613711518598, -0.5663550426199861, -2.094, 0.7924979452316735, 0.6098745828476339, 0.5, 0.35889873246983567, 0.10845342945887403, 0.5, 0.02916333341652683, 0.025597731231524482, 0.16145134862579935, -2.785939904956431, -4.563075760833335, 0.8958690128585236, -0.19634763254425533, -0.7205092487114027, 0.6650461296003519, 0.005260724207565836 ] # 3}}} robot.client.basic.problem.resetRoadmap() robot.client.basic.problem.selectPathOptimizer('None') robot.client.basic.problem.setErrorThreshold(1e-3) robot.client.basic.problem.setMaxIterations(40) from hpp.corbaserver.manipulation import ProblemSolver p = ProblemSolver(robot) # Create constraints. {{{3 from hpp.corbaserver.manipulation import ConstraintGraph cg = ConstraintGraph(robot, 'graph') robot.client.manipulation.problem.createPlacementConstraint( 'box_placement', 'box', 'box/base_joint_SO3', 'box_surface', 'pancake_table_table_top') jointNames = dict() jointNames['all'] = robot.getJointNames() jointNames['pr2'] = list() jointNames['allButPR2LeftArm'] = list() for n in jointNames['all']: if n.startswith("pr2"):
# vim: foldmethod=marker foldlevel=2 from hpp.corbaserver.manipulation.hrp2 import Robot from hpp.corbaserver.manipulation import ProblemSolver, ConstraintGraph from hpp.gepetto.manipulation import Viewer, ViewerFactory from hpp.gepetto import PathPlayer, PathPlayerGui Robot.urdfSuffix = '_capsule_mesh' Robot.srdfSuffix = '_manipulation' # Load HRP2 {{{3 robot = Robot ('hrp2', 'hrp2') ps = ProblemSolver (robot) vf = ViewerFactory (ps) robot.setJointBounds ("hrp2/base_joint_xyz", [-0.2,0.8,-0.5,0.5, 0,2]) ps.selectPathProjector ('Progressive', 0.2) ps.setErrorThreshold (1e-3) ps.setMaxIterations (40) # 3}}} # Define configurations {{{3 half_sitting = q = robot.getInitialConfig () q_init = half_sitting [::] # Open hands ilh = robot.rankInConfiguration ['hrp2/LARM_JOINT6'] q_init [ilh:ilh+6] = [0.75, -0.75, 0.75, -0.75, 0.75, -0.75] irh = robot.rankInConfiguration ['hrp2/RARM_JOINT6'] q_init [irh:irh+6] = [0.75, -0.75, 0.75, -0.75, 0.75, -0.75] ibjxyz = robot.rankInConfiguration ['hrp2/base_joint_xyz']
meshPackageName = 'hpp_tutorial' urdfName = 'box' urdfSuffix = "" srdfSuffix = "" class Environment(object): packageName = 'iai_maps' meshPackageName = 'iai_maps' urdfName = 'kitchen_area' urdfSuffix = "" srdfSuffix = "" robot = Robot('pr2-box', 'pr2') ps = ProblemSolver(robot) ## ViewerFactory is a class that generates Viewer on the go. It means you can ## restart the server and regenerate a new windows. ## To generate a window: ## vf.createViewer () vf = ViewerFactory(ps) vf.loadObjectModel(Box, 'box') vf.loadEnvironmentModel(Environment, "kitchen_area") robot.setJointBounds("pr2/root_joint", [-5, -2, -5.2, -2.7]) robot.setJointBounds("box/root_joint", [-5.1, -2, -5.2, -2.7, 0, 1.5]) # 2}}} # 1}}}
4.108678412429508e-05, 0.09687147183171207, 0.75, -0.75, 0.75, -0.75, 0.75, -0.75, -0.030157742792904667, -0.06148082407263112, 0.000178380167127975, -0.006607688410387017, 7.424531689237967e-05, 0.00026391550679217136, 0.0003776033936788175, 0.000250194383634261, -0.0005725514027501679, 6.818331370664385e-05, -0.00020049176175900826, 0.0004808871166639079, -0.0011302430393443214, 0.00792613666021656, -0.05431204743596144, -0.020779247681766293, 0.026905347698115048, -0.00494217897984072, -0.0011303560351055939, 0.007923626259361372, -0.05448958126663155, -0.019807309704424767, 0.02612488205235436, -0.004939666182290102, 1.9999999999999933, -0.009968182216395699, 0.3399077826251962, 0.8920410834920809, 0.03081996727219969, -0.37686669705702674, -0.2475567159843218 ] #qgoal= p = ProblemSolver(robot) p.createPreGrasp('left-hand-pregrasp', 'hrp2/leftHand', 'screw_gun/handle2') p.createPreGrasp('left-hand-pregrasp-passive', 'hrp2/leftHand', 'screw_gun/handle2') p.createGrasp('left-hand-grasp', 'hrp2/leftHand', 'screw_gun/handle2') p.createGrasp('left-hand-grasp-passive', 'hrp2/leftHand', 'screw_gun/handle2') p.createLockedDofConstraint('screwgun_lock_x', 'screw_gun/base_joint_x', 0, 0, 0) p.createLockedDofConstraint('screwgun_lock_y', 'screw_gun/base_joint_y', 0, 0, 0) p.createLockedDofConstraint('screwgun_lock_z', 'screw_gun/base_joint_z', 0, 0, 0) p.createLockedDofConstraint('screwgun_lock_rx', 'screw_gun/base_joint_SO3', 0, 1, 0) p.createLockedDofConstraint('screwgun_lock_ry', 'screw_gun/base_joint_SO3', 0, 2, 1)
width = bounds[1] - bounds[0] mean = .5 * (bounds[1] + bounds[0]) m = mean - .5 * ratio * width M = mean + .5 * ratio * width robot.setJointBounds(j, [m, M]) print("context=" + args.context) loadServerPlugin(args.context, "manipulation-corba.so") client = CorbaClient(context=args.context) client.manipulation.problem.selectProblem(args.context) robot = Robot("robot", "tiago", rootJointType="planar", client=client) robot.setJointBounds('tiago/root_joint', [-2, 2, -2, 2]) #robot.insertRobotSRDFModel("tiago", "tiago_data", "schunk", "_gripper") ps = ProblemSolver(robot) vf = ViewerFactory(ps) vf.loadRobotModel(Driller, "driller") robot.insertRobotSRDFModel("driller", "gerard_bauzil", "qr_drill", "") robot.setJointBounds('driller/root_joint', [-2, 2, -2, 2, 0, 2]) ps.selectPathValidation("Graph-Dichotomy", 0) ps.selectPathProjector("Progressive", 0.2) ps.addPathOptimizer("EnforceTransitionSemantic") ps.addPathOptimizer("SimpleTimeParameterization") if isSimulation: ps.setMaxIterProjection(1) ps.setParameter("SimpleTimeParameterization/safety", 0.25) ps.setParameter("SimpleTimeParameterization/order", 2) ps.setParameter("SimpleTimeParameterization/maxAcceleration", 1.0)
class Cup(object): rootJointType = "freeflyer" packageName = 'hpp_tutorial' meshPackageName = 'hpp_tutorial' urdfName = 'cup' urdfSuffix = "" srdfSuffix = "" joint = "cup/base_joint" handle = "cup/handle" Robot.srdfSuffix = "_moveit" # 4}}} robot = Robot('romeo-kitchen', 'romeo') ps = ProblemSolver(robot) vf = ViewerFactory(ps) robot.setJointBounds("romeo/base_joint_xyz", [-6, -2, -5.2, -2.7, 0, 2]) vf.loadObjectModel(Kitchen, "kitchen_area") vf.loadObjectModel(Cup, "cup") robot.setJointBounds('cup/base_joint_xyz', [-6, -4, -5, -3, 0, 1.5]) # 3}}} # Define configurations. {{{3 robot.setCurrentConfig(robot.getInitialConfig()) q_init = robot.getHandConfig("both", "open") rank = robot.rankInConfiguration['romeo/base_joint_xyz'] q_init[rank:rank + 7] = [-3.5, -3.7, 0.877, 1, 0, 0, 0] rank = robot.rankInConfiguration['cup/base_joint_xyz']
from hpp.gepetto import PathPlayer, PathPlayerGui class ScrewGun (object): rootJointType = 'freeflyer' packageName = 'airbus_environment' meshPackageName = 'airbus_environment' urdfName = 'screw_gun' urdfSuffix = "" srdfSuffix = "" Robot.urdfSuffix = '_capsule_mesh' Robot.srdfSuffix = '_manipulation' # Load HRP2 and a screwgun {{{3 robot = Robot ('hrp2-screw', 'hrp2') ps = ProblemSolver (robot) ps.selectPathPlanner ("M-RRT") vf = ViewerFactory (ps) vf.loadObjectModel (ScrewGun, 'screw_gun') for d in ["hrp2", "screw_gun"]: robot.setJointBounds (d+"/base_joint_xyz", [-4,4,-4,4,-4,4]) ps.selectPathProjector ("Progressive", 0.2) ps.setErrorThreshold (1e-3) ps.setMaxIterations (40) # 3}}} # Define configurations {{{3 half_sitting = q = robot.getInitialConfig () q_init = half_sitting [::] # Set initial position of screw-driver
packageName = 'hpp_practicals' meshPackageName = 'hpp_practicals' urdfName = 'ur_benchmark/pokeball' urdfSuffix = "" srdfSuffix = "" class Ground (object): rootJointType = 'anchor' packageName = 'hpp_practicals' urdfName = 'ur_benchmark/ground' meshPackageName = 'hpp_practicals' urdfSuffix = "" srdfSuffix = "" class Box (object): rootJointType = 'anchor' packageName = 'hpp_practicals' urdfName = 'ur_benchmark/box' meshPackageName = 'hpp_practicals' urdfSuffix = "" srdfSuffix = "" robot = Robot ('ur5-pokeball', 'ur5') ps = ProblemSolver (robot) ps.setErrorThreshold (1e-4) ps.setMaxIterProjection (40) vf = ViewerFactory (ps) gripperName = 'ur5/wrist_3_joint' ballName = 'pokeball/root_joint'
0, 0, 1.095, 0, 0, 0, 1, 0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, 0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, 0, 0.006761, 0.25847, 0.173046, -0.0002, -0.525366, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 0, -0.25847, -0.173046, 0.0002, -0.525366, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 0, 0, 0 ] robot = HumanoidRobot("talos", "talos", rootJointType="freeflyer") shrinkJointRange(robot, 0.95) # set freeflyerjoint bounds robot.setJointBounds("talos/root_joint", [ -0.5, 0.5, -0.5, 0.5, 0.5, 1.5, -1.01, 1.01, -1.01, 1.01, -1.01, 1.01, -1.01, 1.01 ]) ps = ProblemSolver(robot) ps.setErrorThreshold(1e-4) ps.setMaxIterProjection(40) vf = ViewerFactory(ps) left_arm_lock = createLeftArmLockedJoints(ps) right_arm_lock = createRightArmLockedJoints(ps) if args.arm == 'left': arm_locked = right_arm_lock elif args.arm == 'right': arm_locked = left_arm_lock else: arm_locked = list() left_gripper_lock, right_gripper_lock = createGripperLockedJoints(ps, initConf)
q_goal [rank:rank+3] = [-2.5, -4.4, 0.76] rank = robot.rankInConfiguration ['box/base_joint_SO3'] q_goal [rank:rank+4] = [c, 0, -c, 0] r (q_goal) del c q_inter = [-0.8017105239677402, -0.5977125025958312, -0.796440524800078, 0.6047168680102916, -0.4879605145323316, 0.8728657034488995, -0.988715265911429, 0.1498069522875767, -0.012487804646172175, -0.9999220243274567, -0.9479271854727237, -0.31848712853388694, 0.06700549180802896, -0.9977526066453368, 0.15793164217459785, 0.9874500475467277, 0.9804271799015071, -0.19688205837601827, 0.06981400674906149, 0.9975600254930236, 0.8026666074307995, -0.5964279649006498, -0.8558688410761539, -0.5171929300318802, 0.07633365848037467, 2.5514381844999448, 1.1951774265118278, -0.5864281075389233, 0.24807300661555917, 1.0730239901832896, -0.9931474461781542, 0.5380253563010143, -0.8429286541440898, 0.0, -0.9291311234626017, 0.36975039609596555, 0.5, 0.07192830903452277, 0.0516497980242827, 0.5, 0.2978673015357309, 0.011873305063635719, 0.2207828272342602, 0.9968680838221816, -1.1330407965502385, 0.1474961939381539, -0.9059397450606351, -0.9591666722669869, 0.8241613711518598, -0.5663550426199861, -2.094, 0.7924979452316735, 0.6098745828476339, 0.5, 0.35889873246983567, 0.10845342945887403, 0.5, 0.02916333341652683, 0.025597731231524482, 0.16145134862579935, -2.785939904956431, -4.563075760833335, 0.8958690128585236, -0.19634763254425533, -0.7205092487114027, 0.6650461296003519, 0.005260724207565836] # 3}}} robot.client.basic.problem.resetRoadmap () robot.client.basic.problem.selectPathOptimizer ('None') robot.client.basic.problem.setErrorThreshold (1e-3) robot.client.basic.problem.setMaxIterations (40) from hpp.corbaserver.manipulation import ProblemSolver p = ProblemSolver (robot) # Create constraints. {{{3 from hpp.corbaserver.manipulation import ConstraintGraph cg = ConstraintGraph (robot, 'graph') robot.client.manipulation.problem.createPlacementConstraint ( 'box_placement', 'box', 'box/base_joint_SO3', 'box_surface', 'pancake_table_table_top') jointNames = dict () jointNames['all'] = robot.getJointNames () jointNames['pr2'] = list () jointNames['allButPR2LeftArm'] = list () for n in jointNames['all']: if n.startswith ("pr2"): jointNames['pr2'].append (n)
from hpp.corbaserver.manipulation import ProblemSolver, ConstraintGraph from hpp.gepetto import PathPlayer from hpp.gepetto.manipulation import ViewerFactory class Box (object): rootJointType = 'freeflyer' packageName = 'hpp_tutorial' meshPackageName = 'hpp_tutorial' urdfName = 'box' urdfSuffix = "" srdfSuffix = "" robot = Robot ('tom-box', 'tom') robot.setJointBounds ('tom/base_joint_xy', [-1,1,-1,1]) ps = ProblemSolver (robot) fk = ViewerFactory (ps) fk.loadObjectModel (Box, 'box') robot.setJointBounds ('box/base_joint_xyz', [-1,1,-1,1,-1,1]) openLeftHand = [] for j, v in robot.openHand (None, .75, "left").iteritems () : lockedJointName = "open/"+j openLeftHand.append (lockedJointName) ps.createLockedJoint (lockedJointName, j, [v,]) openRightHand = [] for j, v in robot.openHand (None, .75, "right").iteritems () : lockedJointName = "open/"+j openRightHand.append (lockedJointName)
rootJointType = 'freeflyer' packageName = 'hpp_tutorial' meshPackageName = 'hpp_tutorial' urdfName = 'box' urdfSuffix = "" srdfSuffix = "" class Environment (object): packageName = 'iai_maps' meshPackageName = 'iai_maps' urdfName = 'kitchen_area' urdfSuffix = "" srdfSuffix = "" robot = Robot ('pr2-box', 'pr2') ps = ProblemSolver (robot) ## ViewerFactory is a class that generates Viewer on the go. It means you can ## restart the server and regenerate a new windows. ## To generate a window: ## vf.createViewer () vf = ViewerFactory (ps) vf.loadObjectModel (Box, 'box') vf.loadEnvironmentModel (Environment, "kitchen_area") robot.setJointBounds ("pr2/root_joint", [-5,-2,-5.2,-2.7] ) robot.setJointBounds ("box/root_joint", [-5.1,-2,-5.2,-2.7,0,1.5]) # 2}}} # 1}}}
# vim: foldmethod=marker foldlevel=2 from hpp.corbaserver.manipulation.hrp2 import Robot from hpp.corbaserver.manipulation import ProblemSolver, ConstraintGraph from hpp.gepetto.manipulation import Viewer, ViewerFactory from hpp.gepetto import PathPlayer, PathPlayerGui Robot.urdfSuffix = '_capsule_mesh' Robot.srdfSuffix = '_manipulation' # Load HRP2 {{{3 robot = Robot('hrp2', 'hrp2') ps = ProblemSolver(robot) vf = ViewerFactory(ps) robot.setJointBounds("hrp2/base_joint_xyz", [-0.2, 0.8, -0.5, 0.5, 0, 2]) ps.selectPathProjector('Progressive', 0.2) ps.setErrorThreshold(1e-3) ps.setMaxIterations(40) # 3}}} # Define configurations {{{3 half_sitting = q = robot.getInitialConfig() q_init = half_sitting[::] # Open hands ilh = robot.rankInConfiguration['hrp2/LARM_JOINT6'] q_init[ilh:ilh + 6] = [0.75, -0.75, 0.75, -0.75, 0.75, -0.75] irh = robot.rankInConfiguration['hrp2/RARM_JOINT6'] q_init[irh:irh + 6] = [0.75, -0.75, 0.75, -0.75, 0.75, -0.75] ibjxyz = robot.rankInConfiguration['hrp2/base_joint_xyz']
srdfSuffix = "" class Box (object): rootJointType = "freeflyer" packageName = 'hpp-baxter' meshPackageName = 'hpp-baxter' urdfName = 'box' urdfSuffix = "" srdfSuffix = "" joint = "base_joint" handle = "handle" # 4}}} Robot.urdfSuffix = "" robot = Robot ('baxter-manip', 'baxter') ps = ProblemSolver (robot) vf = ViewerFactory (ps) #robot.setRootJointPosition ("baxter" , [-3.2,-3.9, 0.926, 1, 0, 0, 0]) robot.setRootJointPosition ("baxter" , [-0.8,0.8, 0.926, 1, 0, 0, 0]) vf.loadEnvironmentModel (Table, "table") boxes = list() for i in xrange(K): boxes.append ("box" + str(i)) vf.loadObjectModel (Box, boxes[i]) robot.setJointBounds (boxes[i]+ '/base_joint_xyz', [-1,0.5,-1,2,0.6,1.9]) def setBoxColors (gui): c = Color() for i in xrange(K): gui.setColor (boxes[i], c[i])
srdfSuffix = "" class Box (object): rootJointType = "freeflyer" packageName = 'hpp-baxter' meshPackageName = 'hpp-baxter' urdfName = 'box' urdfSuffix = "" srdfSuffix = "" joint = "base_joint" handle = "handle" # 4}}} Robot.urdfSuffix = "" robot = Robot ('baxter-manip', 'baxter') ps = ProblemSolver (robot) vf = ViewerFactory (ps) #robot.setRootJointPosition ("baxter" , [-3.2,-3.9, 0.926, 1, 0, 0, 0]) robot.setRootJointPosition ("baxter" , [-0.8,0.8, 0.926, 1, 0, 0, 0]) vf.loadEnvironmentModel (Table, "table") vf.loadObjectModel (Box, "box1") vf.loadObjectModel (Box, "box2") robot.setJointBounds ('box1/base_joint_xyz', [-1,0.5,-1,2,0.6,1.9]) robot.setJointBounds ('box2/base_joint_xyz', [-1,0.5,-1,2,0.6,1.9]) # 3}}} # Define configurations. {{{3 q_init = robot.getCurrentConfig ()
class Cup (object): rootJointType = "freeflyer" packageName = 'hpp_tutorial' meshPackageName = 'hpp_tutorial' urdfName = 'cup' urdfSuffix = "" srdfSuffix = "" joint = "cup/base_joint" handle = "cup/handle" Robot.srdfSuffix = "_moveit" # 4}}} robot = Robot ('romeo-kitchen', 'romeo') ps = ProblemSolver (robot) vf = ViewerFactory (ps) robot.setJointBounds ("romeo/base_joint_xyz" , [-6,-2,-5.2,-2.7, 0, 2]) vf.loadObjectModel (Kitchen, "kitchen_area") vf.loadObjectModel (Cup, "cup") robot.setJointBounds ('cup/base_joint_xyz', [-6,-4,-5,-3,0,1.5]) # 3}}} # Define configurations. {{{3 robot.setCurrentConfig (robot.getInitialConfig ()) q_init = robot.getHandConfig ("both", "open") rank = robot.rankInConfiguration ['romeo/base_joint_xyz'] q_init [rank:rank+7] = [-3.5,-3.7, 0.877, 1, 0, 0, 0] rank = robot.rankInConfiguration ['cup/base_joint_xyz']
urdfName = 'placard' urdfSuffix = '' srdfSuffix = '' def __init__(self, name, vf): self.name = name self.vf = vf self.joints = [name + '/root_joint'] self.handles = dict() self.handles['low'] = name + '/low' self.handles['high'] = name + '/high' vf.loadObjectModel(self.__class__, name) self.rank = vf.robot.rankInConfiguration[name + '/root_joint'] ps = ProblemSolver(robot) vf = ViewerFactory(ps) ps.setErrorThreshold(1e-2) ps.setMaxIterProjection(40) robot.setJointBounds('romeo/root_joint', [ -1, 1, -1, 1, 0, 2, -2., 2, -2.,
rootJointType = "freeflyer" packageName = "hpp_tutorial" urdfName = "box" urdfSuffix = "" srdfSuffix = "" class Environment(object): packageName = "hpp_tutorial" urdfName = "kitchen_area" urdfSuffix = "" srdfSuffix = "" robot = Robot("pr2-box", "pr2") ps = ProblemSolver(robot) # ViewerFactory is a class that generates Viewer on the go. It means you can # restart the server and regenerate a new windows. # To generate a window: # vf.createViewer () vf = ViewerFactory(ps) vf.loadObjectModel(Box, "box") vf.loadEnvironmentModel(Environment, "kitchen_area") robot.setJointBounds("pr2/root_joint", [-5, -2, -5.2, -2.7]) robot.setJointBounds("box/root_joint", [-5.1, -2, -5.2, -2.7, 0, 1.5]) # Initialization. # Set parameters.
Robot.packageName = 'ur_description' Robot.meshPackageName = 'ur_description' Robot.urdfName = 'ur5' Robot.urdfSuffix = '_robot' Robot.srdfSuffix = '' class Box (object): rootJointType = 'freeflyer' packageName = 'hpp_tutorial' meshPackageName = 'hpp_tutorial' urdfName = 'box' urdfSuffix = "" srdfSuffix = "" robot = Robot ('ur5-box', 'ur5', rootJointType = 'anchor') ps = ProblemSolver (robot) fk = ViewerFactory (ps) fk.loadObjectModel (Box, 'box') fk.buildCompositeRobot (['ur5', 'box']) robot.setJointBounds ("box/base_joint_xyz", [-2,+2,-2,+2,-2,2]) # 3}}} # Define configurations. {{{3 q_init = robot.getCurrentConfig () rank = robot.rankInConfiguration ['ur5/shoulder_lift_joint'] q_init [rank] = 0 rank = robot.rankInConfiguration ['ur5/elbow_joint'] q_init [rank] = - PI / 2 rank = robot.rankInConfiguration ['box/base_joint_xyz'] q_init [rank:rank+3] = [1,1,1]
robot.client.basic.problem.selectPathPlanner ("M-RRT") robot.loadObjectModel ('box', 'freeflyer', 'fiad_description', 'box', '', '') robot.buildCompositeRobot ('robot', ['ur5', 'box']) for a in ["x","y","z"]: robot.setJointBounds ("box/base_joint_"+a, [-1,1]) robot.client.basic.problem.resetRoadmap () robot.client.basic.problem.selectPathOptimizer ('None') robot.client.basic.problem.setMaxIterations (40) r = MultiRobotPub (robot) half_sitting = robot.getCurrentConfig (); qinit=[0,0,0,0,0,0,0, 0.5,0,1,0,0,0] qgoal=[0,0,0,0,0,0,0,-0.5,0,1,0,0,0] p = ProblemSolver (robot) p.createGrasp ('grasp', 'ur5/ee', 'box/handle') p.createGrasp ('grasp-passive', 'ur5/ee', 'box/handle') if withWaypoint: p.createPreGrasp ('pregrasp', 'ur5/ee', 'box/handle') p.createLockedDofConstraint ('box_lock_x' , 'box/base_joint_x' , 0, 0, 0) p.createLockedDofConstraint ('box_lock_y' , 'box/base_joint_y' , 0, 0, 0) p.createLockedDofConstraint ('box_lock_z' , 'box/base_joint_z' , 0, 0, 0) p.createLockedDofConstraint ('box_lock_rx', 'box/base_joint_SO3', 0, 1, 0) p.createLockedDofConstraint ('box_lock_ry', 'box/base_joint_SO3', 0, 2, 1) p.createLockedDofConstraint ('box_lock_rz', 'box/base_joint_SO3', 0, 3, 2) p.isLockedDofParametric ('box_lock_x' ,True) p.isLockedDofParametric ('box_lock_y' ,True) p.isLockedDofParametric ('box_lock_z' ,True) p.isLockedDofParametric ('box_lock_rx',True) p.isLockedDofParametric ('box_lock_ry',True)
srdfFilename = \ "package://hpp_environments/srdf/construction_set/sphere.srdf" class Ground(object): rootJointType = 'freeflyer' urdfFilename = \ "package://hpp_environments/urdf/construction_set/ground.urdf" srdfFilename = \ "package://hpp_environments/srdf/construction_set/ground.srdf" robot = Robot('2ur5-sphere', 'r0') robot.setJointPosition('r0/root_joint', [-.25, 0, 0, 0, 0, 0, 1]) ps = ProblemSolver(robot) ps.setErrorThreshold(1e-4) ps.setMaxIterProjection(40) vf = ViewerFactory(ps) # # Load robots and objets # - 2 UR3, # - 4 spheres, # - 4 short cylinders, vf.loadRobotModel(Robot, "r1") robot.setJointPosition('r1/root_joint', [.25, 0, 0, 0, 0, 1, 0]) # Change bounds of robots to increase workspace and avoid some collisions
def benchConstraints (constraints, lockDofs): ps.client.basic.problem.resetConstraints() ps.setNumericalConstraints ("test", constraints) ps.setLockedJointConstraints ("test", lockDofs) res = [None] * N q = [None] * N err = [None] * N start = time.time() for i in range(N): res[i], q[i], err[i] = ps.applyConstraints(qs[i]) duration = time.time() - start # return res,q,err,duration return float(res.count(True)) / N,duration / N ps = ProblemSolver (robot) vf = ViewerFactory (ps) ps.setErrorThreshold (1e-2) ps.setMaxIterProjection (40) robot.setJointBounds ('romeo/root_joint' , [-1,1,-1,1, 0, 2,-2.,2,-2.,2,-2.,2, -2.,2,]) placard = Placard ('placard', vf) robot.setJointBounds (placard.name + '/root_joint', [-1,1,-1,1,0,1.5,-2.,2, -2.,2,-2.,2,-2.,2,]) ## Lock both hands locklhand = list() for j,v in robot.leftHandOpen.iteritems(): locklhand.append ('romeo/' + j)
ps.client.basic.problem.resetConstraints() ps.resetConstraints() ps.addNumericalConstraints("test", constraints) ps.addLockedJointConstraints("test", lockDofs) res = [None] * N q = [None] * N err = [None] * N start = time.time() for i in range(N): res[i], q[i], err[i] = ps.applyConstraints(qs[i]) duration = time.time() - start # return res,q,err,duration return float(res.count(True)) / N, duration / N ps = ProblemSolver(robot) vf = ViewerFactory(ps) ps.setErrorThreshold(1e-2) ps.setMaxIterProjection(40) robot.setJointBounds('romeo/root_joint', [ -1, 1, -1, 1, 0, 2, -2., 2, -2.,
packageName = 'hpp_tutorial' meshPackageName = 'hpp_tutorial' urdfName = 'box' urdfSuffix = "" srdfSuffix = "" class Environment (object): packageName = 'iai_maps' meshPackageName = 'iai_maps' urdfName = 'kitchen_area' urdfSuffix = "" srdfSuffix = "" # Load robot and object. {{{3 robot = Robot ('pr2-box', 'pr2') # was anchor joint ps = ProblemSolver (robot) vf = ViewerFactory (ps) vf.loadObjectModel (Box, 'box') vf.loadEnvironmentModel (Environment, "kitchen_area") robot.setJointBounds ("pr2/root_joint" , [-5,-2,-5.2,-2.7] ) robot.setJointBounds ("box/root_joint", [-5.1,-2,-5.2,-2.7,0,1.5]) # 3}}} # Define configurations. {{{3 q_init = robot.getCurrentConfig () q_init[0:4] = [-3.2,-4,1,0] # FIX ME ! (see up ) rank = robot.rankInConfiguration ['pr2/r_gripper_l_finger_joint'] q_init [rank] = 0.5 rank = robot.rankInConfiguration ['pr2/r_gripper_r_finger_joint'] q_init [rank] = 0.5
srdfSuffix = "" class Box (object): rootJointType = "freeflyer" packageName = 'hpp-baxter' meshPackageName = 'hpp-baxter' urdfName = 'box' urdfSuffix = "" srdfSuffix = "" joint = "base_joint" handle = "handle" # 4}}} Robot.urdfSuffix = "" robot = Robot ('baxter-manip', 'baxter') ps = ProblemSolver (robot) vf = ViewerFactory (ps) #robot.setRootJointPosition ("baxter" , [-3.2,-3.9, 0.926, 1, 0, 0, 0]) robot.setRootJointPosition ("baxter" , [-0.8,0.8, 0.926, 0, 0, 0, 1]) vf.loadEnvironmentModel (Table, "table") boxes = list() for i in xrange(K): boxes.append ("box" + str(i)) vf.loadObjectModel (Box, boxes[i]) robot.setJointBounds (boxes[i]+ '/root_joint', [-1,0.5,-1,2,0.6,1.9,-1,1,-1,1,-1,1,-1,1]) def setBoxColors (gui): c = Color() for i in xrange(K): gui.setColor (boxes[i], c[i])
class Box(object): rootJointType = "freeflyer" packageName = 'hpp-baxter' meshPackageName = 'hpp-baxter' urdfName = 'box' urdfSuffix = "" srdfSuffix = "" joint = "base_joint" handle = "handle" # 4}}} Robot.urdfSuffix = "" robot = Robot('baxter-manip', 'baxter') ps = ProblemSolver(robot) vf = ViewerFactory(ps) #robot.setRootJointPosition ("baxter" , [-3.2,-3.9, 0.926, 1, 0, 0, 0]) robot.setRootJointPosition("baxter", [-0.8, 0.8, 0.926, 1, 0, 0, 0]) vf.loadEnvironmentModel(Table, "table") vf.loadObjectModel(Box, "box1") vf.loadObjectModel(Box, "box2") robot.setJointBounds('box1/base_joint_xyz', [-1, 0.5, -1, 2, 0.6, 1.9]) robot.setJointBounds('box2/base_joint_xyz', [-1, 0.5, -1, 2, 0.6, 1.9]) # 3}}} # Define configurations. {{{3 q_init = robot.getCurrentConfig()
class Calibration(object): def __init__(self, client): self.client = client self.robot = None self.robots = list() self.robot_locks = list() self.robots_locks = list() self.robots_gaze = list() self.robots_identity_constraints = list() self.q = [] def initRobot(self): # The first robot is loaded differently by hpp robot_id = len(self.robots) + int(self.robot is not None) robot_name = "talos_" + str(robot_id) if self.robot is None: Robot.packageName = "agimus_demos" Robot.urdfName = "talos" Robot.urdfSuffix = "_calibration_camera" Robot.srdfSuffix = "" self.robot = Robot( "talos", robot_name, rootJointType="freeflyer", client=self.client ) self.ps = ProblemSolver(self.robot) self.ps.setErrorThreshold(0.00000001) self.ps.setMaxIterProjection(100) self.vf = ViewerFactory(self.ps) else: self.robots.append(OtherRobot(name=robot_name, vf=self.vf)) self.robot.setJointBounds(robot_name + "/root_joint", [-1, 1, -1, 1, 0.5, 1.5]) rank, size = self.getRankAndConfigSize(robot_id) self.q[rank : rank + size] = self.robot.getCurrentConfig()[rank : rank + size] return robot_id def setLockJoints(self, robot_id, joints_name_value_tuple): self.ps.createLockedJoint( "talos_" + str(robot_id) + "/root_joint", "talos_" + str(robot_id) + "/root_joint", [0, 0, 1, 0, 0, 0, 1], ) root_joint_rank = self.robot.rankInConfiguration[ "talos_" + str(robot_id) + "/root_joint" ] self.q[root_joint_rank : root_joint_rank + 7] = [0, 0, 1, 0, 0, 0, 1] self.robots_locks.append("talos_" + str(robot_id) + "/root_joint") # self.ps.createLockedJoint("talos_" + str(robot_id) + "/calib_mire_joint_2", "talos_" + str(robot_id) + "/calib_mire_joint_2", [0, 0, 0, 0, 0, 0, 1]) # mire_joint_rank = self.robot.rankInConfiguration["talos_" + str(robot_id) + "/calib_mire_joint_2"] # self.q[mire_joint_rank:mire_joint_rank + 7] = [0, 0, 0, 0, 0, 0, 1] # self.robots_locks.append("talos_" + str(robot_id) + "/calib_mire_joint_2") for name, value in joints_name_value_tuple: joint_name = "talos_" + str(robot_id) + "/" + name self.q[self.robot.rankInConfiguration[joint_name]] = value self.ps.createLockedJoint(joint_name, joint_name, [value]) self.robots_locks.append(joint_name) def getRankAndConfigSize(self, robot_id): robot_name = "talos_" + str(robot_id) rank = self.robot.rankInConfiguration[robot_name + "/root_joint"] size = sum( [ self.robot.getJointConfigSize(joint_name) for joint_name in calib.robot.getJointNames() if robot_name in joint_name ] ) return rank, size def setGaze(self, robot_id, checkerboard_pose): robot_name = "talos_" + str(robot_id) position = ( checkerboard_pose.position.x, checkerboard_pose.position.y, checkerboard_pose.position.z, ) orientation = ( checkerboard_pose.orientation.x, checkerboard_pose.orientation.y, checkerboard_pose.orientation.z, checkerboard_pose.orientation.w, ) self.ps.createPositionConstraint( robot_name + "gaze", robot_name + "/calib_rgb_joint", robot_name + "/calib_mire_joint_2", position, (0, 0, 0), [True, True, True], ) self.ps.createOrientationConstraint( robot_name + "gaze_O", robot_name + "/calib_mire_joint_2", robot_name + "/calib_rgb_joint", orientation, [True, True, True], ) self.robots_gaze.append(robot_name + "gaze") self.robots_gaze.append(robot_name + "gaze_O") def constrainFreeflyers(self): for robot_id in range(1, len(self.robots)): robot_name = "talos_" + str(robot_id) self.client.basic.problem.createIdentityConstraint( robot_name + "_id_rgb", ["talos_0/calib_rgb_joint"], [robot_name + "/calib_rgb_joint"], ) self.client.basic.problem.createIdentityConstraint( robot_name + "_id_mire", ["talos_0/calib_mire_joint_2"], [robot_name + "/calib_mire_joint_2"], ) self.robots_identity_constraints.append(robot_name + "_id_rgb") self.robots_identity_constraints.append(robot_name + "_id_mire") def optimize(self, q_init, robots_id): self.q_proj = self.q client.basic.problem.resetConstraints() calib.robot.setCurrentConfig(q_init) client.basic.problem.addLockedJointConstraints("unused1", calib.robots_locks) gaze = [ c for c in calib.robots_gaze if any(["talos_" + str(robot_id) in c for robot_id in robots_id]) ] num_constraints = gaze + calib.robots_identity_constraints client.basic.problem.addNumericalConstraints( "unused2", num_constraints, [0 for _ in num_constraints] ) # client.basic.problem.addNumericalConstraints("mighty_cam_cost", ["cam_cost"], [ 1 ]) # calib.client.basic.problem.setNumericalConstraintsLastPriorityOptional(True) projOk, self.q_proj, error = client.basic.problem.applyConstraints(q_init) if error < 1.0: optOk, self.q_proj, error = client.basic.problem.optimize(self.q_proj) if optOk: print("All was good! " + str(max(error))) else: print("Optimisation error: " + str(max(error))) else: print("Projection error: " + str(error)) rank_rgb = calib.robot.rankInConfiguration["talos_0/calib_rgb_joint"] rank_mire = calib.robot.rankInConfiguration["talos_0/calib_mire_joint_2"] return ( self.q_proj, self.q_proj[rank_rgb : rank_rgb + 7], max(error), self.q_proj[rank_mire : rank_mire + 7], )
packageName = 'hpp_environments' urdfName = 'construction_set/sphere' urdfSuffix = "" srdfSuffix = "" class Ground (object): rootJointType = 'anchor' packageName = 'hpp_environments' urdfName = 'construction_set/ground' urdfSuffix = "" srdfSuffix = "" robot = Robot ('2ur5-sphere', 'r0') robot.setJointPosition ('r0/root_joint', [-.25, 0, 0, 0, 0, 0, 1]) ps = ProblemSolver (robot) ps.setErrorThreshold (1e-4) ps.setMaxIterProjection (40) vf = ViewerFactory (ps) # # Load robots and objets # - 2 UR3, # - 4 spheres, # - 4 short cylinders, vf.loadRobotModel (Robot, "r1") robot.setJointPosition ('r1/root_joint', [.25, 0, 0, 0, 0, 1, 0]) # Change bounds of robots to increase workspace and avoid some collisions
meshPackageName = 'hpp_tutorial' urdfName = 'box' urdfSuffix = "" srdfSuffix = "" class Environment(object): packageName = 'iai_maps' meshPackageName = 'iai_maps' urdfName = 'kitchen_area' urdfSuffix = "" srdfSuffix = "" robot = Robot('pr2-box', 'pr2') ps = ProblemSolver(robot) ## ViewerFactory is a class that generates Viewer on the go. It means you can ## restart the server and regenerate a new windows. ## To generate a window: ## vf.createRealClient () vf = ViewerFactory(ps) vf.loadObjectModel(Box, 'box') vf.loadEnvironmentModel(Environment, "kitchen_area") robot.setJointBounds("pr2/root_joint", [-5, -2, -5.2, -2.7, -2, 2, -2, 2]) robot.setJointBounds( "box/root_joint", [-5.1, -2, -5.2, -2.7, 0, 1.5, -2, 2, -2, 2, -2, 2, -2, 2]) # 2}}}
from hpp.corbaserver.manipulation import ProblemSolver, ConstraintGraph, ConstraintGraphFactory, Rule, Constraints from hpp.gepetto.manipulation import ViewerFactory from hpp import Transform from desk_robot import Robot as Desk from talos_robot import Robot as Talos import math import numpy as np import CORBA import config_data as coda # Load robot (talos + desk) robot = Talos("talos+desk", "talos") ps = ProblemSolver(robot) vf = ViewerFactory(ps) vf.loadObjectModel (Desk, "desk") v = vf.createViewer() robot.setJointBounds("talos/root_joint", [-1.5, 1.5, -2, 1, 0.5, 1.5]) q = robot.getCurrentConfig() pi = 3.141592654 q[0] = 0.3 q[1] = -1.4 q[2] = 0.921 q[5] = math.sin(0.5 * (pi/2)) q[6] = math.cos(0.5 * (pi/2)) # hips offset
packageName = 'hpp_tutorial' meshPackageName = 'hpp_tutorial' urdfName = 'box' urdfSuffix = "" srdfSuffix = "" class Environment (object): packageName = 'iai_maps' meshPackageName = 'iai_maps' urdfName = 'kitchen_area' urdfSuffix = "" srdfSuffix = "" # Load robot and object. {{{3 robot = Robot ('pr2-box', 'pr2', rootJointType = "anchor") ps = ProblemSolver (robot) vf = ViewerFactory (ps) robot.setJointPosition ('pr2/base_joint', (-3.2, -4, 0, 1, 0, 0, 0)) vf.loadObjectModel (Box, 'box') vf.loadEnvironmentModel (Environment, "kitchen_area") robot.setJointBounds ("box/base_joint_xyz", [-3,-2,-5,-3,0.7,1]) # 3}}} # Define configurations. {{{3 q_init = robot.getCurrentConfig () rank = robot.rankInConfiguration ['pr2/r_gripper_l_finger_joint'] q_init [rank] = 0.5 rank = robot.rankInConfiguration ['pr2/r_gripper_r_finger_joint'] q_init [rank] = 0.5 rank = robot.rankInConfiguration ['pr2/l_gripper_l_finger_joint']
Robot.urdfName = 'ur5' Robot.urdfSuffix = '_robot' Robot.srdfSuffix = '' class Box(object): rootJointType = 'freeflyer' packageName = 'hpp_tutorial' meshPackageName = 'hpp_tutorial' urdfName = 'box' urdfSuffix = "" srdfSuffix = "" robot = Robot('ur5-box', 'ur5', rootJointType='anchor') ps = ProblemSolver(robot) fk = ViewerFactory(ps) fk.loadObjectModel(Box, 'box') fk.buildCompositeRobot(['ur5', 'box']) robot.setJointBounds("box/base_joint_xyz", [-2, +2, -2, +2, -2, 2]) # 3}}} # Define configurations. {{{3 q_init = robot.getCurrentConfig() rank = robot.rankInConfiguration['ur5/shoulder_lift_joint'] q_init[rank] = 0 rank = robot.rankInConfiguration['ur5/elbow_joint'] q_init[rank] = -PI / 2 rank = robot.rankInConfiguration['box/base_joint_xyz'] q_init[rank:rank + 3] = [1, 1, 1]
class Box(object): rootJointType = "freeflyer" packageName = 'hpp-baxter' meshPackageName = 'hpp-baxter' urdfName = 'box' urdfSuffix = "" srdfSuffix = "" joint = "base_joint" handle = "handle" # 4}}} Robot.urdfSuffix = "" robot = Robot('baxter-manip', 'baxter') ps = ProblemSolver(robot) vf = ViewerFactory(ps) #robot.setRootJointPosition ("baxter" , [-3.2,-3.9, 0.926, 1, 0, 0, 0]) robot.setRootJointPosition("baxter", [-0.8, 0.8, 0.926, 1, 0, 0, 0]) vf.loadEnvironmentModel(Table, "table") boxes = list() for i in xrange(K): boxes.append("box" + str(i)) vf.loadObjectModel(Box, boxes[i]) robot.setJointBounds(boxes[i] + '/base_joint_xyz', [-1, 0.5, -1, 2, 0.6, 1.9]) def setBoxColors(gui): c = Color()
urdfSuffix = "" srdfSuffix = "" class Ground(object): rootJointType = 'anchor' packageName = 'hpp_practicals' urdfName = 'ur_benchmark/ground' meshPackageName = 'hpp_practicals' urdfSuffix = "" srdfSuffix = "" class Box(object): rootJointType = 'anchor' packageName = 'hpp_practicals' urdfName = 'ur_benchmark/box' meshPackageName = 'hpp_practicals' urdfSuffix = "" srdfSuffix = "" robot = Robot('ur5-pokeball', 'ur5') ps = ProblemSolver(robot) ps.setErrorThreshold(1e-4) ps.setMaxIterProjection(40) vf = ViewerFactory(ps) gripperName = 'ur5/wrist_3_joint' ballName = 'pokeball/root_joint'
urdfName = 'box' urdfSuffix = "" srdfSuffix = "" class Environment(object): packageName = 'iai_maps' meshPackageName = 'iai_maps' urdfName = 'kitchen_area' urdfSuffix = "" srdfSuffix = "" # Load robot and object. {{{3 robot = Robot('pr2-box', 'pr2', rootJointType="anchor") ps = ProblemSolver(robot) vf = ViewerFactory(ps) robot.setJointPosition('pr2/base_joint', (-3.2, -4, 0, 1, 0, 0, 0)) vf.loadObjectModel(Box, 'box') vf.loadEnvironmentModel(Environment, "kitchen_area") robot.setJointBounds("box/base_joint_xyz", [-3, -2, -5, -3, 0.7, 1]) # 3}}} # Define configurations. {{{3 q_init = robot.getCurrentConfig() rank = robot.rankInConfiguration['pr2/r_gripper_l_finger_joint'] q_init[rank] = 0.5 rank = robot.rankInConfiguration['pr2/r_gripper_r_finger_joint'] q_init[rank] = 0.5 rank = robot.rankInConfiguration['pr2/l_gripper_l_finger_joint']
meshPackageName = 'hpp_tutorial' urdfName = 'box' urdfSuffix = "" srdfSuffix = "" class Environment(object): packageName = 'iai_maps' meshPackageName = 'iai_maps' urdfName = 'kitchen_area' urdfSuffix = "" srdfSuffix = "" robot = Robot('pr2-box', 'pr2') ps = ProblemSolver(robot) ## ViewerFactory is a class that generates Viewer on the go. It means you can ## restart the server and regenerate a new windows. ## To generate a window: ## fk.createRealClient () fk = ViewerFactory(ps) fk.loadObjectModel(Box, 'box') fk.loadEnvironmentModel(Environment, "kitchen_area") robot.setJointBounds("pr2/base_joint_xy", [-5, -2, -5.2, -2.7]) robot.setJointBounds("box/base_joint_xyz", [-5.1, -2, -5.2, -2.7, 0, 1.5]) # 2}}} # Load the Python class ConstraintGraph. {{{2 graph = ConstraintGraph(robot, 'graph')
srdfSuffix = "" class Box (object): rootJointType = "freeflyer" packageName = 'hpp-baxter' meshPackageName = 'hpp-baxter' urdfName = 'box' urdfSuffix = "" srdfSuffix = "" joint = "base_joint" handle = "handle" # 4}}} Robot.urdfSuffix = "" robot = Robot ('baxter-manip', 'baxter') ps = ProblemSolver (robot) vf = ViewerFactory (ps) #robot.setRootJointPosition ("baxter" , [-3.2,-3.9, 0.926, 1, 0, 0, 0]) robot.setRootJointPosition ("baxter" , [-0.8,0.8, 0.926, 1, 0, 0, 0]) vf.loadEnvironmentModel (Table, "table") vf.loadObjectModel (Box, "box1") vf.loadObjectModel (Box, "box2") robot.setJointBounds ('box1/base_joint_xyz', [-1,0.5,-1,2,0.6,1.9]) robot.setJointBounds ('box2/base_joint_xyz', [-1,0.5,-1,2,0.6,1.9]) # 3}}} # Define configurations. {{{3 q_init = robot.getCurrentConfig () rankB1 = robot.rankInConfiguration ['box1/base_joint_xyz']
rootJointType = "freeflyer" packageName = "agimus_demos" urdfName = "calibration_mire" urdfSuffix = "" srdfSuffix = "" name = "mire" handles = ["mire/left", "mire/right"] robot = Robot("dev", "talos", rootJointType="freeflyer") robot.leftAnkle = "talos/leg_left_6_joint" robot.rightAnkle = "talos/leg_right_6_joint" robot.setJointBounds("talos/root_joint", [-1, 1, -1, 1, 0.5, 1.5]) ps = ProblemSolver(robot) ps.setRandomSeed(123) ps.selectPathProjector("Progressive", 0.2) ps.setErrorThreshold(1e-3) ps.setMaxIterProjection(40) ps.addPathOptimizer("SimpleTimeParameterization") vf = ViewerFactory(ps) vf.loadObjectModel(Mire, "mire") robot.setJointBounds("mire/root_joint", [-1, 1, -1, 1, 0, 2]) half_sitting = [ 0, 0, 1.0192720229567027,
urdfName = 'box' urdfSuffix = "" srdfSuffix = "" class Environment(object): packageName = 'iai_maps' meshPackageName = 'iai_maps' urdfName = 'kitchen_area' urdfSuffix = "" srdfSuffix = "" # Load robot and object. {{{3 robot = Robot('pr2-box', 'pr2') # was anchor joint ps = ProblemSolver(robot) vf = ViewerFactory(ps) vf.loadObjectModel(Box, 'box') vf.loadEnvironmentModel(Environment, "kitchen_area") robot.setJointBounds("pr2/root_joint", [-5, -2, -5.2, -2.7]) robot.setJointBounds("box/root_joint", [-5.1, -2, -5.2, -2.7, 0, 1.5]) # 3}}} # Define configurations. {{{3 q_init = robot.getCurrentConfig() q_init[0:4] = [-3.2, -4, 1, 0] # FIX ME ! (see up ) rank = robot.rankInConfiguration['pr2/r_gripper_l_finger_joint'] q_init[rank] = 0.5 rank = robot.rankInConfiguration['pr2/r_gripper_r_finger_joint'] q_init[rank] = 0.5
def makeRobotProblemAndViewerFactory(corbaclient): robot = Robot("dev", "talos", rootJointType="freeflyer", client=corbaclient) robot.leftAnkle = "talos/leg_left_6_joint" robot.rightAnkle = "talos/leg_right_6_joint" robot.setJointBounds("talos/root_joint", [-1, 1, -1, 1, 0.5, 1.5]) ps = ProblemSolver(robot) ps.setRandomSeed(123) ps.selectPathProjector("Progressive", 0.2) ps.setErrorThreshold(1e-3) ps.setMaxIterProjection(40) ps.addPathOptimizer("SimpleTimeParameterization") vf = ViewerFactory(ps) vf.loadObjectModel(Object, "box") robot.setJointBounds("box/root_joint", [-1, 1, -1, 1, 0, 2]) vf.loadEnvironmentModel(Table, "table") return robot, ps, vf
rootJointType = 'freeflyer' packageName = 'hpp_tutorial' meshPackageName = 'hpp_tutorial' urdfName = 'box' urdfSuffix = "" srdfSuffix = "" class Environment (object): packageName = 'iai_maps' meshPackageName = 'iai_maps' urdfName = 'kitchen_area' urdfSuffix = "" srdfSuffix = "" robot = Robot ('pr2-box', 'pr2') ps = ProblemSolver (robot) fk = FakeViewer (ps) fk.loadObjectModel (Box, 'box') fk.buildCompositeRobot (['pr2', 'box']) fk.loadEnvironmentModel (Environment, "kitchen_area") robot.setJointBounds ("pr2/base_joint_xy" , [-5,-2,-5.2,-2.7] ) robot.setJointBounds ("box/base_joint_xyz", [-5.1,-2,-5.2,-2.7,0,1.5]) # 2}}} # Load the Python class ConstraintGraph. {{{2 graph = ConstraintGraph (robot, 'graph') # 2}}} # 1}}}
urdfSuffix = "" srdfSuffix = "" class Ground(object): rootJointType = 'anchor' packageName = 'hpp_environments' urdfName = 'construction_set/ground' urdfSuffix = "" srdfSuffix = "" nSphere = 2 robot = Robot('ur3-spheres', 'ur3') ps = ProblemSolver(robot) ps.setErrorThreshold(1e-4) ps.setMaxIterProjection(40) vf = ViewerFactory(ps) # Change bounds of robots to increase workspace and avoid some collisions robot.setJointBounds('ur3/shoulder_pan_joint', [-pi, 4]) robot.setJointBounds('ur3/shoulder_lift_joint', [-pi, 0]) robot.setJointBounds('ur3/elbow_joint', [-2.6, 2.6]) vf.loadEnvironmentModel(Ground, 'ground') objects = list() p = ps.client.basic.problem.getProblem() r = p.robot()
robot.client.basic.problem.resetRoadmap () robot.client.basic.problem.selectPathOptimizer ('None') robot.client.basic.problem.setErrorThreshold (1e-3) robot.client.basic.problem.setMaxIterations (40) r = MultiRobotPub (robot) half_sitting = robot.getCurrentConfig (); q1=[0.2561522768052704, 0.009968182216397095, -0.0013343337623661373, 0.9822200021750263, -0.01324065709811156, 0.06488994582367685, 0.1756640181081545, 0.3518980306918394, 0.5587732909530961, -0.00014707196704839167, 0.003348199890377334, 0.2851538622381365, 0.019515985023849783, 0.47568241804625144, 0.034325840512149174, 0.48238375657832383, 0.5252200250587239, 0.75, -0.75, 0.75, -0.75, 0.75, -0.75, -0.08183778724042397, -0.002329068502171773, 0.0013672024871911833, -0.03184796508559784, 0.00013468971902929027, -0.006463660186047965, 0.75, -0.75, 0.75, -0.75, 0.75, -0.75, -0.0014454298679420635, 0.008692815372600783, -0.17944952816665183, -0.0349066, 0.08186718484328222, -0.0052176502711966674, -0.0014458867527617663, 0.008689383322591662, -0.1789250482962942, -0.0349066, 0.0813567181568653, -0.00521418794593098, -1.0, -0.009968182216397, 0.3399077826256168, 0.8920410834920809, 0.03081996727219969, -0.37686669705702674, -0.2475567159843218] q2=[0.2561522768052704, 0.009968182216397094, -0.0013343337623677983, 0.9822200021750275, -0.013240657098102699, 0.06488994582365894, 0.17566401810815518, 0.3518980306918404, 0.5587732909530987, -0.00014707196704828986, 0.003348199890377807, 0.2851538622381267, 0.019515985023862044, 0.4756824180462511, 0.03432584051214365, 0.48238375657832383, 0.5252200250587228, 0.75, -0.75, 0.75, -0.75, 0.75, -0.75, -0.08183778724043055, -0.0023290685021591065, 0.0013672024871913863, -0.03184796508560018, 0.00013468971902932646, -0.006463660186048368, 0.75, -0.75, 0.75, -0.75, 0.75, -0.75, -0.001445429867963691, 0.008692815372566085, -0.17944952816666074, -0.0349066, 0.08186718484332876, -0.00521765027117088, -0.0014458867527830789, 0.008689383322556907, -0.17892504829630115, -0.0349066, 0.08135671815690743, -0.005214187945905259, 1.0, -0.009968182216397, 0.3399077826256168, 0.8920410834920809, 0.03081996727219969, -0.37686669705702674, -0.2475567159843218] q3=[0.2561522768052704, 0.009968182216397017, -0.0013072533849402136, 0.9822384333854748, -0.01312068217341367, 0.06461437559545073, 0.17567154051286343, 0.35191845881842604, 0.5584191613132159, -0.00014417077050741273, 0.003264525687460523, 0.2869014360284614, 0.01968512963006628, 0.47563871278773184, 0.0349066, 0.4823374727974044, 0.5253128675582199, 0.00028141477557467405, -9.224156430856334e-05, 9.57339438119022e-05, 0.00043012512285501805, 9.464640215865539e-05, -0.00027454688131241776, -0.08070554282100409, -0.0022150673918241194, 0.0013507126487957277, -0.031445080182081316, 0.00013303660804169588, -0.0063951082465386055, 0.00018071522532470678, -7.008398345853373e-05, 0.0001432918466161822, 5.0663492959904484e-05, 5.932458587315774e-05, -0.0001501990824631989, -0.0016207277515218107, 0.008331445043308924, -0.1782202190053313, -0.0349066, 0.08120820667180957, -0.004973436857255375, -0.0016211611493169267, 0.008328178972275535, -0.17768239866563243, -0.03490564559358417, 0.08066263806839956, -0.00497014215842449, -0.2561522768052704, -0.009968182216396924, 0.33990778262561817, 0.8920410834920809, 0.03081996727219969, -0.37686669705702674, -0.2475567159843218] qinit=[6.351445422051578e-15, -7.64857075802911e-16, -0.00010650933265040068, 0.9997086753828088, -0.001462864329132029, 0.024091994398753543, 1.4078154478767463e-05, -0.00040620371503693694, 0.06720468781884271, 0.09997150487212819, 0.10619128937732075, -0.030984458364104503, 0.06768151515542697, -9.21491333147205e-05, -0.007395092069129794, 4.108678412429508e-05, 0.09687147183171207, 0.75, -0.75, 0.75, -0.75, 0.75, -0.75, -0.030157742792904667, -0.06148082407263112, 0.000178380167127975, -0.006607688410387017, 7.424531689237967e-05, 0.00026391550679217136, 0.0003776033936788175, 0.000250194383634261, -0.0005725514027501679, 6.818331370664385e-05, -0.00020049176175900826, 0.0004808871166639079, -0.0011302430393443214, 0.00792613666021656, -0.05431204743596144, -0.020779247681766293, 0.026905347698115048, -0.00494217897984072, -0.0011303560351055939, 0.007923626259361372, -0.05448958126663155, -0.019807309704424767, 0.02612488205235436, -0.004939666182290102, 1.9999999999999933, -0.009968182216395699, 0.3399077826251962, 0.8920410834920809, 0.03081996727219969, -0.37686669705702674, -0.2475567159843218] #qgoal= p = ProblemSolver (robot) p.createPreGrasp ('left-hand-pregrasp', 'hrp2/leftHand', 'screw_gun/handle2') p.createPreGrasp ('left-hand-pregrasp-passive', 'hrp2/leftHand', 'screw_gun/handle2') p.createGrasp ('left-hand-grasp', 'hrp2/leftHand', 'screw_gun/handle2') p.createGrasp ('left-hand-grasp-passive', 'hrp2/leftHand', 'screw_gun/handle2') p.createLockedDofConstraint ('screwgun_lock_x' , 'screw_gun/base_joint_x' , 0, 0, 0) p.createLockedDofConstraint ('screwgun_lock_y' , 'screw_gun/base_joint_y' , 0, 0, 0) p.createLockedDofConstraint ('screwgun_lock_z' , 'screw_gun/base_joint_z' , 0, 0, 0) p.createLockedDofConstraint ('screwgun_lock_rx', 'screw_gun/base_joint_SO3', 0, 1, 0) p.createLockedDofConstraint ('screwgun_lock_ry', 'screw_gun/base_joint_SO3', 0, 2, 1) p.createLockedDofConstraint ('screwgun_lock_rz', 'screw_gun/base_joint_SO3', 0, 3, 2) p.createLockedDofConstraint ('larm_6' , 'hrp2/LARM_JOINT6' , q1[17], 0, 0) p.createLockedDofConstraint ('lhand_0', 'hrp2/LHAND_JOINT0', q1[18], 0, 0) p.createLockedDofConstraint ('lhand_1', 'hrp2/LHAND_JOINT1', q1[19], 0, 0) p.createLockedDofConstraint ('lhand_2', 'hrp2/LHAND_JOINT2', q1[20], 0, 0) p.createLockedDofConstraint ('lhand_3', 'hrp2/LHAND_JOINT3', q1[21], 0, 0)