Exemple #1
0
 def __init__(self, world):
     GLWidgetPlugin.__init__(self)
     self.world = world
     self.poseWidget = PointPoser()
     self.robotWidget = RobotPoser(world.robot(0))
     self.addWidget(self.poseWidget)
     self.addWidget(self.robotWidget)
Exemple #2
0
    def init(self,world):
        self.world = world
        self.robotPoser = RobotPoser(world.robot(0))
        self.plugin = MyWidgetPlugin(self)
        self.plugin.addWidget(self.robotPoser)
        self.robot = world.robot(0)

        left_arm_link_names = ['left_upper_shoulder','left_lower_shoulder','left_upper_elbow','left_lower_elbow','left_upper_forearm','left_lower_forearm','left_wrist']
        right_arm_link_names = ['right_upper_shoulder','right_lower_shoulder','right_upper_elbow','right_lower_elbow','right_upper_forearm','right_lower_forearm','right_wrist']
        base_link_names = ['base_x','base_y','base_yaw']
        self.left_arm_link_indices = [self.robot.link(l).index for l in left_arm_link_names]
        self.right_arm_link_indices = [self.robot.link(l).index for l in right_arm_link_names]
        self.base_link_indices = [self.robot.link(l).index for l in base_link_names]
        if any(v < 0 for v in self.left_arm_link_indices+self.right_arm_link_indices):
            raise ValueError("Robot in world does not have Baxter's torso?")

        # == subscribe marker position
        self.marker = Marker()
        self.base_height = 1.1
        self.marker_pos = [0.0, 0.0, 0.0]
        self.marker_pos_tf = [0.0, 0.0, 0.0]
        rospy.init_node('marker_listener', anonymous=True)
        rospy.Subscriber("/Marker_glove", Marker, self.callback, None)

        #== auto tracking parameter
        self.tracking_speed_ratio = 1.0
        self.tracking_vel = [0.0, 0.0, 0.0]
        self.left_ee_t = [0.0, 0.0, 0.0]
        self.right_ee_t = [0.0, 0.0, 0.0]
        self.reactive_vel_left_ee = [0.0, 0.0, 0.0]
        self.reactive_vel_right_ee = [0.0, 0.0, 0.0]


        return True
Exemple #3
0
 def init(self, world):
     self.world = world
     self.robotPoser = RobotPoser(world.robot(0))
     self.plugin = MyWidgetPlugin(self)
     self.plugin.addWidget(self.robotPoser)
     robot = world.robot(0)
     left_arm_link_names = [
         'left_upper_shoulder', 'left_lower_shoulder', 'left_upper_elbow',
         'left_lower_elbow', 'left_upper_forearm', 'left_lower_forearm',
         'left_wrist'
     ]
     right_arm_link_names = [
         'right_upper_shoulder', 'right_lower_shoulder',
         'right_upper_elbow', 'right_lower_elbow', 'right_upper_forearm',
         'right_lower_forearm', 'right_wrist'
     ]
     base_link_names = ['base_x', 'base_y', 'base_yaw']
     self.left_arm_link_indices = [
         robot.link(l).index for l in left_arm_link_names
     ]
     self.right_arm_link_indices = [
         robot.link(l).index for l in right_arm_link_names
     ]
     self.base_link_indices = [robot.link(l).index for l in base_link_names]
     if any(v < 0 for v in self.left_arm_link_indices +
            self.right_arm_link_indices):
         raise ValueError("Robot in world does not have Baxter's torso?")
     return True
    def __init__(self, world):
        GLWidgetProgram.__init__(self, "Manual poser")

        self.world = world
        self.robotPoser = RobotPoser(world.robot(0))
        self.addWidget(self.robotPoser)

        robot = world.robot(0)
        left_arm_link_names = [
            'left_upper_shoulder', 'left_lower_shoulder', 'left_upper_elbow',
            'left_lower_elbow', 'left_upper_forearm', 'left_lower_forearm',
            'left_wrist'
        ]
        right_arm_link_names = [
            'right_upper_shoulder', 'right_lower_shoulder',
            'right_upper_elbow', 'right_lower_elbow', 'right_upper_forearm',
            'right_lower_forearm', 'right_wrist'
        ]
        self.left_arm_link_indices = [
            robot.link(l).index for l in left_arm_link_names
        ]
        self.right_arm_link_indices = [
            robot.link(l).index for l in right_arm_link_names
        ]
        self.firstTimeHack = True
        self.world.makeTerrain('terrain')

        self.subscribe()
        self.newPC = None
 def __init__(self,world):
     GLWidgetPlugin.__init__(self)
     self.world = world
     self.poseWidget = PointPoser()
     self.robotWidget = RobotPoser(world.robot(0))
     self.addWidget(self.poseWidget)
     self.addWidget(self.robotWidget)
     self.add_action(self.print_config,'Print config',' ')
     self.add_action(self.save_world,'Save world','s')
    def __init__(self, world):
        GLWidgetProgram.__init__(self, "Manual poser")

        #start up the poser in the currently commanded configuration
        q = motion.robot.getKlamptCommandedPosition()
        world.robot(0).setConfig(q)

        self.world = world
        self.robot = world.robot(0)
        self.robotPoser = RobotPoser(world.robot(0))
        self.addWidget(self.robotPoser)
        self.roadMap = ([], [])

        robot = world.robot(0)
        left_arm_link_names = [
            'left_upper_shoulder', 'left_lower_shoulder', 'left_upper_elbow',
            'left_lower_elbow', 'left_upper_forearm', 'left_lower_forearm',
            'left_wrist'
        ]
        right_arm_link_names = [
            'right_upper_shoulder', 'right_lower_shoulder',
            'right_upper_elbow', 'right_lower_elbow', 'right_upper_forearm',
            'right_lower_forearm', 'right_wrist'
        ]
        self.left_arm_link_names = left_arm_link_names
        self.right_arm_link_names = right_arm_link_names
        missing_left_arm_names = [
            'left_upper_forearm_visual', 'left_upper_elbow_visual'
        ]
        missing_right_arm_names = [
            'right_upper_forearm_visual', 'right_upper_elbow_visual'
        ]
        #self.left_arm_link_names+=missing_left_arm_names
        #self.right_arm_link_names+=missing_right_arm_names
        self.left_arm_link_names = [
            'left_upper_forearm', 'left_lower_forearm', 'left_wrist',
            'left_upper_forearm_visual', 'left_gripper:base'
        ]
        self.right_arm_link_names = [
            'right_upper_forearm', 'right_lower_forearm', 'right_wrist',
            'right_upper_forearm_visual', 'right_gripper:base'
        ]
        self.left_arm_link_indices = [
            robot.link(l).index for l in left_arm_link_names
        ]
        self.right_arm_link_indices = [
            robot.link(l).index for l in right_arm_link_names
        ]
        self.firstTimeHack = True
        self.world.makeTerrain('terrain')

        self.subscribe()
        self.newPC = None

        self.collider = WorldCollider(self.world)
    def __init__(self, world, serviceThread):
        GLWidgetProgram.__init__(self, "Manual poser")

        self.world = world
        self.serviceThread = serviceThread
        self.oldTime = 0.0

        for i in range(10):
            q = self.serviceThread.qcmdGetter.get()
            if q != None:
                break
            time.sleep(0.1)
        if q == None:
            print "Warning, system state service doesn't seem to have .robot.command.q"
            print "Press enter to continue"
            raw_input()
        else:
            world.robot(0).setConfig(q)
        self.robotPoser = RobotPoser(world.robot(0))
        self.addWidget(self.robotPoser)

        robot = world.robot(0)
        left_arm_link_names = [
            'left_upper_shoulder', 'left_lower_shoulder', 'left_upper_elbow',
            'left_lower_elbow', 'left_upper_forearm', 'left_lower_forearm',
            'left_wrist'
        ]
        right_arm_link_names = [
            'right_upper_shoulder', 'right_lower_shoulder',
            'right_upper_elbow', 'right_lower_elbow', 'right_upper_forearm',
            'right_lower_forearm', 'right_wrist'
        ]
        self.left_arm_link_indices = [
            robot.link(l).index for l in left_arm_link_names
        ]
        self.right_arm_link_indices = [
            robot.link(l).index for l in right_arm_link_names
        ]

        for i in range(10):
            ee1 = self.serviceThread.ee1Getter.get()
            ee2 = self.serviceThread.ee1Getter.get()
            if ee1 != None and ee2 != None:
                break
            time.sleep(0.1)
        if ee1 == None:
            print "Warning, system state service doesn't seem to have .robot.endEffectors.0.xform.sensed"
            print "Press enter to continue"
            raw_input()
        if ee2 == None:
            print "Warning, system state service doesn't seem to have .robot.endEffectors.1.xform.sensed"
            print "Press enter to continue"
            raw_input()
 def __init__(self, world):
     GLWidgetProgram.__init__(self, world, "Manual poser")
     self.robotPoser = RobotPoser(world.robot(0))
     self.widgetMaster.add(self.robotPoser)
     robot = world.robot(0)
     left_arm_link_names = [
         "left_upper_shoulder",
         "left_lower_shoulder",
         "left_upper_elbow",
         "left_lower_elbow",
         "left_upper_forearm",
         "left_lower_forearm",
         "left_wrist",
     ]
     right_arm_link_names = [
         "right_upper_shoulder",
         "right_lower_shoulder",
         "right_upper_elbow",
         "right_lower_elbow",
         "right_upper_forearm",
         "right_lower_forearm",
         "right_wrist",
     ]
     self.left_arm_link_indices = [robot.getLink(l).index for l in left_arm_link_names]
     self.right_arm_link_indices = [robot.getLink(l).index for l in right_arm_link_names]
 def __init__(self,world):
     GLWidgetProgram.__init__(self,world,"Manual poser")
     self.robotPoser = RobotPoser(world.robot(0))
     self.widgetMaster.add(self.robotPoser)
     motion.robot.left_limb.enableSelfCollisionAvoidance(False)
     motion.robot.right_limb.enableSelfCollisionAvoidance(False)
     robot = world.robot(0)
     left_arm_link_names = ['left_upper_shoulder','left_lower_shoulder','left_upper_elbow','left_lower_elbow','left_upper_forearm','left_lower_forearm','left_wrist']
     right_arm_link_names = ['right_upper_shoulder','right_lower_shoulder','right_upper_elbow','right_lower_elbow','right_upper_forearm','right_lower_forearm','right_wrist']
     self.left_arm_link_indices = [robot.getLink(l).index for l in left_arm_link_names]
     self.right_arm_link_indices = [robot.getLink(l).index for l in right_arm_link_names]
     self.outputFileLeft = open("sag_calibration_left.csv","w")
     self.outputFileRight = open("sag_calibration_right.csv","w")
     #write headers
     self.outputFileLeft.write("time,")
     self.outputFileLeft.write(",".join("qsns"+str(i) for i in range(1,8)))
     self.outputFileLeft.write(",")
     self.outputFileLeft.write(",".join("qcmd"+str(i) for i in range(1,8)))
     self.outputFileLeft.write("\n")
     self.outputFileRight.write("time,")
     self.outputFileRight.write(",".join("qsns"+str(i) for i in range(1,8)))
     self.outputFileRight.write(",")
     self.outputFileRight.write(",".join("qcmd"+str(i) for i in range(1,8)))
     self.outputFileRight.write("\n")
     self.t0 = time.time()
     self.lastLeftMove = -100
     self.lastRightMove = -100 
    def __init__(self,world):
        GLWidgetProgram.__init__(self,"Manual poser")

        self.world = world
        q = motion.robot.getKlamptCommandedPosition()
        self.world.robot(0).setConfig(q)
        self.robotPoser = RobotPoser(world.robot(0))
        self.addWidget(self.robotPoser)

        robot = world.robot(0)
        left_arm_link_names = ['left_upper_shoulder','left_lower_shoulder','left_upper_elbow','left_lower_elbow','left_upper_forearm','left_lower_forearm','left_wrist']
        right_arm_link_names = ['right_upper_shoulder','right_lower_shoulder','right_upper_elbow','right_lower_elbow','right_upper_forearm','right_lower_forearm','right_wrist']
        head_pan_link_name = 'head'
        self.left_arm_link_indices = [robot.link(l).index for l in left_arm_link_names]
        self.right_arm_link_indices = [robot.link(l).index for l in right_arm_link_names]
        self.head_pan_link_index = robot.link(head_pan_link_name)
Exemple #11
0
def multiwindow_template(world):
    """Tests multiple windows and views."""
    vis.add("world",world)
    vp = vis.getViewport()
    vp.w,vp.h = 800,800
    vis.setViewport(vp)

    vis.setWindowTitle("vis.spin test: will close in 5 seconds...")
    vis.spin(5.0)

    #Now testing ability to re-launch windows
    vis.setWindowTitle("Shown again.  Close me to proceed.")
    vis.spin(float('inf'))

    vis.setWindowTitle("Dialog test. Close me to proceed.")
    vp = vis.getViewport()
    vp.w,vp.h = 400,600
    vis.setViewport(vp)
    vis.dialog()

    vp.w,vp.h = 640,480
    vis.setViewport(vp)
    for i in range(3):
        widgets = GLWidgetPlugin()
        widgets.addWidget(RobotPoser(world.robot(0)))
        vis.addPlugin(widgets)
    vis.setWindowTitle("Split screen test")
    vis.spin(float('inf'))
    
    vis.setPlugin(None)
    vis.setWindowTitle("Back to normal. Close me to quit.")
    vis.dialog()
    vis.kill()
class MyGLViewer(GLWidgetProgram):
    def __init__(self,world):
        GLWidgetProgram.__init__(self,"Manual poser")

        self.world = world
        self.robotPoser = RobotPoser(world.robot(0))
        self.addWidget(self.robotPoser)

        robot = world.robot(0)
        left_arm_link_names = ['left_upper_shoulder','left_lower_shoulder','left_upper_elbow','left_lower_elbow','left_upper_forearm','left_lower_forearm','left_wrist']
        right_arm_link_names = ['right_upper_shoulder','right_lower_shoulder','right_upper_elbow','right_lower_elbow','right_upper_forearm','right_lower_forearm','right_wrist']
        self.left_arm_link_indices = [robot.link(l).index for l in left_arm_link_names]
        self.right_arm_link_indices = [robot.link(l).index for l in right_arm_link_names]

    def display(self):
        #Put your display handler here
        #the current example draws the sensed robot in grey and the
        #commanded configurations in transparent green

        #this line will draw the robot's sensed configuration in the world
        robot = motion.robot
        q = robot.getKlamptSensedPosition()
        self.world.robot(0).setConfig(q)
        self.world.robot(0).drawGL()
        GLWidgetProgram.display(self)

        #draw commanded configuration
        glEnable(GL_BLEND)
        glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA)
        glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[0,1,0,0.5])
        q = robot.getKlamptCommandedPosition()
        self.world.robot(0).setConfig(q)
        self.world.robot(0).drawGL(False)
        glDisable(GL_BLEND)

    def keyboardfunc(self,c,x,y):
        #Put your keyboard handler here
        #the current example toggles simulation / movie mode
        if c == 'h':
            print '[space]: send the current posed milestone'
            print 'q: clean quit'
        elif c == ' ':
            q = self.robotPoser.get()
            robot = motion.robot
            robot.left_mq.setRamp(robot.left_limb.configFromKlampt(q))
            robot.right_mq.setRamp(robot.right_limb.configFromKlampt(q))
            qlg = robot.left_gripper.configFromKlampt(q)
            qrg = robot.right_gripper.configFromKlampt(q)
            robot.left_gripper.command(qlg,[1]*len(qlg),[1]*len(qlg))
            robot.right_gripper.command(qrg,[1]*len(qrg),[1]*len(qrg))
            #robot.left_mq.setRamp([q[i] for i in self.left_arm_link_indices])
            #robot.right_mq.setRamp([q[i] for i in self.right_arm_link_indices])
        elif c == 'q':
            motion.robot.shutdown()
            exit(0)
        else:
            GLWidgetProgram.keyboardfunc(self,c,x,y)
            self.refresh()
