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)
