def DrawCircle(env, center, normal, radius, linewidth=1, colors=None): angles = numpy.arange(0, 2 * numpy.pi + 0.1, 0.1) R = openravepy_int.matrixFromQuat( openravepy_int.quatRotateDirection([0, 0, 1], normal)) right = R[0:3, 0] * radius up = R[0:3, 1] * radius return env.drawlinestrip( c_[numpy.dot(numpy.transpose([numpy.cos(angles)]), [right]) + numpy.dot(numpy.transpose([numpy.sin(angles)]), [up]) + numpy.tile(center, (len(angles), 1))], linewidth, colors=colors)
def fitCircle(points,geometric_refinement=True): """Very simple function to return the best fit circle. Used when fitting real data to joint trajectories. Currently this fits the algebraic distance, a further refinement step for geometric distance should be inserted. :return: center, radius, error """ if points.shape[1] == 3: M = numpy.mean(points,0) points2=points-numpy.tile(M,[len(points),1]) # use the svd to get the plane normal from the null space planenormal=numpy.linalg.svd(numpy.dot(points2.transpose(),points2))[2][-1,:] R=openravepy_int.rotationMatrixFromQuat(openravepy_int.quatRotateDirection([0,0,1],planenormal)) points=numpy.dot(points2,R) x,error = numpy.linalg.lstsq(numpy.c_[points[:,0],points[:,1],numpy.ones(len(points))],-points[:,0]**2-points[:,1]**2)[0:2] if points.shape[1] == 3: # error should also include off-plane offsets! return M+numpy.array(numpy.dot(R,[-0.5*x[0],-0.5*x[1],0])), numpy.sqrt((x[0]**2+x[1]**2)/4-x[2]),error return numpy.array([-0.5*x[0],-0.5*x[1]]), numpy.sqrt((x[0]**2+x[1]**2)/4-x[2]),error
def DrawIkparam2(env,ikparam,dist=1.0,linewidth=1,coloradd=None): """draws an IkParameterization """ if ikparam.GetType() == openravepy_int.IkParameterizationType.Transform6D: return [DrawAxes(env,ikparam.GetTransform6DPose(),dist,linewidth,coloradd)] elif ikparam.GetType() == openravepy_int.IkParameterizationType.TranslationDirection5D: ray = ikparam.GetTranslationDirection5D() colors=numpy.array([[0,0,0],[1,0,0]]) if coloradd is not None: colors = numpy.minimum(1.0, numpy.maximum(0.0, colors + numpy.tile(coloradd,(len(colors),1)))) # have to draw the arrow dirangle = numpy.pi/6 arrowc = numpy.cos(dirangle)*dist*0.1 arrows = numpy.sin(dirangle)*dist*0.1 arrowdirs = numpy.array([[0, 0, 0], [0, 0, dist], [0, 0, dist], [arrows, 0, dist-arrowc], [0, 0, dist], [-arrows, 0, dist-arrowc], [0, 0, dist], [0, arrows, dist-arrowc], [0, 0, dist], [0, -arrows, dist-arrowc]]) q = openravepy_int.quatRotateDirection([0,0,1], ray.dir()) realarrowlines = openravepy_ext.quatRotateArrayT(q, arrowdirs) + numpy.tile(ray.pos(), (len(arrowdirs),1)) return [env.drawlinelist(realarrowlines[:2,:].flatten(),linewidth,colors=colors), env.drawlinelist(realarrowlines[2:].flatten(),linewidth,colors=colors[1])] + [env.plot3(ray.pos(), 4*linewidth, colors=[0.5,0,0])] elif ikparam.GetType() == openravepy_int.IkParameterizationType.Translation3D: if coloradd is not None: colors = numpy.array([coloradd]) else: colors=numpy.array([[0,0,0]]) return [env.plot3(ikparam.GetTranslation3D(),linewidth,colors=colors)] elif ikparam.GetType() == openravepy_int.IkParameterizationType.TranslationXAxisAngleZNorm4D: pos,angle = ikparam.GetTranslationXAxisAngleZNorm4D() T = openravepy_int.matrixFromAxisAngle([0,0,angle]) T[0:3,3] = pos return [DrawAxes(env,T,dist,linewidth,coloradd)] else: raise NotImplemented('iktype %s'%str(ikparam.GetType()))
def DrawCircle(env, center, normal, radius, linewidth=1, colors=None): angles = numpy.arange(0, 2*numpy.pi+0.1, 0.1) R = openravepy_int.matrixFromQuat(openravepy_int.quatRotateDirection([0,0,1],normal)) right = R[0:3,0]*radius up = R[0:3,1]*radius return env.drawlinestrip(c_[numpy.dot(numpy.transpose([numpy.cos(angles)]), [right]) + numpy.dot(numpy.transpose([numpy.sin(angles)]), [up]) + numpy.tile(center, (len(angles),1))], linewidth, colors=colors)