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 q_init [rank:rank+4] = [c, 0, c, 0] rank = robot.rankInConfiguration ['box2/base_joint_xyz'] q_init [rank:rank+3] = [-0.3, 1.3, 0.76] rank = robot.rankInConfiguration ['box2/base_joint_SO3']
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] rank = robot.rankInConfiguration ['box1/base_joint_SO3'] q_init [rank:rank+4] = [c, 0, -c, 0] rank = robot.rankInConfiguration ['box2/base_joint_xyz'] q_init [rank:rank+3] = [-0.4, 0.5, 0.76]
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]) # 3}}} # Define configurations. {{{3 q_init = robot.getCurrentConfig () rankB = list() for i in xrange(K): rankB.append (robot.rankInConfiguration [boxes[i] + '/root_joint']) bb = [-0.3, -0.4, 0.7, 0.9] c = sqrt (2) / 2
# 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]) # 3}}} # Define configurations. {{{3 q_init = robot.getCurrentConfig() rankB = list() for i in xrange(K): rankB.append(robot.rankInConfiguration[boxes[i] + '/base_joint_xyz'])
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]) # 3}}} # Define configurations. {{{3 q_init = robot.getCurrentConfig () rankB = list() for i in xrange(K): rankB.append (robot.rankInConfiguration [boxes[i] + '/base_joint_xyz']) bb = [-0.3, -0.4, 0.7, 0.9] c = sqrt (2) / 2