Exemple #13
0
def multiwindow_template(world):
    """Tests multiple windows and views."""
    vis.add("world", world)
    vp = vis.getViewport()
    vp.w, vp.h = 400, 600
    vis.setViewport(vp)
    vis.addText("label1", "This is Window 1", (20, 20))
    vis.setWindowTitle("Window 1")
    vis.show()
    id1 = vis.getWindow()
    print("First window ID:", id1)

    id2 = vis.createWindow("Window 2")
    vis.add("Lone point", [0, 0, 0])
    vis.setViewport(vp)
    vis.addText("label1", "This is Window 2", (20, 20))
    print("Second window ID:", vis.getWindow())
    vis.setWindow(id2)
    vis.spin(float('inf'))

    #restore back to 1 window, clear the text
    vis.setWindow(id1)
    vis.clearText()

    vp = vis.getViewport()
    vp.w, vp.h = 800, 800
    vis.setViewport(vp)

    vis.setWindowTitle("vis.spin test: will close in 5 seconds...")
    vis.spin(5.0)

    #Now testing ability to re-launch windows
    vis.setWindowTitle("Shown again.  Close me to proceed.")
    vis.spin(float('inf'))

    vis.setWindowTitle("Dialog test. Close me to proceed.")
    vp = vis.getViewport()
    vp.w, vp.h = 400, 600
    vis.setViewport(vp)
    vis.dialog()

    vp.w, vp.h = 640, 480
    vis.setViewport(vp)
    for i in range(3):
        widgets = GLWidgetPlugin()
        widgets.addWidget(RobotPoser(world.robot(0)))
        vis.addPlugin(widgets)
    vis.setWindowTitle("Split screen test")
    vis.spin(float('inf'))

    vis.setPlugin(None)
    vis.setWindowTitle("Back to normal. Close me to quit.")
    vis.dialog()
    vis.kill()
Exemple #14
0
 def __init__(self, world):
     GLWidgetPlugin.__init__(self)
     self.world = world
     self.robot = world.robot(0)
     self.subrobots = []
     for i in range(6):
         self.subrobots.append(
             SubRobotModel(self.robot, list(range(6 + i * 6, 12 + i * 6))))
     (res, val) = editors.run(
         editors.SelectionEditor("subrobot",
                                 self.subrobots[0]._links,
                                 description="sub robot links",
                                 world=world,
                                 robot=self.robot))
     if res:
         print("Return value", val)
         self.subrobots[0]._links = val
     self.startConfig = self.robot.getConfig()
     self.robotWidget = RobotPoser(world.robot(0))
     self.robotWidget.setActiveDofs(self.subrobots[0]._links)
     self.addWidget(self.robotWidget)
    def __init__(self,world):
        GLWidgetProgram.__init__(self,"Manual poser")

        self.world = world
        self.robotPoser = RobotPoser(world.robot(0))
        self.addWidget(self.robotPoser)

        robot = world.robot(0)
        left_arm_link_names = ['left_upper_shoulder','left_lower_shoulder','left_upper_elbow','left_lower_elbow','left_upper_forearm','left_lower_forearm','left_wrist']
        right_arm_link_names = ['right_upper_shoulder','right_lower_shoulder','right_upper_elbow','right_lower_elbow','right_upper_forearm','right_lower_forearm','right_wrist']
        self.left_arm_link_indices = [robot.link(l).index for l in left_arm_link_names]
        self.right_arm_link_indices = [robot.link(l).index for l in right_arm_link_names]
    def __init__(self,world):
        GLWidgetProgram.__init__(self,world,"Manual poser")

        self.world.robot(0).setConfig(motion.robot.getKlamptCommandedPosition())
        #motion.robot.left_limb.enableSelfCollisionAvoidance(False)
        #motion.robot.right_limb.enableSelfCollisionAvoidance(False)

        self.robotPoser = RobotPoser(world.robot(0))
        self.widgetMaster.add(self.robotPoser)
        robot = world.robot(0)
        left_arm_link_names = ['left_upper_shoulder','left_lower_shoulder','left_upper_elbow','left_lower_elbow','left_upper_forearm','left_lower_forearm','left_wrist']
        right_arm_link_names = ['right_upper_shoulder','right_lower_shoulder','right_upper_elbow','right_lower_elbow','right_upper_forearm','right_lower_forearm','right_wrist']
        self.left_arm_link_indices = [robot.getLink(l).index for l in left_arm_link_names]
        self.right_arm_link_indices = [robot.getLink(l).index for l in right_arm_link_names]
class MyGLViewer(GLWidgetPlugin):
    def __init__(self, world):
        GLWidgetPlugin.__init__(self)
        self.world = world
        self.robot = world.robot(0)
        self.subrobots = []
        for i in range(6):
            self.subrobots.append(
                SubRobotModel(self.robot, list(range(6 + i * 6, 12 + i * 6))))
        (res, val) = editors.run(
            editors.SelectionEditor("subrobot",
                                    self.subrobots[0]._links,
                                    description="sub robot links",
                                    world=world,
                                    robot=self.robot))
        if res:
            print("Return value", val)
            self.subrobots[0]._links = val
        self.startConfig = self.robot.getConfig()
        self.robotWidget = RobotPoser(world.robot(0))
        self.robotWidget.setActiveDofs(self.subrobots[0]._links)
        self.addWidget(self.robotWidget)

    def keyboardfunc(self, c, x, y):
        #Put your keyboard handler here
        #the current example prints the config when [space] is pressed
        if c == ' ':
            config = self.robotWidget.get()
            subconfig = self.subrobots[0].fromfull(config)
            print("Config:", subconfig)
            self.subrobots[0].setConfig(subconfig)
        elif c == 'r':
            self.subrobots[0].randomizeConfig()
            self.robotWidget.set(self.robot.getConfig())
        elif c == 'p':
            config = self.robotWidget.get()
            subconfig = self.subrobots[0].fromfull(config)
            self.subrobots[0].setConfig(self.subrobots[0].fromfull(
                self.startConfig))
            settings = {
                'type': "sbl",
                'perturbationRadius': 0.5,
                'bidirectional': 1,
                'shortcut': 1,
                'restart': 1,
                'restartTermCond': "{foundSolution:1,maxIters:1000}"
            }
            plan = robotplanning.planToConfig(self.world,
                                              self.subrobots[0],
                                              subconfig,
                                              movingSubset='all',
                                              **settings)
            plan.planMore(1000)
            print(plan.getPath())
        else:
            GLWidgetPlugin.keyboardfunc(self, c, x, y)
Exemple #18
0
class MyGLViewer(GLWidgetProgram):
    def __init__(self,world):
        GLWidgetProgram.__init__(self,world,"My GL program")
        self.poseWidget = PointPoser()
        self.widgetMaster.add(self.poseWidget)
        self.robotWidget = RobotPoser(world.robot(0))
        self.widgetMaster.add(self.robotWidget)

    def keyboardfunc(self,c,x,y):
        #Put your keyboard handler here
        #the current example prints the config when [space] is pressed
        if c == ' ':
            config = self.robotWidget.get()
            print "Config:",config
            self.world.robot(0).setConfig(config)
        else:
            GLWidgetProgram.keyboardfunc(self,c,x,y)
Exemple #19
0
class MyGLViewer(GLWidgetProgram):
    def __init__(self, world):
        GLWidgetProgram.__init__(self, world, "My GL program")
        self.poseWidget = PointPoser()
        self.widgetMaster.add(self.poseWidget)
        self.robotWidget = RobotPoser(world.robot(0))
        self.widgetMaster.add(self.robotWidget)

    def keyboardfunc(self, c, x, y):
        #Put your keyboard handler here
        #the current example prints the config when [space] is pressed
        if c == ' ':
            config = self.robotWidget.get()
            print "Config:", config
            self.world.robot(0).setConfig(config)
        else:
            GLWidgetProgram.keyboardfunc(self, c, x, y)
class MyGLViewer(GLWidgetPlugin):
    def __init__(self,world):
        GLWidgetPlugin.__init__(self)
        self.world = world
        self.poseWidget = PointPoser()
        self.robotWidget = RobotPoser(world.robot(0))
        self.addWidget(self.poseWidget)
        self.addWidget(self.robotWidget)
        self.add_action(self.print_config,'Print config',' ')
        self.add_action(self.save_world,'Save world','s')

    def print_config(self):
        config = self.robotWidget.get()
        print("Config:",config)
        self.world.robot(0).setConfig(config)
    
    def save_world(self):
        fn = "widgets_test_world.xml"
        print("Saving file to",fn)
        self.world.saveFile(fn)
Exemple #21
0
def coordinates_template(world):
    """Tests integration with the coordinates module."""
    #add the world to the visualizer
    vis.add("world", world)
    coordinates.setWorldModel(world)
    #add the coordinate Manager to the visualizer
    vis.add("coordinates", coordinates.manager())

    vis.setWindowTitle("Coordinates visualiation test")
    if MANUAL_EDITING:
        #manually adds a poser, and adds a callback whenever the widget changes
        widgets = GLWidgetPlugin()
        widgets.addWidget(RobotPoser(world.robot(0)))
        #update the coordinates every time the widget changes
        widgets.widgetchangefunc = (lambda self: coordinates.updateFromWorld())
        vis.pushPlugin(widgets)
        if not MULTITHREADED:
            vis.loop(callback=None, setup=vis.show)
        else:
            vis.show()
            while vis.shown():
                time.sleep(0.01)
    else:
        vis.edit(("world", world.robot(0).getName()))
        if not MULTITHREADED:

            def callback(coordinates=coordinates):
                coordinates.updateFromWorld()

            vis.loop(callback=callback, setup=vis.show)
        else:
            vis.show()
            while vis.shown():
                vis.lock()
                #reads the coordinates from the world
                coordinates.updateFromWorld()
                vis.unlock()
                time.sleep(0.01)

    #quit the visualization thread nicely
    vis.kill()
Exemple #22
0
class MyGLViewer(GLWidgetPlugin):
    def __init__(self, world):
        GLWidgetPlugin.__init__(self)
        self.world = world
        self.poseWidget = PointPoser()
        self.robotWidget = RobotPoser(world.robot(0))
        self.addWidget(self.poseWidget)
        self.addWidget(self.robotWidget)

    def keyboardfunc(self, c, x, y):
        #Put your keyboard handler here
        #the current example prints the config when [space] is pressed
        if c == ' ':
            config = self.robotWidget.get()
            print "Config:", config
            self.world.robot(0).setConfig(config)
        elif c == 's':
            fn = "widgets_test_world.xml"
            print "Saving file to", fn
            self.world.saveFile(fn)
        else:
            GLWidgetPlugin.keyboardfunc(self, c, x, y)
    def __init__(self, simworld=None, planworld=None, controller=None, sim=None, helper=None):

        if simworld == None and planworld == None:
            # Dummy GLViewer
            print "simworld and planworld are None"
            return

        GLRealtimeProgram.__init__(self, "My GL program")

        self.simworld = simworld
        self.planworld = planworld
        self.sim = sim

        self.widgetMaster = WidgetSet()
        self.widgetButton = 2  # right-clicks
        self.draggingWidget = False

        self.poseWidget = PointPoser()
        self.widgetMaster.add(self.poseWidget)
        self.robotWidget = RobotPoser(self.simworld.robot(0))
        self.widgetMaster.add(self.robotWidget)

        self.simulate = True

        if self.sim == None:
            self.simulate = False
        self.controller = controller
        self.command_queue = Queue()

        if helper != None:
            print "helper is not none"
            helper.run()

        self.points = None

        self.extraControllers = {}
