def buildHorizontalDivider(self, layout):
     frame = QFrame();
     frame.setFrameShape(QFrame.HLine);
     #*******frame.setFrameStyle(QFrame.Shadow.Sunken.value());
     frame.setLineWidth(3);
     frame.setMidLineWidth(3);
     layout.addWidget(frame);
Beispiel #2
0
 def get_horizontal_line(self):
     """
     http://stackoverflow.com/questions/5671354/how-to-programmatically-make-a-horizontal-line-in-qt
     """
     hline = QFrame()
     hline.setFrameShape(QFrame.HLine)
     hline.setFrameShadow(QFrame.Sunken)
     return hline
Beispiel #3
0
 def get_horizontal_line(self):
     """
     http://stackoverflow.com/questions/5671354/how-to-programmatically-make-a-horizontal-line-in-qt
     """
     hline = QFrame()
     hline.setFrameShape(QFrame.HLine)
     hline.setFrameShadow(QFrame.Sunken)
     return hline
    def setup_group_frame(self, group):
        layout = QVBoxLayout()

        # grid for buttons for named targets
        grid = QGridLayout()
        grid.setSpacing(1)

        self.button_group = QButtonGroup(self)

        row = 0
        column = 0
        named_targets = self.get_named_targets(group)
        for target in named_targets:
            button = QPushButton(target)
            self.button_group.addButton(button)
            grid.addWidget(button, row, column)
            column += 1
            if column >= self.MAX_COLUMNS:
                row += 1
                column = 0

        self.button_group.buttonClicked.connect(self._handle_button_clicked)

        # grid for show joint value and move arm buttons/text field
        joint_values_grid = QGridLayout()
        joint_values_grid.setSpacing(1)

        button_show_joint_values = QPushButton('Current Joint Values')
        button_show_joint_values.clicked[bool].connect(
            self._handle_show_joint_values_clicked)

        line_edit = QLineEdit()
        self.text_joint_values.append(line_edit)

        button_move_to = QPushButton('Move to')
        button_move_to.clicked[bool].connect(self._handle_move_to_clicked)

        joint_values_grid.addWidget(button_show_joint_values, 0, 1)
        joint_values_grid.addWidget(line_edit, 0, 2)
        joint_values_grid.addWidget(button_move_to, 0, 3)

        layout.addLayout(grid)
        line = QFrame()
        line.setFrameShape(QFrame.HLine)
        line.setFrameShadow(QFrame.Sunken)
        layout.addWidget(line)
        layout.addLayout(joint_values_grid)

        frame = QFrame()
        frame.setLayout(layout)
        return frame
    def joint_widget(self, main_vbox, index):
        joint = self.chain[index]
        frame = QFrame()
        frame.setFrameShape(QFrame.StyledPanel)
        frame.setFrameShadow(QFrame.Raised)

        hbox = QHBoxLayout()
        # hbox.addWidget(frame)

        self.prior_time = 0.0

        robot_joint = self.robot.joints[self.joint_list[joint.name]]
        joint.lower_limit = robot_joint.limit.lower
        joint.upper_limit = robot_joint.limit.upper
        ramp_range = robot_joint.limit.upper - robot_joint.limit.lower

        print "  ", joint.name, "  limits(", joint.lower_limit, ", ", joint.upper_limit, ") range=", ramp_range

        self.cmd_spinbox.append(QDoubleSpinBox())
        self.cmd_spinbox[index].setDecimals(5)
        self.cmd_spinbox[index].setRange(joint.lower_limit, joint.upper_limit)
        self.cmd_spinbox[index].setSingleStep((robot_joint.limit.upper - robot_joint.limit.lower) / 50.0)
        self.cmd_spinbox[index].valueChanged.connect(joint.on_cmd_value)

        self.ramp_up_spinbox.append(QDoubleSpinBox())
        self.ramp_up_spinbox[index].setDecimals(5)
        self.ramp_up_spinbox[index].setRange(-ramp_range, ramp_range)
        self.ramp_up_spinbox[index].setSingleStep(ramp_range / 50.0)
        self.ramp_up_spinbox[index].valueChanged.connect(joint.on_ramp_up_value)

        self.ramp_down_spinbox.append(QDoubleSpinBox())
        self.ramp_down_spinbox[index].setDecimals(5)
        self.ramp_down_spinbox[index].setRange(-ramp_range, ramp_range)
        self.ramp_down_spinbox[index].setSingleStep(ramp_range / 50.0)
        self.ramp_down_spinbox[index].valueChanged.connect(joint.on_ramp_down_value)

        hbox.addWidget(QLabel(joint.name))
        hbox.addWidget(self.cmd_spinbox[index])
        hbox.addWidget(self.ramp_up_spinbox[index])
        hbox.addWidget(self.ramp_down_spinbox[index])

        # Add horizontal layout to frame for this joint group
        frame.setLayout(hbox)

        # Add frame to main vertical layout
        main_vbox.addWidget(frame)
    def __init__(self, parent, fileName, top_widget_layout):

        self.controllers = []
        self.parent      = parent

        self.loadFile(fileName)


        print "Initialize controllers..."
        for controller in self.controllers:
            frame = QFrame()
            frame.setFrameShape(QFrame.StyledPanel);
            frame.setFrameShadow(QFrame.Raised);

            vbox = QVBoxLayout()
            label = QLabel()
            label.setText(controller.label)
            vbox.addWidget(label);

            print controller.name

            for joint in controller.joints:
                label = QLabel()
                label.setText(joint.name)
                vbox.addWidget(label);

                #Add input for setting the biases
                widget = QWidget()
                hbox = QHBoxLayout()

                hbox.addWidget(joint.sensor_bias_spinbox)
                hbox.addWidget(joint.control_bias_spinbox)
                hbox.addWidget(joint.gearing_bias_spinbox)

                widget.setLayout(hbox)
                vbox.addWidget(widget)

            label = QLabel()
            label.setText("      Sensor           Control           Gearing")
            vbox.addWidget(label);
            vbox.addStretch()

            frame.setLayout(vbox)
            top_widget_layout.addWidget(frame)
        print "Done loading controllers"
    def joint_widget( self, main_vbox, index):
        joint = self.chain[index]
        frame = QFrame()
        frame.setFrameShape(QFrame.StyledPanel);
        frame.setFrameShadow(QFrame.Raised);

        hbox = QHBoxLayout()
        #hbox.addWidget(frame)

        self.prior_time = 0.0

        robot_joint = self.robot.joints[self.joint_list[joint.name]]
        joint.lower_limit = robot_joint.limit.lower
        joint.upper_limit = robot_joint.limit.upper
        amplitude_range = (robot_joint.limit.upper-robot_joint.limit.lower)
        frequency_range = self.frequency_limit
        iterations_range = 10000

        print "  ",joint.name, "  limits(", joint.lower_limit,", ",joint.upper_limit,") range=",amplitude_range

        self.cur_position_spinbox.append(QDoubleSpinBox())
        self.cur_position_spinbox[index].setDecimals(5)
        self.cur_position_spinbox[index].setRange(joint.lower_limit, joint.upper_limit)
        self.cur_position_spinbox[index].setSingleStep((robot_joint.limit.upper-robot_joint.limit.lower)/50.0)
        self.cur_position_spinbox[index].valueChanged.connect(joint.on_position_value)

        self.amplitude_spinbox.append(QDoubleSpinBox())
        self.amplitude_spinbox[index].setDecimals(5)
        self.amplitude_spinbox[index].setRange(-amplitude_range, amplitude_range)
        self.amplitude_spinbox[index].setSingleStep(amplitude_range/50.0)
        self.amplitude_spinbox[index].valueChanged.connect(joint.on_amplitude_value)
        self.amplitude_spinbox[index].setValue(joint.amplitude)

        hbox.addWidget(QLabel(joint.name))
        hbox.addWidget(self.cur_position_spinbox[index])
        hbox.addWidget(self.amplitude_spinbox[index])

        # Add horizontal layout to frame for this joint group
        frame.setLayout(hbox)

        # Add frame to main vertical layout
        main_vbox.addWidget(frame)
    def __init__(self, main, fileName, main_hbox):
        self.main       = main
        self.chain      = []
        self.poses      = []
        self.chain_name  = ""
        frame = QFrame()
        frame.setFrameShape(QFrame.StyledPanel);
        frame.setFrameShadow(QFrame.Raised);

        vbox = QVBoxLayout()

        label = QLabel()
        vbox.addWidget(label);

        self.load_file(fileName, vbox)
        self.trajectoryPublisher = rospy.Publisher('/trajectory_controllers/'+self.chain_name+'_traj_controller/trajectory'  ,JointTrajectory, queue_size=10)
        self.positionPublisher   = rospy.Publisher('/trajectory_controllers/'+self.chain_name+'_traj_controller/joint_states',JointState,      queue_size=10)
        self.ghostPublisher      = rospy.Publisher('/flor/ghost/set_joint_states',JointState, queue_size=10)

        label.setText(self.chain_name)
        vbox.addStretch(1)

        frame.setLayout(vbox)
        main_hbox.addWidget(frame)
    def __init__(self, context):
        super(SinusoidalTrajectoryDialog, self).__init__(context)
        self.setObjectName('SinusoidalTrajectoryDialog')
        #self.updateStateSignal = Signal(object)
        self.updateStateSignal.connect(self.on_updateState)

        self.robot = URDF.from_parameter_server()
        self.joint_list = {}

    	for ndx,jnt in enumerate(self.robot.joints):
            self.joint_list[jnt.name] = ndx
            #print jnt.name, " ",self.joint_list[jnt.name]

        self.chain=[]
        self.chain_file = rospy.get_param("chain_file")
        self.chain_name = rospy.get_param("chain_name")

        yaml_file = self.chain_file+self.chain_name+"_chain.yaml"
        print yaml_file

        #define structures
        self.robot_state = JointState()
        self.robot_command = JointTrajectory()

        stream = open(yaml_file, "r")
        jointChain = yaml.load_all(stream)
        print '\n\n'
        for ndx, data in enumerate(jointChain):
            print ndx," : ", data
            self.delay_time          = data["delay_time"]
            self.amplitude           = data["amplitude"]
            self.frequency           = data["frequency"]
            self.frequency_limit     = data["frequency_limit"]
            self.iterations          = data["iterations"]
            self.joint_state_topic   = data["joint_state_topic"]
            self.trajectory_topic    = data["trajectory_topic"]

            joints = rospy.get_param(data["chain_param_name"])
            for joint in joints:
                print joint
                self.robot_state.name.append(joint)
                self.chain.append(JointData(self, joint) )
        self.robot_command.joint_names = self.robot_state.name

        stream.close()


        self.robot_state.position = [0.0]*len(self.robot_state.name)
        self.robot_state.velocity = [0.0]*len(self.robot_state.name)
        self.robot_state.effort   = [0.0]*len(self.robot_state.name)
        self.robot_joint_state = JointState()

        print "delay_time  =",self.delay_time
        print "amplitude   =",self.amplitude
        print "frequency   =",self.frequency
        print "iterations  =",self.iterations

        print "Robot State Structure",self.robot_state
        print "Robot Command Structure",self.robot_command

        # initialize structure to hold widget handles
        self.cur_position_spinbox=[]
        self.amplitude_spinbox=[]
        self.frequency_spinbox=[]
        self.iterations_spinbox=[]

        self._widget = QWidget()
        vbox = QVBoxLayout()

        # Push buttons
        hbox = QHBoxLayout()

        snap_command = QPushButton("Snap Position")
        snap_command.clicked.connect(self.snap_current_callback)
        hbox.addWidget(snap_command)

        check_limits = QPushButton("Check Limits Gains")
        check_limits.clicked.connect(self.check_limits_callback)
        hbox.addWidget(check_limits)

        apply_command = QPushButton("Send Trajectory")
        apply_command.clicked.connect(self.apply_command_callback)
        hbox.addWidget(apply_command)

        save_trajectory = QPushButton("Save Trajectory")
        save_trajectory.clicked.connect(self.save_trajectory_callback)
        hbox.addWidget(save_trajectory)

        zero_ramp = QPushButton("Zero Values")
        zero_ramp.clicked.connect(self.zero_values_callback)
        hbox.addWidget(zero_ramp)

        vbox.addLayout(hbox)

        time_hbox = QHBoxLayout()

        vbox_frequqency = QVBoxLayout()
        vbox_frequqency.addWidget(QLabel("Frequency"))
        self.frequency_spinbox = QDoubleSpinBox()
        self.frequency_spinbox.setDecimals(5)
        self.frequency_spinbox.setRange(0, self.frequency_limit)
        self.frequency_spinbox.setSingleStep(0.05)
        self.frequency_spinbox.valueChanged.connect(self.on_frequency_value)
        self.frequency_spinbox.setValue(self.frequency)
        vbox_frequqency.addWidget(self.frequency_spinbox)
        time_hbox.addLayout(vbox_frequqency)

        vbox_iterations = QVBoxLayout()
        vbox_iterations.addWidget(QLabel("Iterations"))
        self.iterations_spinbox = QDoubleSpinBox()
        self.iterations_spinbox.setDecimals(5)
        self.iterations_spinbox.setRange(0, 10)
        self.iterations_spinbox.setSingleStep(1)
        self.iterations_spinbox.valueChanged.connect(self.on_iterations_value)
        self.iterations_spinbox.setValue(self.iterations)
        vbox_iterations.addWidget(self.iterations_spinbox)
        time_hbox.addLayout(vbox_iterations)

        vbox.addLayout(time_hbox)

        # Joints title
        title_frame = QFrame()
        title_frame.setFrameShape(QFrame.StyledPanel);
        title_frame.setFrameShadow(QFrame.Raised);

        title_hbox = QHBoxLayout()
        title_hbox.addWidget(QLabel("Joints"))
        title_hbox.addWidget(QLabel("Current Position"))
        title_hbox.addWidget(QLabel("Amplitude"))
        title_frame.setLayout(title_hbox)
        vbox.addWidget(title_frame)


        # Define the widgets for each joint
        for i,joint in enumerate(self.chain):
            #print i,",",joint
            self.joint_widget( vbox, i)


        #add stretch at end so all GUI elements are at top of dialog
        vbox.addStretch(1)

        self._widget.setLayout(vbox)


        # Define the connections to the outside world
        self.jointSubscriber  = rospy.Subscriber(self.joint_state_topic, JointState, self.stateCallbackFnc)
        self.commandPublisher = rospy.Publisher(self.trajectory_topic  , JointTrajectory, queue_size=10)

        # Add the widget
        context.add_widget(self._widget)
    def __init__(self, context):
        super(TrapezoidalTrajectoryDialog, self).__init__(context)
        self.setObjectName("TrapezoidalTrajectoryDialog")
        # self.updateStateSignal = Signal(object)
        self.updateStateSignal.connect(self.on_updateState)

        self.robot = URDF.from_parameter_server()
        self.joint_list = {}

        for ndx, jnt in enumerate(self.robot.joints):
            self.joint_list[jnt.name] = ndx

        self.chain = []
        self.chain_file = rospy.get_param("~chain_file")
        self.chain_name = rospy.get_param("~chain_name")

        print
        print "Define order of splines used to approximate trapezoid"
        self.spline_order = rospy.get_param("~spline_order", 5)  # 1 # 3 # 5 # linear, cubic, quintic
        if (self.spline_order == 1) or (self.spline_order == 3) or (self.spline_order == 5):
            print "Spline order=", self.spline_order
        else:
            print "Invalid spline order!  Must be 1, 3, or 5"
            print "Spline order=", self.spline_order
            sys.exit(-1)

        yaml_file = self.chain_file + self.chain_name + "_chain.yaml"
        print "Chain definition file:"
        print yaml_file
        print

        # define structures
        self.robot_state = JointState()
        self.robot_command = JointTrajectory()

        stream = open(yaml_file, "r")
        jointChain = yaml.load_all(stream)

        for ndx, data in enumerate(jointChain):
            print ndx, " : ", data
            self.delay_time = data["delay_time"]
            self.ramp_up_time = data["ramp_up_time"]
            self.dwell_time = data["dwell_time"]
            self.ramp_down_time = data["ramp_down_time"]
            self.hold_time = data["hold_time"]
            self.ramp_start_fraction = data["ramp_start_fraction"]
            self.ramp_end_fraction = data["ramp_end_fraction"]

            self.joint_state_topic = data["joint_state_topic"]
            self.trajectory_topic = data["trajectory_topic"]

            if rospy.search_param(data["chain_param_name"]):
                print "Found ", data["chain_param_name"]
            else:
                print "Failed to find the ", data["chain_param_name"], " in the parameter server!"
                sys.exit(-1)

            joint_names = rospy.get_param(data["chain_param_name"])
            for joint in joint_names:
                print joint
                self.robot_state.name.append(joint)
                self.chain.append(JointData(self, joint))

        self.robot_command.joint_names = self.robot_state.name

        stream.close()

        self.robot_state.position = [0.0] * len(self.robot_state.name)
        self.robot_state.velocity = [0.0] * len(self.robot_state.name)
        self.robot_state.effort = [0.0] * len(self.robot_state.name)
        self.robot_joint_state = JointState()

        print "delay_time    =", self.delay_time
        print "ramp_up_time  =", self.ramp_up_time
        print "dwell_time    =", self.dwell_time
        print "ramp_down_time=", self.ramp_down_time
        print "hold_time     =", self.hold_time
        print "spline order  =", self.spline_order

        if (
            (self.ramp_start_fraction < 0.001)
            or (self.ramp_end_fraction > 0.999)
            or (self.ramp_start_fraction >= self.ramp_end_fraction)
        ):
            print "Invalid ramp fractions - abort!"
            print "0.0 < ", self.ramp_start_fraction, " < ", self.ramp_end_fraction, " < 1.0"
            return

        print "Robot State Structure", self.robot_state
        print "Robot Command Structure", self.robot_command

        # initialize structure to hold widget handles
        self.cmd_spinbox = []
        self.ramp_up_spinbox = []
        self.ramp_down_spinbox = []

        self._widget = QWidget()
        vbox = QVBoxLayout()

        # Push buttons
        hbox = QHBoxLayout()

        snap_command = QPushButton("Snap Position")
        snap_command.clicked.connect(self.snap_current_callback)
        hbox.addWidget(snap_command)

        check_limits = QPushButton("Check Limits Gains")
        check_limits.clicked.connect(self.check_limits_callback)
        hbox.addWidget(check_limits)

        apply_command = QPushButton("Send Trajectory")
        apply_command.clicked.connect(self.apply_command_callback)
        hbox.addWidget(apply_command)

        save_trajectory = QPushButton("Save Trajectory")
        save_trajectory.clicked.connect(self.save_trajectory_callback)
        hbox.addWidget(save_trajectory)

        zero_ramp = QPushButton("Zero Ramps")
        zero_ramp.clicked.connect(self.zero_ramp_callback)
        hbox.addWidget(zero_ramp)

        vbox.addLayout(hbox)

        time_hbox = QHBoxLayout()

        vbox_delay = QVBoxLayout()
        vbox_delay.addWidget(QLabel("Delay"))
        self.delay_time_spinbox = QDoubleSpinBox()
        self.delay_time_spinbox.setDecimals(5)
        self.delay_time_spinbox.setRange(0, 10.0)
        self.delay_time_spinbox.setSingleStep(0.1)
        self.delay_time_spinbox.valueChanged.connect(self.on_delay_time_value)
        self.delay_time_spinbox.setValue(self.delay_time)
        vbox_delay.addWidget(self.delay_time_spinbox)
        time_hbox.addLayout(vbox_delay)

        vbox_ramp_up = QVBoxLayout()
        vbox_ramp_up.addWidget(QLabel("Ramp Up"))
        self.ramp_up_time_spinbox = QDoubleSpinBox()
        self.ramp_up_time_spinbox.setDecimals(5)
        self.ramp_up_time_spinbox.setRange(0, 10.0)
        self.ramp_up_time_spinbox.setSingleStep(0.1)
        self.ramp_up_time_spinbox.valueChanged.connect(self.on_ramp_up_time_value)
        self.ramp_up_time_spinbox.setValue(self.ramp_up_time)
        vbox_ramp_up.addWidget(self.ramp_up_time_spinbox)
        time_hbox.addLayout(vbox_ramp_up)

        #
        vbox_dwell = QVBoxLayout()
        vbox_dwell.addWidget(QLabel("Dwell"))
        self.dwell_time_spinbox = QDoubleSpinBox()
        self.dwell_time_spinbox.setDecimals(5)
        self.dwell_time_spinbox.setRange(0, 10.0)
        self.dwell_time_spinbox.setSingleStep(0.1)
        self.dwell_time_spinbox.valueChanged.connect(self.on_dwell_time_value)
        self.dwell_time_spinbox.setValue(self.dwell_time)
        vbox_dwell.addWidget(self.dwell_time_spinbox)
        time_hbox.addLayout(vbox_dwell)

        vbox_ramp_down = QVBoxLayout()
        vbox_ramp_down.addWidget(QLabel("Down"))
        self.ramp_down_time_spinbox = QDoubleSpinBox()
        self.ramp_down_time_spinbox.setDecimals(5)
        self.ramp_down_time_spinbox.setRange(0, 10.0)
        self.ramp_down_time_spinbox.setSingleStep(0.1)
        self.ramp_down_time_spinbox.valueChanged.connect(self.on_ramp_down_time_value)
        self.ramp_down_time_spinbox.setValue(self.ramp_down_time)
        vbox_ramp_down.addWidget(self.ramp_down_time_spinbox)
        time_hbox.addLayout(vbox_ramp_down)

        vbox_hold = QVBoxLayout()
        vbox_hold.addWidget(QLabel("Hold"))
        self.hold_time_spinbox = QDoubleSpinBox()
        self.hold_time_spinbox.setDecimals(5)
        self.hold_time_spinbox.setRange(0, 10.0)
        self.hold_time_spinbox.setSingleStep(0.1)
        self.hold_time_spinbox.valueChanged.connect(self.on_hold_time_value)
        self.hold_time_spinbox.setValue(self.hold_time)
        vbox_hold.addWidget(self.hold_time_spinbox)
        time_hbox.addLayout(vbox_hold)

        vbox.addLayout(time_hbox)

        # Joints title
        title_frame = QFrame()
        title_frame.setFrameShape(QFrame.StyledPanel)
        title_frame.setFrameShadow(QFrame.Raised)

        title_hbox = QHBoxLayout()
        title_hbox.addWidget(QLabel("Joints"))
        title_hbox.addWidget(QLabel("Start"))
        title_hbox.addWidget(QLabel("Ramp Up"))
        title_hbox.addWidget(QLabel("Ramp Down"))
        title_frame.setLayout(title_hbox)
        vbox.addWidget(title_frame)

        # Define the widgets for each joint
        for i, joint in enumerate(self.chain):
            # print i,",",joint
            self.joint_widget(vbox, i)

        # add stretch at end so all GUI elements are at top of dialog
        vbox.addStretch(1)

        self._widget.setLayout(vbox)

        # Define the connections to the outside world
        self.jointSubscriber = rospy.Subscriber(self.joint_state_topic, JointState, self.stateCallbackFnc)
        self.commandPublisher = rospy.Publisher(self.trajectory_topic, JointTrajectory, queue_size=10)

        # Add the widget
        context.add_widget(self._widget)
    def __init__(self, parent, fileName, widget):

        self.controllers = []
        self.parent      = parent

        self.loadFile(fileName)

        robot = URDF.from_parameter_server()
        joint_list = {}
	for ndx,jnt in enumerate(robot.joints):
		joint_list[jnt.name] = ndx

        for controller in self.controllers:
            frame = QFrame()
            frame.setFrameShape(QFrame.StyledPanel);
            frame.setFrameShadow(QFrame.Raised);

            vbox = QVBoxLayout()
            label = QLabel()
            label.setText(controller.label)
            vbox.addWidget(label);
            print controller.name

            controller.snap_to_ghost_button = QPushButton("SnapGhost")
            controller.snap_to_ghost_button.pressed.connect(controller.on_snapGhostPressed)
            vbox.addWidget(controller.snap_to_ghost_button)
            controller.snap_to_current_button = QPushButton("SnapCurrent")
            controller.snap_to_current_button.pressed.connect(controller.on_snapCurrentPressed)
            vbox.addWidget(controller.snap_to_current_button)
            controller.apply_to_robot_button = QPushButton("ApplyRobot")
            controller.apply_to_robot_button.pressed.connect(controller.on_applyRobotPressed)
            vbox.addWidget(controller.apply_to_robot_button)
            controller.save_joints_to_file_button = QPushButton("SaveJoints")
            controller.save_joints_to_file_button.pressed.connect(controller.on_saveJointsPressed)
            vbox.addWidget(controller.save_joints_to_file_button)
            controller.undo_last_action_button = QPushButton("Undo Last")
            controller.undo_last_action_button.pressed.connect(controller.on_undoPressed)
            vbox.addWidget(controller.undo_last_action_button)


            for joint in controller.joints:
                label = QLabel()
                label.setText(joint.name)
                vbox.addWidget(label);

                robot_joint = robot.joints[joint_list[joint.name]]
                lower = robot_joint.limit.lower - (math.fabs(robot_joint.limit.upper)+math.fabs(robot_joint.limit.lower))*0.2
                upper = robot_joint.limit.upper + (math.fabs(robot_joint.limit.upper)+math.fabs(robot_joint.limit.lower))*0.2
                print "  ",joint.name, "  limits(", robot_joint.limit.lower,", ",robot_joint.limit.upper,") num"

                joint.slider = QSlider(Qt.Horizontal)
                joint.slider.setRange(int(lower*10000.0), int(upper*10000.0))
                joint.slider.setValue(int(lower*10000.0))
                joint.slider.setSingleStep((upper-lower)/20.0)
                joint.slider.valueChanged.connect(joint.on_sliderMoved)
                vbox.addWidget(joint.slider)
                joint.progress_bar = QProgressBar()
                joint.progress_bar.setRange(int(lower*10000.0), int(upper*10000.0))
                joint.progress_bar.setValue(int(lower*10000.0))
                vbox.addWidget(joint.progress_bar)

            vbox.addStretch()

            frame.setLayout(vbox)
            widget.addWidget(frame)
    def create_controller_ui(self):
        self.controllers = []

        if not self.load_robot_model():
            # if no groups config is on the parameter server, request the list from controller manager
            self.load_controllers()
        robot = URDF.from_parameter_server()

        joint_list = {}
        for ndx, jnt in enumerate(robot.joints):
            joint_list[jnt.name] = ndx

        for controller in self.controllers:
            frame = QFrame()
            frame.setFrameShape(QFrame.StyledPanel)
            frame.setFrameShadow(QFrame.Raised)

            vbox = QVBoxLayout()
            label = QLabel()
            label.setText(controller.label)
            vbox.addWidget(label)

            controller.snap_to_ghost_button = QPushButton("SnapGhost")
            controller.snap_to_ghost_button.pressed.connect(controller.on_snap_ghost_pressed)
            vbox.addWidget(controller.snap_to_ghost_button)
            controller.snap_to_current_button = QPushButton("SnapCurrent")
            controller.snap_to_current_button.pressed.connect(controller.on_snap_current_pressed)
            vbox.addWidget(controller.snap_to_current_button)
            controller.apply_to_robot_button = QPushButton("ApplyRobot")
            controller.apply_to_robot_button.pressed.connect(controller.on_apply_robot_pressed)
            vbox.addWidget(controller.apply_to_robot_button)
            # Removed because it is hardcoded
            # controller.save_joints_to_file_button = QPushButton("SaveJoints")
            # controller.save_joints_to_file_button.pressed.connect(controller.on_save_joints_pressed)
            # vbox.addWidget(controller.save_joints_to_file_button)
            controller.undo_last_action_button = QPushButton("Undo Last")
            controller.undo_last_action_button.pressed.connect(controller.on_undo_pressed)
            vbox.addWidget(controller.undo_last_action_button)

            print 'Loading limits for controller:', controller.topic
            for joint in controller.joints:
                label = QLabel()
                label.setText(joint.name)
                vbox.addWidget(label)

                try:
                    robot_joint = robot.joints[joint_list[joint.name]]
                except KeyError:
                    print 'No limits found for', joint.name
                    limit_lower = -1.0
                    limit_upper = 1
                else:
                    limit_lower = robot_joint.limit.lower
                    limit_upper = robot_joint.limit.upper
                print "  ", joint.name, "  limits(", limit_lower, ", ", limit_upper, ") num"

                joint.slider = QSlider(Qt.Horizontal)
                joint.slider.setRange(int(limit_lower * 10000.0), int(limit_upper * 10000.0))
                joint.slider.setValue(int(limit_lower * 10000.0))
                joint.slider.setSingleStep((limit_upper - limit_lower) / 20.0)
                joint.slider.valueChanged.connect(joint.on_slider_moved)
                vbox.addWidget(joint.slider)
                joint.progress_bar = QProgressBar()
                joint.progress_bar.setRange(int(limit_lower * 10000.0),
                                            int(limit_upper * 10000.0))
                joint.progress_bar.setValue(int(limit_lower * 10000.0))
                vbox.addWidget(joint.progress_bar)

            vbox.addStretch()

            frame.setLayout(vbox)
            self.widget.addWidget(frame)