40323131日誌 105/07/31

手臂的推導角度公式

角度推導公式

先用圖解法解，再來用python-solvespace 來驗證點坐標是否相符

底下是程式碼，可以跑出所有結果

from slvs import *
from math import *

def arm(degree1, degree2):
    sys = System(500)
    g = 1

    #原點Point0
    p0 = sys.add_param(0.0)
    p1 = sys.add_param(0.0)
    p2 = sys.add_param(0.0)
    Point0 = Point3d(p0, p1, p2)

    #XY法線
    qw, qx, qy, qz = Slvs_MakeQuaternion(1, 0, 0, 0, 1, 0)
    p3 = sys.add_param(qw)
    p4 = sys.add_param(qx)
    p5 = sys.add_param(qy)
    p6 = sys.add_param(qz)
    Normal1 = Normal3d(p3, p4, p5, p6)

    #工作平面
    Workplane1 = Workplane(Point0, Normal1)

    #3D版的Point0=>Point1
    p7 = sys.add_param(0.0)
    p8 = sys.add_param(0.0)
    Point1 = Point2d(Workplane1, p7, p8)
    Constraint.dragged(Workplane1, Point1)

    #Point2
    p9 = sys.add_param(10.0)
    p10 = sys.add_param(10.0)
    Point2 = Point2d(Workplane1, p9, p10)
    p11 = sys.add_param(-20.0)
    p12 = sys.add_param(20.0)
    Point3 = Point2d(Workplane1, p11, p12)
    p13 = sys.add_param(-30.0)
    p14 = sys.add_param(15.0)
    Point4 = Point2d(Workplane1, p13, p14)
    p15 = sys.add_param(-40.0)
    p16 = sys.add_param(10.0)
    Point5 = Point2d(Workplane1, p15, p16)
    #基線點
    p17 = sys.add_param(10.0)
    p18 = sys.add_param(0.0)
    Point6 = Point2d(Workplane1, p17, p18)
    Constraint.dragged(Workplane1, Point6)
    Line0 = LineSegment2d(Workplane1, Point1, Point6)
    Constraint.distance(55, Workplane1, Point1, Point2)
    Constraint.distance(135, Workplane1, Point2, Point3)
    Constraint.distance(55, Workplane1, Point3, Point4)
    Constraint.distance(145, Workplane1, Point4, Point5)
    Constraint.distance(135, Workplane1, Point4, Point1)
    Line1 = LineSegment2d(Workplane1, Point1, Point2)
    Line2 = LineSegment2d(Workplane1, Point3, Point4)
    Line3 = LineSegment2d(Workplane1, Point1, Point4)
    Constraint.on(Workplane1, Point5, Line2)
    Constraint.angle(Workplane1, degree1, Line0,  Line3, False)
    Constraint.angle(Workplane1, degree2, Line0,  Line1, False)


    sys.solve()
    if (sys.result == SLVS_RESULT_OKAY):
        print ("點座標：")
        print(("P5(%.3f %.3f %.3f)")%(sys.get_param(15).val, sys.get_param(16).val, sys.get_param(2).val))
        print(("P4(%.3f %.3f %.3f)")%(sys.get_param(13).val, sys.get_param(14).val, sys.get_param(2).val))
        #print(("P3(%.3f %.3f %.3f)")%(sys.get_param(11).val, sys.get_param(12).val, sys.get_param(2).val))
        #print(("P2(%.3f %.3f %.3f)")%(sys.get_param(9).val, sys.get_param(10).val, sys.get_param(2).val))
        print ("%d DOF" % sys.dof)
    elif (sys.result == SLVS_RESULT_INCONSISTENT):
        print ("solve failed")
        print ("SLVS_RESULT_INCONSISTENT")
        print ("%d DOF" % sys.dof)
        for i in range(sys.faileds):
            print(" %lu", sys.failed[i]);
    elif (sys.result == SLVS_RESULT_DIDNT_CONVERGE):
        print ("solve failed")
        print ("SLVS_RESULT_DIDNT_CONVERGE")
        print ("%d DOF" % sys.dof)
    elif (sys.result == SLVS_RESULT_TOO_MANY_UNKNOWNS):
        print ("solve failed")
        print ("SLVS_RESULT_TOO_MANY_UNKNOWNS")
        print ("%d DOF" % sys.dof)

#主程式
#限制範圍內的角度的所有組合
for i in range(90, 151):
    for j in range(0, 61):
        print (i,j)
        arm(i, j)