Создание объектно-ориентированного обратного кинематического решателя - scipy.optimize.minimize только итерация один раз - PullRequest
Новая
       43

Создание объектно-ориентированного обратного кинематического решателя - scipy.optimize.minimize только итерация один раз

0 голосов
/ 24 сентября 2019

Я пытаюсь создать пакет обратной кинематики Python в стиле ООП с использованием DH-матрицы и scipy.

Я создал ссылку с этим атрибутом dh_matrix, чтобы сгенерировать матрицу dh для каждой ссылки.

Затем каждая ссылка отправляется в цепочку, и создается метод DH цепочки путем умножения всей матрицы dh

x, y, z - верхний правый угол DH-матрицы цепочки, как определеноDH-матрица.

Целевая функция здесь генерирует абсолютное расстояние от целевого значения путем обновления тета каждой ссылки до нового значения scipy

Я пытался создать его с использованием чистой геометрии, которая является специфическойкаждому роботу, и он смог сходиться.Однако, я не использовал для этого программирование в стиле ООП.

Я пытался использовать разные методы.

class DHLink():
@property
def dh_matrix(self):
    ct = cos(radians(self.theta))
    st = sin(radians(self.theta))
    ca = cos(radians(self.alpha))
    sa = sin(radians(self.alpha))

    return round_(array([[ct, -ca * st, sa * st, self.a * ct],
                  [st, ca * ct, -sa * ct, self.a * st],
                  [0, sa, ca, self.d],
                   [0, 0, 0, 1]]), decimals = 3)

class Chain():
@property
def dh_eqn(self):
    dh_eqn = self.links[0].dh_matrix
    for link in self.links[1:]:
        dh_eqn = dh_eqn.dot(link.dh_matrix)
    return dh_eqn
@property
def x(self):
    return self.dh_eqn[0][3]

@property
def y(self):
    return self.dh_eqn[1][3]

@property
def z(self):
    return self.dh_eqn[2][3]

def objective(self, theta, x, y, z):
    '''Objective function to be minimized by optimizer to reduce RMS error of target and actual
    ------------------------
    Input x,y,z = position of target position'''
    for count, link in enumerate(self.links):
        link.theta = theta[count]
    dist = sqrt((self.x - x)**2) + sqrt((self.y - y)**2) + sqrt((self.z - z)**2)
    return dist

def inverse_kinematics(self, bounds, constraints, x, y, z, default_target = [0,0]):
    default_target = array(default_target)  #Convert to numpy.array
    sol = minimize(self.objective, default_target, args=(x, y, z),bounds = bounds, method = 'SLSQP', options={'disp':True, 'eps':1e2})
    return sol.x

Это просто немного изменит значение и вернет число, близкое к целевому значению по умолчанию

Оптимизация успешно завершена.(Выход из режима 0) Текущее значение функции: 0.1902600000000001 Итерации: 1 Оценки функции: 15 Оценки градиента: 1 Выход [296]: массив ([44.99997959, 44.99998853])

...