def DrawIkparam(env,ikparam,dist=1.0,linewidth=1,coloradd=None):
"""draws an IkParameterization
"""
if ikparam.GetType() == openravepy_int.IkParameterizationType.Transform6D:
return DrawAxes(env,ikparam.GetTransform6D(),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))))
return env.drawlinelist(numpy.array([ray.pos(),ray.pos()+ray.dir()*dist]),linewidth,colors=colors)
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 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()))
