Esempio n. 1
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 == ' ':
            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)
Esempio n. 2
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)
Esempio n. 3
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
Esempio n. 4
0
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()
Esempio n. 5
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