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