Exemple #24
0
class KeyboardAutoTrackTaskGenerator(TaskGenerator):
    """Allows the user to interact with the model by right clicking
    using the mouse and pressing space bar to send the milestone.

    Also demonstrates how to write a Python plugin task with an OpenGL
    widget.
    """
    def __init__(self):
        self.plugin = None
        self.q = None

    def callback(self, data):
        self.marker = data
        self.marker_pos[0] = self.marker.pose.position.x
        self.marker_pos[1] = self.marker.pose.position.y
        self.marker_pos[2] = self.marker.pose.position.z
        #=== transform to world coordinate
        marker = se3.apply(kinect_frame_to_pedestal, tuple(self.marker_pos))
        self.marker_pos_tf = list(marker)
        self.marker_pos_tf[2] = self.marker_pos_tf[2] + self.base_height
        # print "marker_pos_tf = ", self.marker_pos_tf

    def name(self): return "Keyboard"

    def init(self,world):
        self.world = world
        self.robotPoser = RobotPoser(world.robot(0))
        self.plugin = MyWidgetPlugin(self)
        self.plugin.addWidget(self.robotPoser)
        self.robot = world.robot(0)

        left_arm_link_names = ['left_upper_shoulder','left_lower_shoulder','left_upper_elbow','left_lower_elbow','left_upper_forearm','left_lower_forearm','left_wrist']
        right_arm_link_names = ['right_upper_shoulder','right_lower_shoulder','right_upper_elbow','right_lower_elbow','right_upper_forearm','right_lower_forearm','right_wrist']
        base_link_names = ['base_x','base_y','base_yaw']
        self.left_arm_link_indices = [self.robot.link(l).index for l in left_arm_link_names]
        self.right_arm_link_indices = [self.robot.link(l).index for l in right_arm_link_names]
        self.base_link_indices = [self.robot.link(l).index for l in base_link_names]
        if any(v < 0 for v in self.left_arm_link_indices+self.right_arm_link_indices):
            raise ValueError("Robot in world does not have Baxter's torso?")

        # == subscribe marker position
        self.marker = Marker()
        self.base_height = 1.1
        self.marker_pos = [0.0, 0.0, 0.0]
        self.marker_pos_tf = [0.0, 0.0, 0.0]
        rospy.init_node('marker_listener', anonymous=True)
        rospy.Subscriber("/Marker_glove", Marker, self.callback, None)

        #== auto tracking parameter
        self.tracking_speed_ratio = 1.0
        self.tracking_vel = [0.0, 0.0, 0.0]
        self.left_ee_t = [0.0, 0.0, 0.0]
        self.right_ee_t = [0.0, 0.0, 0.0]
        self.reactive_vel_left_ee = [0.0, 0.0, 0.0]
        self.reactive_vel_right_ee = [0.0, 0.0, 0.0]


        return True

    def start(self):
        self._status = 'ok'
        self.robotPoser.set(self.world.robot(0).getConfig())
        self.serviceThread = ServiceThread()
        self.serviceThread.start()
        return True

    def stop(self):
        if self.serviceThread:
            self.serviceThread.kill()
            print "Waiting for thread join..."
            self.serviceThread.join()
            print "Done"
            self.serviceThread = None

    def getEE(self):
        R1,t1 = self.serviceThread.eeGetter_left.get()
        R2,t2 = self.serviceThread.eeGetter_right.get()
        self.left_ee_t = list(t1)
        self.right_ee_t = list(t2)
        print "left hand EE = ", self.left_ee_t
        print "right hand EE = ", self.right_ee_t

    def cal_reactive_vel(self, pos_hand, pos_target, vel_ratio):
        reactive_vel = [0.0, 0.0, 0.0]
        pos_diff = vectorops.sub(tuple(pos_target), tuple(pos_hand))
        pos_diff_norm = vectorops.norm(pos_diff)
        if pos_diff_norm >= 0.02:
            vel = vectorops.mul(pos_diff, vel_ratio)
            reactive_vel = list(vel)
        return reactive_vel


    def get(self):
        res = self.do_logic()
        return res


    def do_logic(self):

        rospy.Subscriber("/Marker_glove", Marker, self.callback, None)

        ''' retract arm to initial position '''

        if self.plugin.initialPose:
            self.plugin.initialPose = False
            self.q = self.robotPoser.get()
            if self.plugin.selectedLimb == "both":
                for i in range(len(self.left_arm_link_indices)):
                    idx = self.left_arm_link_indices[i]
                    self.q[idx] = q_init_left[i]
                for i in range(len(self.right_arm_link_indices)):
                    idx = self.right_arm_link_indices[i]
                    self.q[idx] = q_init_right[i]
                print "set both arms to initial pose"
            if self.plugin.selectedLimb == "left":
                for i in range(len(self.left_arm_link_indices)):
                    idx = self.left_arm_link_indices[i]
                    self.q[idx] = q_init_left[i]
                print "set left arms to initial pose"
            if self.plugin.selectedLimb == "right":
                for i in range(len(self.right_arm_link_indices)):
                    idx = self.right_arm_link_indices[i]
                    self.q[idx] = q_init_right[i]
                print "set right arms to initial pose"
            self.robotPoser.set(self.q)

        if self.plugin.tuckArmPose:
            self.plugin.tuckArmPose = False
            self.q = self.robotPoser.get()
            if self.plugin.selectedLimb == "both":
                for i in range(len(self.left_arm_link_indices)):
                    idx = self.left_arm_link_indices[i]
                    self.q[idx] = q_tuckarm_left[i]
                for i in range(len(self.right_arm_link_indices)):
                    idx = self.right_arm_link_indices[i]
                    self.q[idx] = q_tuckarm_right[i]
                print "Tuck both arms"
            if self.plugin.selectedLimb == "left":
                for i in range(len(self.left_arm_link_indices)):
                    idx = self.left_arm_link_indices[i]
                    self.q[idx] = q_tuckarm_left[i]
                print "Tuck left arm"
            if self.plugin.selectedLimb == "right":
                for i in range(len(self.right_arm_link_indices)):
                    idx = self.right_arm_link_indices[i]
                    self.q[idx] = q_tuckarm_right[i]
                print "Tuck right arm"
            self.robotPoser.set(self.q)


        ''' Control - Robot arm and mobile base'''

        if self.plugin.sendMilestone:
            self.plugin.sendMilestone = False
            self.q = self.robotPoser.get()
            qcmd = self.world.robot(0).getConfig()
            baseMoved = any(qcmd[i] != self.q[i] for i in self.base_link_indices)
            leftArmMoved = any(qcmd[i] != self.q[i] for i in self.left_arm_link_indices)
            rightArmMoved = any(qcmd[i] != self.q[i] for i in self.right_arm_link_indices)
            moved = []
            if baseMoved: moved.append('base')
            if leftArmMoved: moved.append('left')
            if rightArmMoved: moved.append('right')
            targets = []
            if baseMoved: targets.append([self.q[i] for i in self.base_link_indices])
            if leftArmMoved: targets.append([self.q[i] for i in self.left_arm_link_indices])
            if rightArmMoved: targets.append([self.q[i] for i in self.right_arm_link_indices])
            print "Sending milestone",self.q,"parts",",".join(moved)
            #CONSTRUCT THE TASK HERE
            msg = {}
            msg['type'] = 'JointPose'
            msg['parts'] = moved
            msg['positions'] = targets
            msg['speed'] = 1
            msg['safe'] = 0
            return msg

        ''' Control - gripper '''
        if self.plugin.gripperControl == True:
            self.plugin.gripperControl = False
            if self.plugin.gripperState == "open":
                p = [1.0, 1.0, 1.0, 1.0]
                if self.plugin.selectedLimb == "both":
                    p = p + p
            elif self.plugin.gripperState == "close":
                p = [0.3, 0.3, 0.3, 1.0]
                if self.plugin.selectedLimb == "both":
                    p = p + p
            msg = {}
            msg['type'] = 'Gripper'
            msg['limb'] = self.plugin.selectedLimb
            msg['position'] = p
            msg['force'] = 0.2
            msg['speed'] = 0.5
            return msg


        ''' Auto tracking '''

        if self.plugin.trackPosition == True:
            self.plugin.trackPosition_lastState = True
            self.plugin.trackPosition = False
            print "move to target: ", self.marker_pos_tf
            self.getEE()
            if self.plugin.selectedLimb == "both":
                self.reactive_vel_left_ee = self.cal_reactive_vel(self.left_ee_t, self.marker_pos_tf, self.tracking_speed_ratio)
                self.reactive_vel_right_ee = self.cal_reactive_vel(self.right_ee_t, self.marker_pos_tf, self.tracking_speed_ratio)
                print "Use both left or right arm to track"
                linear_vel = self.reactive_vel_left_ee + self.reactive_vel_right_ee
                angular_vel = [0.0, 0.0, 0.0] + [0.0, 0.0, 0.0]
            if self.plugin.selectedLimb == "left":
                self.reactive_vel_left_ee = self.cal_reactive_vel(self.left_ee_t, self.marker_pos_tf, self.tracking_speed_ratio)
                print "Move left arm to target at velocity: ", self.reactive_vel_left_ee
                linear_vel = self.reactive_vel_left_ee
                angular_vel = [0.0, 0.0, 0.0]
            if self.plugin.selectedLimb == "right":
                self.reactive_vel_right_ee = self.cal_reactive_vel(self.right_ee_t, self.marker_pos_tf, self.tracking_speed_ratio)
                print "Move right arm to target"
                linear_vel = self.reactive_vel_right_ee
                angular_vel = [0.0, 0.0, 0.0]
            msg = {}
            msg['type'] = 'CartesianVelocity'
            msg['limb'] = self.plugin.selectedLimb
            msg['linear'] = linear_vel
            msg['angular'] = angular_vel
            return msg

        if self.plugin.trackPosition_lastState == True:
            self.plugin.trackPosition_lastState = False
            print "last command is cartesian velocity ... "
            if self.plugin.selectedLimb == "both":
                linear_vel = [0.0, 0.0, 0.0] + [0.0, 0.0, 0.0]
                angular_vel = [0.0, 0.0, 0.0] + [0.0, 0.0, 0.0]
            if self.plugin.selectedLimb == "left":
                linear_vel = [0.0, 0.0, 0.0]
                angular_vel = [0.0, 0.0, 0.0]
            if self.plugin.selectedLimb == "right":
                linear_vel = [0.0, 0.0, 0.0]
                angular_vel = [0.0, 0.0, 0.0]
            msg = {}
            msg['type'] = 'CartesianVelocity'
            msg['limb'] = self.plugin.selectedLimb
            msg['linear'] = linear_vel
            msg['angular'] = angular_vel
            return msg

        return None







    def glPlugin(self):
        return self.plugin
