def meridian2(P0,Rn,sign,q0):
print 'meridian2'
Q=[q0]
dt = 1e-5
y=array([float(P0[0]),float(P0[1]),float(P0[2])])
d = sqrt(P0[0]**2+P0[1]**2+P0[2]**2)
dif = d-Rn
S=(dif>0)
tol=1e-6
notstop = True
while abs(dif)>1e-4:
tand = tangentd(y,sign)*dt
pnew = y+tand
res = MLS_QP.optmizeTan(y, pnew, v,tol)
if dot(res.x-y,res.x-y)==0:
tol*=0.1
y=res.x
if notstop:
norm = MLS_QP.dfn4(y[0],y[1],y[2])
norm /= sqrt(dot(norm,norm))
ray = array([y[0],y[1],y[2],norm[0],norm[1],norm[2]])
resQ = coating.optmizeQ(robot,ikmodel,manip,ray,Q[-1])
if resQ.success:
Q.append(resQ.x)
else:
print 'Meridian Error'
d = sqrt(res.x[0]**2+res.x[1]**2+res.x[2]**2)
dif = d-Rn
if S!=(dif>0):
notstop = False
sign*=-1
dt*=0.5
S=(dif>0)
norm = MLS_QP.dfn4(y[0],y[1],y[2])
norm /= sqrt(dot(norm,norm))
ray = array([y[0],y[1],y[2],norm[0],norm[1],norm[2]])
resQ = coating.optmizeQ(robot,ikmodel,manip,ray,Q[-1])
if resQ.success:
Q.append(resQ.x)
else:
print 'Meridian Error'
return ray, Q
def meridian2(P0, Rn, sign, q0):
print 'meridian2'
Q = [q0]
dt = 1e-5
y = array([float(P0[0]), float(P0[1]), float(P0[2])])
d = sqrt(P0[0]**2 + P0[1]**2 + P0[2]**2)
dif = d - Rn
S = (dif > 0)
tol = 1e-6
notstop = True
while abs(dif) > 1e-4:
tand = tangentd(y, sign) * dt
pnew = y + tand
res = moving_LS.optmizeTan(y, pnew, v, tol)
if dot(res.x - y, res.x - y) == 0:
tol *= 0.1
y = res.x
if notstop:
norm = moving_LS.dfn4(y[0], y[1], y[2])
norm /= sqrt(dot(norm, norm))
ray = array([y[0], y[1], y[2], norm[0], norm[1], norm[2]])
resQ = coating.optmizeQ(robot, ikmodel, manip, ray, Q[-1])
if resQ.success:
Q.append(resQ.x)
else:
print 'Meridian Error'
d = sqrt(res.x[0]**2 + res.x[1]**2 + res.x[2]**2)
dif = d - Rn
if S != (dif > 0):
notstop = False
sign *= -1
dt *= 0.5
S = (dif > 0)
norm = moving_LS.dfn4(y[0], y[1], y[2])
norm /= sqrt(dot(norm, norm))
ray = array([y[0], y[1], y[2], norm[0], norm[1], norm[2]])
resQ = coating.optmizeQ(robot, ikmodel, manip, ray, Q[-1])
if resQ.success:
Q.append(resQ.x)
else:
print 'Meridian Error'
return ray, Q
def Qvector_backtrack(y, Q):
for rev_y in reversed(y):
res = coating.optmizeQ(robot, ikmodel, manip, rev_y, Q[-1])
Q.append(res.x)
if not res.success:
print 'Qvector_backtrack error'
return list(reversed(Q))
def tangentOptm(ray,q0):
tan = cross(ray[3:6],ray[0:3])
tan = tan/sqrt(dot(tan,tan))
res = coating.optmizeQ(robot,ikmodel,manip,ray,q0)
angle_suc = isViable(res.x,ray[3:6])
return tan, res.x, (res.success and angle_suc)
def Qvector_backtrack(y,Q):
for rev_y in reversed(y):
res = coating.optmizeQ(robot,ikmodel,manip,rev_y,Q[-1])
Q.append(res.x)
if not res.success:
print 'Qvector_backtrack error'
return list(reversed(Q))
def tangentOptm(ray, q0):
tan = cross(ray[3:6], ray[0:3])
tan = tan / sqrt(dot(tan, tan))
res = coating.optmizeQ(robot, ikmodel, manip, ray, q0)
angle_suc = isViable(res.x, ray[3:6])
return tan, res.x, (res.success and angle_suc)
def tangentOptm(ray, q0):
global manipulabilityNUM
global globalManipPos
global globalManipOri
tan = cross(ray[3:6], ray[0:3])
tan *= 1.0 / sqrt(dot(tan, tan))
res = coating.optmizeQ(robot, ikmodel, manip, ray, q0)
angle_suc = isViable(res.x, ray[3:6])
# print 'NORM: ', ray[3:6], ' POINT: ', ray[0:3]
return tan, res.x, (res.success and angle_suc)
def tangentOptm(ray,q0):
global manipulabilityNUM
global globalManipPos
global globalManipOri
tan = cross(ray[3:6],ray[0:3])
tan *= 1.0/sqrt(dot(tan,tan))
res = coating.optmizeQ(robot,ikmodel,manip,ray,q0)
angle_suc = isViable(res.x,ray[3:6])
# print 'NORM: ', ray[3:6], ' POINT: ', ray[0:3]
return tan, res.x, (res.success and angle_suc)
def tangentOptm(ray,q0):
tan = cross(ray[3:6],ray[0:3])
tan *= (40.0/60)/sqrt(dot(tan,tan))
angle_suc = True
res = coating.optmizeQ(robot,ikmodel,manip,ray,q0)
## robot.SetDOFValues(res.x,ikmodel.manip.GetArmIndices())
## T=manip.GetTransform()
## Rx = T[0:3,0]/sqrt(dot(T[0:3,0],T[0:3,0]))
# print 'tangentOptm: res.success -',res.success , ' angle -', 180*math.acos(dot(ray[3:6],Rx))/math.pi
if res.success and not isViable(res.x,ray[3:6]):
angle_suc=False
return tan, res.x, (res.success and angle_suc)
def tangentOptm(ray, q0):
tan = cross(ray[3:6], ray[0:3])
tan *= (40.0 / 60) / sqrt(dot(tan, tan))
angle_suc = True
res = coating.optmizeQ(robot, ikmodel, manip, ray, q0)
## robot.SetDOFValues(res.x,ikmodel.manip.GetArmIndices())
## T=manip.GetTransform()
## Rx = T[0:3,0]/sqrt(dot(T[0:3,0],T[0:3,0]))
# print 'tangentOptm: res.success -',res.success , ' angle -', 180*math.acos(dot(ray[3:6],Rx))/math.pi
if res.success and not isViable(res.x, ray[3:6]):
angle_suc = False
return tan, res.x, (res.success and angle_suc)
def meridian2(P0,sign,q0):
print 'meridian2'
Q=[q0]
P0=array([P0[0],P0[1],P0[2]])
y = curvepoint(P0)
norm = polynomial_spline.df(y)
norm /= sqrt(dot(norm,norm))
ray = array([y[0],y[1],y[2],norm[0],norm[1],norm[2]])
resQ = coating.optmizeQ(robot,ikmodel,manip,ray,Q[-1])
if resQ.success:
Q.append(resQ.x)
else:
print 'Meridian Error'
return ray, Q
def meridian2(P0, sign, q0):
print 'meridian2'
Q = [q0]
P0 = array([P0[0], P0[1], P0[2]])
y = curvepoint(P0)
norm = polynomial_spline.df(y)
norm /= sqrt(dot(norm, norm))
ray = array([y[0], y[1], y[2], norm[0], norm[1], norm[2]])
resQ = coating.optmizeQ(robot, ikmodel, manip, ray, Q[-1])
if resQ.success:
Q.append(resQ.x)
else:
print 'Meridian Error'
return ray, Q
