def plotVectors(ax, centers, specials=[], dirs=[], label='_', cradius=0.1, ccolor='r'): for i, p in enumerate(centers): dx, dy = -2 * cradius, cradius if (p[0] >= 0): dx, dy = cradius, cradius / 2.0 if (label != '_'): plt.annotate(r'$%s_%d$' % (label, i), xy=p, xytext=(p[0] + dx, p[1] + dy)) if (len(specials) > 0): if (i in specials): drawCircle(ax, p, cradius, color=ccolor) else: drawCircle(ax, p, cradius - 0.05, color='b') else: drawCircle(ax, p, cradius, color=ccolor) if (len(dirs) > 0): d = dirs[i] vel = (3 * d[0], 3 * d[1]) if (VectorFigUtils.vnorm(vel)): ax.arrow(p[0], p[1], vel[0], vel[1], head_width=0.07, head_length=0.11, fc='k', ec='k')
def plotAll(ax, alpha=0.3, color='b', centers=[], specials=[], ccolor='r', cradius=0.1, label='_', xlabel="", ylabel="", title="", mainfont=16): plotWorld(ax, alpha=alpha, color=color, centers=centers, specials=specials, cradius=cradius, ccolor=ccolor, label=label) VectorFigUtils.decorate(xlabel=xlabel, ylabel=ylabel, title=title, mainfont=mainfont)
def plotVectors(ax, centers, specials=[], dirs = [], label='_', cradius=0.1, ccolor='r'): for i,p in enumerate(centers): dx,dy = -2*cradius,cradius if(p[0] >= 0): dx,dy = cradius,cradius/2.0 if(label != '_'): plt.annotate(r'$%s_%d$'%(label,i), xy=p, xytext=(p[0]+dx,p[1]+dy)) if(len(specials)>0): if(i in specials): drawCircle(ax,p,cradius,color=ccolor) else: drawCircle(ax,p,cradius-0.05,color='b') else: drawCircle(ax,p,cradius,color=ccolor) if(len(dirs)>0): d = dirs[i] vel = (3*d[0],3*d[1]) if(VectorFigUtils.vnorm(vel)): ax.arrow(p[0], p[1], vel[0], vel[1], head_width=0.07, head_length=0.11, fc='k', ec='k')
def getJointSpeedNorm(self): return VectorFigUtils.vnorm(self.getMotorSpeeds())
def dmnorm(self, m): mplus = np.array(self.getJointAngles()) return VectorFigUtils.vnorm(mplus - np.array(m))
def plotAll(ax, alpha = 0.3, color = 'b', centers=[], specials=[], ccolor='r', cradius = 0.1, label='_', xlabel="", ylabel="", title="", mainfont=16): plotWorld(ax,alpha=alpha, color=color, centers=centers, specials=specials, cradius=cradius, ccolor=ccolor, label=label) VectorFigUtils.decorate(xlabel=xlabel,ylabel=ylabel,title=title,mainfont=mainfont)
def dmnorm(self,m): mplus=np.array(self.getJointAngles()) return VectorFigUtils.vnorm(mplus - np.array(m))