Exemple #25
0
    vis.show()
    while vis.shown():
        time.sleep(0.01)
    """

    print "Doing a dialog..."
    vis.setWindowTitle("Dialog test")
    print "calling dialog()"
    vis.dialog()

    print "Doing a split screen program..."
    vp.w, vp.h = 640, 480
    vis.setViewport(vp)
    for i in range(3):
        widgets = GLWidgetPlugin()
        widgets.addWidget(RobotPoser(world.robot(0)))
        #update the coordinates every time the widget changes
        widgets.widgetchangefunc = (lambda self: coordinates.updateFromWorld())
        vis.addPlugin(widgets)
    vis.setWindowTitle("Split screen test")
    vis.show()
    while vis.shown():
        time.sleep(0.1)

    print "Showing a dialog, back to normal..."
    vis.setPlugin(None)
    vis.dialog()
    print "Showing again, back to normal..."
    vis.setWindowTitle("Basic visualization test")
    vis.show()
    while vis.shown():
class MyGLViewer(GLWidgetProgram):
    def __init__(self, world):
        GLWidgetProgram.__init__(self, "Manual poser")

        self.world = world
        self.robotPoser = RobotPoser(world.robot(0))
        self.addWidget(self.robotPoser)

        robot = world.robot(0)
        left_arm_link_names = [
            'left_upper_shoulder', 'left_lower_shoulder', 'left_upper_elbow',
            'left_lower_elbow', 'left_upper_forearm', 'left_lower_forearm',
            'left_wrist'
        ]
        right_arm_link_names = [
            'right_upper_shoulder', 'right_lower_shoulder',
            'right_upper_elbow', 'right_lower_elbow', 'right_upper_forearm',
            'right_lower_forearm', 'right_wrist'
        ]
        self.left_arm_link_indices = [
            robot.link(l).index for l in left_arm_link_names
        ]
        self.right_arm_link_indices = [
            robot.link(l).index for l in right_arm_link_names
        ]
        self.firstTimeHack = True
        self.world.makeTerrain('terrain')

        self.subscribe()
        self.newPC = None

    def display(self):
        #Put your display handler here
        #the current example draws the sensed robot in grey and the
        #commanded configurations in transparent green

        #this line with draw the world
        robot = motion.robot
        q = robot.getKlamptSensedPosition()
        self.world.robot(0).setConfig(q)
        self.world.robot(0).drawGL()
        self.world.terrain(0).drawGL()
        GLWidgetProgram.display(self)

        #draw commanded configuration
        glEnable(GL_BLEND)
        glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
        glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, [0, 1, 0, 0.5])
        q = robot.getKlamptCommandedPosition()
        self.world.robot(0).setConfig(q)
        self.world.robot(0).drawGL(False)
        glDisable(GL_BLEND)

    def keyboardfunc(self, c, x, y):
        #Put your keyboard handler here
        #the current example toggles simulation / movie mode
        if c == 'h':
            print '[space]: send the current posed milestone'
            print 'q: clean quit'
        elif c == ' ':
            q = self.robotPoser.get()
            q0 = motion.robot.getKlamptCommandedPosition()
            t1 = time.time()
            #collisions = obstaclecollision(WorldCollider(self.world),self.world.robot(0),q0,q)
            collides = bisection(WorldCollider(self.world),
                                 self.world.robot(0), q0, q)
            print "Obstacle collision detection done in time", +time.time() - t1
            exit(0)
            for i in range(self.world.robot(0).numLinks()):
                self.world.robot(0).link(i).appearance().setColor(
                    0.5, 0.5, 0.5, 1)
            #if not self.firstTimeHack and selfcollision(self.world.robot(0),q0,q):
            if collides:
                print "Invalid configuration, it self-collides"

            elif not self.firstTimeHack and collisions != None:
                #clear all links to gray
                for pairs in collisions:
                    print "Link " + str(
                        pairs[0].getIndex()) + " collides with obstacle"
                    self.world.robot(0).link(
                        pairs[0].getIndex()).appearance().setColor(1, 0, 0, 1)
            else:
                self.firstTimeHack = False
                robot = motion.robot
                robot.left_mq.setRamp(robot.left_limb.configFromKlampt(q))
                robot.right_mq.setRamp(robot.right_limb.configFromKlampt(q))
                qlg = robot.left_gripper.configFromKlampt(q)
                qrg = robot.right_gripper.configFromKlampt(q)
                robot.left_gripper.command(qlg, [1] * len(qlg), [1] * len(qlg))
                robot.right_gripper.command(qrg, [1] * len(qrg),
                                            [1] * len(qrg))
                #robot.left_mq.setRamp([q[i] for i in self.left_arm_link_indices])
                #robot.right_mq.setRamp([q[i] for i in self.right_arm_link_indices])
        elif c == 'q':
            motion.robot.shutdown()
            exit(0)
        else:
            GLWidgetProgram.keyboardfunc(self, c, x, y)
            self.refresh()

    def subscribe(self):
        rospy.init_node('subscribe', anonymous=True)
        rospy.Subscriber("/kinect2/sd/points", PointCloud2, self.callback)

    def callback(self, data):
        if self.newPC == None:
            data_out = pc2.read_points(data, skip_nans=True)
            #data_out = pc2.read_points(data)
            pc = PointCloud()
            pc.propertyNames.append('rgba')
            t0 = time.time()
            points = list(data_out)

            #filtered = self.radiusfilter(points,0.1,0)
            filtered = self.voxelgridfilter(points, 0.1, 0.1, 0.1, 10)
            print "Noise cleaned up in time", time.time() - t0
            #filtered = points
            for p in filtered:
                coordinates = (p[0], p[1], p[2])
                if vectorops.norm(coordinates) > 0.4:
                    index = pc.addPoint(coordinates)
                    i = struct.unpack("i", struct.pack("f", p[3]))[0]
                    pc.setProperty(index, 0, i)
            #print "Point cloud copied to Klampt format in time",time.time()-t0

            q = (0.566222, 0.423455, 0.424871, 0.5653)
            R1 = so3.from_quaternion(q)
            t1 = (0.228665, 0.0591513, 0.0977748)
            T1 = (R1, t1)
            R2 = so3.rotation((0, 0, 1), math.pi / 2)
            t2 = (0, 0, 1)
            T2 = (R2, t2)
            T = se3.mul(T2, T1)
            (R, t) = T
            pc.transform(R, t)
            #print "Point cloud transformed in time",time.time()-t0
            self.newPC = pc
        else:

            print "Not ready yet to receive new point cloud..."

    # Color data will be lost
    def statisticalfilter(self, points, k, mul):
        pclCloud = pcl.PointCloud()
        pclCloud.from_list(points)
        fil = pclCloud.make_statistical_outlier_filter()
        fil.set_mean_k(k)

        fil.set_std_dev_mul_thresh(mul)
        filtered = fil.filter().to_list()
        return filtered

    # Under construction
    def radiusfilter(self, points, radius, num):
        filtered = []
        coordinates = []
        for p in points:
            coordinates.append([p[0], p[1], p[2]])
        #print np.array(coordinates).reshape(-len(coordinates),3)
        tree = KDTree(np.array(coordinates))
        #tree = KDTree(np.random.random((10, 3)), leaf_size=2)
        for i, p in enumerate(coordinates):
            #print tree.query_radius(p,r=radius,count_only=True)
            if tree.query_radius(coordinates[i], r=radius,
                                 count_only=True) >= num:
                filtered.append(points[i])
        return points

    def voxelgridfilter(self, points, gridX, gridY, gridZ, num):
        filtered = []
        dictionary = {}
        for i, p in enumerate(points):
            x = p[0]
            y = p[1]
            z = p[2]
            xindex = int(math.floor(x / gridX))
            yindex = int(math.floor(y / gridY))
            zindex = int(math.floor(z / gridZ))
            key = (xindex, yindex, zindex)
            if key not in dictionary:
                dictionary[key] = []
            else:
                dictionary[key].append(i)
        for key, value in dictionary.iteritems():
            if len(value) >= num:
                for j in value:
                    filtered.append(points[j])
        return filtered

    #def countNeighbors(self, points, radius):
    #    dictionary = defaultdict(int)
    #    for i in range(len(points)-1):
    #        for j in range(i+1,len(points)):
    #            pointA = points[i]
    #            pointB = points[j]
    #            if(vectorops.distance((pointA[0],pointA[1],pointA[2]),(pointB[0],pointB[1],pointB[2]))<radius):
    #                dictionary[i]+=1
    #                dictionary[j]+=1
    #    return dictionary

    def idle(self):
        if self.newPC != None:
            #print "Setting the terrain geometry"
            self.world.terrain(0).geometry().setPointCloud(self.newPC)
            #print "Setting the collision margin"
            self.world.terrain(0).geometry().setCollisionMargin(0.01)
            self.newPC = None
            #print "Done"
            self.refresh()
Exemple #27
0
class MyGLViewer(GLWidgetProgram):
    def __init__(self, world, serviceThread):
        GLWidgetProgram.__init__(self, "Manual poser")

        self.world = world
        self.serviceThread = serviceThread

        for i in range(10):
            q = self.serviceThread.qcmdGetter.get()
            if q != None:
                break
            time.sleep(0.1)
        if q == None:
            print "Warning, system state service doesn't seem to have .robot.command.q"
            print "Press enter to continue"
            raw_input()
        else:
            world.robot(0).setConfig(q)
        self.robotPoser = RobotPoser(world.robot(0))
        self.addWidget(self.robotPoser)

        robot = world.robot(0)
        left_arm_link_names = [
            'left_upper_shoulder', 'left_lower_shoulder', 'left_upper_elbow',
            'left_lower_elbow', 'left_upper_forearm', 'left_lower_forearm',
            'left_wrist'
        ]
        right_arm_link_names = [
            'right_upper_shoulder', 'right_lower_shoulder',
            'right_upper_elbow', 'right_lower_elbow', 'right_upper_forearm',
            'right_lower_forearm', 'right_wrist'
        ]
        self.left_arm_link_indices = [
            robot.link(l).index for l in left_arm_link_names
        ]
        self.right_arm_link_indices = [
            robot.link(l).index for l in right_arm_link_names
        ]

    def display(self):
        #Put your display handler here
        #the example draws the posed robot in grey, the sensed
        #configuration in transparent red and the commanded
        #configuration in transparent green

        #this line will draw the world
        GLWidgetProgram.display(self)

        q = self.serviceThread.qsnsGetter.get()
        if q:
            glEnable(GL_BLEND)
            glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
            glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE,
                         [1, 0, 0, 0.5])
            self.world.robot(0).setConfig(q)
            self.world.robot(0).drawGL(False)

        #draw commanded configuration
        glEnable(GL_BLEND)
        glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
        glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, [0, 1, 0, 0.5])
        q = self.serviceThread.qcmdGetter.get()
        if q:
            self.world.robot(0).setConfig(q)
            self.world.robot(0).drawGL(False)
            glDisable(GL_BLEND)

    def keyboardfunc(self, c, x, y):
        #Put your keyboard handler here
        #the current example toggles simulation / movie mode
        if c == 'h':
            print '[space]: send the current posed milestone'
            print 'q: clean quit'
        elif c == ' ':
            q = self.robotPoser.get()
            #CONSTRUCT THE TASK HERE
            msg = {}
            msg['type'] = 'JointPose'
            msg['limb'] = 'both'
            msg['position'] = [
                q[i] for i in self.left_arm_link_indices +
                self.right_arm_link_indices
            ]
            msg['speed'] = 1
            msg['safe'] = 0
            print "Sending message", msg
            #SEND THE TASK TO THE SYSTEM STATE SERVICE
            self.serviceThread.taskSetter.set(msg)
        elif c == 'q':
            self.serviceThread.kill()
            exit(0)
        else:
            GLWidgetProgram.keyboardfunc(self, c, x, y)
            self.refresh()
Exemple #28
0
class MyGLViewer(GLWidgetPlugin):
    def __init__(self, world):
        GLWidgetPlugin.__init__(self)
        self.world = world
        self.robot = world.robot(0)
        self.subrobots = []
        for i in range(6):
            self.subrobots.append(
                SubRobotModel(self.robot, list(range(6 + i * 6, 12 + i * 6))))
        (res, val) = editors.run(
            editors.SelectionEditor("subrobot",
                                    self.subrobots[0]._links,
                                    description="sub robot links",
                                    world=world,
                                    robot=self.robot))
        if res:
            print("Return value", val)
            self.subrobots[0]._links = val
        self.startConfig = self.robot.getConfig()
        self.robotWidget = RobotPoser(world.robot(0))
        self.robotWidget.setActiveDofs(self.subrobots[0]._links)
        self.addWidget(self.robotWidget)

    def keyboardfunc(self, c, x, y):
        #Put your keyboard handler here
        #the current example prints the config when [space] is pressed
        if c == 'h' or c == '?':
            print("Controls:")
            print("- [space]: print the widget's sub-robot configuration")
            print("- r: randomize the sub-robot configuration")
            print("- p: plan to the widget's sub-robot configuration")
            print("- i: test the IK functions")
        elif c == ' ':
            config = self.robotWidget.get()
            subconfig = self.subrobots[0].fromfull(config)
            print("Config:", subconfig)
            self.subrobots[0].setConfig(subconfig)
        elif c == 'r':
            self.subrobots[0].randomizeConfig()
            self.robotWidget.set(self.robot.getConfig())
        elif c == 'p':
            config = self.robotWidget.get()
            subconfig = self.subrobots[0].fromfull(config)
            self.subrobots[0].setConfig(self.subrobots[0].fromfull(
                self.startConfig))
            settings = {
                'type': "sbl",
                'perturbationRadius': 0.5,
                'bidirectional': 1,
                'shortcut': 1,
                'restart': 1,
                'restartTermCond': "{foundSolution:1,maxIters:1000}"
            }
            plan = robotplanning.planToConfig(self.world,
                                              self.subrobots[0],
                                              subconfig,
                                              movingSubset='all',
                                              **settings)
            plan.planMore(1000)
            print(plan.getPath())
        elif c == 'i':
            link = self.subrobots[0].link(self.subrobots[0].numLinks() - 1)
            print("IK solve for ee to go 10cm upward...")
            obj = ik.objective(link,
                               local=[0, 0, 0],
                               world=vectorops.add(
                                   link.getWorldPosition([0, 0, 0]),
                                   [0, 0, 0.1]))
            solver = ik.solver(obj)
            res = solver.solve()
            print("  result", res)
            print("  residual", solver.getResidual())
            print(self.robotWidget.set(self.robot.getConfig()))
        else:
            GLWidgetPlugin.keyboardfunc(self, c, x, y)
Exemple #29
0
 def __init__(self, world):
     GLWidgetProgram.__init__(self, world, "My GL program")
     self.poseWidget = PointPoser()
     self.widgetMaster.add(self.poseWidget)
     self.robotWidget = RobotPoser(world.robot(0))
     self.widgetMaster.add(self.robotWidget)
class MyGLViewer(GLWidgetProgram):
    def __init__(self, world):
        GLWidgetProgram.__init__(self, "Manual poser")

        #start up the poser in the currently commanded configuration
        q = motion.robot.getKlamptCommandedPosition()
        world.robot(0).setConfig(q)

        self.world = world
        self.robot = world.robot(0)
        self.robotPoser = RobotPoser(world.robot(0))
        self.addWidget(self.robotPoser)
        self.roadMap = ([], [])

        robot = world.robot(0)
        left_arm_link_names = [
            'left_upper_shoulder', 'left_lower_shoulder', 'left_upper_elbow',
            'left_lower_elbow', 'left_upper_forearm', 'left_lower_forearm',
            'left_wrist'
        ]
        right_arm_link_names = [
            'right_upper_shoulder', 'right_lower_shoulder',
            'right_upper_elbow', 'right_lower_elbow', 'right_upper_forearm',
            'right_lower_forearm', 'right_wrist'
        ]
        self.left_arm_link_names = left_arm_link_names
        self.right_arm_link_names = right_arm_link_names
        missing_left_arm_names = [
            'left_upper_forearm_visual', 'left_upper_elbow_visual'
        ]
        missing_right_arm_names = [
            'right_upper_forearm_visual', 'right_upper_elbow_visual'
        ]
        #self.left_arm_link_names+=missing_left_arm_names
        #self.right_arm_link_names+=missing_right_arm_names
        self.left_arm_link_names = [
            'left_upper_forearm', 'left_lower_forearm', 'left_wrist',
            'left_upper_forearm_visual', 'left_gripper:base'
        ]
        self.right_arm_link_names = [
            'right_upper_forearm', 'right_lower_forearm', 'right_wrist',
            'right_upper_forearm_visual', 'right_gripper:base'
        ]
        self.left_arm_link_indices = [
            robot.link(l).index for l in left_arm_link_names
        ]
        self.right_arm_link_indices = [
            robot.link(l).index for l in right_arm_link_names
        ]
        self.firstTimeHack = True
        self.world.makeTerrain('terrain')

        self.subscribe()
        self.newPC = None

        self.collider = WorldCollider(self.world)

    def display(self):
        #Put your display handler here
        #the current example draws the sensed robot in grey and the
        #commanded configurations in transparent green

        #this line with draw the world
        robot = motion.robot
        q = robot.getKlamptSensedPosition()
        self.world.robot(0).setConfig(q)
        self.world.robot(0).drawGL()
        self.world.terrain(0).drawGL()
        GLWidgetProgram.display(self)

        #draw commanded configuration
        glEnable(GL_BLEND)
        glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
        glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, [0, 1, 0, 0.5])
        q = robot.getKlamptCommandedPosition()
        self.world.robot(0).setConfig(q)
        self.world.robot(0).drawGL(False)
        glDisable(GL_BLEND)
        V, E = self.roadMap
        positions = []
        glDisable(GL_LIGHTING)
        glColor3f(0, 1, 0)
        glLineWidth(5.0)
        glBegin(GL_LINE_STRIP)
        #print len(V)
        #P = []
        #for j in range(len(V)/2):
        #    P.append(V[j])
        #for v in V:
        #    self.robot.setConfig(v)
        #    positions.append(self.robot.link("left_gripper:base").getTransform()[1])
        #print "endEffector:", self.robot.getLink("left_gripper:base").getTransform()[1]
        #    glVertex3f(*self.robot.getLink("left_gripper:base").getTransform()[1])
        #for (i,j) in E:
        #    glVertex3f(*positions[i])
        #    glVertex3f(*positions[j])
        #glEnd()
        #glEnable(GL_LIGHTING)

    def keyboardfunc(self, c, x, y):
        #Put your keyboard handler here
        #the current example toggles simulation / movie mode
        if c == 'h':
            print '[space]: send the current posed milestone'
            print 'q: clean quit'
        elif c == ' ':
            q = self.robotPoser.get()
            #print "space q:", q
            q0 = motion.robot.getKlamptCommandedPosition()
            #print q0
            #collider = WorldCollider(self.world)

            collisions = obstaclecollision(WorldCollider(self.world),
                                           self.world.robot(0), q0, q)

            for i in range(self.world.robot(0).numLinks()):
                self.world.robot(0).link(i).appearance().setColor(
                    0.5, 0.5, 0.5, 1)
            if not self.firstTimeHack and selfcollision(
                    self.world.robot(0), q0, q):
                print "Invalid configuration, it self-collides"

            elif not self.firstTimeHack and collisions != None:
                #clear all links to gray
                for pairs in collisions:
                    print "Link " + str(
                        pairs[0].getIndex()) + " collides with obstacle"
                    self.world.robot(0).link(
                        pairs[0].getIndex()).appearance().setColor(1, 0, 0, 1)
            else:
                self.firstTimeHack = False
                robot = motion.robot
                robot.left_mq.setRamp(robot.left_limb.configFromKlampt(q))
                robot.right_mq.setRamp(robot.right_limb.configFromKlampt(q))
                print
                print
                print "Moving", q0, "->", q
                print
                print
                qlg = robot.left_gripper.configFromKlampt(q)
                qrg = robot.right_gripper.configFromKlampt(q)
                print "space prg:", qrg
                robot.left_gripper.command(qlg, [1] * len(qlg), [1] * len(qlg))
                robot.right_gripper.command(qrg, [1] * len(qrg),
                                            [1] * len(qrg))
                #robot.left_mq.setRamp([q[i] for i in self.left_arm_link_indices])
                #robot.right_mq.setRamp([q[i] for i in self.right_arm_link_indices])
        elif c == 'q':
            motion.robot.shutdown()
            exit(0)
        elif c == 'p':
            """given a target object position, automatically moves the end effector close to the target object"""
            #print "Joint limits:", self.robot.getJointLimits()
            start = self.world.robot(0).getConfig()
            #print "end effector original position:", self.robot.link('left_gripper:base').getWorldPosition((0,0,0))
            #print "start", start
            """target position currently fixed at (0.8,0.1,1) for testing"""
            target = self.getClosestConfig(self.world.robot(0), (0.8, 0.1, 1),
                                           100, 0.1, 100)
            if target == None:
                #    print "Cannot solve IK"
                return
            self.robot.setConfig(target)
            #print "ik result:", target
            #print "end effector position:", self.robot.link('left_gripper:base').getWorldPosition((0,0,0))
            path = self.getCollisionFreePath(start, target, 10)
            #print "path:", path

            for q in path:
                robot = motion.robot
                robot.left_mq.setRamp(robot.left_limb.configFromKlampt(q))
                robot.right_mq.setRamp(robot.right_limb.configFromKlampt(q))

                qlg = robot.left_gripper.configFromKlampt(q)

                qrg = robot.right_gripper.configFromKlampt(q)

                if qlg:
                    robot.left_gripper.command(qlg, [1] * len(qlg),
                                               [1] * len(qlg))
                if qrg:

                    robot.right_gripper.command(qrg, [1] * len(qrg),
                                                [1] * len(qrg))
            print "getKlamptCommandedPosition:", robot.getKlamptCommandedPosition(
            )

        else:
            GLWidgetProgram.keyboardfunc(self, c, x, y)
            self.refresh()

    def subscribe(self):
        rospy.init_node('subscribe', anonymous=True)
        rospy.Subscriber("/kinect2/sd/points", PointCloud2, self.callback)

    def callback(self, data):
        if self.newPC == None:
            data_out = pc2.read_points(data, skip_nans=False)
            pc = PointCloud()
            pc.propertyNames.append('rgba')
            t0 = time.time()
            points = list(data_out)
            """ needs better external calibration"""
            q = (0.564775, 0.425383, 0.426796, 0.563848)
            R1 = so3.from_quaternion(q)
            t1 = (0.283078, 0.043398, 1.01 - 0.00182539)
            (Rinv, tinv) = se3.inv((R1, t1))
            denoised = self.voxelgridfilter(points, 0.1, 0.1, 0.1, 50)
            print "Noise filtered in time", time.time() - t0
            filtered = denoised
            print "Robot arms filtered in time", time.time() - t0
            counting = 0
            for p in filtered:
                if p == None:
                    counting = counting + 1
                    continue
                coordinates = (p[0], p[1], p[2])
                index = pc.addPoint(coordinates)
                i = struct.unpack("i", struct.pack("f", p[3]))[0]
                pc.setProperty(index, 0, i)

            print "^^^^^^^^", counting
            print "Point cloud copied to Klampt format in time", time.time(
            ) - t0

            pc.transform(R1, t1)
            print "Point cloud transformed in time", time.time() - t0
            self.newPC = pc
        else:
            print "Not ready yet to receive new point cloud..."

    # def statisticalfilter(self,points,k,mul):
    #         pclCloud = pcl.PointCloud()
    #         pclCloud.from_list(points)
    #         fil = pclCloud.make_statistical_outlier_filter()
    #         fil.set_mean_k(k)

    #         fil.set_std_dev_mul_thresh(mul)
    #         filtered = fil.filter().to_list()
    #         return filtered

    # def radiusfilter(self,points,radius,num):
    #     filtered = []
    #     coordinates = []
    #     for p in points:
    #         coordinates.append([p[0],p[1],p[2]])

    #     tree = KDTree(np.array(coordinates))

    #     for i,p in enumerate(coordinates):

    #         if tree.query_radius(coordinates[i],r=radius,count_only=True)>=num:
    #             filtered.append(points[i])
    #     return points

    def voxelgridfilter(self, points, gridX, gridY, gridZ, num):
        """ a simple voxelgrid filter to get rid of noise in point cloud data"""
        dictionary = {}
        for i, p in enumerate(points):
            x = p[0]
            y = p[1]
            z = p[2]

            if math.isnan(x) or vectorops.norm((x, y, z)) < 0.31:
                points[i] = None
                continue
            xindex = int(math.floor(x / gridX))
            yindex = int(math.floor(y / gridY))
            zindex = int(math.floor(z / gridZ))
            key = (xindex, yindex, zindex)
            if key not in dictionary:
                dictionary[key] = []
            dictionary[key].append(i)
        for key, value in dictionary.iteritems():
            if len(value) < num:
                for j in value:
                    points[j] = None
        return points

    def robotSelfFilter(self, points, Rinv, tinv):
        """ Get rid of points that belong to the robot's arms"""
        robot = self.world.robot(0)
        arm_link_names = self.left_arm_link_names + self.right_arm_link_names
        bbMargin = 0.1
        #upperThreshold = 0.1 # Need to tune
        #lowerThreshold = -0.1 # Need to tune
        armBBs = []
        transformedG = []
        maxArmDepth = 0
        for i in arm_link_names:
            currentT = robot.link(i).getTransform()
            (newR, newt) = se3.mul((Rinv, tinv), currentT)
            g = robot.link(i).geometry()
            g.setCurrentTransform(newR, newt)
            transformedG.append(g)
            myBB = g.getBB()
            if myBB[1][2] > maxArmDepth:
                maxArmDepth = myBB[1][2]
            expandedBB = ((myBB[0][0] - bbMargin, myBB[0][1] - bbMargin,
                           myBB[0][2] - bbMargin),
                          (myBB[1][0] + bbMargin, myBB[1][1] + bbMargin,
                           myBB[1][2] + bbMargin))
            armBBs.append(expandedBB)
        allIndices = self.indicesOfInterest(armBBs)
        count = 0
        self.count2 = 0
        for j, indices in enumerate(allIndices):
            if indices == None:
                continue
            for i in indices:

                p = points[i]
                if p == None:
                    continue
                x = p[0]
                y = p[1]
                z = p[2]

                if vectorops.norm((x, y, z)) > maxArmDepth + 0.3:
                    continue
                points[i] = None
                continue
                #point = (x,y,z)
                #if(z<1.2) and self.insideGeometry(point,transformedG[j],upperThreshold,lowerThreshold):
                #    count = count + 1
                #    points[i] = None
        return points

    # def insideGeometryBB(self, point, bb, bbMargin):

    #     mins = vectorops.sub(bb[0],bbMargin)
    #     maxes = vectorops.sub(bb[1], -bbMargin)

    #     insideX = point[0]>=mins[0] and point[0]<=maxes[0]
    #     insideY = point[1]>=mins[1] and point[1]<=maxes[1]
    #     insideZ = point[2]>=mins[2] and point[2]<=maxes[2]

    #     return insideX and insideY and insideZ

    # def insideGeometry(self, point, g, upperThreshold, lowerThreshold):

    #     direction = vectorops.unit(point)

    #     (hit, pt) = g.rayCast((0,0,0), direction)
    #     depthPoint = vectorops.norm(point)
    #     depthHit = vectorops.norm(pt)
    #     diff = depthPoint - depthHit

    #     if hit:

    #         self.count2 = self.count2 + 1
    #         if diff < upperThreshold and diff > lowerThreshold: # If point lies inside robot body
    #             return True
    #     return False

    def indicesOfInterest(self, armBBs):
        """ Get indices of point cloud data that belong to robot's arms"""
        allIndices = []
        for i, bb in enumerate(armBBs):

            xs = (bb[0][0], bb[1][0])
            ys = (bb[0][1], bb[1][1])
            zs = (bb[0][2], bb[1][2])
            allIndices.append(self.indicesOfSingleBB(xs, ys, zs))

        return allIndices

    def indicesOfSingleBB(self, xs, ys, zs):
        """ Get indices of point cloud data that belong to the bounding box of a single arm link"""
        indices = []
        xminAngle = 180
        xmaxAngle = -180
        yminAngle = 180
        ymaxAngle = -180
        for x in xs:
            for z in zs:
                xangle = math.degrees(math.atan2(x, z)) + 35.3
                if xangle < xminAngle:
                    xminAngle = xangle
                if xangle > xmaxAngle:
                    xmaxAngle = xangle

        if xminAngle > 70.6 or xmaxAngle < 0:

            return None
        if xminAngle < 0:
            xminAngle = 0
        if xmaxAngle > 70.6:
            xmaxAngle = 70.6

        for y in ys:
            for z in zs:
                yangle = math.degrees(math.atan2(y, z)) + 30
                if yangle < yminAngle:
                    yminAngle = yangle
                if yangle > ymaxAngle:
                    ymaxAngle = yangle

        if yminAngle > 60 or ymaxAngle < 0:

            return None
        if yminAngle < 0:
            yminAngle = 0
        if ymaxAngle > 60:
            ymaxAngle = 60

        xminPixel = int(math.floor(512 * xminAngle / 70.6))
        xmaxPixel = int(math.floor(512 * xmaxAngle / 70.6))
        yminPixel = int(math.floor(424 * yminAngle / 60))
        ymaxPixel = int(math.floor(424 * ymaxAngle / 60))

        for i in range(yminPixel, ymaxPixel):
            for j in range(xminPixel, xmaxPixel):
                indices.append(512 * i + j)

        return indices

    def idle(self):
        if self.newPC != None:
            print "Setting the terrain geometry"
            self.world.terrain(0).geometry().setPointCloud(self.newPC)
            print "Setting the collision margin"
            self.world.terrain(0).geometry().setCollisionMargin(0.01)
            self.newPC = None
            print "Done"
            self.refresh()

    def getCollisionFreePath(self, start, goal, iterations):
        """ Given a start and a goal configuration, returns a collision-free path between the two configurations"""
        """ Currently takes forever to find a path... Needs more work"""
        #MotionPlan.setOptions(type="rrt", perturbationRadius=2.0, bidirectional=True)
        #MotionPlan.setOptions(type="prm", knn=10, connectionThreshold=0.25)
        MotionPlan.setOptions(type="sbl",
                              perturbationRadius=2.0,
                              connectionThreshold=0.5,
                              bidirectional=True)
        #MotionPlan.setOptions(type="lazyrrg*")
        #space = ObstacleCSpace(self.collider, self.robot)
        #planner = MotionPlan(space)

        #planner = robotplanning.planToConfig(self.world, self.robot, goal, type="prm", knn=10, connectionThreshold=0.1)

        space = robotcspace.RobotCSpace(self.robot, WorldCollider(self.world))
        jointLimits = self.robot.getJointLimits()
        lower = jointLimits[0]
        higher = jointLimits[1]
        for i in range(12):
            lower[i] = 0
            higher[i] = 0
        newLimits = (lower, higher)
        space.bound = zip(*newLimits)

        planner = cspace.MotionPlan(space)

        planner.setEndpoints(start, goal)

        planner.planMore(iterations)

        V, E = planner.getRoadmap()
        self.roadMap = (V, E)

        return planner.getPath()

    def perturb(self, q, radius):
        newq = []
        for i in q:
            newq.append(i + random.uniform(-radius, radius))
        return newq

    def getClosestConfig(self, robot, target, iterations, c, numsteps):
        """ given a target object position, returns a configuration with end effector close to the target object but without colliding with it"""
        cost = 9999
        res = None
        start = robot.getConfig()

        s = IKSolver(robot)
        objective = IKObjective()
        objective.setFixedPoint(
            robot.link("left_gripper:base").getIndex(), (0, 0, 0), target)
        s.add(objective)
        s.setActiveDofs([12, 13, 14, 15, 16, 18, 19])

        for i in range(iterations):
            (ret, iters) = s.solve(100, 1e-4)
            if ret:
                end = robot.getConfig()
                qmin, qmax = robot.getJointLimits()
                flag = False
                for k in range(len(end)):
                    if end[k] < qmin[k] or end[k] > qmax[k]:
                        flag = True
                        break
                if flag:
                    start = self.perturb(start, 0.1)
                    robot.setConfig(start)
                    continue
                for j in xrange(numsteps + 1):
                    u = float(j) / numsteps
                    q = robot.interpolate(end, start, u)
                    if not inCollision(WorldCollider(self.world), robot, q):
                        newcost = vectorops.distance(
                            q, end) + c * vectorops.distance(q, start)
                        if newcost < cost:
                            res = q
                            cost = newcost
                        break
            start = self.perturb(start, 0.1)
            robot.setConfig(start)

        return res
