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
0.20387672048126043, 0.9007626913161502, -0.39038645767349395, 0.31725226129015516, 1.5475253831101246, -0.0104572058777634, 0.32681856374063933, 0.24476959944940427, 1.06, 1.06, 1.06, 1.06, 1.06, 1.06, 1.0, 1.06, 1.06, -1.06, 1.06, 1.06, 0.412075621240969, 0.020809907186176854, 1.056724788359247, 0.0, 0.0, 0.0, 1.0 ] q_init = q_goal[::] q_init[r + 3:r + 7] = [0, 0, 1, 0] n = 'romeo/l_hand grasps placard/low' res, q_init, err = cg.applyNodeConstraints(n, q_init) if not res: raise RuntimeError("Failed to project initial configuration.") res, q_goal, err = cg.applyNodeConstraints(n, q_goal) if not res: raise RuntimeError("Failed to project initial configuration.") ps.selectPathProjector("Progressive", .05) import datetime as dt totalTime = dt.timedelta(0) totalNumberNodes = 0 N = 20 for i in range(N): ps.clearRoadmap() ps.resetGoalConfigs() ps.setInitialConfig(q_init) ps.addGoalConfig(q_goal) t1 = dt.datetime.now() ps.solve() t2 = dt.datetime.now() totalTime += t2 - t1 print(t2 - t1)
#q_init [rank:rank+4] = [c, 0, c, 0] q_init[rank:rank + 7] = [-2.5, -3.6, 0.76, 0, c, 0, c] #rank = robot.rankInConfiguration ['box/base_joint_SO3'] rank = robot.rankInConfiguration['box/root_joint'] q_goal[rank:rank + 7] = [-2.5, -4.4, 0.76, 0, -c, 0, c] #rank = robot.rankInConfiguration ['box/base_joint_SO3'] #q_goal [rank:rank+4] = [c, 0, -c, 0] del c # 3}}} robot.client.basic.problem.resetRoadmap() robot.client.basic.problem.setErrorThreshold(1e-3) robot.client.basic.problem.setMaxIterProjection(40) ps.selectPathProjector('Progressive', 0.2) # Create constraints. {{{3 robot.client.manipulation.problem.createPlacementConstraint \ ('box_placement', ['box/box_surface',], ['kitchen_area/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) if not n.startswith("pr2/l_gripper"):
rank = robot.rankInConfiguration ['box1/base_joint_xyz'] q1 = q_init[:] q1[rank:rank+7] = [ -0.14391940018238783, 1.092383723903555, 0.7460100959532432, 0.44542854851747, 0.5489390643072047, 0.4452361192941091, -0.5495672023684386] q2 = q0[:] q2[rank:rank+3] = [ -0.545, 0.268, 0.7460100959532432] rank = robot.rankInConfiguration ['box2/base_joint_xyz'] q1[rank:rank+7] = [ -0.14391940018238783, 1.092383723903555, 0.7460100959532432, 0.44542854851747, 0.5489390643072047, 0.4452361192941091, -0.5495672023684386] q2[rank:rank+3] = [ -0.445, 0.268, 0.7460100959532432] # 3}}} robot.client.basic.problem.resetRoadmap () robot.client.basic.problem.setErrorThreshold (1e-3) robot.client.basic.problem.setMaxIterations (20) #ps.selectPathValidation ('Graph-Discretized', 0.05) ps.selectPathProjector ('Progressive', 0.1) # ps.selectPathProjector ('Global', 0.2) # Create constraints. {{{3 from hpp.corbaserver.manipulation import ConstraintGraph cg = ConstraintGraph (robot, 'graph') # Create passive DOF lists {{{4 jointNames = dict () jointNames['all'] = robot.getJointNames () jointNames['baxter'] = list () jointNames['baxterRightSide'] = list () jointNames['baxterLeftSide'] = list () jointNames['box1'] = list () jointNames['box2'] = list () for n in jointNames['all']:
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.74,0,1.0192720229567027,0,0,0,1, # root_joint 0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, # leg_left
cg.addConstraints (graph = True, constraints = commonConstraints) cg.initialize () # Define initial and final configurations q_goal = [-0.003429678026293006, 7.761615492429529e-05, 0.8333148411182841, -0.08000440760954532, 0.06905332841243099, -0.09070086400314036, 0.9902546570793265, 0.2097693637044623, 0.19739743868699455, -0.6079135018296973, 0.8508704420155889, -0.39897628829947995, -0.05274298289004072, 0.20772797293264825, 0.1846394290733244, -0.49824886682709824, 0.5042013065348324, -0.16158420369261683, -0.039828502509861335, -0.3827070014985058, -0.24118425356319423, 1.0157846623463191, 0.5637424355124602, -1.3378817283780955, -1.3151786907256797, -0.392409481224193, 0.11332560818107676, 1.06, 1.06, 1.06, 1.06, 1.06, 1.06, 1.0, 1.06, 1.06, -1.06, 1.06, 1.06, 0.35936687035487364, -0.32595302056157444, -0.33115291290191723, 0.20387672048126043, 0.9007626913161502, -0.39038645767349395, 0.31725226129015516, 1.5475253831101246, -0.0104572058777634, 0.32681856374063933, 0.24476959944940427, 1.06, 1.06, 1.06, 1.06, 1.06, 1.06, 1.0, 1.06, 1.06, -1.06, 1.06, 1.06, 0.412075621240969, 0.020809907186176854, 1.056724788359247, 0.0, 0.0, 0.0, 1.0] q_init = q_goal [::] q_init [r+3:r+7] = [0, 0, 1, 0] n = 'romeo/l_hand grasps placard/low' res, q_init, err = cg.applyNodeConstraints (n, q_init) if not res: raise RuntimeError ("Failed to project initial configuration.") res, q_goal, err = cg.applyNodeConstraints (n, q_goal) if not res: raise RuntimeError ("Failed to project initial configuration.") ps.selectPathProjector ("Progressive", .05) import datetime as dt totalTime = dt.timedelta (0) totalNumberNodes = 0 N = 20 for i in range (N): ps.clearRoadmap () ps.resetGoalConfigs () ps.setInitialConfig (q_init) ps.addGoalConfig (q_goal) t1 = dt.datetime.now () ps.solve () t2 = dt.datetime.now () totalTime += t2 - t1 print (t2-t1)
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'] q_init [rank:rank+7] = [-4.8, -4.6, 0.91,0,sqrt(2)/2,sqrt(2)/2,0] q_goal = q_init [::] q_goal [rank:rank+7] = [-4.8, -3.35, 0.9, 0,sqrt(2)/2,sqrt(2)/2,0] # 3}}} robot.client.basic.problem.resetRoadmap () robot.client.basic.problem.setErrorThreshold (1e-3) robot.client.basic.problem.setMaxIterations (40) # ps.selectPathProjector ('Progressive', 0.2) ps.selectPathProjector ('Global', 0.2) # Create constraints. {{{3 from hpp.corbaserver.manipulation import ConstraintGraph cg = ConstraintGraph (robot, 'graph') # Create passive DOF lists {{{4 jointNames = dict () jointNames['all'] = robot.getJointNames () jointNames['romeo'] = list () jointNames['romeoWithoutLeftArm'] = list () jointNames['romeoWithoutRightArm'] = list () jointNames['kitchen_area'] = list () jointNames['cup'] = list () for n in jointNames['all']: if n.startswith ("romeo"):
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'] q_init[rank:rank + 7] = [-4.8, -4.6, 0.91, 0, sqrt(2) / 2, sqrt(2) / 2, 0] q_goal = q_init[::] q_goal[rank:rank + 7] = [-4.8, -3.35, 0.9, 0, sqrt(2) / 2, sqrt(2) / 2, 0] # 3}}} robot.client.basic.problem.resetRoadmap() robot.client.basic.problem.setErrorThreshold(1e-3) robot.client.basic.problem.setMaxIterations(40) # ps.selectPathProjector ('Progressive', 0.2) ps.selectPathProjector('Global', 0.2) # Create constraints. {{{3 from hpp.corbaserver.manipulation import ConstraintGraph cg = ConstraintGraph(robot, 'graph') # Create passive DOF lists {{{4 jointNames = dict() jointNames['all'] = robot.getJointNames() jointNames['romeo'] = list() jointNames['romeoWithoutLeftArm'] = list() jointNames['romeoWithoutRightArm'] = list() jointNames['kitchen_area'] = list() jointNames['cup'] = list() for n in jointNames['all']: if n.startswith("romeo"):
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 # q_init [-7:] = [2, 1, 0.65, 0.7071067811865476, 0, -0.7071067811865475, 0] q_init [-7:] = [2, 1, 0.65, 0.7071067811865476, 0.5, -0.5, 0] # Open left hand ilh = robot.rankInConfiguration ['hrp2/LARM_JOINT6'] q_init [ilh:ilh+6] = [0.75, -0.75, 0.75, -0.75, 0.75, -0.75] q_goal = q_init [::]