def __init__(self, path, config, fit_type=FitType.CUBIC):
self.path = path
self.config = config
self.splineGenerator = SplineGenerator(fit_type)
self.trajectory = Trajectory()
self.splineUtils = SplineUtils()
self.planner = TrajectoryPlanner(config)
self.prepare()
def __init__(self, rexarm, planner, kinect):
self.rexarm = rexarm
self.tp = planner
self.kinect = kinect
self.status_message = "State: Idle"
self.current_state = "idle"
self.next_state = "idle"
self.config = [[0] * self.rexarm.num_joints
] #change when gripper is added
# self.coeff_rgb_2_world = np.zeros((3,3),float)
self.tp = TrajectoryPlanner(rexarm)
self.world_mouse = [0, 0, 0]
# self.offset = 85
# self.remainder = 55
self.offset = 110
self.blob_height = 40
self.remainder = 70
self.config_event1 = []
self.config_event3 = []
self.config_event4 = []
self.config_event5 = []
def __init__(self,parent=None):
QWidget.__init__(self,parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
""" Set GUI to track mouse """
QWidget.setMouseTracking(self,True)
"""
Dynamixel bus
TODO: add other motors here as needed with their correct address"""
self.dxlbus = DXL_BUS(DEVICENAME, BAUDRATE)
port_num = self.dxlbus.port()
base = DXL_MX(port_num, 1)
shld = DXL_MX(port_num, 2)
elbw = DXL_MX(port_num, 3)
wrst = DXL_AX(port_num, 4)
wrst2 = DXL_AX(port_num, 5)
wrst3 = DXL_XL(port_num, 6)
gripper = DXL_XL(port_num, 7)
"""Objects Using Other Classes"""
self.kinect = Kinect()
self.rexarm = Rexarm((base,shld,elbw,wrst,wrst2,wrst3),gripper)
self.tp = TrajectoryPlanner(self.rexarm)
self.sm = StateMachine(self.rexarm, self.tp, self.kinect)
"""
Attach Functions to Buttons & Sliders
TODO: NAME AND CONNECT BUTTONS AS NEEDED
"""
self.ui.btn_exec.clicked.connect(self.execute)
self.ui.btn_estop.clicked.connect(self.estop)
self.ui.btnUser1.setText("Calibrate")
self.ui.btnUser1.clicked.connect(partial(self.sm.set_next_state, "calibrate"))
self.ui.sldrBase.valueChanged.connect(self.sliderChange)
self.ui.sldrShoulder.valueChanged.connect(self.sliderChange)
self.ui.sldrElbow.valueChanged.connect(self.sliderChange)
self.ui.sldrWrist.valueChanged.connect(self.sliderChange)
self.ui.sldrWrist2.valueChanged.connect(self.sliderChange)
self.ui.sldrWrist3.valueChanged.connect(self.sliderChange)
#self.ui.sldrGrip1.valueChanged.connect(self.sliderChange)
self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange)
self.ui.sldrSpeed.valueChanged.connect(self.sliderChange)
self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk)
self.ui.rdoutStatus.setText("Waiting for input")
"""Team10 section for buttons"""
self.ui.btnUser2.setText("teach")
self.ui.btnUser2.clicked.connect(partial(self.sm.set_next_state, "teach"))
self.ui.btnUser3.setText("repeat")
self.ui.btnUser3.clicked.connect(partial(self.sm.set_next_state, "repeat"))
self.ui.btnUser4.setText("Set ROI")
self.ui.btnUser4.clicked.connect(partial(self.sm.set_next_state, "set_roi"))
self.ui.btnUser5.setText("Set Exclusion")
self.ui.btnUser5.clicked.connect(partial(self.sm.set_next_state, "set_exclusion"))
self.ui.btnUser6.setText("Save frames")
self.ui.btnUser6.clicked.connect(partial(self.sm.set_next_state, "save_frames"))
self.ui.btn_task3.clicked.connect(partial(self.sm.set_next_state, "task3"))
self.ui.btnUser7.setText("Click & Grab")
self.ui.btnUser7.clicked.connect(partial(self.sm.set_next_state, "ClickandGrab"))
"""initalize manual control off"""
self.ui.SliderFrame.setEnabled(False)
"""initalize rexarm"""
self.rexarm.initialize()
"""Setup Threads"""
self.videoThread = VideoThread(self.kinect)
self.videoThread.updateFrame.connect(self.setImage)
self.videoThread.start()
self.logicThread = LogicThread(self.sm)
self.logicThread.start()
self.displayThread = DisplayThread(self.rexarm, self.sm)
self.displayThread.updateJointReadout.connect(self.updateJointReadout)
self.displayThread.updateEndEffectorReadout.connect(self.updateEndEffectorReadout)
self.displayThread.updateStatusMessage.connect(self.updateStatusMessage)
self.displayThread.start()
"""
Setup Timer
this runs the trackMouse function every 50ms
"""
self._timer = QTimer(self)
self._timer.timeout.connect(self.trackMouse)
self._timer.start(50)
dxlbus = DXL_BUS(DEVICENAME, BAUDRATE)
port_num = dxlbus.port()
base = DXL_MX(port_num, 0)
shld = DXL_MX(port_num, 1)
elbw = DXL_MX(port_num, 2)
wrst = DXL_AX(port_num, 3)
wrst2 = DXL_XL(port_num, 4)
grip = DXL_XL(port_num, 5)
rexarm = Rexarm((base,shld,elbw,wrst,wrst2), grip)
rexarm.set_speeds_normalized_global(0.15)
rexarm.initialize()
rexarm.open_gripper()
tp = TrajectoryPlanner(rexarm)
time.sleep(1)
waypoints = [[1.0,0.8,1.0,1.0,1.0],
[-1.0,-0.8,-1.0,-1.0,-1.0],
[-1.0,0.8,1.0,1.0,1.0],
[1.0,-0.8,-1.0,-1.0,-1.0],
[0.0,0.0,0.0,0.0,0.0]]
for wp in waypoints:
goal_wp = wp
tp.set_initial_wp()
tp.set_final_wp(goal_wp)
tp.go()
rexarm.toggle_gripper()
def __init__(self,parent=None):
QWidget.__init__(self,parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
""" Set GUI to track mouse """
QWidget.setMouseTracking(self,True)
"""dynamixel bus -- add other motors here"""
self.dxlbus = DXL_BUS(DEVICENAME, BAUDRATE)
port_num = self.dxlbus.port()
base = DXL_MX(port_num, 0)
shld = DXL_MX(port_num, 1)
elbw = DXL_MX(port_num, 2)
wrst = DXL_AX(port_num, 3)
wrst2 = DXL_XL(port_num, 4) # NIC 10/4
grip = DXL_XL(port_num, 5) # NIC 10/4
"""Objects Using Other Classes"""
self.kinect = Kinect()
self.rexarm = Rexarm((base,shld,elbw,wrst,wrst2),grip) # NIC 10/4
self.tp = TrajectoryPlanner(self.rexarm)
self.sm = StateMachine(self.rexarm, self.tp, self.kinect)
"""
Attach Functions to Buttons & Sliders
TODO: NAME AND CONNECT BUTTONS AS NEEDED
"""
self.ui.btn_estop.clicked.connect(self.estop)
self.ui.btn_exec.clicked.connect(partial(self.sm.set_next_state,"execute_plan"))
self.ui.btn_task1.clicked.connect(partial(self.sm.set_next_state,"task_1"))
self.ui.btn_task2.clicked.connect(partial(self.sm.set_next_state,"task_2"))
self.ui.btn_task4.clicked.connect(partial(self.sm.set_next_state,"task_5"))
#self.ui.btn_exec.clicked.connect(partial(self.sm.set_next_state,"execute"))
self.ui.btnUser1.setText("Calibrate")
self.ui.btnUser1.clicked.connect(partial(self.sm.set_next_state, "calibrate"))
self.ui.btnUser2.setText("Record Waypoints")
self.ui.btnUser2.clicked.connect(partial(self.sm.set_next_state, "record"))
self.ui.btnUser3.setText("Play")
self.ui.btnUser3.clicked.connect(partial(self.sm.set_next_state, "play"))
self.ui.btnUser4.setText("Open Gripper") # NIC 10/4
self.ui.btnUser4.clicked.connect(self.rexarm.open_gripper) # NIC 10/4
self.ui.btnUser5.setText("Close Gripper") # NIC 10/4
self.ui.btnUser5.clicked.connect(self.rexarm.close_gripper) # NIC 10/4
self.ui.sldrBase.valueChanged.connect(self.sliderChange)
self.ui.sldrShoulder.valueChanged.connect(self.sliderChange)
self.ui.sldrElbow.valueChanged.connect(self.sliderChange)
self.ui.sldrWrist.valueChanged.connect(self.sliderChange)
self.ui.sldrGrip1.valueChanged.connect(self.sliderChange)
self.ui.sldrGrip2.valueChanged.connect(self.sliderChange)
self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange)
self.ui.sldrSpeed.valueChanged.connect(self.sliderChange)
self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk)
self.ui.rdoutStatus.setText("Waiting for input")
"""initalize manual control off"""
self.ui.SliderFrame.setEnabled(False)
"""initalize rexarm"""
self.rexarm.initialize()
"""Setup Threads"""
self.videoThread = VideoThread(self.kinect)
self.videoThread.updateFrame.connect(self.setImage)
self.videoThread.start()
self.logicThread = LogicThread(self.sm)
self.logicThread.start()
self.displayThread = DisplayThread(self.rexarm, self.sm)
self.displayThread.updateJointReadout.connect(self.updateJointReadout)
self.displayThread.updateEndEffectorReadout.connect(self.updateEndEffectorReadout)
self.displayThread.updateStatusMessage.connect(self.updateStatusMessage)
self.displayThread.start()
"""
Setup Timer
this runs the trackMouse function every 50ms
"""
self._timer = QTimer(self)
self._timer.timeout.connect(self.trackMouse)
self._timer.start(50)
def __init__(self, parent=None):
QWidget.__init__(self, parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
""" Groups of ui commonents """
self.error_lcds = [
self.ui.rdoutBaseErrors, self.ui.rdoutShoulderErrors,
self.ui.rdoutElbowErrors, self.ui.rdoutWristErrors,
self.ui.rdoutWrist2Errors, self.ui.rdoutWrist3Errors
]
self.joint_readouts = [
self.ui.rdoutBaseJC, self.ui.rdoutShoulderJC, self.ui.rdoutElbowJC,
self.ui.rdoutWristJC, self.ui.rdoutWrist2JC, self.ui.rdoutWrist3JC
]
self.joint_slider_rdouts = [
self.ui.rdoutBase, self.ui.rdoutShoulder, self.ui.rdoutElbow,
self.ui.rdoutWrist, self.ui.rdoutWrist2, self.ui.rdoutWrist3
]
self.joint_sliders = [
self.ui.sldrBase, self.ui.sldrShoulder, self.ui.sldrElbow,
self.ui.sldrWrist, self.ui.sldrWrist2, self.ui.sldrWrist3
]
"""Objects Using Other Classes"""
self.kinect = Kinect()
self.rexarm = Rexarm()
self.tp = TrajectoryPlanner(self.rexarm)
self.sm = StateMachine(self.rexarm, self.tp, self.kinect)
self.sm.is_logging = False
"""
Attach Functions to Buttons & Sliders
TODO: NAME AND CONNECT BUTTONS AS NEEDED
"""
# Video
self.ui.videoDisplay.setMouseTracking(True)
self.ui.videoDisplay.mouseMoveEvent = self.trackMouse
self.ui.videoDisplay.mousePressEvent = self.calibrateMousePress
# Buttons
# Handy lambda function that can be used with Partial to only set the new state if the rexarm is initialized
nxt_if_arm_init = lambda next_state: self.sm.set_next_state(
next_state if self.rexarm.initialized else None)
self.ui.btn_estop.clicked.connect(self.estop)
self.ui.btn_init_arm.clicked.connect(self.initRexarm)
self.ui.btnUser1.setText("Calibrate")
self.ui.btnUser1.clicked.connect(partial(nxt_if_arm_init, 'calibrate'))
##OUR_CODE
self.ui.btn_exec.clicked.connect(self.execute)
self.ui.btnUser2.clicked.connect(self.record)
self.ui.btnUser3.clicked.connect(self.playback)
self.ui.btnUser4.clicked.connect(self.execute_tp)
self.ui.btnUser5.clicked.connect(self.toggle_logging)
self.ui.btnUser1.clicked.connect(self.calibrate)
self.ui.btnUser6.clicked.connect(self.blockDetect)
self.ui.btnUser7.clicked.connect(self.openGripper)
self.ui.btnUser8.clicked.connect(self.closeGripper)
self.ui.btnUser9.clicked.connect(self.clickGrab)
# Sliders
for sldr in self.joint_sliders:
sldr.valueChanged.connect(self.sliderChange)
self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange)
self.ui.sldrSpeed.valueChanged.connect(self.sliderChange)
# Direct Control
self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk)
# Status
self.ui.rdoutStatus.setText("Waiting for input")
# Auto exposure
self.ui.chkAutoExposure.stateChanged.connect(self.autoExposureChk)
"""initalize manual control off"""
self.ui.SliderFrame.setEnabled(False)
"""Setup Threads"""
# Rexarm runs its own thread
# Video
self.videoThread = VideoThread(self.kinect)
self.videoThread.updateFrame.connect(self.setImage)
self.videoThread.start()
# State machine
self.logicThread = LogicThread(self.sm)
self.logicThread.start()
# Display
self.displayThread = DisplayThread(self.rexarm, self.sm)
self.displayThread.updateJointReadout.connect(self.updateJointReadout)
self.displayThread.updateEndEffectorReadout.connect(
self.updateEndEffectorReadout)
self.displayThread.updateStatusMessage.connect(
self.updateStatusMessage)
self.displayThread.updateJointErrors.connect(self.updateJointErrors)
self.displayThread.start()
class TrajectoryGenerator:
def __init__(self, path, config, fit_type=FitType.CUBIC):
self.path = path
self.config = config
self.splineGenerator = SplineGenerator(fit_type)
self.trajectory = Trajectory()
self.splineUtils = SplineUtils()
self.planner = TrajectoryPlanner(config)
self.prepare()
def prepare(self):
if(len(self.path) < 2):
print("Error: TrajectoryGenerator preparation failed: path length is less than 2")
return
self.trajectory.spline_list = []
self.trajectory.length_list = []
self.trajectory.total_length = 0
for i in range(len(self.path) - 1):
s = self.splineGenerator.fit(self.path[i], self.path[i + 1])
dist = self.splineUtils.get_arc_length(s, self.config.sample_count)
self.trajectory.spline_list.append(s)
self.trajectory.length_list.append(dist)
self.trajectory.total_length = self.trajectory.total_length + dist
self.config.dest_pos = self.trajectory.total_length
self.config.src_theta = self.path[0].angle
self.config.dest_theta = self.path[0].angle
self.planner.prepare()
self.trajectory.length = self.planner.info.length
self.trajectory.path_length = len(self.path)
self.trajectory.config = self.config
def generate(self):
trajectory_length = self.trajectory.length
path_length = self.trajectory.path_length
splines = self.trajectory.spline_list
spline_lengths = self.trajectory.length_list
segments = self.planner.create()
spline_i = 0
spline_pos_initial = 0.0
for i in range(trajectory_length):
displacement = segments[i].displacement
while True:
pos_relative = displacement - spline_pos_initial
# all but the last point on a spline
if(pos_relative <= spline_lengths[spline_i]):
si = splines[spline_i]
percentage = self.splineUtils.get_progress_for_distance(si, pos_relative, self.config.sample_count)
coords = self.splineUtils.get_coords(si, percentage)
segments[i].heading = self.splineUtils.get_angle(si, percentage)
segments[i].x = coords.x
segments[i].y = coords.y
break
# finish a spline
elif spline_i < path_length - 2:
spline_pos_initial += spline_lengths[spline_i]
spline_i += 1
# Very last point
else:
si = splines[path_length - 2]
segments[i].heading = self.splineUtils.get_angle(si, 1.0)
coords = self.splineUtils.get_coords(si, 1.0)
segments[i].x = coords.x
segments[i].y = coords.y
break
splines = []
spline_lengths = []
return segments
def __init__(self, parent=None):
QWidget.__init__(self, parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
""" Set GUI to track mouse """
QWidget.setMouseTracking(self, True)
"""dynamixel bus -- add other motors here"""
self.dxlbus = DXL_BUS(DEVICENAME, BAUDRATE)
port_num = self.dxlbus.port()
base = DXL_MX(port_num, 0)
shld = DXL_MX(port_num, 1)
elbw = DXL_MX(port_num, 2)
wrst = DXL_AX(port_num, 3)
wrst2 = DXL_XL(port_num, 4)
grip = DXL_XL(port_num, 5)
"""Objects Using Other Classes"""
self.rexarm = Rexarm((base, shld, elbw, wrst, wrst2), grip)
self.kinect = Kinect(self.rexarm)
self.tp = TrajectoryPlanner(self.rexarm)
self.sm = StateMachine(self.rexarm, self.tp, self.kinect)
"""
Attach Functions to Buttons & Sliders
TODO: NAME AND CONNECT BUTTONS AS NEEDED
"""
self.ui.btn_estop.clicked.connect(self.estop)
self.ui.btnUser1.setText("Calibrate")
self.ui.btnUser1.clicked.connect(
partial(self.sm.set_next_state, "calibrate"))
self.ui.sldrBase.valueChanged.connect(self.sliderChange)
self.ui.sldrShoulder.valueChanged.connect(self.sliderChange)
self.ui.sldrElbow.valueChanged.connect(self.sliderChange)
self.ui.sldrWrist.valueChanged.connect(self.sliderChange)
### sliders gripper
self.ui.sldrGrip1.valueChanged.connect(self.sliderChange)
self.ui.sldrGrip2.valueChanged.connect(self.sliderChange)
### button gripper open & close
self.ui.btnUser5.clicked.connect(self.btn2clicked)
self.ui.btnUser6.clicked.connect(self.btn1clicked)
self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange)
self.ui.sldrSpeed.valueChanged.connect(self.sliderChange)
self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk)
self.ui.rdoutStatus.setText("Waiting for input")
self.ui.btn_exec.clicked.connect(
partial(self.sm.set_next_state, "execute"))
self.ui.btnUser2.setText("Record wp")
self.ui.btnUser3.setText("Clear wp")
self.ui.btnUser4.setText("Click and pick")
self.ui.btnUser2.clicked.connect(
partial(self.sm.set_next_state, "add_wp"))
self.ui.btnUser3.clicked.connect(
partial(self.sm.set_next_state, "clear_wp"))
self.ui.btnUser4.clicked.connect(
partial(self.sm.set_next_state, "click_and_pick"))
self.ui.btnUser7.setText("Save Calibration Points")
self.ui.btnUser8.setText("Load Previous Calibration")
self.ui.btnUser7.clicked.connect(
partial(self.sm.set_next_state, "save_calibration_points"))
self.ui.btnUser8.clicked.connect(
partial(self.sm.set_next_state, "load_previous_calibration"))
self.ui.btnUser9.setText("Record Block Position")
self.ui.btnUser9.clicked.connect(
partial(self.sm.set_next_state, "record_block_position"))
self.ui.btnUser5.setText("Open Gripper")
self.ui.btnUser6.setText("Close Gripper")
self.ui.btn_task1.clicked.connect(
partial(self.sm.set_next_state, "mirror"))
self.ui.btn_task2.clicked.connect(
partial(self.sm.set_next_state, "stack_3"))
self.ui.btn_task3.clicked.connect(
partial(self.sm.set_next_state, "line_em_up"))
self.ui.btn_task4.clicked.connect(
partial(self.sm.set_next_state, "stack_em_high"))
self.ui.btn_exec_6.clicked.connect(
partial(self.sm.set_next_state, "pyramid5"))
"""initalize manual control off"""
self.ui.SliderFrame.setEnabled(False)
"""initalize rexarm"""
self.rexarm.initialize()
"""Setup Threads"""
self.videoThread = VideoThread(self.kinect)
self.videoThread.updateFrame.connect(self.setImage)
self.videoThread.start()
self.logicThread = LogicThread(self.sm)
self.logicThread.start()
self.displayThread = DisplayThread(self.rexarm, self.sm)
self.displayThread.updateJointReadout.connect(self.updateJointReadout)
self.displayThread.updateEndEffectorReadout.connect(
self.updateEndEffectorReadout)
self.displayThread.updateStatusMessage.connect(
self.updateStatusMessage)
self.displayThread.start()
"""
Setup Timer
this runs the trackMouse function every 50ms
"""
self._timer = QTimer(self)
self._timer.timeout.connect(self.trackMouse)
self._timer.start(50)
def __init__(self, parent=None):
QWidget.__init__(self, parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
""" Set GUI to track mouse """
QWidget.setMouseTracking(self, True)
"""
Dynamixel bus
TODO: add other motors here as needed with their correct address"""
self.dxlbus = DXL_BUS(DEVICENAME, BAUDRATE)
port_num = self.dxlbus.port()
base = DXL_MX(port_num, 1)
shld = DXL_MX(port_num, 2)
elbw = DXL_MX(port_num, 3)
wrst = DXL_AX(port_num, 4)
wrst2 = DXL_AX(port_num, 5)
#wrst3 = DXL_XL(port_num, 6)
#grip = DXL_XL(port_num, 7)
#wrst2.set_compliance(8,64)
"""Objects Using Other Classes"""
self.kinect = Kinect()
self.rexarm = Rexarm((base, shld, elbw, wrst, wrst2), 0)
self.tp = TrajectoryPlanner(self.rexarm)
self.sm = StateMachine(self.rexarm, self.tp, self.kinect)
"""
Attach Functions to Buttons & Sliders
TODO: NAME AND CONNECT BUTTONS AS NEEDED
"""
self.ui.btn_estop.clicked.connect(self.estop)
self.ui.btn_exec.clicked.connect(self.execute)
self.ui.btn_task1.clicked.connect(self.record)
self.ui.btn_task2.clicked.connect(self.clear_waypoints)
self.ui.btn_task3.clicked.connect(self.toggle_gripper)
self.ui.btnUser1.setText("Calibrate")
self.ui.btnUser1.clicked.connect(
partial(self.sm.set_next_state, "calibrate"))
self.ui.btnUser2.setText("Block Detector")
self.ui.btnUser2.clicked.connect(
partial(self.sm.set_next_state, "block detection"))
self.ui.btnUser2.setText("Color Buckets")
self.ui.btnUser2.clicked.connect(
partial(self.sm.set_next_state, "color buckets"))
self.ui.btnUser3.setText("Click Grab/Drop Mode")
self.ui.btnUser3.clicked.connect(
partial(self.sm.set_next_state, "click grab drop"))
self.ui.btnUser4.setText("Pick n' Stack")
self.ui.btnUser4.clicked.connect(
partial(self.sm.set_next_state, "pick and stack"))
self.ui.btnUser5.setText("Line 'em up")
self.ui.btnUser5.clicked.connect(
partial(self.sm.set_next_state, "line them up"))
self.ui.btnUser6.setText("Stack 'em high")
self.ui.btnUser6.clicked.connect(
partial(self.sm.set_next_state, "stack them high"))
self.ui.btnUser7.setText("Block slider")
self.ui.btnUser7.clicked.connect(
partial(self.sm.set_next_state, "block slider"))
self.ui.btnUser8.setText("Hot swap")
self.ui.btnUser8.clicked.connect(
partial(self.sm.set_next_state, "hot swap"))
self.ui.btnUser9.setText("Calibration Accuracy")
self.ui.btnUser9.clicked.connect(
partial(self.sm.set_next_state, "Calibration Accuracy"))
self.ui.sldrBase.valueChanged.connect(self.sliderChange)
self.ui.sldrShoulder.valueChanged.connect(self.sliderChange)
self.ui.sldrElbow.valueChanged.connect(self.sliderChange)
self.ui.sldrWrist.valueChanged.connect(self.sliderChange)
self.ui.sldrWrist2.valueChanged.connect(self.sliderChange)
self.ui.sldrWrist3.valueChanged.connect(self.sliderChange)
self.ui.sldrGrip1.valueChanged.connect(self.sliderChange)
self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange)
self.ui.sldrSpeed.valueChanged.connect(self.sliderChange)
self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk)
self.ui.rdoutStatus.setText("Waiting for input")
"""initalize manual control off"""
self.ui.SliderFrame.setEnabled(False)
"""initalize rexarm"""
self.rexarm.initialize()
"""Setup Threads"""
self.videoThread = VideoThread(self.kinect)
self.videoThread.updateFrame.connect(self.setImage)
self.videoThread.start()
self.logicThread = LogicThread(self.sm)
self.logicThread.start()
self.displayThread = DisplayThread(self.rexarm, self.sm)
self.displayThread.updateJointReadout.connect(self.updateJointReadout)
self.displayThread.updateEndEffectorReadout.connect(
self.updateEndEffectorReadout)
self.displayThread.updateStatusMessage.connect(
self.updateStatusMessage)
self.displayThread.start()
"""
Setup Timer
this runs the trackMouse function every 50ms
"""
self._timer = QTimer(self)
self._timer.timeout.connect(self.trackMouse)
self._timer.start(50)
tmp_waypoints = [[0.0, 0.0, 0.0, 0.0], [np.pi * 0.1, 0.0, np.pi / 2, 0.0],
[np.pi * 0.25, np.pi / 2, -np.pi / 2, np.pi / 2],
[np.pi * 0.4, np.pi / 2, -np.pi / 2, 0.0],
[np.pi * 0.55, 0, 0, 0], [np.pi * 0.7, 0.0, np.pi / 2, 0.0],
[np.pi * 0.85, np.pi / 2, -np.pi / 2, np.pi / 2],
[np.pi, np.pi / 2, -np.pi / 2, 0.0],
[0.0, np.pi / 2, np.pi / 2, 0.0],
[np.pi / 2, -np.pi / 2, np.pi / 2, 0.0]]
waypoints = []
for wp in tmp_waypoints:
full_wp = [0.0] * rexarm.num_joints
full_wp[0:len(wp)] = wp
waypoints.append(full_wp)
if args.trajectory_planner:
tp = TrajectoryPlanner(rexarm)
for wp in waypoints:
tp.set_initial_wp()
tp.set_final_wp(wp)
tp.go(max_speed=2.5)
time.sleep(1.0)
else:
for wp in waypoints:
rexarm.set_positions(wp)
time.sleep(1)
rexarm.disable_torque()
time.sleep(0.1)
def __init__(self, parent=None):
QWidget.__init__(self, parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
""" Set GUI to track mouse """
QWidget.setMouseTracking(self, True)
"""
Dynamixel bus
TODO: add other motors here as needed with their correct address"""
self.dxlbus = DXL_BUS(DEVICENAME, BAUDRATE)
port_num = self.dxlbus.port()
base = DXL_XL(port_num, 1)
shld = DXL_XL(port_num, 2)
elbw = DXL_XL(port_num, 3)
wrst = DXL_XL(port_num, 4)
grip = DXL_XL(port_num, 5)
"""Objects Using Other Classes"""
self.camera = PiCamera()
self.rexarm = Rexarm((base, shld, elbw, wrst, grip))
self.tp = TrajectoryPlanner(self.rexarm)
self.sm = StateMachine(self.rexarm, self.tp)
self.taskThread = TaskThread(self.sm)
self.rgb_image = None
"""
Attach Functions to Buttons & Sliders
TODO: NAME AND CONNECT BUTTONS AS NEEDED
"""
self.ui.btn_estop.clicked.connect(self.estop)
self.ui.btnUser1.setText("Calibrate")
self.ui.btnUser1.clicked.connect(
partial(self.sm.set_current_state, "calibrate"))
self.ui.btn_task1.clicked.connect(
partial(self.taskThread.set_task_num, 1))
self.ui.btn_task2.clicked.connect(
partial(self.taskThread.set_task_num, 2))
self.ui.btn_task3.clicked.connect(
partial(self.taskThread.set_task_num, 3))
self.ui.btn_task4.clicked.connect(
partial(self.taskThread.set_task_num, 4))
self.ui.sldrBase.valueChanged.connect(self.sliderChange)
self.ui.sldrShoulder.valueChanged.connect(self.sliderChange)
self.ui.sldrElbow.valueChanged.connect(self.sliderChange)
self.ui.sldrWrist.valueChanged.connect(self.sliderChange)
self.ui.sldrWrist2.valueChanged.connect(self.sliderChange)
self.ui.sldrGrip1.valueChanged.connect(self.sliderChange)
self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange)
self.ui.sldrSpeed.valueChanged.connect(self.sliderChange)
self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk)
self.ui.rdoutStatus.setText("Waiting for input")
"""initalize manual control off"""
self.ui.SliderFrame.setEnabled(False)
"""initalize rexarm"""
self.rexarm.initialize()
"""Setup Threads"""
self.videoThread = VideoThread(self.camera)
self.videoThread.updateFrame.connect(self.setImage)
self.videoThread.updateAprilTags.connect(self.updateAprilTags)
self.videoThread.updateImage.connect(self.updateImage)
self.videoThread.start()
self.logicThread = LogicThread(self.sm)
self.logicThread.start()
self.taskThread.start()
self.displayThread = DisplayThread(self.rexarm, self.sm)
self.displayThread.updateJointReadout.connect(self.updateJointReadout)
self.displayThread.updateEndEffectorReadout.connect(
self.updateEndEffectorReadout)
self.displayThread.updateStatusMessage.connect(
self.updateStatusMessage)
self.displayThread.start()
"""
Setup Timer
this runs the trackMouse function every 50ms
"""
self._timer = QTimer(self)
self._timer.timeout.connect(self.trackMouse)
self._timer.start(50)
def __init__(self,parent=None):
QWidget.__init__(self,parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
""" Set GUI to track mouse """
QWidget.setMouseTracking(self,True)
"""
Dynamixel bus
TODO: add other motors here as needed with their correct address"""
self.dxlbus = DXL_BUS(DEVICENAME, BAUDRATE)
port_num = self.dxlbus.port()
"""
467TODO:
Based on motors we are using, modify ports and definition of rexarm
Assume only use shld, elbw, wrst for now
"""
base = DXL_MX(port_num, 1)
shld = DXL_MX(port_num, 2)
elbw = DXL_MX(port_num, 3)
wrst = DXL_AX(port_num, 4)
#wrst2 = DXL_AX(port_num, 1)
"""Objects Using Other Classes"""
self.kinect = None #Kinect()
#self.rexarm = Rexarm((base,shld,elbw,wrst,wrst2),0)
self.rexarm = Rexarm((base,shld,elbw, wrst),0)
self.tp = TrajectoryPlanner(self.rexarm)
self.sm = StateMachine(self.rexarm, self.tp, self.kinect)
"""
Attach Functions to Buttons & Sliders
TODO: NAME AND CONNECT BUTTONS AS NEEDED
"""
self.ui.btn_estop.clicked.connect(self.estop)
self.ui.btnUser1.setText("Calibrate")
self.ui.btnUser1.clicked.connect(partial(self.sm.set_next_state, "calibrate"))
self.ui.sldrBase.valueChanged.connect(self.sliderChange)
self.ui.sldrShoulder.valueChanged.connect(self.sliderChange)
self.ui.sldrElbow.valueChanged.connect(self.sliderChange)
self.ui.sldrWrist.valueChanged.connect(self.sliderChange)
self.ui.sldrWrist2.valueChanged.connect(self.sliderChange)
self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange)
self.ui.sldrSpeed.valueChanged.connect(self.sliderChange)
self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk)
self.ui.rdoutStatus.setText("Waiting for input")
"""initalize manual control off"""
self.ui.SliderFrame.setEnabled(False)
"""initalize rexarm"""
self.rexarm.initialize()
"""Setup Threads"""
# self.videoThread = VideoThread(self.kinect)
# self.videoThread.updateFrame.connect(self.setImage)
# self.videoThread.start()
self.logicThread = LogicThread(self.sm)
self.logicThread.start()
self.displayThread = DisplayThread(self.rexarm, self.sm)
self.displayThread.updateJointReadout.connect(self.updateJointReadout)
self.displayThread.updateEndEffectorReadout.connect(self.updateEndEffectorReadout)
self.displayThread.updateStatusMessage.connect(self.updateStatusMessage)
self.displayThread.start()
"""
Setup Timer
this runs the trackMouse function every 50ms
"""
self._timer = QTimer(self)
self._timer.timeout.connect(self.trackMouse)
self._timer.start(50)
def __init__(self,parent=None):
QWidget.__init__(self,parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
""" Set GUI to track mouse """
QWidget.setMouseTracking(self,True)
"""dynamixel bus -- add other motors here"""
self.dxlbus = DXL_BUS(DEVICENAME, BAUDRATE)
port_num = self.dxlbus.port()
base = DXL_MX(port_num, 0)
shld = DXL_MX(port_num, 1)
elbw = DXL_MX(port_num, 2)
wrst = DXL_AX(port_num, 3)
wrst2 = DXL_XL(port_num, 4)
grip = DXL_XL(port_num, 5)
"""Objects Using Other Classes"""
self.kinect = Kinect()
self.rexarm = Rexarm((base,shld,elbw,wrst,wrst2,grip),0)
self.tp = TrajectoryPlanner(self.rexarm)
self.sm = StateMachine(self.rexarm, self.tp, self.kinect)
"""
Attach Functions to Buttons & Sliders
TODO: NAME AND CONNECT BUTTONS AS NEEDED
"""
self.ui.btn_estop.clicked.connect(self.estop)
self.ui.btn_exec.clicked.connect(partial(self.sm.set_next_state, "execute"))
self.ui.btn_cali.clicked.connect(partial(self.sm.set_next_state, "calibrate"))
# self.ui.btnUser1.setText("Calibrate")
# self.ui.btnUser1.clicked.connect(partial(self.sm.set_next_state, "calibrate"))
self.ui.btnUser1.setText("Teach and Repeat")
self.ui.btnUser1.clicked.connect(partial(self.sm.set_next_state, "teachNRepeat"))
self.ui.btnUser2.setText("Record Waypoints")
self.ui.btnUser2.clicked.connect(partial(self.sm.set_next_state, "recordWaypoint"))
self.ui.btnUser3.setText("Play")
self.ui.btnUser3.clicked.connect(partial(self.sm.set_next_state, "play"))
self.ui.btnUser4.setText("Block Detection Start")
self.ui.btnUser4.clicked.connect(partial(self.sm.set_next_state, "blockDetectionStart"))
self.ui.btnUser5.setText("Block Message")
self.ui.btnUser5.clicked.connect(partial(self.sm.set_next_state, "blockMessage"))
self.ui.btnUser6.setText("Block Detection End")
self.ui.btnUser6.clicked.connect(partial(self.sm.set_next_state, "blockDetectionEnd"))
self.ui.btnUser7.setText("Click and Grab")
self.ui.btnUser7.clicked.connect(partial(self.sm.set_next_state, "clickNGrab"))
self.ui.btnUser8.setText("Pick n' Place")
self.ui.btnUser8.clicked.connect(partial(self.sm.set_next_state, "pickNPlace"))
self.ui.btnUser9.setText("Pick n' Stack")
self.ui.btnUser9.clicked.connect(partial(self.sm.set_next_state, "pickNStack"))
self.ui.btnUser10.setText("Line 'em Up!")
self.ui.btnUser10.clicked.connect(partial(self.sm.set_next_state, "lineUp"))
self.ui.btnUser11.setText("Stack 'em High!")
self.ui.btnUser11.clicked.connect(partial(self.sm.set_next_state, "stackHigh"))
self.ui.btnUser12.setText("Pyramid Builder!")
self.ui.btnUser12.clicked.connect(partial(self.sm.set_next_state, "buildPyramid"))
self.ui.sldrBase.valueChanged.connect(self.sliderChange)
self.ui.sldrShoulder.valueChanged.connect(self.sliderChange)
self.ui.sldrElbow.valueChanged.connect(self.sliderChange)
self.ui.sldrWrist.valueChanged.connect(self.sliderChange)
self.ui.sldrGrip1.valueChanged.connect(self.sliderChange)
self.ui.sldrGrip2.valueChanged.connect(self.sliderChange)
self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange)
self.ui.sldrSpeed.valueChanged.connect(self.sliderChange)
self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk)
self.ui.rdoutStatus.setText("Waiting for input")
"""initalize manual control off"""
self.ui.SliderFrame.setEnabled(False)
"""initalize rexarm"""
self.rexarm.initialize()
"""Setup Threads"""
self.videoThread = VideoThread(self.kinect)
self.videoThread.updateFrame.connect(self.setImage)
self.videoThread.start()
self.logicThread = LogicThread(self.sm)
self.logicThread.start()
self.displayThread = DisplayThread(self.rexarm, self.sm)
self.displayThread.updateJointReadout.connect(self.updateJointReadout)
self.displayThread.updateEndEffectorReadout.connect(self.updateEndEffectorReadout)
self.displayThread.updateStatusMessage.connect(self.updateStatusMessage)
self.displayThread.start()
"""
Setup Timer
this runs the trackMouse function every 50ms
"""
self._timer = QTimer(self)
self._timer.timeout.connect(self.trackMouse)
self._timer.start(50)
class StateMachine():
def __init__(self, rexarm, planner, kinect):
self.rexarm = rexarm
self.tp = planner
self.kinect = kinect
self.status_message = "State: Idle"
self.current_state = "idle"
self.next_state = "idle"
self.config = [[0] * self.rexarm.num_joints
] #change when gripper is added
# self.coeff_rgb_2_world = np.zeros((3,3),float)
self.tp = TrajectoryPlanner(rexarm)
self.world_mouse = [0, 0, 0]
# self.offset = 85
# self.remainder = 55
self.offset = 110
self.blob_height = 40
self.remainder = 70
self.config_event1 = []
self.config_event3 = []
self.config_event4 = []
self.config_event5 = []
def set_next_state(self, state):
self.next_state = state
""" This function is run continuously in a thread"""
def run(self):
if (self.current_state == "execute"):
if (self.next_state == "idle"):
self.idle()
if (self.next_state == "estop"):
self.estop()
if (self.next_state == "manual"):
self.manual()
if (self.current_state == "teach"):
if (self.next_state == "idle"):
self.idle()
if (self.next_state == "estop"):
self.estop()
if (self.next_state == "manual"):
self.manual()
if (self.current_state == "repeat"):
if (self.next_state == "idle"):
self.idle()
if (self.next_state == "estop"):
self.estop()
if (self.next_state == "manual"):
self.manual()
if (self.current_state == "manual"):
if (self.next_state == "manual"):
self.manual()
if (self.next_state == "idle"):
self.idle()
if (self.next_state == "estop"):
self.estop()
if (self.current_state == "idle"):
if (self.next_state == "manual"):
self.manual()
if (self.next_state == "rls"):
self.rls()
if (self.next_state == "teach"):
self.teach()
if (self.next_state == "repeat"):
self.repeat()
if (self.next_state == "execute"):
self.execute()
if (self.next_state == "idle"):
self.idle()
if (self.next_state == "estop"):
self.estop()
if (self.next_state == "calibrate"):
self.calibrate()
if (self.next_state == "grab"):
self.grab()
if (self.next_state == "drop_off"):
self.drop_off()
if (self.next_state == "move"):
self.move()
if (self.next_state == "event_1"):
self.event_1()
if (self.next_state == "event_2"):
self.event_2()
if (self.next_state == "event_3"):
self.event_3()
if (self.next_state == "event_4"):
self.event_4()
if (self.next_state == "event_5"):
self.event_5()
if (self.current_state == "estop"):
self.next_state = "estop"
self.estop()
if (self.current_state == "calibrate"):
if (self.next_state == "idle"):
self.idle()
if (self.current_state == "grab"):
if (self.next_state == "idle"):
self.idle()
if (self.current_state == "drop_off"):
if (self.next_state == "idle"):
self.idle()
if (self.current_state == "move"):
if (self.next_state == "idle"):
self.idle()
if (self.current_state == "event_1"):
if (self.next_state == "idle"):
self.idle()
if (self.current_state == "event_2"):
if (self.next_state == "idle"):
self.idle()
if (self.current_state == "event_3"):
if (self.next_state == "idle"):
self.idle()
if (self.current_state == "event_4"):
if (self.next_state == "idle"):
self.idle()
if (self.current_state == "event_5"):
if (self.next_state == "idle"):
self.idle()
"""Functions run for each state"""
def manual(self):
self.status_message = "State: Manual - Use sliders to control arm"
self.current_state = "manual"
self.rexarm.send_commands()
self.rexarm.get_feedback()
def idle(self):
self.status_message = "State: Idle - Waiting for input"
self.current_state = "idle"
self.rexarm.get_feedback()
def estop(self):
self.status_message = "EMERGENCY STOP - Check Rexarm and restart program"
self.current_state = "estop"
self.rexarm.disable_torque()
self.rexarm.get_feedback()
def execute(self):
self.status_message = "EXECUTE PLAN - Arm moves according to preset configuration"
self.current_state = "execute"
# config_space = [[ 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.5],
# [ 1.0, 0.8, 1.0, 0.5, 1.0, -2.0, -1.0],
# [-1.0,-0.8,-1.0,-0.5, -1.0, 3.0, 0.5],
# [-1.0, 0.8, 1.0, 0.5, 1.0, 1.0, -1.0],
# [1.0, -0.8,-1.0,-0.5, -1.0, -1.0, 0.5],
# [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.0]]
# config_space = [[ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0],
# [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.0],
# [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0],
# [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.0],
# [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0],
# [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.0]]
#IK test
self.rexarm.pause(4)
tmp = self.world_mouse
# pose = [tmp[0],tmp[1],-20,0,0,0] #input desired position and orientation
# pose = world2ik(tmp[0],tmp[1],-20,0,0,0)
# print('pose = ',pose)
oc = [-10, 10, 110]
# config_space1 = IK(pose,oc)
# config_space1 = np.append(config_space1,0)
# print(config_space1)
# #self.rexarm.set_positions(config_space1)
# pose = [30,30,160,0,0,0]
# pose1 = world2ik(150,100,-20,0,0,0)
# config_space1 = IK(pose1,oc)
# config_space1 = np.append(config_space1,0)
# config_space2 = IK(pose,oc)
# config_space2 = np.append(config_space2,0)
# config_space3 = deepcopy(config_space2)
# config_space3[-1] = 60
# pose = world2ik(0,0,0,0,0,0)
# config_space4 = IK(pose,oc)
# config_space4 = np.append(config_space4,np.deg2rad(80.0))
# pose2 = world2ik (150,100,-40,0,0,0)
# config_space5 = IK(pose2,oc)
# config_space5 = np.append(config_space5,0)
#print('config_space = ',config_space3)
offset = 85
remainder = 55
x_factor = 15 / 150
y_factor = 15 / 150
pose = world2ik(200 * 1.1, 200 * 1.1, 0 - self.offset, -np.pi / 2,
np.pi / 2, -np.pi / 2)
# pose = world2ik(-150 - 20,100 + 10,40-self.offset,-np.pi/3,np.pi,0)
config_space = IK(pose)
cur_config = self.rexarm.get_positions()
self.tp.execute_plan([cur_config, config_space])
self.rexarm.pause(1)
# pose = world2ik(-100 *1.1,100 * 1.1,0-self.offset ,-np.pi/2,np.pi/2,np.pi/2)
# config_space = IK(pose,1)
# cur_config = self.rexarm.get_positions();
# self.tp.execute_plan([cur_config,config_space])
# self.rexarm.pause(1)
#print(self.rexarm.gripper_state)
'''
pose = world2ik(tmp[0],tmp[1],tmp[2]-offset+remainder,0,np.pi,-np.pi/2)
config_space6 = IK(pose,oc)
pose = world2ik(tmp[0],tmp[1],tmp[2]-offset,0,np.pi,-np.pi/2)
config_space7 = IK(pose,oc)
for i in [config_space6,config_space7]:#,config_space2,config_space4]:
cur_config = self.rexarm.get_positions();
self.tp.execute_plan([cur_config,i]);
#self.rexarm.set_positions(i)
self.rexarm.pause(2)
self.rexarm.toggle_gripper()
self.rexarm.pause(1)
self.tp.execute_plan([self.rexarm.get_positions(),[0.0]*6])
self.rexarm.pause(4)
tmp = self.world_mouse
pose = world2ik(tmp[0],tmp[1],tmp[2]-offset+remainder,0,-np.pi/2,-np.pi/2)
config_space8 = IK(pose,oc)
pose = world2ik(tmp[0],tmp[1],tmp[2]-offset,0,-np.pi/2,-np.pi/2)
config_space9 = IK(pose,oc)
for i in [config_space8,config_space9]:#,config_space2,config_space4]:
cur_config = self.rexarm.get_positions();
self.tp.execute_plan([cur_config,i]);
#self.rexarm.set_positions(i)
self.rexarm.pause(2)
self.rexarm.toggle_gripper()
cur_config = self.rexarm.get_positions();
self.rexarm.pause(1)
self.tp.execute_plan([cur_config,config_space8]);
self.tp.execute_plan([self.rexarm.get_positions(),[0.0]*6])
'''
# self.rexarm.close_gripper()
# self.rexarm.pause(1)
# cur_config = self.rexarm.get_positions();
# self.tp.execute_plan([cur_config,config_space4]);
# #self.rexarm.set_positions(i)
# self.rexarm.pause(2)
# print(self.rexarm.get_positions())
# self.rexarm.pause(2)
# print(self.world_mouse)
'''
init = [0.0]*self.rexarm.num_joints;
final = deepcopy(init) #[1.0]*self.rexarm.num_joints;
final[1] = 2
plan = [init,final]
self.tp.execute_plan(plan)
time.sleep(5)
#self.rexarm.speeds = [1.0]*self.rexarm.num_joints
init = deepcopy(final)
final = [0.0]*self.rexarm.num_joints
plan = [init,final]
self.tp.execute_plan(plan)
'''
self.set_next_state("idle")
def rls(self):
self.status_message = "disable torque"
self.rexarm.disable_torque()
self.config = [[0] * self.rexarm.num_joints]
self.set_next_state("idle")
def teach(self):
self.status_message = "TEACH - Please move the arm"
#self.rexarm.disable_torque()
#self.rexarm.pause(2)
cur_config = self.rexarm.get_positions()
#print(cur_config)
self.config.append(deepcopy(cur_config))
#print(self.config)
#self.rexarm.enable_torque()
config = np.genfromtxt("A.csv", delimiter=",")
print(config)
m, n = config.shape
config_angles = []
for i in range(m):
row = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
for j in range(n):
row[j] = config[i][j]
config_angles.append(row)
config_angles.append(deepcopy(cur_config))
np.savetxt("A.csv", config_angles, delimiter=",")
print("configuration acquired\n")
self.set_next_state("idle")
def repeat(self):
self.rexarm.enable_torque()
self.status_message = "REPEAT - Arm moves according to what is taught"
self.current_state = "repeat"
self.rexarm.set_positions([0.0] * self.rexarm.num_joints)
#print(self.config)
# for j in self.config:
# self.rexarm.set_positions(j)
# #print(j)
# self.rexarm.pause(2)
print(self.config)
self.config_event5 = deepcopy(
self.config) #change event when necessary
print(self.config_event1)
self.set_next_state("idle")
def calibrate(self):
self.current_state = "calibrate"
self.set_next_state("idle")
#self.tp.go(max_speed=2.0)
# location_strings = ["lower left corner of board",
# "upper left corner of board",
# "upper right corner of board",
# "lower right corner of board",
# "center of 1 layer blob, right",
# "center of 2 layer blob, right",
# "center of 3 layer blob, right, upper board",
# "center of 3 layer blob, right, lower board",
# "center of 1 layer blob, left",
# "center of 2 layer blob, left",
# "center of 3 layer blob, left, upeer board",
# "center of 3 layer blob, left, lower board"]
location_strings = [
"lower left corner of board", "upper left corner of board",
"upper right corner of board", "lower right corner of board",
"center of 1 layer blob, right", "center of 2 layer blob, right",
"center of 1 layer blob, left", "center of 2 layer blob, left"
]
# "center of shoulder motor",
i = 0
for j in range(len(location_strings)):
self.status_message = "Calibration - Click %s in RGB image" % location_strings[
j]
while (i <= j):
self.rexarm.get_feedback()
if (self.kinect.new_click == True):
self.kinect.rgb_click_points[
i] = self.kinect.last_click.copy()
i = i + 1
self.kinect.new_click = False
i = 0
for j in range(len(location_strings)):
self.status_message = "Calibration - Click %s in depth image" % location_strings[
j]
while (i <= j):
self.rexarm.get_feedback()
if (self.kinect.new_click == True):
self.kinect.depth_click_points[
i] = self.kinect.last_click.copy()
i = i + 1
self.kinect.new_click = False
print self.kinect.rgb_click_points.shape
print self.kinect.depth_click_points.shape
# print self.kinect.currentDepthFrame
# print freenect.sync_get_depth()
"""TODO Perform camera calibration here"""
# print ('----self.kinect.currentDepthFrame----')
# print self.kinect.currentDepthFrame()
# co_eff = self.kinect.getAffineTransform(self.kinect.rgb_click_points, self.kinect.depth_click_points)
co_eff = self.kinect.getAffineTransform(self.kinect.depth_click_points,
self.kinect.rgb_click_points)
# print co_eff
# ------rgb_2_world, need better measurement--------------------
# hlf_len = 287.5
# left_lower_world = np.array([-hlf_len, hlf_len])
# left_upper_world = np.array([-hlf_len, -hlf_len])
# right_upper_world = np.array([hlf_len, -hlf_len])
# right_lower_world = np.array([hlf_len, hlf_len])
# world_points = np.append([left_lower_world],[left_upper_world,right_upper_world, right_lower_world], axis=0)
# print world_points
# self.coeff_rgb_2_world = self.kinect.getAffineTransform(self.kinect.rgb_click_points, world_points)
# print ('rgb_2_world',self.coeff_rgb_2_world)
self.status_message = "Calibration - Completed Calibration"
self.kinect.depth2rgb_affine = co_eff[0:2][0:]
self.kinect.kinectCalibrated = True
print('---print self.kinect.depth2rgb_affine-----')
print self.kinect.depth2rgb_affine
"""TODO Perform camera calibration here"""
self.kinect.intrinsic_matrix = self.kinect.loadCameraCalibration()
# print('----intrinsic_matrix---')
# print self.kinect.intrinsic_matrix
# convert d to Zc, for d use only 10 bits
d = np.zeros((len(location_strings), ), dtype=int)
time.sleep(2)
self.kinect.currentDepthFrame = deepcopy(
self.kinect.registerDepthFrame(freenect.sync_get_depth()[0]))
# rows,cols = self.kinect.currentDepthFrame.shape
# self.kinect.currentDepthFrame = cv2.warpAffine(self.kinect.currentDepthFrame, self.kinect.depth2rgb_affine, (cols,rows),1)
# self.kinect.captureDepthFrame()
for i in range(len(location_strings)):
yi = self.kinect.rgb_click_points[i][1]
xi = self.kinect.rgb_click_points[i][0]
d[i] = self.kinect.currentDepthFrame[yi][
xi] # the depth corresponding to points in rgb frame
print('---before clip----')
print d
np.clip(d, 0, 2**10 - 1, d)
print('---after clip')
print d
Zc = np.zeros((len(location_strings), ), dtype=float)
for i in range(len(location_strings)):
Zc[i] = 0.1236 * math.tan(d[i] / 2842.5 + 1.1863)
# print('---Zc-----')
# print Zc
# find the corresponding coordinate in camera frame
camera_points = np.zeros((len(location_strings), 3))
for i in range(len(location_strings)):
XYZ = Zc[i] * np.matmul(
np.linalg.inv(self.kinect.intrinsic_matrix),
np.append(self.kinect.rgb_click_points[i], 1))
camera_points[i][0] = XYZ[0]
camera_points[i][1] = XYZ[1]
camera_points[i][2] = XYZ[2]
# print('----camera_points----')
# print camera_points
# world frame coordinate
hlf_len = 50
blob_height = 38
left_lower_world = np.array([-6 * hlf_len, 6 * hlf_len, 0])
left_upper_world = np.array([-6 * hlf_len, -6 * hlf_len, 0])
right_upper_world = np.array([6 * hlf_len, -6 * hlf_len, 0])
right_lower_world = np.array([6 * hlf_len, 6 * hlf_len, 0])
# center_shoulder_world = np.array([0, 0, 11*blob_height])
one_layer_blob_world_right = np.array(
[3 * hlf_len, -hlf_len, blob_height])
two_layer_blob_world_right = np.array(
[3 * hlf_len, 3 * hlf_len, 2 * blob_height])
three_layer_blob_world_right_upper = np.array(
[4 * hlf_len, -4 * hlf_len, 3 * blob_height])
three_layer_blob_world_right_lower = np.array(
[4 * hlf_len, 4 * hlf_len, 3 * blob_height])
one_layer_blob_world_left = np.array(
[-3 * hlf_len, -hlf_len, blob_height])
two_layer_blob_world_left = np.array(
[-2 * hlf_len, 3 * hlf_len, 2 * blob_height])
three_layer_blob_world_left_upper = np.array(
[-4 * hlf_len, -4 * hlf_len, 3 * blob_height])
three_layer_blob_world_left_lower = np.array(
[-4 * hlf_len, 4 * hlf_len, 3 * blob_height])
# world_points = np.append([left_lower_world],[left_upper_world,right_upper_world, right_lower_world, \
# center_shoulder_world, one_layer_blob_world, two_layer_blob_world], axis=0)
# world_points = np.append([left_lower_world],[left_upper_world,right_upper_world, right_lower_world, \
# one_layer_blob_world_right, two_layer_blob_world_right,\
# three_layer_blob_world_right_upper, three_layer_blob_world_right_lower,\
# one_layer_blob_world_left, two_layer_blob_world_left,\
# three_layer_blob_world_left_upper, three_layer_blob_world_left_lower], axis=0)
world_points = np.append([left_lower_world],[left_upper_world,right_upper_world, right_lower_world, \
one_layer_blob_world_right, two_layer_blob_world_right,\
one_layer_blob_world_left, two_layer_blob_world_left], axis=0)
print('----world_points----')
print world_points
# get affine transformation between camera frame and world frame, inverse extrinsic matrix
self.kinect.co_eff_camera_2_world = self.kinect.getAffineTransform(
camera_points, world_points)
print('---co_eff_camera_2_world---')
print self.kinect.co_eff_camera_2_world
# find transformation from rgb to world
# find corresponding camera frame value
# the corresponding world frame value
# self.coeff_rgb_2_world =
# depth_frame = self.kinect.detectBlocksInDepthImage()
# Setup SimpleBlobDetector parameters.
# im_with_keypoints = self.kinect.detectBlocksInDepthImage()
# cv2.imshow("Keypoints", im_with_keypoints)
# cv2.waitKey(0)
# ######## error: too many values to unpack
# keypoints_world, keypoints_color, keypoints_orientation = self.kinect.blockDetector()
# ############################
# print('---keypoints_world----')
# print keypoints_world
# print('---keypoints_color----')
# print keypoints_color
# print('---keypoints_orientation----')
# print keypoints_orientation
time.sleep(1)
def event_1(self):
self.rexarm.open_gripper()
self.current_state = "event_1"
self.status_message = "State: Event 1 - Pick n stack! "
keypoints_world, keypoints_color, keypoints_orientation = self.kinect.blockDetector(
)
# print('------keypoints_world-----')
# print keypoints_world
# print('-----keypoints_color----')
# print keypoints_color
# print('-----keypoints_orientation----')
# print keypoints_orientation
config_space_init_final = np.zeros((3, 6))
config_space = np.zeros((3, 6))
config_before = np.zeros((3, 6))
destination = np.zeros((3, 6))
# destination[0][0] = -100
# destination[0][1] = -100
# destination[1][0] = -100
# destination[1][1] = -100
# destination[2][0] = -100
# destination[2][1] = -100
print('-----destination-----')
print destination
destination_before = np.zeros((3, 6))
destination_final = np.zeros((3, 6))
# ##---------------new----------------
# config_obj_pre = np.zeros((3,6))
# config_obj = np.zeros((3,6))
factor = 1.08
for i in range(3):
#------------new-----------------
# first move x, y position
# pose_obj_pre = world2ik(keypoints_world[i][0], keypoints_world[i][1], keypoints_world[i][2] -self.offset -10 + 100, 0, np.pi, -np.pi-keypoints_orientation[i])
# config_obj_pre[i] = IK(pose_obj_pre)
# pose_obj = world2ik(keypoints_world[i][0], keypoints_world[i][1], keypoints_world[i][2] -self.offset -10, 0, np.pi, -np.pi-keypoints_orientation[i])
# config_obj[i] = IK(pose_obj)
#-------------------------------------------
#------convert angle
# if keypoints_orientation[i]
# if keypoints_world[i][0] > 0:
# keypoints_world[i][0] = keypoints_world[i][0] + 10
# else:
# keypoints_world[i][0] = keypoints_world[i][0] - 10
keypoints_world[i][0] = keypoints_world[i][0] * factor
keypoints_world[i][1] = keypoints_world[i][1] * factor
pose = world2ik(keypoints_world[i][0], keypoints_world[i][1],
keypoints_world[i][2] - self.offset, 0, np.pi,
keypoints_orientation[i] * 3.141592 / 180.0)
config_space[i] = IK(pose, 1)
# pose = world2ik(keypoints_world[i][0],keypoints_world[i][1],keypoints_world[i][2]-self.offset + 40,0,np.pi,-np.pi-keypoints_orientation[i])
# config_before[i] = IK(pose)
pose = world2ik(
keypoints_world[i][0], keypoints_world[i][1],
keypoints_world[i][2] - self.offset + self.remainder, 0, np.pi,
keypoints_orientation[i] * 3.141592 / 180.0)
config_space_init_final[i] = IK(pose, 1)
# pose = world2ik(-100,200,-60,0,np.pi,0)
#if 1:
pose = world2ik(
-150, 0,
keypoints_world[i][2] - self.offset + self.blob_height * i, 0,
np.pi, np.pi)
destination[i] = IK(pose, 1)
pose = world2ik(
-150, 0, keypoints_world[i][2] - self.offset +
self.blob_height * i + self.remainder, 0, np.pi, np.pi)
destination_before[i] = IK(pose, 1)
pose = world2ik(
-150, 0, keypoints_world[i][2] - self.offset +
self.blob_height * (i + 1) + self.remainder, 0, np.pi, np.pi)
destination_final[i] = IK(pose, 1)
# else:
# pose = world2ik(-200,0, keypoints_world[i][2] - self.offset + self.blob_height*i, 0,np.pi/2,np.pi)
# destination[i] = IK(pose,1)
# pose = world2ik(-200,0, keypoints_world[i][2] - self.offset + self.blob_height*i + self.remainder, 0,np.pi/2,np.pi)
# destination_before[i] = IK(pose,1)
# # pose = world2ik(-100,100, keypoints_world[i][2] - self.offset + self.blob_height*(i+1) + self.remainder,0,np.pi/2,0)
# # destination_final[i] = IK(pose,1)
# destination_final[i] = deepcopy(destination[i])
# destination_final[i][-2] = 0
for j in range(3):
# self.tp.execute_plan([self.rexarm.get_positions(),config_obj_pre[j]])
# self.rexarm.pause(3)
# self.tp.execute_plan([self.rexarm.get_positions(),config_obj[j]])
# self.rexarm.pause(3)
# self.tp.execute_plan([self.rexarm.get_positions(),config_obj_pre[j]])
##################################################################
self.tp.execute_plan(
[self.rexarm.get_positions(), config_space_init_final[j]])
#self.rexarm.set_positions(config_space_init_final[j])
self.rexarm.pause(1)
self.tp.execute_plan(
[self.rexarm.get_positions(), config_space[j]])
#self.rexarm.set_positions(config_space[j])
self.rexarm.pause(1)
self.rexarm.close_gripper()
self.rexarm.pause(1)
self.tp.execute_plan(
[self.rexarm.get_positions(), config_space_init_final[j]])
#self.rexarm.set_positions(config_space_init_final[j])
self.rexarm.pause(1)
# self.tp.execute_plan([self.rexarm.get_positions(),[0.0]*6])
# #self.rexarm.set_positions([0]*6)
# self.rexarm.pause(1)
'''
self.tp.execute_plan([self.rexarm.get_positions(),destination_before[j]])
#self.rexarm.set_positions(destination_before[j])
self.rexarm.pause(1)
self.tp.execute_plan([self.rexarm.get_positions(),destination[j]])
#self.rexarm.set_positions(destination[j])
self.rexarm.pause(1)
self.rexarm.open_gripper()
self.rexarm.pause(1)
self.tp.execute_plan([self.rexarm.get_positions(),destination_final[j]])
#self.rexarm.set_positions(destination_final[j])
'''
self.tp.execute_plan([
self.rexarm.get_positions(),
config_tasks.task1_position[2 * j + 1]
])
self.rexarm.pause(1)
self.tp.execute_plan([
self.rexarm.get_positions(), config_tasks.task1_position[2 * j]
])
self.rexarm.pause(1)
self.rexarm.open_gripper()
self.rexarm.pause(1)
self.tp.execute_plan([
self.rexarm.get_positions(),
config_tasks.task1_position[2 * j + 1]
])
self.rexarm.pause(1)
self.tp.execute_plan([self.rexarm.get_positions(), [0] * 6])
self.set_next_state("idle")
def event_2(self):
self.current_state = "event_2"
self.status_message = "State: Event 2 - Wacky Pick n place!"
D2R = 3.141592 / 180.0
keypoints_world, keypoints_color, keypoints_orientation = self.kinect.blockDetector(
)
print('---keypoints_orientation------')
print keypoints_orientation
print('---keypoints_world------')
print keypoints_world
for i in range(len(keypoints_color)):
if keypoints_world[i][0] >= 0:
pose1 = world2ik(
keypoints_world[i][0] * 1.35, keypoints_world[i][1] * 1.35,
keypoints_world[i][2] - 20 - self.offset + 80 + 10,
-keypoints_orientation[i] * D2R, 0, np.pi + np.pi / 6)
pose = world2ik(
keypoints_world[i][0] - 20 *
(keypoints_world[i][0] / abs(keypoints_world[i][0])),
keypoints_world[i][1] - 20 *
(keypoints_world[i][1] / abs(keypoints_world[i][1])),
keypoints_world[i][2] - self.offset + 80 + 60,
-keypoints_orientation[i] * D2R, 0, np.pi + np.pi / 6)
else:
pose1 = world2ik(
keypoints_world[i][0] * 1.35, keypoints_world[i][1] * 1.35,
keypoints_world[i][2] - 20 - self.offset + 80 + 10,
-keypoints_orientation[i] * D2R, 0, np.pi - np.pi / 6)
pose = world2ik(
keypoints_world[i][0] - 20 *
(keypoints_world[i][0] / abs(keypoints_world[i][0])),
keypoints_world[i][1] - 20 *
(keypoints_world[i][1] / abs(keypoints_world[i][1])),
keypoints_world[i][2] - self.offset + 80 + 60,
-keypoints_orientation[i] * D2R, 0, np.pi - np.pi / 6)
config_heigh_space = IK(pose)
if keypoints_orientation[i] < -80:
config_heigh_space[-1] = config_heigh_space[-1] + np.pi / 2
config_space = IK(pose1)
config_space[-1] += np.pi / 2
config_heigh_space[-1] += np.pi / 2
self.tp.execute_plan(
[self.rexarm.get_positions(), config_heigh_space])
self.rexarm.pause(1)
self.tp.execute_plan([self.rexarm.get_positions(), config_space])
self.rexarm.pause(1)
self.rexarm.toggle_gripper()
self.rexarm.pause(1)
self.tp.execute_plan(
[self.rexarm.get_positions(), config_heigh_space])
self.rexarm.pause(1)
self.tp.execute_plan([self.rexarm.get_positions(), [0] * 6])
self.rexarm.pause(1)
self.set_next_state("idle")
def event_3(self):
self.current_state = "event_3"
self.status_message = "State: Event 3 - Line them up! "
self.rexarm.open_gripper()
used_color_name = []
block_interval = 50
D2R = 3.141592 / 180.0
# self.rexarm.pause(4)
# tmp1 = self.world_mouse
# self.rexarm.pause(4)
# tmp2 = self.world_mouse
# length = math.hypot(tmp1[0]-tmp2[0], tmp1[1]-tmp2[1])
# temp_x = (tmp2[0]-tmp1[0])*block_interval/length
# temp_y = (tmp2[0]-tmp1[0])*block_interval/length
# location_list = []
# for i in range(8):
# location_list.append([tmp1[0]+temp_x*i,tmp1[1]+temp_y*i])
while True:
keypoints_world, keypoints_color, keypoints_orientation = self.kinect.blockDetector(
)
print('------keypoints_world-----')
print keypoints_world
print('-----keypoints_color----')
print keypoints_color
print('-----keypoints_orientation----')
print keypoints_orientation
print('-----number of blocks detected----')
num = len(keypoints_color)
print num
flag2 = 0
color_list = [
'black', 'red', 'orange', 'yellow', 'green', 'blue', 'purple',
'pink'
]
color_name_list = []
for color in keypoints_color:
for i in range(8):
if color == color_list[i]:
color_name_list.append(i)
for i in range(num):
flag1 = 0
for used in used_color_name:
if color_name_list[i] == used:
flag1 = 1
if flag1 == 0:
pose = world2ik(keypoints_world[i][0] * 1.08,
keypoints_world[i][1] * 1.08,
keypoints_world[i][2] - self.offset - 15,
0, 0, keypoints_orientation[i] *
D2R) # NEED TO EDIT: horizontally grab
initial_position = IK(pose, 1)
pose = world2ik(keypoints_world[i][0] * 1.08,
keypoints_world[i][1] * 1.08,
keypoints_world[i][2] - self.offset - 15 +
self.remainder, 0, 0,
keypoints_orientation[i] *
D2R) # NEED TO EDIT: horizontally grab
initial_heigh_position = IK(pose, 1)
final_heigh_position = config_tasks.task3_config_destination[
color_name_list[i] * 2 + 1]
final_position = config_tasks.task3_config_destination[
color_name_list[i] * 2]
# pose = world2ik(location_list[color_name_list[i]][0],location_list[color_name_list[i]][1],0,0,0,0) # NEED TO EDIT: horizontally grab
# final_position = IK(pose,1)
# pose = world2ik(location_list[color_name_list[i]][0],location_list[color_name_list[i]][1],self.remainder,0,0,0) # NEED TO EDIT: horizontally grab
# final_heigh_position = IK(pose,1)
self.tp.execute_plan(
[self.rexarm.get_positions(), initial_heigh_position])
self.rexarm.pause(.5)
self.tp.execute_plan(
[self.rexarm.get_positions(), initial_position])
self.rexarm.pause(.5)
self.rexarm.close_gripper()
self.rexarm.pause(.5)
self.tp.execute_plan(
[self.rexarm.get_positions(), initial_heigh_position])
self.rexarm.pause(.5)
self.tp.execute_plan(
[self.rexarm.get_positions(), [0.0] * 6])
self.rexarm.pause(.5)
self.tp.execute_plan(
[self.rexarm.get_positions(), final_heigh_position])
self.rexarm.pause(.5)
self.tp.execute_plan(
[self.rexarm.get_positions(), final_position])
self.rexarm.pause(.5)
self.rexarm.open_gripper()
self.rexarm.pause(1)
self.tp.execute_plan(
[self.rexarm.get_positions(), final_heigh_position])
self.rexarm.pause(1)
self.tp.execute_plan(
[self.rexarm.get_positions(), [0.0] * 6])
self.rexarm.pause(1)
else:
flag2 += 1
if flag2 == len(color_name_list):
break
for color in color_name_list:
flag = 0
for used in used_color_name:
if used == color:
flag = 1
if flag == 0:
used_color_name.append(color)
self.set_next_state("idle")
def event_4(self):
self.current_state = "event_4"
self.status_message = "State: Event 4 - Stack them high!"
self.rexarm.open_gripper()
D2R = 3.141592 / 180.0
used_color_name = []
block_interval = 50
# NEED TO ADD DEFINE OF IK &IK2
# self.rexarm.pause(4)
# tmp = self.world_mouse
# stack_location = [tmp[0],tmp[1]]
# stack_location_list = []
# for i in range(8):
# stack_location_list.append([tmp[0],tmp[1],i*block_interval])
for k in [3, 2]:
keypoints_world, keypoints_color, keypoints_orientation = self.kinect.blockDetector(
)
print('------keypoints_world-----')
print keypoints_world
print('-----keypoints_color----')
print keypoints_color
print('-----keypoints_orientation----')
print keypoints_orientation
print('-----number of blocks detected----')
num = len(keypoints_color)
print num
# used_location = []
# used_location = [stack_location]
# for i in range(num):
# used_location.append([keypoints_world[i][0],keypoints_world[i][1]])
reput_block_num = 0
xy_candidate = []
location_candidate = []
candidate_num = 0
for j in range(len(config_tasks.task4_xy_waitlist)):
flag = 0
for i in range(num):
if math.hypot(
keypoints_world[i][0] -
config_tasks.task4_xy_waitlist[j][0],
keypoints_world[i][1] -
config_tasks.task4_xy_waitlist[j][1]) < 55:
flag = 1
if flag == 0:
xy_candidate.append(config_tasks.task4_xy_waitlist[j])
location_candidate.append(
config_tasks.task4_position_waitlist[2 * j])
location_candidate.append(
config_tasks.task4_position_waitlist[2 * j + 1])
candidate_num += 1
for i in range(num):
if keypoints_world[i][
2] >= k * 30: # NEED TO EDIT: this 30 can be changed
# print('-----used_location-----'+str(used_location))
# num_tried = 0
# while True:
# temp_location = [randint(-200,200),randint(-200,200)]
# if temp_location[0]<70 and temp_location[0]>-70 and temp_location[1]>0:
# num_tried += 1
# print('-----'+str(num_tried)+'random points tried-----')
# pass
# else:
# if math.hypot(temp_location[0],temp_location[1])<=math.hypot(120,150) and math.hypot(temp_location[0],temp_location[1])>100:
# flag = 0
# for j in range(len(used_location)):
# if math.hypot(used_location[j][0]-temp_location[0],used_location[j][1]-temp_location[1])<70:
# falg = 1
# if flag==0:
# used_location.append(temp_location)
# print('-----temp_location-----'+str(temp_location))
# break
# else:
# num_tried += 1
# print('-----'+str(num_tried)+'random points tried-----')
# else:
# num_tried += 1
# print('-----'+str(num_tried)+'random points tried-----')
pose = world2ik(keypoints_world[i][0],
keypoints_world[i][1],
keypoints_world[i][2] - self.offset, 0, 0,
keypoints_orientation[i] *
D2R) # NEED TO EDIT: horizontally grab
initial_position = IK(pose, 1)
pose = world2ik(
keypoints_world[i][0], keypoints_world[i][1],
keypoints_world[i][2] - self.offset + self.remainder,
0, 0, keypoints_orientation[i] *
D2R) # NEED TO EDIT: horizontally grab
initial_heigh_position = IK(pose, 1)
print('-----size & index-----')
print(candidate_num)
print(reput_block_num)
final_position = location_candidate[2 * reput_block_num]
final_heigh_position = location_candidate[2 *
reput_block_num +
1]
# pose = world2ik(temp_location[0],temp_location[1],38-self.offset,0,0,0) # NEED TO EDIT: horizontally grab
# final_position = IK(pose,1)
# pose = world2ik(temp_location[0],temp_location[1],38-self.offset+self.remainder,0,0,0) # NEED TO EDIT: horizontally grab
# final_heigh_position = IK(pose,1)
self.tp.execute_plan(
[self.rexarm.get_positions(), initial_heigh_position])
self.rexarm.pause(1)
self.tp.execute_plan(
[self.rexarm.get_positions(), initial_position])
self.rexarm.pause(1)
self.rexarm.toggle_gripper()
self.rexarm.pause(1)
self.tp.execute_plan(
[self.rexarm.get_positions(), initial_heigh_position])
self.rexarm.pause(1)
self.tp.execute_plan(
[self.rexarm.get_positions(), [0.0] * 6])
self.rexarm.pause(1)
self.tp.execute_plan(
[self.rexarm.get_positions(), final_heigh_position])
self.rexarm.pause(1)
self.tp.execute_plan(
[self.rexarm.get_positions(), final_position])
self.rexarm.pause(1)
self.rexarm.toggle_gripper()
self.rexarm.pause(1)
self.tp.execute_plan(
[self.rexarm.get_positions(), final_heigh_position])
self.rexarm.pause(1)
self.tp.execute_plan(
[self.rexarm.get_positions(), [0.0] * 6])
self.rexarm.pause(1)
reput_block_num += 1
keypoints_world, keypoints_color, keypoints_orientation = self.kinect.blockDetector(
)
print('------keypoints_world-----')
print keypoints_world
print('-----keypoints_color----')
print keypoints_color
print('-----keypoints_orientation----')
print keypoints_orientation
print('-----number of blocks detected----')
num = len(keypoints_color)
print num
color_list = [
'black', 'red', 'orange', 'yellow', 'green', 'blue', 'purple',
'pink'
]
color_name_list = []
for color in color_list:
for i in range(num):
if color == keypoints_color[i]:
color_name_list.append(i)
level = 0
for name in color_name_list:
pose = world2ik(keypoints_world[name][0], keypoints_world[name][1],
keypoints_world[name][2] - self.offset, 0, 0,
keypoints_orientation[name] *
D2R) # NEED TO EDIT: horizontally grab
initial_position = IK(pose, 1)
pose = world2ik(keypoints_world[name][0], keypoints_world[name][1],
keypoints_world[name][2] - self.offset +
self.remainder, 0, 0, keypoints_orientation[name] *
D2R) # NEED TO EDIT: horizontally grab
initial_heigh_position = IK(pose, 1)
final_position = config_tasks.task4_config_destination[level]
final_heigh_position = config_tasks.task4_config_destination[level
+ 1]
# pose = world2ik(stack_location_list[level][0],stack_location_list[level][1],stack_location_list[level][2],0,0,0) # NEED TO EDIT: horizontally grab
# final_position = IK(pose,1)
# pose = world2ik(stack_location_list[level][0],stack_location_list[level][1],stack_location_list[level][2]+self.remainder,0,0,0) # NEED TO EDIT: horizontally grab
# final_heigh_position = IK(pose,1)
self.tp.execute_plan(
[self.rexarm.get_positions(), initial_heigh_position])
self.rexarm.pause(1)
self.tp.execute_plan(
[self.rexarm.get_positions(), initial_position])
self.rexarm.pause(1)
self.rexarm.toggle_gripper()
self.rexarm.pause(1)
self.tp.execute_plan(
[self.rexarm.get_positions(), initial_heigh_position])
self.rexarm.pause(1)
self.tp.execute_plan([self.rexarm.get_positions(), [0.0] * 6])
self.rexarm.pause(1)
self.tp.execute_plan(
[self.rexarm.get_positions(), final_heigh_position])
self.rexarm.pause(1)
self.tp.execute_plan([self.rexarm.get_positions(), final_position])
self.rexarm.pause(1)
self.rexarm.toggle_gripper()
self.rexarm.pause(1)
self.tp.execute_plan(
[self.rexarm.get_positions(), final_heigh_position])
self.rexarm.pause(1)
self.tp.execute_plan([self.rexarm.get_positions(), [0.0] * 6])
self.rexarm.pause(1)
level += 1
self.set_next_state("idle")
def event_5(self):
self.current_state = "event_5"
self.status_message = "State: Event 5 - Block Mover!"
for i in range(len(config_tasks.task5_position)):
# for i in [len(config_tasks.task5_position)-1:0]:
self.tp.execute_plan(
[self.rexarm.get_positions(), config_tasks.task5_position[i]])
self.rexarm.pause(1)
if i == 2:
self.rexarm.toggle_gripper()
self.rexarm.pause(1)
if i == len(config_tasks.task5_position) - 2:
self.rexarm.toggle_gripper()
self.rexarm.pause(1)
self.tp.execute_plan([self.rexarm.get_positions(), [0.0] * 6])
self.rexarm.pause(1)
self.set_next_state("idle")
pass
def grab(self):
self.current_state = "grab"
self.status_message = "Grab block"
#----grab
self.rexarm.close_gripper()
self.set_next_state("idle")
pass
def drop_off(self):
self.current_state = "drop_off"
self.status_message = "Drop off block"
self.rexarm.open_gripper()
# drop off
self.set_next_state("idle")
# pass
def move(self):
self.current_state = "move"
self.status_message = "Move from current location to destination"
self.set_next_state("idle")
pass
def __init__(self, parent=None):
QWidget.__init__(self, parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
"""Set the running mode to simulation"""
self.mode = 'SIM'
""" Set GUI to track mouse """
# QWidget.setMouseTracking(self, True)
"""
Dynamixel bus
TODO: add other motors here as needed with their correct address"""
self.dxlbus = None
# port_num = self.dxlbus.port()
base = DXL_MX(None, 1)
shld = DXL_MX(None, 2)
elbw = DXL_MX(None, 3)
wrst = DXL_AX(None, 4)
# wrst2 = DXL_AX(5)
self.dxl = [base, shld, elbw, wrst]
"""Objects Using Other Classes"""
self.gzclient = GzSimClient(8081)
self.gzrexarm = gzrexarm.GzRexarm(self.gzclient)
self.real_rexarm = rexarm.Rexarm((base, shld, elbw, wrst), 0)
self.rexarm = rexarm_com.RexarmCom(self.gzrexarm, self.real_rexarm,
self.mode)
# self.kinect = Kinect()
self.tp = TrajectoryPlanner(self.rexarm)
# self.sm = StateMachine(self.rexarm, self.tp, self.kinect)
self.sm = StateMachine(self.rexarm, self.tp, None)
"""
Attach Functions to Buttons & Sliders
TODO: NAME AND CONNECT BUTTONS AS NEEDED
"""
self.ui.btn_estop.clicked.connect(self.estop)
self.ui.btnUser1.setText("Calibrate")
self.ui.btnUser1.clicked.connect(
partial(self.sm.set_next_state, "calibrate"))
self.ui.btnClearTrj.clicked.connect(self.clear_trj)
self.ui.btnAddCube.clicked.connect(self.add_cube)
self.ui.sldrBase.valueChanged.connect(self.sliderChange)
self.ui.sldrShoulder.valueChanged.connect(self.sliderChange)
self.ui.sldrElbow.valueChanged.connect(self.sliderChange)
self.ui.sldrWrist.valueChanged.connect(self.sliderChange)
self.ui.sldrGrip1.valueChanged.connect(self.sliderChange)
self.ui.sldrGrip2.valueChanged.connect(self.sliderChange)
self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange)
self.ui.sldrSpeed.valueChanged.connect(self.sliderChange)
# self.ui.sldrMaxTorque.valueChanged.connect(self.sliderTqSpChange)
# self.ui.sldrSpeed.valueChanged.connect(self.sliderTqSpChange)
self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk)
self.ui.chk_simulation.stateChanged.connect(self.simulationChk)
self.ui.chk_attached.stateChanged.connect(self.attachChk)
self.ui.chk_showtrj.stateChanged.connect(self.trjChk)
self.ui.rdoutStatus.setText("Waiting for input")
"""Initalize manual control off"""
self.ui.SliderFrame.setEnabled(False)
"""Lock torque and speed control soider in simulation"""
# self.lock_tqspslider()
"""Initalize the simulation"""
self.gzclient.start()
self.gzrexarm.initialize()
"""Setup Threads"""
# self.videoThread = VideoThread(self.kinect)
# self.videoThread.updateFrame.connect(self.setImage)
# self.videoThread.start()
self.logicThread = LogicThread(self.sm)
self.logicThread.start()
self.displayThread = DisplayThread(self.sm, self.real_rexarm,
self.gzrexarm, self.mode)
self.displayThread.updateJointReadout.connect(self.updateJointReadout)
self.displayThread.updateEndEffectorReadout.connect(
self.updateEndEffectorReadout)
self.displayThread.updateStatusMessage.connect(
self.updateStatusMessage)
self.displayThread.start()
"""