def __init__(self, legId=None, pos=None, rotate=None):
super().__init__(pos, rotate)
self.legId = legId
self.link_length_array = []
self.start_angles = []
self.solver = IKSolver(self.link_length_array, self.start_angles)
self.coord = CoordinateConverter()
def __init__(self):
super().__init__()
self.coord = CoordinateConverter()
self.setFixedSize(self.coord.scrWidth, self.coord.scrHeight)
self.draggableRectMap = {}
self.currentRect = None
def __init__(self, pos, z, leg):
QObject.__init__(self)
adjusted_pos = QPoint(pos.x() - self.size.width() / 2,
pos.y() - self.size.height() / 2)
self.pos = adjusted_pos
self.rect = QRect(adjusted_pos, self.size)
self.leg = leg
self.coord = CoordinateConverter()
self.z = z # z is the z coordinate of the leg in world space
class LinearTrajectory:
def __init__(self, leg, start_point, end_point):
self.leg = leg
self.linearInterpolator = LinearInterpolator(start_point, end_point,
10)
self.next = None
def setNext(self, nextTrajectory):
self.next = nextTrajectory
def getLastTrajectory(self):
if self.next:
return self.next.getLastTrajectory()
else:
return self
def go(self):
next_pos = self.linearInterpolator.get_next()
if next_pos:
self.leg.set_end_pos_local(next_pos)
# print("current leg position:", self.leg.get_target_pos())
return True
elif self.next:
return self.next.go()
return False
coord = CoordinateConverter()
@staticmethod
def genTrajectory(leg, world_positions):
retTraj = None
last_pos = None
for pos in world_positions:
if last_pos is None:
last_pos = pos
else:
last_obj_pos = LinearTrajectory.coord.worldToObject(
last_pos, leg.get_init_transformation_matrix())
next_obj_pos = LinearTrajectory.coord.worldToObject(
pos, leg.get_init_transformation_matrix())
newTraj = LinearTrajectory(leg, last_obj_pos, next_obj_pos)
last_pos = pos
if retTraj is None:
retTraj = newTraj
else:
retTraj.getLastTrajectory().setNext(newTraj)
return retTraj
class RoboLeg(LinkSystem):
def __init__(self, legId=None, pos=None, rotate=None):
super().__init__(pos, rotate)
self.legId = legId
self.link_length_array = []
self.start_angles = []
self.solver = IKSolver(self.link_length_array, self.start_angles)
self.coord = CoordinateConverter()
def add_link(self, length, axis, init_theta=0.0):
self.link_length_array.append(length)
self.start_angles.append(init_theta)
super().add_link(length, axis, init_theta)
def set_link_angle(self, link_id, theta):
self.links[link_id].setTheta(theta)
if RobotConfig.enable_serial:
GlobalContext.getSerial().set_angle(self.legId, link_id, theta)
def set_link_callback(self, link_id, callback):
self.links[link_id].angleChanged.connect(callback)
def set_end_pos_local(self, target_obj_pos):
print("Setting local pos:" + str(target_obj_pos))
thetas = self.solver.solve(target_obj_pos)
print("theta:" + str(thetas))
if thetas is not None:
# 3. Update angles
for i in range(len(thetas)):
self.set_link_angle(i, thetas[i])
def set_end_pos(self, target_world_pos):
print("Setting world pos:" + str(target_world_pos))
target_obj_pos = self.coord.worldToObject(
target_world_pos, self.get_init_transformation_matrix())
self.set_end_pos_local(target_obj_pos)
def getStatus(self):
retStr = ""
for linkIdx in range(len(self.links)):
link = self.links[linkIdx]
retStr += " link{}: {:4.2f}".format(linkIdx, link.getTheta())
return retStr
class IKWidget(QWidget):
legSelected = pyqtSignal(object)
def __init__(self):
super().__init__()
self.coord = CoordinateConverter()
self.setFixedSize(self.coord.scrWidth, self.coord.scrHeight)
self.draggableRectMap = {}
self.currentRect = None
def mousePressEvent(self, event):
for leg in self.draggableRectMap:
rect = self.draggableRectMap[leg]
if rect.contains(event.pos()):
self.currentRect = rect
self.legSelected.emit(self.currentRect)
self.update()
return
self.legSelected.emit(None)
self.currentRect = None
self.update()
def mouseMoveEvent(self, event):
if self.currentRect:
current_pos = event.pos()
self.currentRect.setPos(current_pos)
self.updateRobot()
self.update()
def paintEvent(self, event):
qp = QPainter()
qp.begin(self)
color = QColor(0, 0, 0)
color.setNamedColor('#d4d4d4')
qp.setPen(color)
qp.setBrush(QColor(127, 127, 127))
body_length = RobotConfig.bodyLength
body_width = RobotConfig.bodyWidth
body_rect = self.coord.convertRectToScr(-body_width / 2,
body_length / 2, body_width,
body_length)
qp.drawRect(body_rect)
# Draw all the leg targets
robot_legs = GlobalContext.getRobot().getLegs()
for leg in robot_legs:
leg_start_point = leg.get_start_pos()
leg_target_point = leg.get_target_pos()
target_scr_point = self.coord.worldToScr(
leg_target_point[0].item(0), leg_target_point[1].item(0))
if leg not in self.draggableRectMap:
self.draggableRectMap[leg] = DraggableRect(
target_scr_point, leg_target_point[2].item(0), leg)
self.draggableRectMap[leg].valueChanged.connect(
self.updateRobot)
qp.drawLine(
self.coord.worldToScr(leg_start_point[0], leg_start_point[1]),
target_scr_point)
if self.draggableRectMap[leg] is self.currentRect:
self.draggableRectMap[leg].draw(qp, True)
leg.setDrawCoordinate(True)
else:
self.draggableRectMap[leg].draw(qp, False)
leg.setDrawCoordinate(False)
self.drawCoordinate(qp)
qp.end()
def updateRobot(self):
if self.currentRect is not None:
cur_leg = self.currentRect.getLeg()
# use IK to find link position of the leg
world_pos = self.coord.scrToWorld(self.currentRect.getPos())
world_pos.append(self.currentRect.getZ())
cur_leg.set_end_pos(world_pos)
def drawCoordinate(self, qp):
color = QColor(0, 0, 0)
# Draw Coordinate
color.setNamedColor('#ff0000')
qp.setPen(color)
qp.drawLine(self.coord.worldToScr(0.0, 0.0),
self.coord.worldToScr(1.0, 0.0))
color.setNamedColor('#00ff00')
qp.setPen(color)
qp.drawLine(self.coord.worldToScr(0.0, 0.0),
self.coord.worldToScr(0.0, 1.0))
class DraggableRect(QObject):
valueChanged = pyqtSignal()
size = QSize(10, 10)
# leg means this rect corresponds to which leg.
def __init__(self, pos, z, leg):
QObject.__init__(self)
adjusted_pos = QPoint(pos.x() - self.size.width() / 2,
pos.y() - self.size.height() / 2)
self.pos = adjusted_pos
self.rect = QRect(adjusted_pos, self.size)
self.leg = leg
self.coord = CoordinateConverter()
self.z = z # z is the z coordinate of the leg in world space
def setZ(self, z):
self.z = z
self.valueChanged.emit()
def getZ(self):
return self.z
def getLeg(self):
return self.leg
def setPos(self, pos):
adjusted_pos = QPoint(pos.x() - self.size.width() / 2,
pos.y() - self.size.height() / 2)
self.pos = adjusted_pos
self.rect = QRect(adjusted_pos, self.size)
def getPos(self):
return self.pos
def draw(self, qp, selected=False):
old_pen = qp.pen()
if selected:
qp.setBrush(QColor(0, 0, 127))
else:
qp.setBrush(QColor(127, 127, 127))
qp.drawRect(self.rect)
if selected:
color = QColor(0, 0, 0)
color.setNamedColor('#ff0000')
qp.setPen(color)
# draw the local coordinate of the leg
leg_start_point = self.leg.get_start_pos()
leg_scr_position = self.coord.worldToScr(leg_start_point[0],
leg_start_point[1])
# Local x coordinate
worldPosX = self.coord.objectToWorld(
[1, 0, 0], self.leg.get_init_transformation_matrix())
scrPosX = self.coord.worldToScr(worldPosX[0], worldPosX[1])
qp.drawLine(leg_scr_position, scrPosX)
# Local y coordinate
color.setNamedColor('#00ff00')
qp.setPen(color)
worldPosY = self.coord.objectToWorld(
[0, 1, 0], self.leg.get_init_transformation_matrix())
scrPosY = self.coord.worldToScr(worldPosY[0], worldPosY[1])
qp.drawLine(leg_scr_position, scrPosY)
# Local z coordinate
color.setNamedColor('#0000ff')
qp.setPen(color)
worldPosZ = self.coord.objectToWorld(
[0, 0, 1], self.leg.get_init_transformation_matrix())
scrPosZ = self.coord.worldToScr(worldPosZ[0], worldPosZ[1])
qp.drawLine(leg_scr_position, scrPosZ)
qp.setPen(old_pen)
def contains(self, p):
return self.rect.contains(p)