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()
Exemple #2
0
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))
Exemple #3
0
#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([
Exemple #5
0
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()
Exemple #6
0
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;