class MyGLViewer(GLWidgetProgram):
    def __init__(self,world):
        GLWidgetProgram.__init__(self,world,"Manual poser")
        self.robotPoser = RobotPoser(world.robot(0))
        self.widgetMaster.add(self.robotPoser)
        motion.robot.left_limb.enableSelfCollisionAvoidance(False)
        motion.robot.right_limb.enableSelfCollisionAvoidance(False)
        robot = world.robot(0)
        left_arm_link_names = ['left_upper_shoulder','left_lower_shoulder','left_upper_elbow','left_lower_elbow','left_upper_forearm','left_lower_forearm','left_wrist']
        right_arm_link_names = ['right_upper_shoulder','right_lower_shoulder','right_upper_elbow','right_lower_elbow','right_upper_forearm','right_lower_forearm','right_wrist']
        self.left_arm_link_indices = [robot.getLink(l).index for l in left_arm_link_names]
        self.right_arm_link_indices = [robot.getLink(l).index for l in right_arm_link_names]
        self.outputFileLeft = open("sag_calibration_left.csv","w")
        self.outputFileRight = open("sag_calibration_right.csv","w")
        #write headers
        self.outputFileLeft.write("time,")
        self.outputFileLeft.write(",".join("qsns"+str(i) for i in range(1,8)))
        self.outputFileLeft.write(",")
        self.outputFileLeft.write(",".join("qcmd"+str(i) for i in range(1,8)))
        self.outputFileLeft.write("\n")
        self.outputFileRight.write("time,")
        self.outputFileRight.write(",".join("qsns"+str(i) for i in range(1,8)))
        self.outputFileRight.write(",")
        self.outputFileRight.write(",".join("qcmd"+str(i) for i in range(1,8)))
        self.outputFileRight.write("\n")
        self.t0 = time.time()
        self.lastLeftMove = -100
        self.lastRightMove = -100 

    def display(self):
        #Put your display handler here
        #the current example draws the sensed robot in grey and the
        #commanded configurations in transparent green

        #this line with draw the world
        robot = motion.robot
        q = robot.getKlamptSensedPosition()
        self.world.robot(0).setConfig(q)
        self.world.robot(0).drawGL()
        GLWidgetProgram.display(self)

        #draw commanded configuration
        glEnable(GL_BLEND)
        glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA)
        glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[0,1,0,0.5])
        q = robot.getKlamptCommandedPosition()
        self.world.robot(0).setConfig(q)
        self.world.robot(0).drawGL(False)
        glDisable(GL_BLEND)

    def idle(self):
        GLWidgetProgram.idle(self)
        t = time.time()-self.t0
        if motion.robot.left_mq.moving():
            self.lastLeftMove = t
        if motion.robot.right_mq.moving():
            self.lastRightMove = t
        if t < self.lastLeftMove + 3:
            print "Saving left",t
            qsensed = motion.robot.left_limb.sensedPosition()
            qcmd = motion.robot.left_limb.commandedPosition()
            self.outputFileLeft.write(str(t)+",")
            self.outputFileLeft.write(','.join(str(v) for v in qsensed+qcmd))
            self.outputFileLeft.write('\n')
        if t < self.lastRightMove + 3:
            print "Saving right",t
            qsensed = motion.robot.right_limb.sensedPosition()
            qcmd = motion.robot.right_limb.commandedPosition()
            self.outputFileRight.write(str(t)+",")
            self.outputFileRight.write(','.join(str(v) for v in qsensed+qcmd))
            self.outputFileRight.write('\n')

    def keyboardfunc(self,c,x,y):
        #Put your keyboard handler here
        #the current example toggles simulation / movie mode
        if c == 'h':
            print '[space]: send the current posed milestone'
            print 'q: clean quit'
        elif c == ' ':
            q = self.robotPoser.get()
            robot = motion.robot
            robot.left_mq.setRamp(robot.left_limb.configFromKlampt(q))
            robot.right_mq.setRamp(robot.right_limb.configFromKlampt(q))
            qlg = robot.left_gripper.configFromKlampt(q)
            qrg = robot.right_gripper.configFromKlampt(q)
            robot.left_gripper.command(qlg,[1]*len(qlg),[1]*len(qlg))
            robot.right_gripper.command(qrg,[1]*len(qrg),[1]*len(qrg))
            #robot.left_mq.setRamp([q[i] for i in self.left_arm_link_indices])
            #robot.right_mq.setRamp([q[i] for i in self.right_arm_link_indices])
        elif c == 'q':
            motion.robot.shutdown()
            exit(0)
        else:
            GLWidgetProgram.keyboardfunc(self,c,x,y)
            self.refresh()
