def __init__(self, basex, basey, length1, length2):
self.bbox_scale = 1.0
self.nsteps = 50
self.link1 = ArmPart(length1, scale=1.0)
self.link2 = ArmPart(length2, scale=1.0)
self.basex = basex
self.basey = basey
self.l2_angle = 2.5
from tkinter import ttk
"""Parámetros para Comunicación Serial"""
port= serial.Serial('COM4', 115200, timeout=1.0)
port.set_buffer_size(5, 1)
port.flushInput()
"""Parametros para Pygame"""
black = (0, 0, 0)
white = (255, 255, 255)
pygame.init()
width = 850
height = 750
display = pygame.display.set_mode((width, height))
fpsClock = pygame.time.Clock()
upperarm = ArmPart('upperarm.png', scale=.7)
forearm = ArmPart('forearm.png', scale=.8)
hand = ArmPart('hand.png', scale=1.0)
origin = (width / 2, height / 2)
m = 0
n = 0
"""Funciones a utilizar"""
def popupmsg(msg):
popup = tk.Tk()
popup.wm_title("!")
label = ttk.Label(popup, text=msg)
ttk.Label()
label.pack(side="top", fill="x", pady=10)
B1 = ttk.Button(popup, text="Okay", command = popup.destroy)
import pygame
import pygame.locals
from armpart import ArmPart
black = (0, 0, 0)
white = (255, 255, 255)
pygame.init()
width = 750
height = 750
display = pygame.display.set_mode((width, height))
fpsClock = pygame.time.Clock()
upperarm = ArmPart('upperarm.png', scale=.7)
forearm = ArmPart('forearm.png', scale=.8)
hand = ArmPart('hand.png', scale=1.0)
origin = (width / 2, height / 2)
while 1:
display.fill(white)
# rotate our joints
ua_image, ua_rect = upperarm.rotate(.03)
fa_image, fa_rect = forearm.rotate(-.02)
h_image, h_rect = hand.rotate(.03)
# generate (x,y) positions of each of the joints
import pygame.locals
from armpart import ArmPart
black = (0, 0, 0)
white = (255, 255, 255)
arm_color = (50, 50, 50, 200) # fourth value specifies transparency
pygame.init()
width = 750
height = 750
display = pygame.display.set_mode((width, height))
fpsClock = pygame.time.Clock()
upperarm = ArmPart('upperarm.png', scale=.7)
forearm = ArmPart('forearm.png', scale=.8)
hand = ArmPart('my_hand.png', scale=1.)
line_width = 15
line_upperarm = pygame.Surface((upperarm.scale, line_width), pygame.SRCALPHA,
32)
line_forearm = pygame.Surface((forearm.scale, line_width), pygame.SRCALPHA, 32)
line_hand = pygame.Surface((hand.scale, line_width), pygame.SRCALPHA, 32)
line_upperarm.fill(arm_color)
line_forearm.fill(arm_color)
line_hand.fill(arm_color)
origin = (width / 2, height / 2)
import pygame
import pygame.locals
from armpart import ArmPart
black = (0, 0, 0)
white = (255, 255, 255)
pygame.init()
width = 500
height = 500
display = pygame.display.set_mode((width, height))
fpsClock = pygame.time.Clock()
upperarm = ArmPart('upperarm.png', scale=.7)
base = (width / 2, height / 2)
while 1:
display.fill(white)
ua_image, ua_rect = upperarm.rotate(.01)
ua_rect.center += np.asarray(base)
ua_rect.center -= np.array([np.cos(upperarm.rotation) * upperarm.offset,
-np.sin(upperarm.rotation) * upperarm.offset])
display.blit(ua_image, ua_rect)
pygame.draw.circle(display, black, base, 30)
class Arm:
def __init__(self, basex, basey, length1, length2):
self.bbox_scale = 1.0
self.nsteps = 50
self.link1 = ArmPart(length1, scale=1.0)
self.link2 = ArmPart(length2, scale=1.0)
self.basex = basex
self.basey = basey
self.l2_angle = 2.5
def createBodies(self, world):
self.link1.pos = [ (self.basex, self.basey) ]
self.link2.pos = [ (self.basex+self.link1.length-20, self.basey-10) ]
vertices1 = [(-self.link1.length/2.0, -self.link1.line_width/2.0), (self.link1.length/2.0, -self.link1.line_width/2.0), (self.link1.length/2.0, self.link1.line_width/2.0), (-self.link1.length/2.0, self.link1.line_width/2.0)]
vertices2 = [(-self.link2.length/2.0, -self.link2.line_width/2.0), (self.link2.length/2.0, -self.link2.line_width/2.0), (self.link2.length/2.0, self.link2.line_width/2.0), (-self.link2.length/2.0, self.link2.line_width/2.0)]
rotated_vertices2 = []
for vertex in vertices2:
new_vertex = (((vertex[0] - (vertices2[0][0])) * math.cos(self.l2_angle)) - ((vertex[1] - ((vertices2[3][1]-vertices2[0][1])/2)) * math.sin(self.l2_angle)) + (vertices2[0][0]), \
((vertex[0] - (vertices2[0][0])) * math.sin(self.l2_angle)) + ((vertex[1] - ((vertices2[3][1]-vertices2[0][1])/2)) * math.cos(self.l2_angle)) + ((vertices2[3][1]-vertices2[0][1])/2) )
rotated_vertices2.append( new_vertex )
self.link1.createBody(world, vertices1, "link1", density=1.5)
self.link2.createBody(world, rotated_vertices2, "link2", density=1.0)
self.set_pivot_positions()
self.pivot1 = world.CreateKinematicBody(position=self.pivot_position1)
circle=b2CircleShape(pos=self.pivot1.position, radius=1.0)
self.pivot1.CreateFixture(shape=circle, density=0.0, friction=0.0)
self.pivot1.userData = "pivot1"
self.joint1 = world.CreateRevoluteJoint(bodyA=self.pivot1, bodyB=self.link1.body, anchor=self.pivot1.position, enableMotor=True, maxMotorTorque=100000000, motorSpeed=0.0)
self.joint2 = world.CreateRevoluteJoint(bodyA=self.link1.body, bodyB=self.link2.body, anchor=self.pivot_position2, enableMotor=True, maxMotorTorque=10000000, motorSpeed=0.0, enableLimit=True, lowerAngle=-5.5, upperAngle=0.2)
self.tool = Tool(self.pivot_position3[0], self.pivot_position3[1], world, 100.0)
self.joint3 = world.CreateRevoluteJoint(bodyA=self.link2.body, bodyB=self.tool.body1, anchor=self.pivot_position3, enableMotor=True, maxMotorTorque=10000000, motorSpeed=0.0, enableLimit=True, lowerAngle=-math.pi/4.0, upperAngle=(3.0/4.0)*math.pi)
def set_pivot_positions(self):
self.pivot_position1 = (self.basex-self.link1.length/2.0, self.basey)
self.pivot_position2 = (self.pivot_position1[0]+(self.link1.length * math.cos(self.link1.body.angle)), \
self.pivot_position1[1]+(self.link1.length * math.sin(self.link1.body.angle)))
self.pivot_position3 = (self.pivot_position2[0]+((self.link2.length-2) * math.cos(self.l2_angle))+15, \
self.pivot_position2[1]+((self.link2.length-2) * math.sin(self.l2_angle)))
def move_arm_absolute(self, theta1, theta2):
self.link1.rotation = theta1
self.link2.rotation = theta2
self.link1.pos = [(self.basex, self.basey), \
(self.basex + self.link1.length * math.cos(self.link1.rotation), self.basey + self.link1.length * math.sin(self.link1.rotation))
]
self.link2.pos = [self.link1.pos[1], \
(self.link1.pos[1][0] + self.link1.length * math.cos(self.link1.rotation) + self.link2.length * math.cos(self.link1.rotation + self.link2.rotation), self.link1.pos[1][1] + self.link1.length * math.sin(self.link1.rotation) + self.link2.length * math.sin(self.link1.rotation + self.link2.rotation)) \
]
def inverse_kinematics(self, desired_x, desired_y):
#c2 needs to be in [-1,1], otherwise point is outside reachable workspace
c2 = (desired_x**2 + desired_y**2 - self.link1.length**2 - self.link2.length**2) / (2*self.link1.length * self.link2.length)
if c2 < -1 or c2 > 1:
return None, None
s2 = -math.sqrt(1 - c2**2)
theta2 = math.atan2(s2, c2)
theta1 = math.atan2(desired_y, desired_x) - math.atan2(self.link2.length * s2, self.link1.length + self.link2.length * c2)
return theta1, theta2
