如何为双连杆操纵器的代码添加角度约束？

编程问答 2022-05-27

问题描述

我正在编写代码，从建模角度为双连杆机械手解决逆运动学问题。 我了解逆运动学解决方案的数学，从中我编写了解决方案的代码。但是我没有想到如何对角度（操纵器不能移动的度数）创建约束。 如果您能帮我解决这个问题，我将不胜感激！

import numpy as np
import math

L1 = 70
L2 = 100

# 2nd point of the manipulator Р2
x2 = 86
y2 = 129.89

# Finding the length of line B
B = (x2**2+y2**2)**(1/2)

q1 = math.acos(x2/B)

#q1 = math.atan(y2/x2) - 2nd formula for q1
#q2 we find using the cosine theorem a^2 = b^2 + c^2 - 2*b*c*cos(alpha)

q2 = math.acos((L1**2-L2**2+B**2)/(2*B*L1))
Q1 = (q1-q2)*180/math.pi
# Q2 = 180-Oax
Q2 = (math.pi - math.acos((L1**2 + L2**2 - B**2)/(2*L1*L2)))*180/math.pi

print("Angle 1 =",Q1)
print("Angle 2 =",Q2)

解决方法

暂无找到可以解决该程序问题的有效方法，小编努力寻找整理中！

如果你已经找到好的解决方法，欢迎将解决方案带上本链接一起发送给小编。

小编邮箱:dio#foxmail.com (将#修改为@）

kinematicsmanipulatorsmathpythonrobotics