Exemple #32
0
class MyGLViewer(GLWidgetProgram):
    def __init__(self, world):
        GLWidgetProgram.__init__(self, "Manual poser")

        #start up the poser in the currently commanded configuration
        q = motion.robot.getKlamptCommandedPosition()
        world.robot(0).setConfig(q)

        self.world = world
        self.robot = world.robot(0)
        self.robotPoser = RobotPoser(world.robot(0))
        self.addWidget(self.robotPoser)
        self.roadMap = ([], [])

        robot = world.robot(0)
        left_arm_link_names = [
            'left_upper_shoulder', 'left_lower_shoulder', 'left_upper_elbow',
            'left_lower_elbow', 'left_upper_forearm', 'left_lower_forearm',
            'left_wrist'
        ]
        right_arm_link_names = [
            'right_upper_shoulder', 'right_lower_shoulder',
            'right_upper_elbow', 'right_lower_elbow', 'right_upper_forearm',
            'right_lower_forearm', 'right_wrist'
        ]
        self.left_arm_link_names = left_arm_link_names
        self.right_arm_link_names = right_arm_link_names
        missing_left_arm_names = [
            'left_upper_forearm_visual', 'left_upper_elbow_visual'
        ]
        missing_right_arm_names = [
            'right_upper_forearm_visual', 'right_upper_elbow_visual'
        ]
        #self.left_arm_link_names+=missing_left_arm_names
        #self.right_arm_link_names+=missing_right_arm_names
        self.left_arm_link_names = [
            'left_upper_forearm', 'left_lower_forearm', 'left_wrist',
            'left_upper_forearm_visual', 'left_gripper:base'
        ]
        self.right_arm_link_names = [
            'right_upper_forearm', 'right_lower_forearm', 'right_wrist',
            'right_upper_forearm_visual', 'right_gripper:base'
        ]
        self.left_arm_link_indices = [
            robot.link(l).index for l in left_arm_link_names
        ]
        self.right_arm_link_indices = [
            robot.link(l).index for l in right_arm_link_names
        ]
        self.firstTimeHack = True
        self.world.makeTerrain('terrain')

        self.subscribe()
        self.newPC = None

        self.collider = WorldCollider(self.world)

    def display(self):
        #Put your display handler here
        #the current example draws the sensed robot in grey and the
        #commanded configurations in transparent green

        #this line with draw the world
        robot = motion.robot
        q = robot.getKlamptSensedPosition()
        self.world.robot(0).setConfig(q)
        self.world.robot(0).drawGL()
        self.world.terrain(0).drawGL()
        GLWidgetProgram.display(self)

        #draw commanded configuration
        glEnable(GL_BLEND)
        glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
        glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, [0, 1, 0, 0.5])
        q = robot.getKlamptCommandedPosition()
        self.world.robot(0).setConfig(q)
        self.world.robot(0).drawGL(False)
        glDisable(GL_BLEND)
        V, E = self.roadMap
        positions = []
        glDisable(GL_LIGHTING)
        glColor3f(0, 1, 0)
        glLineWidth(5.0)
        glBegin(GL_LINE_STRIP)
        #print len(V)
        #P = []
        #for j in range(len(V)/2):
        #    P.append(V[j])
        for v in V:
            self.robot.setConfig(v)
            #    positions.append(self.robot.link("left_gripper:base").getTransform()[1])
            #print "endEffector:", self.robot.getLink("left_gripper:base").getTransform()[1]
            glVertex3f(
                *self.robot.getLink("left_gripper:base").getTransform()[1])
        #for (i,j) in E:
        #    glVertex3f(*positions[i])
        #    glVertex3f(*positions[j])
        glEnd()
        glEnable(GL_LIGHTING)

    def keyboardfunc(self, c, x, y):
        #Put your keyboard handler here
        #the current example toggles simulation / movie mode
        if c == 'h':
            print '[space]: send the current posed milestone'
            print 'q: clean quit'
        elif c == ' ':
            q = self.robotPoser.get()
            #print "space q:", q
            q0 = motion.robot.getKlamptCommandedPosition()
            #print q0
            #collider = WorldCollider(self.world)

            collisions = obstaclecollision(WorldCollider(self.world),
                                           self.world.robot(0), q0, q)

            for i in range(self.world.robot(0).numLinks()):
                self.world.robot(0).link(i).appearance().setColor(
                    0.5, 0.5, 0.5, 1)
            if not self.firstTimeHack and selfcollision(
                    self.world.robot(0), q0, q):
                print "Invalid configuration, it self-collides"

            elif not self.firstTimeHack and collisions != None:
                #clear all links to gray
                for pairs in collisions:
                    print "Link " + str(
                        pairs[0].getIndex()) + " collides with obstacle"
                    self.world.robot(0).link(
                        pairs[0].getIndex()).appearance().setColor(1, 0, 0, 1)
            else:
                self.firstTimeHack = False
                robot = motion.robot
                robot.left_mq.setRamp(robot.left_limb.configFromKlampt(q))
                robot.right_mq.setRamp(robot.right_limb.configFromKlampt(q))
                print
                print
                print "Moving", q0, "->", q
                print
                print
                qlg = robot.left_gripper.configFromKlampt(q)
                qrg = robot.right_gripper.configFromKlampt(q)
                print "space prg:", qrg
                robot.left_gripper.command(qlg, [1] * len(qlg), [1] * len(qlg))
                robot.right_gripper.command(qrg, [1] * len(qrg),
                                            [1] * len(qrg))
                #robot.left_mq.setRamp([q[i] for i in self.left_arm_link_indices])
                #robot.right_mq.setRamp([q[i] for i in self.right_arm_link_indices])
        elif c == 'q':
            motion.robot.shutdown()
            exit(0)
        elif c == 'p':
            print "Joint limits:", self.robot.getJointLimits()
            start = self.world.robot(0).getConfig()
            print "end effector original position:", self.robot.link(
                'left_gripper:base').getWorldPosition((0, 0, 0))
            print "start", start
            target = self.getClosestConfig(self.world.robot(0), (0.8, 0.1, 1),
                                           100, 0.1, 100)
            if target == None:
                print "Cannot solve IK"
                return
            self.robot.setConfig(target)
            print "ik result:", target
            print "end effector position:", self.robot.link(
                'left_gripper:base').getWorldPosition((0, 0, 0))
            path = self.getCollisionFreePath(start, target, 10)
            print "path:", path
            #q = path[1]
            #robot = motion.robot
            #robot.left_mq.setRamp(robot.left_limb.configFromKlampt(q))
            #robot.right_mq.setRamp(robot.right_limb.configFromKlampt(q))
            #     #print
            #     #print
            #     #print "Moving",q0,"->",q
            #     #print
            #     #print
            #qlg = robot.left_gripper.configFromKlampt(q)
            #     #print "q is:", q
            #qrg = robot.right_gripper.configFromKlampt(q)
            #     #print qrg
            #if qlg:
            #    robot.left_gripper.command(qlg,[1]*len(qlg),[1]*len(qlg))
            #if qrg:

            #    robot.right_gripper.command(qrg,[1]*len(qrg),[1]*len(qrg))

            for q in path:
                robot = motion.robot
                robot.left_mq.setRamp(robot.left_limb.configFromKlampt(q))
                robot.right_mq.setRamp(robot.right_limb.configFromKlampt(q))
                #print
                #print
                #print "Moving",q0,"->",q
                #print
                #print
                qlg = robot.left_gripper.configFromKlampt(q)
                #print "q is:", q
                qrg = robot.right_gripper.configFromKlampt(q)
                #print qrg
                if qlg:
                    robot.left_gripper.command(qlg, [1] * len(qlg),
                                               [1] * len(qlg))
                if qrg:

                    robot.right_gripper.command(qrg, [1] * len(qrg),
                                                [1] * len(qrg))
            print "getKlamptCommandedPosition:", robot.getKlamptCommandedPosition(
            )

        else:
            GLWidgetProgram.keyboardfunc(self, c, x, y)
            self.refresh()

    def subscribe(self):
        rospy.init_node('subscribe', anonymous=True)
        rospy.Subscriber("/kinect2/sd/points", PointCloud2, self.callback)

    def callback(self, data):
        if self.newPC == None:
            data_out = pc2.read_points(data, skip_nans=False)
            #data_out = pc2.read_points(data)
            #print data.width, data.height
            pc = PointCloud()
            pc.propertyNames.append('rgba')
            t0 = time.time()
            points = list(data_out)
            #print points
            #sys.exit(0)
            #print len(points)
            #print "converted to list in time", time.time()-t0
            q = (0.564775, 0.425383, 0.426796, 0.563848)
            R1 = so3.from_quaternion(q)
            ##t1 = (0.24539, 0.0893425, 1.00112145)
            #t1 = (0.24539, -0.05, 1.00112145)
            ####t1 = (0.273078, 0.0893398, 1.0-0.00182539)
            t1 = (0.283078, 0.043398, 1.01 - 0.00182539)
            #filtered = self.radiusfilter(points,0.1,0)
            (Rinv, tinv) = se3.inv((R1, t1))
            #for p in points:
            #    print p

            #tm = se3.apply((R1,t1), p)
            #t = se3.apply((Rinv, tinv), tm)
            #print t

            #self.xmin = 100
            #self.xmax = 0
            #self.ymin = 100
            #self.ymax = 0

            denoised = self.voxelgridfilter(points, 0.1, 0.1, 0.1, 50)
            print "Noise filtered in time", time.time() - t0
            #filtered = points
            filtered = denoised
            #print self.xmin, self.xmax, self.ymin, self.ymax

            #filtered = self.robotSelfFilter(denoised, Rinv, tinv)
            #print len(denoised)-len(filtered)
            print "Robot arms filtered in time", time.time() - t0
            #print len(filtered)

            counting = 0
            for p in filtered:
                if p == None:
                    counting = counting + 1
                    continue
                coordinates = (p[0], p[1], p[2])
                #if vectorops.norm(coordinates)>0.31:
                index = pc.addPoint(coordinates)
                i = struct.unpack("i", struct.pack("f", p[3]))[0]
                pc.setProperty(index, 0, i)

            print "^^^^^^^^", counting
            print "Point cloud copied to Klampt format in time", time.time(
            ) - t0

            #q = (0.566222,0.423455,0.424871,0.5653)
            #q = (0.564775, 0.425383, 0.426796, 0.563848)
            #R1 = so3.from_quaternion(q)
            #t1 = (0.228665,0.0591513,0.0977748)
            #t1 = (0.24539, 0.0893425, 0.00112145)
            #t1 = (0.24539, 0.0893425, 1.00112145)
            #T1 = (R1, t1)
            #R2 = so3.rotation((0,0,1),math.pi/2)
            #t2 = (0,0,1)
            #T2 = (R2, t2)
            #T = se3.mul(T2,T1)
            #(R,t)=T
            #pc.transform(R,t)
            pc.transform(R1, t1)
            print "Point cloud transformed in time", time.time() - t0
            self.newPC = pc

        else:

            print "Not ready yet to receive new point cloud..."

    # Color data is lost
    def statisticalfilter(self, points, k, mul):
        pclCloud = pcl.PointCloud()
        pclCloud.from_list(points)
        fil = pclCloud.make_statistical_outlier_filter()
        fil.set_mean_k(k)

        fil.set_std_dev_mul_thresh(mul)
        filtered = fil.filter().to_list()
        return filtered

    def radiusfilter(self, points, radius, num):
        filtered = []
        coordinates = []
        for p in points:
            coordinates.append([p[0], p[1], p[2]])
        #print np.array(coordinates).reshape(-len(coordinates),3)
        tree = KDTree(np.array(coordinates))
        #tree = KDTree(np.random.random((10, 3)), leaf_size=2)
        for i, p in enumerate(coordinates):
            #print tree.query_radius(p,r=radius,count_only=True)
            if tree.query_radius(coordinates[i], r=radius,
                                 count_only=True) >= num:
                filtered.append(points[i])
        return points

    def voxelgridfilter(self, points, gridX, gridY, gridZ, num):
        #filtered = []
        dictionary = {}
        for i, p in enumerate(points):

            x = p[0]
            y = p[1]
            z = p[2]

            #xangle = math.degrees(math.atan2(x,z))+35.3
            #yangle = math.degrees(math.atan2(y,z)) + 30
            #if xangle>self.xmax:
            # 	self.xmax = xangle
            #if xangle<self.xmin:
            #	self.xmin = xangle
            #if yangle>self.ymax:
            #	self.ymax = yangle
            #if yangle<self.ymin:
            #	self.ymin = yangle

            if math.isnan(x) or vectorops.norm((x, y, z)) < 0.31:
                points[i] = None
                continue
            xindex = int(math.floor(x / gridX))
            yindex = int(math.floor(y / gridY))
            zindex = int(math.floor(z / gridZ))
            key = (xindex, yindex, zindex)
            if key not in dictionary:
                dictionary[key] = []

            dictionary[key].append(i)
        for key, value in dictionary.iteritems():
            #print value
            if len(value) < num:
                for j in value:
                    points[j] = None
                    #print points[j]
        return points

    def robotSelfFilter(self, points, Rinv, tinv):
        #t1 = time.time()
        #filtered = []
        robot = self.world.robot(0)
        #camPos = (0.24539, 0.0893425, 0.00112145) # Not needed anymore in camera frame
        arm_link_names = self.left_arm_link_names + self.right_arm_link_names
        bbMargin = 0.1
        upperThreshold = 0.1  # Need to tune
        lowerThreshold = -0.1  # Need to tune
        armBBs = []
        transformedG = []
        #testPoint = (0.26839444608864566, -0.6676082722078356, 1.2409759988857147)
        #testPoint = (0.6351629925287235, -0.8301655827368236, 1.2909760000026105)
        #print se3.apply((Rinv, tinv), testPoint)
        maxArmDepth = 0
        for i in arm_link_names:
            #robot.link(i).appearance().setColor(0,0,1,1)
            currentT = robot.link(i).getTransform()
            #pt = se3.apply(currentT, (0,0,0.1))
            #print se3.apply((Rinv, tinv), pt)
            #print currentT
            (newR, newt) = se3.mul((Rinv, tinv), currentT)
            #print se3.apply((newR,newt), (0,0,0.1))
            g = robot.link(i).geometry()
            g.setCurrentTransform(newR, newt)
            #g.transform(Rinv, tinv)
            transformedG.append(g)
            myBB = g.getBB()
            if myBB[1][2] > maxArmDepth:
                maxArmDepth = myBB[1][2]
            expandedBB = ((myBB[0][0] - bbMargin, myBB[0][1] - bbMargin,
                           myBB[0][2] - bbMargin),
                          (myBB[1][0] + bbMargin, myBB[1][1] + bbMargin,
                           myBB[1][2] + bbMargin))
            armBBs.append(expandedBB)
        #robot.link(arm_link_names[10]).appearance().setColor(0,0,1,1)
        #print armBBs[10]
        #print "Arm geometry transformed in time: ",time.time()-t1
        allIndices = self.indicesOfInterest(armBBs)

        #for i, p in enumerate(points):
        #print points[15000]
        count = 0
        self.count2 = 0
        for j, indices in enumerate(allIndices):
            #print indices[len(indices)-1]

            if indices == None:
                continue
            #print len(indices)
            for i in indices:

                #print points[i][3]
                #if points[i] !=None:

                #	newpoint = (points[i][0], points[i][1], points[i][2], -1.7014118346e+38)

                #	points[i] = newpoint

                #continue

                p = points[i]
                if p == None:
                    continue
                x = p[0]
                y = p[1]
                z = p[2]

                if vectorops.norm((x, y, z)) > maxArmDepth + 0.3:
                    continue
                points[i] = None
                continue

                point = (x, y, z)
                #discard = False
                #for j,bb in enumerate(armBBs):
                #if(z<1.2):

                #g = robot.link(j).geometry()
                #g.transform(Rinv, tinv)    # geometry transformed to camera frame
                #if(self.insideGeometryBB(point,bb,bbMargin)):
                #    if(self.insideGeometry(point,transformedG[j],upperThreshold,lowerThreshold)):
                #        discard = True
                #break
                #pass
                #if discard:
                #    points[i] = None
                #print "**********************"
                #     count2 = count2+1
                if (z < 1.2) and self.insideGeometry(point, transformedG[j],
                                                     upperThreshold,
                                                     lowerThreshold):
                    #print "detected"
                    count = count + 1
                    points[i] = None
        print "detected: ", count
        print "hits: ", self.count2
        return points

    def insideGeometryBB(self, point, bb, bbMargin):
        #t1 = time.time()

        mins = vectorops.sub(bb[0], bbMargin)
        maxes = vectorops.sub(bb[1], -bbMargin)
        #print "bounding box checked first two lines in time: ",time.time()-t1
        insideX = point[0] >= mins[0] and point[0] <= maxes[0]
        insideY = point[1] >= mins[1] and point[1] <= maxes[1]
        insideZ = point[2] >= mins[2] and point[2] <= maxes[2]
        #print "bounding box checked in time: ",time.time()-t1
        #print maxes[0], maxes[1]
        #print "point:", point[0], point[1]
        return insideX and insideY and insideZ

    def insideGeometry(self, point, g, upperThreshold, lowerThreshold):
        #t1 = time.time()
        #print "using rayCast"
        direction = vectorops.unit(point)
        #print g.getBB()[0][0]
        (hit, pt) = g.rayCast((0, 0, 0), direction)
        depthPoint = vectorops.norm(point)
        depthHit = vectorops.norm(pt)
        diff = depthPoint - depthHit
        #print "rayCast done in: ",time.time()-t1
        #self.count2 = self.count2 + 1
        if hit:
            #print "hit!!!!!!"
            self.count2 = self.count2 + 1
            if diff < upperThreshold and diff > lowerThreshold:  # If point lies inside robot body
                return True
        return False

    def indicesOfInterest(self, armBBs):
        allIndices = []
        for i, bb in enumerate(armBBs):
            #if i<len(self.left_arm_link_names):
            #	print self.left_arm_link_names[i]
            xs = (bb[0][0], bb[1][0])
            ys = (bb[0][1], bb[1][1])
            zs = (bb[0][2], bb[1][2])
            allIndices.append(self.indicesOfSingleBB(xs, ys, zs))
        #print allIndices
        return allIndices

    def indicesOfSingleBB(self, xs, ys, zs):
        #print xs, ys, zs
        indices = []
        xminAngle = 180
        xmaxAngle = -180
        yminAngle = 180
        ymaxAngle = -180
        for x in xs:
            for z in zs:
                xangle = math.degrees(math.atan2(x, z)) + 35.3
                if xangle < xminAngle:
                    xminAngle = xangle
                if xangle > xmaxAngle:
                    xmaxAngle = xangle
        #print "*********x: ", xminAngle, xmaxAngle
        if xminAngle > 70.6 or xmaxAngle < 0:

            return None
        if xminAngle < 0:
            xminAngle = 0
        if xmaxAngle > 70.6:
            xmaxAngle = 70.6

        for y in ys:
            for z in zs:
                yangle = math.degrees(math.atan2(y, z)) + 30
                if yangle < yminAngle:
                    yminAngle = yangle
                if yangle > ymaxAngle:
                    ymaxAngle = yangle
        #print "*********y: ", yminAngle, ymaxAngle
        #print "*********y coordiantes: ", ys
        if yminAngle > 60 or ymaxAngle < 0:

            return None
        if yminAngle < 0:
            yminAngle = 0
        if ymaxAngle > 60:
            ymaxAngle = 60

        #print "*********",xminAngle, xmaxAngle, yminAngle, ymaxAngle
        #print "******* lalalala"
        xminPixel = int(math.floor(512 * xminAngle / 70.6))
        xmaxPixel = int(math.floor(512 * xmaxAngle / 70.6))
        yminPixel = int(math.floor(424 * yminAngle / 60))
        ymaxPixel = int(math.floor(424 * ymaxAngle / 60))
        #print "*********", xminPixel, xmaxPixel, yminPixel, ymaxPixel
        for i in range(yminPixel, ymaxPixel):
            for j in range(xminPixel, xmaxPixel):
                indices.append(512 * i + j)
        #print "******** indices: ", indices[len(indices)-1]
        return indices

    #def countNeighbors(self, points, radius):
    #    dictionary = defaultdict(int)
    #    for i in range(len(points)-1):
    #        for j in range(i+1,len(points)):
    #            pointA = points[i]
    #            pointB = points[j]
    #            if(vectorops.distance((pointA[0],pointA[1],pointA[2]),(pointB[0],pointB[1],pointB[2]))<radius):
    #                dictionary[i]+=1
    #                dictionary[j]+=1
    #    return dictionary

    def idle(self):
        if self.newPC != None:
            print "Setting the terrain geometry"
            self.world.terrain(0).geometry().setPointCloud(self.newPC)
            print "Setting the collision margin"
            self.world.terrain(0).geometry().setCollisionMargin(0.01)
            self.newPC = None
            print "Done"
            self.refresh()

    #def calibrate(self, point):
    #    q = (0.566222,0.423455,0.424871,0.5653)
    #    R = so3.from_quaternion(q)
    #    t = (0.228665,0.0591513,0.0977748)
    #    T = (R, t)
    #    newPoint = se3.apply(T,point)
    #    x = newPoint[0]
    #    y = newPoint[1]
    #    z = newPoint[2]
    #    return (-y,x,z)
    def getCollisionFreePath(self, start, goal, iterations):
        #MotionPlan.setOptions(type="rrt", perturbationRadius=2.0, bidirectional=True)
        #MotionPlan.setOptions(type="prm", knn=10, connectionThreshold=0.25)
        MotionPlan.setOptions(type="sbl",
                              perturbationRadius=2.0,
                              connectionThreshold=0.5,
                              bidirectional=True)
        #MotionPlan.setOptions(type="lazyrrg*")
        #space = ObstacleCSpace(self.collider, self.robot)
        #planner = MotionPlan(space)

        #planner = robotplanning.planToConfig(self.world, self.robot, goal, type="prm", knn=10, connectionThreshold=0.1)
        print "milestone 1"
        space = robotcspace.RobotCSpace(self.robot, WorldCollider(self.world))
        jointLimits = self.robot.getJointLimits()
        lower = jointLimits[0]
        higher = jointLimits[1]
        for i in range(12):
            lower[i] = 0
            higher[i] = 0
        newLimits = (lower, higher)
        space.bound = zip(*newLimits)
        print "milestone 2"
        planner = cspace.MotionPlan(space)
        print "milestone 3"
        planner.setEndpoints(start, goal)
        print "before planning"
        planner.planMore(iterations)
        print "after planning"
        V, E = planner.getRoadmap()
        self.roadMap = (V, E)
        print "No. of vertices:", len(V)
        print "Vertices:", V
        print "Edges:", E

        return planner.getPath()

    def perturb(self, q, radius):
        newq = []
        for i in q:
            newq.append(i + random.uniform(-radius, radius))
        return newq

    def getClosestConfig(self, robot, target, iterations, c, numsteps):
        cost = 9999
        res = None
        start = robot.getConfig()
        #goal = ik.objective(robot.link("left_gripper:base"), local = [(0,0,0)], world = [target])

        #goal = ik.objective(robot.link("left_wrist"), local = [(0,0,0)], world = [target])
        s = IKSolver(robot)
        objective = IKObjective()
        objective.setFixedPoint(
            robot.link("left_gripper:base").getIndex(), (0, 0, 0), target)
        s.add(objective)
        s.setActiveDofs([12, 13, 14, 15, 16, 18, 19])
        print "Active DOFs:", s.getActiveDofs()
        for i in range(iterations):
            #if ik.solve(goal):
            (ret, iters) = s.solve(100, 1e-4)
            if ret:
                end = robot.getConfig()
                print "*****************************", end
                qmin, qmax = robot.getJointLimits()
                #print qmin
                #print qmax
                flag = False
                for k in range(len(end)):
                    if end[k] < qmin[k] or end[k] > qmax[k]:

                        flag = True
                        break
                if flag:
                    start = self.perturb(start, 0.1)
                    robot.setConfig(start)
                    continue
                for j in xrange(numsteps + 1):
                    u = float(j) / numsteps
                    print "u is:", u
                    q = robot.interpolate(end, start, u)
                    #q = end
                    print "interpolated q is:", q
                    if not inCollision(WorldCollider(self.world), robot, q):
                        newcost = vectorops.distance(
                            q, end) + c * vectorops.distance(q, start)
                        if newcost < cost:
                            res = q
                            cost = newcost
                        break
            start = self.perturb(start, 0.1)
            robot.setConfig(start)
        print "res is:", res
        return res
