0
私は円をパラメータ化するスクリプトを持っています。これは、シミュレーションプログラムに使用されます。しかし、私はスクリプトのいくつかの部分を理解するのに苦労しており、これを学びたいと思っています。私はいくつかのpythonオンラインコースを取ったが、それがそれを理解するのに十分であるかどうかはわからない。たとえば以下の部分。スクリプトを理解する
サークルの円周上に3つの点があり、円であることがわかりますか？コード全体が他のコードの下にあります。式fabs((V1*V2)/(sqrt(V1*V1*V2*V2))) > 0.95にあるV1,V2およびV3の意味は？
for i in range(skipCount):
P1 = points[i]
P2 = points[i+1]
if i+2 < skipCount:
P3 = points[i+2]
else:
P3 = points[i-1]
#endif
全体コード：
def OnAbort():
cmd.Checked = False
pass
#-------------------------------------------------------------------------------
def firstState():
if selProg.Count != 1:
print "Circular Move: No program selected. Aborting..."
return False
#endif
cmd.Checked = True
program = selProg.getItem(0)
routine = program.CurrentRoutine
controller = program.Controller
addLimitProperty(cmd, VC_REAL, "CircularSpeed", True, False, False, 100., None, "Velocity", 0., controller.MaxCartesianSpeed)
cmp = controller.Component
# Fix for Main Program selection BUG
if routine != program.MainRoutine:
if selStmt.Count:
stmt = selStmt.getItem(0)
if stmt.Routine != routine:
routine = program.MainRoutine
#endif
#endif
#endif
if not displayPropertyDialog(cmd.Name, "Circular Motion Settings", ''):
OnAbort()
return False
#endif
phName = 'CircularMove'
ph = cmp.findBehaviour(phName)
processName = 'CircularMove'
if ph != None:
if ph.Script[:12] != "#VERSION 1.2":
print "Updating Script " + processName
ph.delete()
ph = None
#endif
#endif
if ph == None:
ph = cmp.createBehaviour(VC_PYTHON_PROCESS_HANDLER, phName)
# set the bitmap of the process, that appears in the RSL statement grid
iconuri = getCommandPath()
iconuri = iconuri[:-3] + ".bmp"
icon = getApplication().loadBitmap(iconuri)
ph.Icon = icon
# register process handler to the rsl executor behavior(s) of the robot
xl = cmp.findBehavioursByType(VC_RSLEXECUTOR)
for x in xl:
phl = x.ProcessHandlers
phl.append(ph)
x.ProcessHandlers = phl
#endfor
ph.Process = processName
ph.Script = """#VERSION 1.2
from vcRslProcessHandler import *
from math import *
import vcMatrix
RAD_TO_DEG = 90.0/atan2(1.,0.)
def OnExecuteProcess(executor, process_statement):
# Check if at end of Circle Move
skipProp = process_statement.getProperty("SkipCount")
if skipProp != None:
#delete all dummy statements to trick RSL program pointer
routine = process_statement.Routine
s = process_statement.Index - skipProp.Value + 1
for i in range(skipProp.Value):
routine.deleteStatement(s)
return
routine.endBatch()
#endif
cntr = executor.Controller
routine = process_statement.Routine
stmts = routine.Statements
if process_statement.getProperty("NumPoints"):
skipCount = process_statement.NumPoints
else:
skipCount = 2
#endif
idx = process_statement.Index
if idx+skipCount >= routine.StatementCount:
return
#endif
for i in range(skipCount):
if stmts[idx+1+i].Type not in [VC_STATEMENT_LINMOTION, VC_STATEMENT_PTPMOTION]:
return
#endif
#endfor
cntr.clearTargets()
trg = cntr.createTarget()
points = [ trg.Target ]
for i in range(skipCount):
points.append(stmts[idx+1+i].Target)
#endfor
for i in range(skipCount):
P1 = points[i]
P2 = points[i+1]
if i+2 < skipCount:
P3 = points[i+2]
else:
P3 = points[i-1]
#endif
V1 = P1.P - P2.P
V2 = P2.P - P3.P
V3 = P3.P - P1.P
if fabs((V1*V2)/(sqrt(V1*V1*V2*V2))) > 0.95:
print "ERROR: CircularMove: Points are colinear"
getSimulation().halt()
return
#endif
N = V1^V2
circleRadius = V1.length()*V2.length()*V3.length()/(2.0*N.length())
alpha = -1.0*(V2*V2)*(V1*V3)
beta = -1.0*(V3*V3)*(V2*V1)
gamma = -1.0*(V1*V1)*(V3*V2)
cP = (alpha*P1.P + beta*P2.P + gamma*P3.P)*(1.0/(2.0*N*N))
R = P1.P - cP
xx = P1.P - cP
xx.normalize()
N.normalize()
yy = N^xx
yy.normalize()
V = P2.P - cP
V.normalize()
if process_statement.FullCircle:
circleAngle = 360.0
P3 = P1
else:
circleAngle = atan2(V*yy, V*xx)*RAD_TO_DEG
if circleAngle < 0.0 :
circleAngle += 360.0
#endif
#endif
vel = process_statement.CircularSpeed
acc = cntr.MaxCartesianAccel
arg = vel*vel/(4.*circleRadius*acc)
if arg > 1.0:
arg = 1.0
#endif
dAngle = 2.0*asin(arg) * RAD_TO_DEG
i = (int) (circleAngle/dAngle)
if i < 2:
i = 2
#endif
dAngle = circleAngle/i
if dAngle < 5.0:
dAngle = 5.0
#endif
trg.MotionType = VC_MOTIONTARGET_MT_LINEAR
trg.CartesianSpeed = vel
trg.AccuracyMethod = VC_MOTIONTARGET_AM_VELOCITY
trg.AccuracyValue = vel
dC = vcMatrix.new()
dC.setAxisAngle(N.X, N.Y, N.Z, circleAngle)
dC.translateAbs(cP.X, cP.Y, cP.Z)
invP1 = dC*P1
invP1.invert()
P12 = invP1*P2
k12 = P12.getAxisAngle()
if circleAngle > 180.0 and abs(k12.W) > 0.1:
k12.W = -(360.0 - k12.W)
#endif
if k12.Z < -0.1:
k12.W *= -1.0
#endif
dTheta = k12.W/circleAngle*dAngle
dP12 = vcMatrix.new()
C0 = vcMatrix.new(P1)
C0.translateAbs(-cP.X, -cP.Y, -cP.Z)
theta = 0.0
angle = 0.0
while angle < circleAngle:
dC.setAxisAngle(N.X, N.Y, N.Z, angle)
dP12.setAxisAngle(k12.X, k12.Y, k12.Z, theta)
C = dC*C0*dP12
trg.Target = C
cntr.addTarget(trg)
angle += dAngle
theta += dTheta
#endwhile
#endif
trg.AccuracyValue = 0.0
trg.Target = P2
cntr.addTarget(trg)
cntr.move()
# Mark end of Circle Move
routine.beginBatch()
stmt = routine.createStatement(VC_STATEMENT_PROCESS)
stmt.Index = process_statement.Index+1
stmt.Process = "CircularMove"
prop = stmt.createProperty(VC_INTEGER, "SkipCount")
prop.Value = skipCount
# Add dummy Comment statements to trick RSL program pointer
for i in range(skipCount-1):
stmt = routine.createStatement(VC_STATEMENT_COMMENT)
stmt.Index = process_statement.Index+1
#endfor
#-------------------------------------------------------------------------------
def OnFinalize():
pass
#-------------------------------------------------------------------------------
def OnReset():
pass
#-------------------------------------------------------------------------------
"""
#endif
ps = routine.createStatement(VC_STATEMENT_PROCESS)
ps.Name = "CM1"
ps.Process = processName
for p in ps.Properties:
if p.Name == "Process":
p.IsVisible = False
addLimitProperty(ps, VC_INTEGER, "NumPoints", True, False, False, 2, None, "", 2, 8)
copyProperties(cmd, ps)
cmd.Checked = False
stmt = selStmt.getItem(0)
if stmt:
ps.Index = stmt.Index+1
return True
addState(firstState)
#endif