def plot():
"""A function for plotting an arm, and having it calculate the
inverse kinematics such that given the mouse (x, y) position it
finds the appropriate joint angles to reach that point."""
# create an instance of the arm
arm = Arm.Arm3Link(L=np.array([300, 200, 100]))
# make our window for drawin'
window = pyglet.window.Window()
label = pyglet.text.Label('Mouse (x,y)',
font_name='Times New Roman',
font_size=36,
x=window.width // 2,
y=window.height // 2,
anchor_x='center',
anchor_y='center')
def get_joint_positions():
"""This method finds the (x,y) coordinates of each joint"""
x = np.array([
0, arm.L[0] * np.cos(arm.q[0]), arm.L[0] * np.cos(arm.q[0]) +
arm.L[1] * np.cos(arm.q[0] + arm.q[1]), arm.L[0] * np.cos(arm.q[0])
+ arm.L[1] * np.cos(arm.q[0] + arm.q[1]) +
arm.L[2] * np.cos(np.sum(arm.q))
]) + window.width / 2
y = np.array([
0, arm.L[0] * np.sin(arm.q[0]), arm.L[0] * np.sin(arm.q[0]) +
arm.L[1] * np.sin(arm.q[0] + arm.q[1]),
arm.L[0] * np.sin(arm.q[0]) +
arm.L[1] * np.sin(arm.q[0] + arm.q[1]) +
arm.L[2] * np.sin(np.sum(arm.q))
])
return np.array([x, y]).astype('int')
window.jps = get_joint_positions()
@window.event
def on_draw():
window.clear()
label.draw()
for i in range(3):
pyglet.graphics.draw(
2, pyglet.gl.GL_LINES,
('v2i', (window.jps[0][i], window.jps[1][i],
window.jps[0][i + 1], window.jps[1][i + 1])))
@window.event
def on_mouse_motion(x, y, dx, dy):
# call the inverse kinematics function of the arm
# to find the joint angles optimal for pointing at
# this position of the mouse
label.text = '(x,y) = (%.3f, %.3f)' % (x, y)
arm.q = arm.inv_kin([x - window.width / 2, y]) # get new arm angles
window.jps = get_joint_positions() # get new joint (x,y) positions
pyglet.app.run()
import Arm
from numpy import pi
import numpy as np
def me_round(x):
return np.round(x, decimals=3)
arm = Arm.Arm3Link(L=[1, 1, 1])
print(list(map(me_round, arm.get_xy([pi / 3, pi / 4, pi / 2]))))
a = arm.inv_kin([-0.725, 1.573])
print(list(map(me_round, arm.get_xy(a))))
print(a)
tan = list(map(np.tan, a))
p = list(map(np.arctan, tan))
print(list(i * 180 / pi for i in p))
#setup section #Time Library Import import time #Cartesian Space to Joint Space Libraries from math import * import numpy as np #Arm Calculation Code Import import Arm wmroboarm = Arm.Arm3Link(L=np.array([255, 255, 1])) #Scale mouse input to correct size targety = 400 targetz = 200 Shoulder_Current = 50 Elbow_Current = -50 #Plug Variables into function and store output as angles variable wmroboarm.q = [radians(Shoulder_Current), radians(Elbow_Current), 0] angles = wmroboarm.inv_kin(xy=(targety, targetz)) #Find angles to set arm at Shoulder_Set_Angle = (degrees(angles[0])) Elbow_Set_Angle = (degrees(angles[1])) #Print angles
çalýþtýrarak projeyi baþlatmýþ oluyoruz """
import numpy as np
import pyglet
import Arm
def plot():
"" " Bir kolu çizmek için bir iþlevi ve bunu hesaplamak
Ters kinematik iþlemi için, fare (x, y) koordinatlarý verildiðinde
Bu noktaya ulaþmak için uygun eklem açýlarýný bulur. """
# kolun bir örneðini oluþturmak
arm = Arm.Arm3Link ( L = np.array ([ 300 , 200 , 100 ]))
# penceremizi oluþturmak
window = pyglet.window.Window ()
window = pyglet.window.Window()
label = pyglet.text.Label(
'Mouse (x,y)', font_name='Times New Roman',
font_size=36, x=window.width//2, y=window.height//2,
anchor_x='center', anchor_y='center')
def get_joint_positions():
"" " Bu yöntem her bir eklemin (x, y) koordinatlarýný bulur" ""
x = np.array([
def plot():
"""A function for plotting an arm, and having it calculate the
inverse kinematics such that given the mouse (x, y) position it
finds the appropriate joint angles to reach that point."""
# create an instance of the arm
arm = Arm.Arm3Link(L=np.array([140, 152, 0]))
# make our window for drawin'
window = pyglet.window.Window()
label = pyglet.text.Label('Mouse (x,y)',
font_name='Times New Roman',
font_size=36,
x=window.width // 2,
y=window.height // 2,
anchor_x='center',
anchor_y='center')
def get_joint_positions():
"""This method finds the (x,y) coordinates of each joint"""
x = np.array([
0, arm.L[0] * np.cos(arm.q[0]), arm.L[0] * np.cos(arm.q[0]) +
arm.L[1] * np.cos(arm.q[0] + arm.q[1]), arm.L[0] * np.cos(arm.q[0])
+ arm.L[1] * np.cos(arm.q[0] + arm.q[1]) +
arm.L[2] * np.cos(np.sum(arm.q))
]) + window.width / 2
y = np.array([
0, arm.L[0] * np.sin(arm.q[0]), arm.L[0] * np.sin(arm.q[0]) +
arm.L[1] * np.sin(arm.q[0] + arm.q[1]),
arm.L[0] * np.sin(arm.q[0]) +
arm.L[1] * np.sin(arm.q[0] + arm.q[1]) +
arm.L[2] * np.sin(np.sum(arm.q))
])
return np.array([x, y]).astype('int')
window.jps = get_joint_positions()
@window.event
def on_draw():
window.clear()
label.draw()
for i in range(3):
pyglet.graphics.draw(
2, pyglet.gl.GL_LINES,
('v2i', (window.jps[0][i], window.jps[1][i],
window.jps[0][i + 1], window.jps[1][i + 1])))
@window.event
def on_mouse_motion(x, y, dx, dy):
# call the inverse kinematics function of the arm
# to find the joint angles optimal for pointing at
# this position of the mouse
# label.text = '(y,x) = (%.3f, %.3f)' % (y, x)
#label.text = '(x,y) = (%.3f, %.3f)' % (x, y)
a = int(180 - arm.q[0] * 180 / np.pi)
b = int(arm.q[1] * 180 / np.pi - 70)
pub2.publish(b)
pub3.publish(a)
label.text = '(x,y) = (%.3f, %.3f)' % (a, b)
arm.q = arm.inv_kin([x - window.width / 2, y]) # get new arm angles
window.jps = get_joint_positions() # get new joint (x,y) positions
# xy = [x,y]
#
# print('-------------------------')
# print('Initial joint angles', arm.q)
# print('Final joint angles: ', q)
# print('Desired hand position: ', xy)
# print('Actual hand position: ', actual_xy)
# print('Error: ', error)
# print('-------------------------')
pyglet.app.run()
import serial
import Arm
import re
ser = serial.Serial('COM3', 115200, timeout=3)
#ser.baudrate=9600
tmp_pos = [0.0,0.0,0.0]
#define dimensions for the MeArm
linkage_1 = 8.5 #length of link 1 in cm
linkage_1_angle = 0.0
linkage_2 = 8.5 #length of link 2 in cm
linkage_2_angle = 0.0
linkage_3 = 4.0 #length of claw in cm
arm = Arm.Arm3Link(L = np.array([linkage_1*10,linkage_2*10,1]))
#move it a bit to make sure it's facing to the right
arm.q = arm.inv_kin([-31,1]) # get new arm angles
arm.q = arm.inv_kin([31,1]) # get new arm angles
arm.q = arm.inv_kin([120,1]) # get new arm angles
mearm_base_initial=90
mearm_left_initial=148
mearm_right_initial=68
mearm_gripper_initial=90
mearm_base=mearm_base_initial
mearm_left=mearm_left_initial
mearm_right=mearm_right_initial
mearm_gripper=mearm_gripper_initial
mearm1_offset=417;