Exemple #33
0
class MyGLViewer(GLWidgetProgram):
    def __init__(self,world):
        GLWidgetProgram.__init__(self,"Manual poser")

        self.world = world
        self.robotPoser = RobotPoser(world.robot(0))
        self.addWidget(self.robotPoser)

        robot = world.robot(0)
        left_arm_link_names = ['left_upper_shoulder','left_lower_shoulder','left_upper_elbow','left_lower_elbow','left_upper_forearm','left_lower_forearm','left_wrist']
        right_arm_link_names = ['right_upper_shoulder','right_lower_shoulder','right_upper_elbow','right_lower_elbow','right_upper_forearm','right_lower_forearm','right_wrist']
        self.left_arm_link_indices = [robot.link(l).index for l in left_arm_link_names]
        self.right_arm_link_indices = [robot.link(l).index for l in right_arm_link_names]
        self.firstTimeHack = True

    def display(self):
        #Put your display handler here
        #the current example draws the sensed robot in grey and the
        #commanded configurations in transparent green

        #this line with draw the world
        robot = motion.robot
        q = robot.getKlamptSensedPosition()
        self.world.robot(0).setConfig(q)
        self.world.robot(0).drawGL()
        GLWidgetProgram.display(self)

        #draw commanded configuration
        glEnable(GL_BLEND)
        glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA)
        glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[0,1,0,0.5])
        q = robot.getKlamptCommandedPosition()
        self.world.robot(0).setConfig(q)
        self.world.robot(0).drawGL(False)
        glDisable(GL_BLEND)

    def keyboardfunc(self,c,x,y):
        #Put your keyboard handler here
        #the current example toggles simulation / movie mode
        if c == 'h':
            print '[space]: send the current posed milestone'
            print 'q: clean quit'
        elif c == ' ':
            q = self.robotPoser.get()
            q0 = motion.robot.getKlamptCommandedPosition()
            if not self.firstTimeHack and selfcollision(self.world.robot(0),q0,q):
                print "Invalid configuration, it self-collides"
            else:
                self.firstTimeHack = False
                robot = motion.robot
                robot.left_mq.setRamp(robot.left_limb.configFromKlampt(q))
                robot.right_mq.setRamp(robot.right_limb.configFromKlampt(q))
                qlg = robot.left_gripper.configFromKlampt(q)
                qrg = robot.right_gripper.configFromKlampt(q)
                robot.left_gripper.command(qlg,[1]*len(qlg),[1]*len(qlg))
                robot.right_gripper.command(qrg,[1]*len(qrg),[1]*len(qrg))
                #robot.left_mq.setRamp([q[i] for i in self.left_arm_link_indices])
                #robot.right_mq.setRamp([q[i] for i in self.right_arm_link_indices])
        elif c == 'q':
            motion.robot.shutdown()
            exit(0)
        else:
            GLWidgetProgram.keyboardfunc(self,c,x,y)
            self.refresh()
