def setUp(self):
'''
Initialize the client.
'''
super(DatabaseDependentTestCase, self).setUp()
self.client = PensiveClient(self.get_url(''),
client=FakeHTTPClient(self))
def __init__(self, host=None, port=None, pin=None, store=None):
self._store = store or PensiveClient().default()
if self._store.get('/simulate/vacuum', True):
self._rio = None
logger.warn('vacuum simulated')
else:
self._connect(host, port, pin)
logger.info('vacuum connected')
def __init__(self, robot='left', port=1000, store=None):
Assert(robot in robots, 'Unrecognized robot {}'.format(robot))
self.client = TrajClient(host=robots[robot], port=port)
self.name = self.client.name
self.store = store or PensiveClient().default()
self.receivedMilestones = []
# Vacuum
self.vacuum = Vacuum(store=self.store)
# Queue
self.startIndex = None
def __init__(self):
super(RealsenseCalibration, self).__init__()
self.initUI()
self.thread = VideoThread()
self.thread.signal.connect(self.updateView)
cameramat, cameracoeff = self.thread.getIntrinsics()
if cameramat is None:
logger.warning(
"Unable to get camera coefficients for camera. Setting to zeros..."
)
cameramat = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
cameracoeff = np.zeros((5))
self.camera_matrix = cameramat
self.camera_coeff = cameracoeff
self.cameraXform = np.identity(4)
self.db_client = PensiveClient(host='http://10.10.1.60:8888')
self.store = self.db_client.default()
register_numpy()
def __init__(self,
events=None,
callbacks=None,
finStates=None,
store=None):
self.events = {}
self.callbacks = {}
self.transitions = {}
self.finStates = {}
self.pastEvents = []
self.current = None
self.store = store or PensiveClient().default()
def __init__(self, robot='left', milestones=None, speed=1., store=None):
# Robot
self.robot = Robot(robot=robot, store=store)
if milestones:
self.trajectory = Trajectory(robot=self.robot,
milestones=milestones,
speed=speed)
else:
self.trajectory = None
self.freq = 10.
# Database
self.store = store or PensiveClient().default()
def __init__(self, robot='left', milestones=None, speed=1., store=None):
# Robot
if milestones:
self.trajectory = SimulatedTrajectory(milestones=milestones,
speed=speed,
store=store)
self.freq = 1 / milestones[0][0]
else:
self.trajectory = None
self.freq = 60
self.startTime = None
# Database
self.store = store or PensiveClient().default()
def __init__(self,
fromState,
toState,
altState,
condition=None,
checkpoint=None,
store=None,
checkState=None):
self.fromState = fromState.upper()
self.toState = toState.upper()
self.altState = altState.upper()
self.checkState = checkState
self.condition = condition
self.checkpoint = checkpoint
self.store = store or PensiveClient().default()
def __init__(self, milestones, speed=1, store=None):
self.milestones = {}
self.curr_index = None
self.curr_milestone = None
self.speed = speed
self.complete = False
self.vacuum = Vacuum(store=store)
# Process milestones
Assert(0 < self.speed <= 1,
'Speed {} is not in (0,1]'.format(self.speed))
self.milestones['database'] = milestones
self.milestones['robot'] = [
convertConfigToRobot(mi, speed) for mi in milestones
]
# store
self.store = store or PensiveClient().default()
def run(modal=True, store=None):
from pensive.client import PensiveClient
store = store or PensiveClient().default()
from PyQt4.QtGui import QApplication
app = QApplication.instance()
if app:
embedded = True
else:
embedded = False
app = QApplication([])
app.setApplicationName('ARC Reactor')
window = WorldViewerWindow(store)
if modal:
from PyQt4.QtCore import Qt
window.setWindowModality(Qt.ApplicationModal)
window.show()
if not embedded:
app.exec_()
def client_entry():
from random import choice, randint, uniform
from string import ascii_lowercase
from pensive.client import PensiveClient
store = PensiveClient().default()
for i in range(n_operations):
s = '/'.join([
''.join([choice(ascii_lowercase) for i in range(randint(1, 10))])
for j in range(randint(1, 4))
])
if uniform(0, 1) > 0.5:
store.get()
else:
store.put(s, uniform(0, 1))
def checkMilestone(self, m0, m1, dt, index):
"""Checks that the change in joint space distance does not exceed the max"""
Assert(
len(m0) == len(m1),
'Unequal milestone lengths: len({}) != len({})'.format(m0, m1))
for i, (m_p, m) in enumerate(zip(m0, m1)):
#Joint distance
dq = abs(m - m_p)
if dq > DQ_MAX:
self.failedMilestones.append(
('Exceeded joint distance', index, i))
logger.error(
'Milestone {}, Joint {} exceeded dq: {} > {}'.format(
index, i, round(dq, 2), DQ_MAX))
#Joint velocity
dv = dq / float(dt)
if dv > DV_MAX:
self.failedMilestones.append(
('Exceeded joint velocity', index, i))
logger.error(
'Milestone {}, Joint {} exceeded dv: {} > {}'.format(
index, i, round(dv, 2), DV_MAX))
if __name__ == "__main__":
plan = PensiveClient().default().get('robot/waypoints')
c = MotionPlanChecker(plan)
for error in c.check():
print error
def __init__(self, name, store=None):
self.name = name.upper()
self.store = store or PensiveClient().default()
class RealsenseCalibration(QtWidgets.QWidget):
def __init__(self):
super(RealsenseCalibration, self).__init__()
self.initUI()
self.thread = VideoThread()
self.thread.signal.connect(self.updateView)
cameramat, cameracoeff = self.thread.getIntrinsics()
if cameramat is None:
logger.warning(
"Unable to get camera coefficients for camera. Setting to zeros..."
)
cameramat = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
cameracoeff = np.zeros((5))
self.camera_matrix = cameramat
self.camera_coeff = cameracoeff
self.cameraXform = np.identity(4)
self.db_client = PensiveClient(host='http://10.10.1.60:8888')
self.store = self.db_client.default()
register_numpy()
def initUI(self):
#make the layout vertical
self.layout = QtWidgets.QVBoxLayout()
self.setLayout(self.layout)
#add places to enter/show the 4x4 matrix of the current postion of the robot
self.horzMid = QtWidgets.QHBoxLayout()
self.gridRobot = QtWidgets.QGridLayout()
self.gridCamera = QtWidgets.QGridLayout()
self.horzMid.addItem(self.gridRobot)
spacer = QtWidgets.QLabel(" ", self)
self.horzMid.addWidget(spacer)
spacer.show()
self.horzMid.addItem(self.gridCamera)
self.robotTextBoxes = []
self.cameraTextBoxes = []
for row in range(4):
self.robotTextBoxes.append([])
self.cameraTextBoxes.append([])
for col in range(4):
#add textbox to the grid
tbox = QtWidgets.QLineEdit(self)
self.robotTextBoxes[row].append(tbox)
self.gridRobot.addWidget(self.robotTextBoxes[row][col], row,
col)
tbox = QtWidgets.QLineEdit(self)
self.cameraTextBoxes[row].append(tbox)
self.gridCamera.addWidget(self.cameraTextBoxes[row][col], row,
col)
label = QtWidgets.QLabel("Camera Xform", self)
self.gridCamera.addWidget(label, 5, 2)
label.show()
label = QtWidgets.QLabel("Robot Xform", self)
self.gridRobot.addWidget(label, 5, 2)
label.show()
#set initial values for the matrices
eye = np.identity(4)
for row in range(4):
for col in range(4):
self.robotTextBoxes[row][col].setText(str(eye[row, col]))
self.cameraTextBoxes[row][col].setText(str(eye[row, col]))
#add a push button at the top
self.horzTop = QtWidgets.QHBoxLayout()
self.horzTop.addStretch(1)
self.calibrate_button = QtWidgets.QPushButton("Calibrate", self)
self.save_button = QtWidgets.QPushButton("Save", self)
self.horzTop.addWidget(self.calibrate_button)
self.horzTop.addWidget(self.save_button)
self.horzTop.addStretch(1)
self.calibrate_button.clicked.connect(self.calibrate_camera)
self.save_button.clicked.connect(self.save_calibration)
#add widget to show what the camera sees
self.camera_display = QtWidgets.QLabel(self)
zeroImg = np.zeros((480, 640, 3), dtype='uint8')
image = QtGui.QImage(zeroImg, zeroImg.shape[1], zeroImg.shape[0],
zeroImg.shape[1] * 3, QtGui.QImage.Format_RGB888)
pix = QtGui.QPixmap(image)
self.camera_display.setPixmap(pix)
self.horzBot = QtWidgets.QHBoxLayout()
self.horzBot.addStretch(1)
self.horzBot.addWidget(self.camera_display)
self.horzBot.addStretch(1)
#add everthing to the layout
self.layout.addItem(self.horzTop)
self.layout.addItem(self.horzMid)
self.layout.addItem(self.horzBot)
self.calibrate_button.show()
def calibrate_camera(self):
if self.thread.isRunning():
self.thread.keepRunning = False
else:
self.thread.keepRunning = True
self.thread.start()
def updateView(self, image, aligned_image, point_cloud):
#set the dictionary
dictionary = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_5X5_50)
#need to measure out a target
board = cv2.aruco.GridBoard_create(4, 6, .021, 0.003, dictionary)
#get the camera transform
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
res = cv2.aruco.detectMarkers(gray, dictionary)
if len(res[0]) > 0:
pose = cv2.aruco.estimatePoseBoard(res[0], res[1], board,
self.camera_matrix,
self.camera_coeff)
rotMat = cv2.Rodrigues(
pose[1]) #returns rotation vector and translation vector
rotMat = rotMat[0] #returns rotation matrix and jacobian
xform = np.zeros((4, 4))
xform[0:3, 0:3] = rotMat
xform[0:3, 3] = pose[2].flatten()
xform[3, 3] = 1
self.cameraXform = xform
#draw what opencv is tracking
#flip r and b channels to draw
image = image[:, :, ::-1].copy()
cv2.aruco.drawAxis(image, self.camera_matrix, self.camera_coeff,
pose[1], pose[2], 0.1)
image = image[:, :, ::-1].copy()
#flip back
else:
logger.warning(
"Unable to get the camera transform. Camera cannot see Charuco."
)
xform = np.zeros((4, 4))
xform[0, 0] = 1
xform[1, 1] = 1
xform[2, 2] = 1
xform[3, 3] = 1
self.cameraXform = xform
pix_img = QtGui.QImage(image, image.shape[1], image.shape[0],
image.shape[1] * 3, QtGui.QImage.Format_RGB888)
pix = QtGui.QPixmap(pix_img)
self.camera_display.setPixmap(pix)
#get the base to end effector transform
base2ee = self.store.get(key='robot/tcp_pose')
#get the end effector to aruco pose
ee2aruco = self.store.get(key='calibration/target_xform')
target_pose = base2ee.dot(ee2aruco)
#update the gui
for row in range(4):
for col in range(4):
self.robotTextBoxes[row][col].setText(
str(target_pose[row, col]))
#multiply to get the camera world pose
self.cameraXform = target_pose.dot(np.linalg.inv(self.cameraXform))
#update the gui
for row in range(4):
for col in range(4):
self.cameraTextBoxes[row][col].setText(
str(self.cameraXform[row, col]))
# logger.info(time.time()-t)
self.thread.can_emit = True
#update the database with the camera world location
self.store.put(key="camera/shelf0/pose", value=self.cameraXform)
#push out the point cloud
self.store.put(key="camera/shelf0/point_cloud", value=point_cloud)
self.store.put(key="camera/shelf0/aligned_image", value=aligned_image)
self.store.put(key="camera/shelf0/timestamp", value=time.time())
def save_calibration(self):
#get the camera serial number
sn = self.thread.serialNum
#get the robot matrix
robotmat = np.identity(4)
for row in range(4):
for col in range(4):
num = float(self.robotTextBoxes[row][col].text())
robotmat[row, col] = num
#multiply the transforms
full_transform = self.cameraXform * robotmat
print(full_transform)
#save it
np.save(sn + "_xform", full_transform)
def sync(db):
# update the robot config
rc = RobotController()
rc.updateCurrentConfig()
# build the world
world = build_world(db)
# update the robot tool pose
robot = world.robot('tx90l')
db.put('/robot/tcp_pose', klampt2numpy(robot.link(robot.numLinks() - 1).getTransform()))
if __name__ == '__main__':
from argparse import ArgumentParser, ArgumentDefaultsHelpFormatter
parser = ArgumentParser(description='synchronize with real robot', formatter_class=ArgumentDefaultsHelpFormatter)
parser.add_argument('-a', '--address', metavar='HOST', help='database server host')
parser.add_argument('-s', '--store', metavar='STORE', help='database store')
args = parser.parse_args()
# connect to the database
from pensive.client import PensiveClient
client = PensiveClient(args.address)
# get the store
store = client.store(args.store)
sync(store)
import matplotlib
matplotlib.use('Qt4Agg')
from matplotlib import pyplot
import numpy
from math import pi
from pensive.client import PensiveClient
s = PensiveClient().default()
mp = s.get('/robot/waypoints')
t = numpy.cumsum([w[0] for w in mp])
q = numpy.array([w[1]['robot'][1:] for w in mp])
v = numpy.array([w[1]['vacuum'][0] for w in mp])
pyplot.figure()
pyplot.plot(t, q * 180 / pi, '-+')
pyplot.plot(t, v * 100, '-+')
pyplot.xlabel('time (s)')
pyplot.ylabel('value')
pyplot.legend(['q{} (deg)'.format(i + 1) for i in range(len(q[0]))] + ['vacuum (%)'], bbox_to_anchor=(1,1))
pyplot.grid(True)
pyplot.show()