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
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() for i in range(nSphere): vf.loadObjectModel(Sphere, 'sphere{0}'.format(i)) robot.setJointBounds('sphere{0}/root_joint'.format(i), [ -1., 1., -1., 1., -.1, 1., -1.0001, 1.0001,
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}}} # 1}}} # Initialization. {{{1 # Set parameters. {{{2 robot.client.basic.problem.resetRoadmap () ps.setErrorThreshold (1e-3) ps.setMaxIterProjection (40)
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 rank = robot.rankInConfiguration['pr2/l_gripper_l_finger_joint'] q_init[rank] = 0.5 rank = robot.rankInConfiguration['pr2/l_gripper_r_finger_joint'] q_init[rank] = 0.5
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 () q0 = q_init [::] rank = robot.rankInConfiguration ['box1/base_joint_xyz'] q_init [rank:rank+3] = [-0.3, 1.3, 0.76] rank = robot.rankInConfiguration ['box1/base_joint_SO3'] c = sqrt (2) / 2
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'] rankB2 = robot.rankInConfiguration ['box2/base_joint_xyz'] c = sqrt (2) / 2 rank = robot.rankInConfiguration ['box1/base_joint_xyz'] q_init [rank:rank+3] = [-0.3, 0.5, 0.76]
# - 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 robot.setJointBounds ('r0/shoulder_pan_joint', [-pi, 4]) robot.setJointBounds ('r1/shoulder_pan_joint', [-pi, 4]) robot.setJointBounds ('r0/shoulder_lift_joint', [-pi, 0]) robot.setJointBounds ('r1/shoulder_lift_joint', [-pi, 0]) robot.setJointBounds ('r0/elbow_joint', [-2.6, 2.6]) robot.setJointBounds ('r1/elbow_joint', [-2.6, 2.6]) vf.loadEnvironmentModel (Ground, 'ground') nSphere = 2 nCylinder = 2 objects = list () for i in range (nSphere): vf.loadObjectModel (Sphere, 'sphere{0}'.format (i)) robot.setJointBounds ('sphere{0}/root_joint'.format (i), [-1.,1.,-1.,1.,-.1,1.,-1.0001, 1.0001,-1.0001, 1.0001, -1.0001, 1.0001,-1.0001, 1.0001,]) objects.append ('sphere{0}'.format (i)) for i in range (nCylinder): vf.loadObjectModel (Cylinder_08, 'cylinder{0}'.format (i)) robot.setJointBounds ('cylinder{0}/root_joint'.format (i),