class CustomGLViewer(GLRealtimeProgram):
    def __init__(self, simworld=None, planworld=None, controller=None, sim=None, helper=None):

        if simworld == None and planworld == None:
            # Dummy GLViewer
            print "simworld and planworld are None"
            return

        GLRealtimeProgram.__init__(self, "My GL program")

        self.simworld = simworld
        self.planworld = planworld
        self.sim = sim

        self.widgetMaster = WidgetSet()
        self.widgetButton = 2  # right-clicks
        self.draggingWidget = False

        self.poseWidget = PointPoser()
        self.widgetMaster.add(self.poseWidget)
        self.robotWidget = RobotPoser(self.simworld.robot(0))
        self.widgetMaster.add(self.robotWidget)

        self.simulate = True

        if self.sim == None:
            self.simulate = False
        self.controller = controller
        self.command_queue = Queue()

        if helper != None:
            print "helper is not none"
            helper.run()

        self.points = None

        self.extraControllers = {}

    def idle(self):

        if self.extraControllers != {}:
            remove_indices = []
            for controller in self.extraControllers:
                self.dt = 0.01
                controller.sim.simulate(self.dt)
                glutPostRedisplay()

                if controller.remainingTime() <= 0:
                    remove_indices.append(controller)

            for controller in remove_indices:
                del self.extraControllers[controller]
                print "controller removed"

        if self.simulate:
            self.dt = 0.01
            self.sim.simulate(self.dt)
            glutPostRedisplay()
        self.widgetMaster.idle()
        if self.widgetMaster.wantsRedraw():
            print "widgetMaster wants redraw"
            self.refresh()

    def printStuff(self):
        print "shelf xform:", knowledge.shelf_xform
        print self.simworld.terrain(0).geometry()
        # print "terrain xform:", self.simworld.terrain(0).geometry().getTransform(), "\n\n"

    def updatePoints(self, points):
        self.points = points

    def addController(self, new_controller, rgb):
        self.extraControllers[new_controller] = rgb

    def glShowPointCloud(self, pc, downsample_rate=5, pt_size=None):
        # print glGetFloatv(GL_CURRENT_COLOR)
        pc = np.array(pc)
        # print 'Rendering %d points'%pc.shape[0]
        try:
            d = pc.shape[1]
            assert d == 3 or d == 4, "Unrecognized point cloud shape: " + str(pc.shape)
            pc = pc[::downsample_rate, :]
            pc = pc.tolist()
            glDisable(GL_LIGHTING)
            glColor3f(0, 1, 0)
            if pt_size is not None:
                glPointSize(pt_size)
            glBegin(GL_POINTS)
            for p in pc:
                if d == 4:
                    r, g, b = pcl_float_to_rgb(p[3])
                    r /= 255.0
                    g /= 255.0
                    b /= 255.0
                    glColor3f(r, g, b)
                glVertex3f(p[0], p[1], p[2])
            glEnd()
            glEnable(GL_LIGHTING)
            glColor3f(1, 0, 0)
        except:
            # print 'Insufficient data'
            pass

    def updateBox(self, index, vertices):
        BOX_COORDS[index] = vertices

    def display(self):

        global BOX_COORDS

        glEnable(GL_BLEND)

        glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
        glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, [1, 0, 0, 0.5])
        # only 1 robot in this case, but still use for-loop for generality
        for i in xrange(self.simworld.numRobots()):
            r = self.controller.robotModel
            q = self.controller.getCommandedConfig()
            if self.widgetMaster.wantsRedraw():
                # restart sim - start it with the new robot config
                # if simulating
                if self.sim != None:

                    # put more checks on mouse function here
                    # might accidentally reset if you mouse over
                    q = self.robotWidget.get()
                    # print 'self.sim is not None'
                    self.controller.robotModel.setConfig(q)
                    self.simworld.robot(0).setConfig(q)
                    self.controller.setLinear(q, 0.01)
            else:
                self.robotWidget.set(q)

            # q = self.controller.getSensedConfig()
            r.setConfig(q)
            r.drawGL(False)
        glDisable(GL_BLEND)

        for controller in self.extraControllers:
            glEnable(GL_BLEND)
            glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
            glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, self.extraControllers[controller])
            r = controller.robotModel
            q = controller.getCommandedConfig()
            r.setConfig(q)
            r.drawGL(False)
            glDisable(GL_BLEND)

        for i in xrange(self.simworld.numTerrains()):
            self.simworld.terrain(i).drawGL()

        if self.points != None:
            self.glShowPointCloud(self.points)

        for i in BOX_COORDS:
            self.draw_wire_box(*BOX_COORDS[i])

        self.widgetMaster.drawGL(self.viewport())

    def draw_wire_box(self, bmin, bmax):
        """Helper: draws a wireframe box"""
        glBegin(GL_LINE_LOOP)
        glVertex3f(bmin[0], bmin[1], bmin[2])
        glVertex3f(bmin[0], bmin[1], bmax[2])
        glVertex3f(bmin[0], bmax[1], bmax[2])
        glVertex3f(bmin[0], bmax[1], bmin[2])
        glEnd()
        glBegin(GL_LINE_LOOP)
        glVertex3f(bmax[0], bmin[1], bmin[2])
        glVertex3f(bmax[0], bmin[1], bmax[2])
        glVertex3f(bmax[0], bmax[1], bmax[2])
        glVertex3f(bmax[0], bmax[1], bmin[2])
        glEnd()
        glBegin(GL_LINES)
        glVertex3f(bmin[0], bmin[1], bmin[2])
        glVertex3f(bmax[0], bmin[1], bmin[2])
        glVertex3f(bmin[0], bmin[1], bmax[2])
        glVertex3f(bmax[0], bmin[1], bmax[2])
        glVertex3f(bmin[0], bmax[1], bmax[2])
        glVertex3f(bmax[0], bmax[1], bmax[2])
        glVertex3f(bmin[0], bmax[1], bmin[2])
        glVertex3f(bmax[0], bmax[1], bmin[2])
        glEnd()

    def keyboardfunc(self, c, x, y):
        # print c,"pressed"
        # print 'int value =', ord(c)

        self.widgetMaster.keypress(c)
        self.refresh()
        if c == "`":
            self.simulate = not self.simulate
            print "Simulating:", self.simulate
        else:
            self.command_queue.put(c)
            # print int(c)
            if c == chr(27):
                # c == esc
                # self.picking_thread.join()
                exit(0)
        glutPostRedisplay()

    def mousefunc(self, button, state, x, y):
        # print "mouse",button,state,x,y

        if button == self.widgetButton:
            if state == 0:
                # print 'state was zero'
                if self.widgetMaster.beginDrag(x, self.height - y, self.viewport()):
                    # print 'beginning drag'
                    self.draggingWidget = True
            else:
                # print 'state of button not zero'
                if self.draggingWidget:
                    # print 'dragging widget true - ending drag'
                    self.widgetMaster.endDrag()
                    self.draggingWidget = False
            if self.widgetMaster.wantsRedraw():
                self.refresh()
            self.lastx, self.lasty = x, y
            return
        GLRealtimeProgram.mousefunc(self, button, state, x, y)

    def motionfunc(self, x, y, dx, dy):
        if self.draggingWidget:
            self.widgetMaster.drag(dx, -dy, self.viewport())
            if self.widgetMaster.wantsRedraw():
                self.refresh()
        else:
            res = self.widgetMaster.hover(x, self.height - y, self.viewport())
            if self.widgetMaster.wantsRedraw():
                self.refresh()
            GLRealtimeProgram.motionfunc(self, x, y, dx, dy)

    def specialfunc(self, c, x, y):
        # Put your keyboard special character handler here
        # print c,"pressed"
        self.widgetMaster.keypress(c)
        self.refresh()
class MyGLViewer(GLWidgetProgram):
    def __init__(self, world, serviceThread):
        GLWidgetProgram.__init__(self, "Manual poser")

        self.world = world
        self.serviceThread = serviceThread
        self.oldTime = 0.0

        for i in range(10):
            q = self.serviceThread.qcmdGetter.get()
            if q != None:
                break
            time.sleep(0.1)
        if q == None:
            print "Warning, system state service doesn't seem to have .robot.command.q"
            print "Press enter to continue"
            raw_input()
        else:
            world.robot(0).setConfig(q)
        self.robotPoser = RobotPoser(world.robot(0))
        self.addWidget(self.robotPoser)

        robot = world.robot(0)
        left_arm_link_names = [
            'left_upper_shoulder', 'left_lower_shoulder', 'left_upper_elbow',
            'left_lower_elbow', 'left_upper_forearm', 'left_lower_forearm',
            'left_wrist'
        ]
        right_arm_link_names = [
            'right_upper_shoulder', 'right_lower_shoulder',
            'right_upper_elbow', 'right_lower_elbow', 'right_upper_forearm',
            'right_lower_forearm', 'right_wrist'
        ]
        self.left_arm_link_indices = [
            robot.link(l).index for l in left_arm_link_names
        ]
        self.right_arm_link_indices = [
            robot.link(l).index for l in right_arm_link_names
        ]

        for i in range(10):
            ee1 = self.serviceThread.ee1Getter.get()
            ee2 = self.serviceThread.ee1Getter.get()
            if ee1 != None and ee2 != None:
                break
            time.sleep(0.1)
        if ee1 == None:
            print "Warning, system state service doesn't seem to have .robot.endEffectors.0.xform.sensed"
            print "Press enter to continue"
            raw_input()
        if ee2 == None:
            print "Warning, system state service doesn't seem to have .robot.endEffectors.1.xform.sensed"
            print "Press enter to continue"
            raw_input()

    def display(self):
        #Put your display handler here
        #the example draws the posed robot in grey, the sensed
        #configuration in transparent red and the commanded
        #configuration in transparent green

        #this line will draw the world
        GLWidgetProgram.display(self)

        q = self.serviceThread.qsnsGetter.get()
        ee1 = self.serviceThread.ee1Getter.get()
        ee2 = self.serviceThread.ee1Getter.get()

        # print "End Effector 1 = ", ee1

        if q:
            glEnable(GL_BLEND)
            glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
            glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE,
                         [1, 0, 0, 0.5])
            self.world.robot(0).setConfig(q)
            self.world.robot(0).drawGL(False)

        #draw commanded configuration
        glEnable(GL_BLEND)
        glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
        glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, [0, 1, 0, 0.5])
        q = self.serviceThread.qcmdGetter.get()
        if q:
            self.world.robot(0).setConfig(q)
            self.world.robot(0).drawGL(False)
            glDisable(GL_BLEND)

        self.getMsg()

    def getMsg(self):
        deviceState = self.serviceThread.updater.deviceState
        ee1 = self.serviceThread.ee1Getter.get()
        ee2 = self.serviceThread.ee1Getter.get()
        ee = [ee1, ee2]
        haptic_task_type = "cartesian"
        if haptic_task_type == "cartesian":
            msg = self.renderCartesianVelocityMsg(deviceState)
        elif haptic_task_type == "multiIK":
            msg = self.renderMultiIKMsg(deviceState, ee)

        # === Generate cartesian velocity task ===
        for i, dstate in enumerate(deviceState):
            if dstate['buttonDown']:
                if dstate['newupdate']:
                    if haptic_task_type == "cartesian":
                        print "send cartesian control msg: ", msg
                    elif haptic_task_type == "multiIK":
                        print "send Multi-IK control msg: ", msg
                    self.serviceThread.taskSetter.set(msg)

        # === Generate Multi-IK task ===

    def renderCartesianVelocityMsg(self, deviceState):
        """
        Prepare a task message for Controller dispatcher - Send a multiIK Task
        """
        msg = {}
        msg['type'] = 'CartesianVelocity'
        msg['limb'] = 'both'
        # msg['speed'] = 1
        msg['linear'] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        msg['angular'] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        msg['safe'] = 0

        if len(deviceState) == 0: return

        for i, dstate in enumerate(deviceState):
            if dstate['buttonDown']:
                if dstate['newupdate']:
                    msg['linear'][0 + i * 3:3 + i * 3] = dstate['linearVel']
                    msg['angular'][0 + i * 3:3 + i * 3] = dstate['angularVel']
        return msg

    def renderMultiIKMsg(self, deviceState, ee):
        """
        Prepare a task message for Controller dispatcher - Send a multiIK Task
        """
        msg = {}
        msg['components'] = []

        if len(deviceState) == 0: return

        for i, dstate in enumerate(deviceState):
            if dstate['buttonDown']:
                if dstate['newupdate']:
                    ikgoal = {}
                    ikgoal['link'] = endEffectorIndices[i]
                    Rlocal, tlocal = endEffectorLocalTransforms[i]
                    ikgoal['localPosition'] = tlocal
                    ikgoal['endPosition'] = [
                        dstate['moveToTransform'][1][k] + ee[i][1][k]
                        for k in range(0, 3)
                    ]
        msg['components'].append(ikgoal)
        return msg

    def keyboardfunc(self, c, x, y):
        #Put your keyboard handler here
        #the current example toggles simulation / movie mode
        if c == 'h':
            print '[space]: send the current posed milestone'
            print 'q: clean quit'
        elif c == ' ':
            q = self.robotPoser.get()
            #CONSTRUCT THE TASK HERE
            msg = {}
            msg['type'] = 'JointPose'
            msg['limb'] = 'both'
            msg['position'] = [
                q[i] for i in self.left_arm_link_indices +
                self.right_arm_link_indices
            ]
            msg['speed'] = 1
            msg['safe'] = 0
            print "Sending message", msg
            #SEND THE TASK TO THE SYSTEM STATE SERVICE
            self.serviceThread.taskSetter.set(msg)
        elif c == 'q':
            self.serviceThread.kill()
            exit(0)

        elif c == 'i':
            ql = [0.25, -0.5, -0.5, 1.5, -0.25, 1.0, 0.5]
            qr = [-0.25, -0.5, 0.5, 1.5, 0.25, 1.0, 0.5]
            msg = {}
            msg['type'] = 'JointPose'
            msg['limb'] = 'both'
            msg['position'] = ql + qr
            msg['speed'] = 1
            msg['safe'] = 0
            print "Sending message", msg
            self.serviceThread.taskSetter.set(msg)

        else:
            GLWidgetProgram.keyboardfunc(self, c, x, y)
            self.refresh()
Exemple #36
0
 def __init__(self,world):
     GLWidgetProgram.__init__(self,world,"My GL program")
     self.poseWidget = PointPoser()
     self.widgetMaster.add(self.poseWidget)
     self.robotWidget = RobotPoser(world.robot(0))
     self.widgetMaster.add(self.robotWidget)
Exemple #37
0
class MousePoserTaskGenerator(TaskGenerator):
    """Allows the user to interact with the model by right clicking
    using the mouse and pressing space bar to send the milestone.
    
    Also demonstrates how to write a Python plugin task with an OpenGL
    widget.
    """
    def __init__(self):
        self.plugin = None

    def name(self):
        return "MousePoser"

    def init(self, world):
        self.world = world
        self.robotPoser = RobotPoser(world.robot(0))
        self.plugin = MyWidgetPlugin(self)
        self.plugin.addWidget(self.robotPoser)
        robot = world.robot(0)
        left_arm_link_names = [
            'left_upper_shoulder', 'left_lower_shoulder', 'left_upper_elbow',
            'left_lower_elbow', 'left_upper_forearm', 'left_lower_forearm',
            'left_wrist'
        ]
        right_arm_link_names = [
            'right_upper_shoulder', 'right_lower_shoulder',
            'right_upper_elbow', 'right_lower_elbow', 'right_upper_forearm',
            'right_lower_forearm', 'right_wrist'
        ]
        base_link_names = ['base_x', 'base_y', 'base_yaw']
        self.left_arm_link_indices = [
            robot.link(l).index for l in left_arm_link_names
        ]
        self.right_arm_link_indices = [
            robot.link(l).index for l in right_arm_link_names
        ]
        self.base_link_indices = [robot.link(l).index for l in base_link_names]
        if any(v < 0 for v in self.left_arm_link_indices +
               self.right_arm_link_indices):
            raise ValueError("Robot in world does not have Baxter's torso?")
        return True

    def start(self):
        self._status = 'ok'
        self.robotPoser.set(self.world.robot(0).getConfig())
        return True

    def stop(self):
        pass

    def get(self):
        if self.plugin.initialPose:
            self.plugin.initialPose = False
            q = self.robotPoser.get()
            for i in range(len(self.left_arm_link_indices)):
                idx = self.left_arm_link_indices[i]
                q[idx] = q_init_left[i]
            for i in range(len(self.right_arm_link_indices)):
                idx = self.right_arm_link_indices[i]
                q[idx] = q_init_right[i]
            self.robotPoser.set(q)
            print "set up to initial pose"

        if self.plugin.sendMilestone:
            self.plugin.sendMilestone = False
            q = self.robotPoser.get()
            qcmd = self.world.robot(0).getConfig()
            baseMoved = any(qcmd[i] != q[i] for i in self.base_link_indices)
            leftArmMoved = any(qcmd[i] != q[i]
                               for i in self.left_arm_link_indices)
            rightArmMoved = any(qcmd[i] != q[i]
                                for i in self.right_arm_link_indices)
            moved = []
            if baseMoved: moved.append('base')
            if leftArmMoved: moved.append('left')
            if rightArmMoved: moved.append('right')
            targets = []
            if baseMoved:
                targets.append([q[i] for i in self.base_link_indices])
            if leftArmMoved:
                targets.append([q[i] for i in self.left_arm_link_indices])
            if rightArmMoved:
                targets.append([q[i] for i in self.right_arm_link_indices])
            print "Sending milestone", q, ", moving parts:", ",".join(moved)
            #CONSTRUCT THE TASK HERE
            msg = {}
            msg['type'] = 'JointPose'
            msg['parts'] = moved
            msg['positions'] = targets
            msg['speed'] = 1
            #TEST: only allow safe configurations (self collision checking)
            msg['safe'] = 1
            return msg
        return None

    def glPlugin(self):
        return self.plugin