def remove(self):
"""
Remove body from OpenRAVE environment.
"""
env = get_openrave_env()
with env:
env.Remove(self.rave)
def draw_arrow(origin, end, color='r', linewidth=0.02):
"""
Draw an arrow between two points.
Parameters
----------
origin : array, shape=(3,)
World coordinates of the origin of the arrow.
end : array, shape=(3,)
World coordinates of the end of the arrow.
color : char or triplet, optional
Color letter or RGB values, default is 'g' for green.
linewidth : scalar, optional
Thickness of arrow.
Returns
-------
handle : openravepy.GraphHandle
OpenRAVE graphical handle. Must be stored in some variable, otherwise
the drawn object will vanish instantly.
"""
if type(color) is str:
color = matplotlib_to_rgb(color)
env = get_openrave_env()
return env.drawarrow(origin, end, linewidth=linewidth, color=color)
def draw_line(start_point, end_point, color='g', linewidth=1.):
"""
Draw a line between two points.
Parameters
----------
start_point : array, shape=(3,)
One end of the line, in world frame coordinates.
end_point : array, shape=(3,)
Other end of the line, in world frame coordinates.
color : char or triplet, optional
Color letter or RGB values, default is 'g' for green.
linewidth : scalar
Thickness of drawn line.
Returns
-------
handle : openravepy.GraphHandle
OpenRAVE graphical handle. Must be stored in some variable, otherwise
the drawn object will vanish instantly.
"""
if type(color) is str:
color = matplotlib_to_rgb(color)
return get_openrave_env().drawlinelist(
array([start_point, end_point]), linewidth=linewidth, colors=color)
def draw_contact_force_lines(contact, length=0.25):
"""
Draw friction cones from each vertex of the surface patch.
INPUT:
- ``length`` -- (optional) length of friction rays in [m]
OUTPUT:
A list of OpenRAVE GUI handles.
"""
env = get_openrave_env()
handles = []
for c in contact.vertices:
color = [0.1, 0.1, 0.1]
color[randint(3)] += 0.2
for f in contact.force_rays:
handles.append(env.drawlinelist(
array([c, c + length * f]),
linewidth=1, colors=color))
handles.append(env.drawlinelist(
array([c, c + length * contact.n]),
linewidth=5, colors=color))
return handles
def __init__(self,
X,
Y,
Z,
pos=None,
rpy=None,
pose=None,
color='r',
visible=True,
transparency=None,
name=None,
dZ=0.):
self.X = X
self.Y = Y
self.Z = Z
aabb = [0., 0., dZ, X, Y, Z]
env = get_openrave_env()
with env:
box = openravepy.RaveCreateKinBody(env, '')
box.InitFromBoxes(array([array(aabb)]), True)
super(Box, self).__init__(box,
pos=pos,
rpy=rpy,
pose=pose,
color=color,
visible=visible,
transparency=transparency,
name=name)
env.Add(box, True)
def __init__(self, X, Y, Z, pos=None, rpy=None, pose=None, color='r',
visible=True, transparency=None, name=None, dZ=0.):
"""
Create a new rectangular box.
INPUT:
- ``X`` -- box half-length
- ``Y`` -- box half-width
- ``Z`` -- box half-height
- ``pos`` -- initial position in inertial frame
- ``rpy`` -- initial orientation in inertial frame
- ``color`` -- color letter in ['r', 'g', 'b']
- ``name`` -- object's name (optional)
- ``pose`` -- initial pose (supersedes pos and rpy)
- ``visible`` -- initial box visibility
- ``transparency`` -- (optional) from 0 for opaque to 1 for invisible
- ``dZ`` -- special value used to make Contact slabs
"""
self.X = X
self.Y = Y
self.Z = Z
aabb = [0., 0., dZ, X, Y, Z]
env = get_openrave_env()
with env:
box = openravepy.RaveCreateKinBody(env, '')
box.InitFromBoxes(array([array(aabb)]), True)
super(Box, self).__init__(
box, pos=pos, rpy=rpy, pose=pose, color=color, visible=visible,
transparency=transparency, name=name)
env.Add(box, True)
def __init__(self, path=None, xml=None):
assert path is not None or xml is not None
name = basename(splitext(path)[0])
if xml is None:
xml = Robot.__default_xml % (path, name)
env = get_openrave_env()
env.LoadData(xml)
rave = env.GetRobot(name)
nb_dofs = rave.GetDOF()
q_min, q_max = rave.GetDOFLimits()
rave.SetDOFVelocityLimits([1000.] * nb_dofs)
self.has_free_flyer = False
self.ik = None
self.is_visible = True
self.mass = sum([link.GetMass() for link in rave.GetLinks()])
self.nb_dofs = nb_dofs
self.q_max = q_max
self.q_max.flags.writeable = False
self.q_min = q_min
self.q_min.flags.writeable = False
self.rave = rave
self.stance = None
self.tau_max = None # set by hand in child robot class
self.transparency = 0. # initially opaque
#
self.ik = IKSolver(self)
def __init__(self, X, Y, Z, pos=None, rpy=None, pose=None, color='r',
dZ=0.):
aabb = [0., 0., dZ, X, Y, Z]
env = get_openrave_env()
with env:
box = openravepy.RaveCreateKinBody(env, '')
box.InitFromBoxes(array([array(aabb)]), True)
super(Box, self).__init__(
box, pos=pos, rpy=rpy, pose=pose, color=color)
env.Add(box, True)
def draw_cone(apex,
axis,
section,
combined='r-#',
color=None,
linewidth=2.,
pointsize=0.05):
"""
Draw a 3D cone defined from its apex, axis vector and a cross-section
polygon (defined in the plane orthogonal to the axis vector).
Parameters
----------
apex : array
Position of the origin of the cone in world coordinates.
axis : array
Unit vector directing the cone axis and lying inside.
combined : string, optional
Drawing spec in matplotlib fashion. Default is 'g-#'.
linewidth : scalar, optional
Thickness of the edges of the cone.
pointsize : scalar, optional
Point size in [m].
Returns
-------
handles : list of GUI handles
Must be stored in some variable, otherwise the drawn object will
vanish instantly.
"""
if len(section) < 1:
warn("Trying to draw an empty cone")
return []
from pymanoid.draw import matplotlib_to_rgba
color = color if color is not None else matplotlib_to_rgba(combined[0])
handles = draw_polygon(points=section,
normal=axis,
combined=combined,
color=color)
edges = vstack([[apex, vertex] for vertex in section])
edges = array(edges)
edge_color = array(color) * 0.7
edge_color[3] = 1.
handles.append(get_openrave_env().drawlinelist(edges,
linewidth=linewidth,
colors=edge_color))
return handles
def draw(self, apex, size=1., combined='g-#', color=None, linewidth=2):
"""
Draw cone with apex at a given world position.
Parameters
----------
apex : array
Position of the origin of the cone in world coordinates.
size : scalar, optional
Scale factor.
combined : string, optional
Drawing spec in matplotlib fashion. Default is 'g-#'.
color : char or triplet, optional
Color letter or RGB values, default is 'g' for green.
linewidth : scalar
Thickness of drawn line.
Returns
-------
handles : list of openravepy.GraphHandle
OpenRAVE graphical handles. Must be stored in some variable,
otherwise the drawn object will vanish instantly.
"""
assert len(apex) == 3, "apex is not a 3D point"
assert len(self.rays[0]) == 3, "only 3D cones can be drawn"
rays = self.rays
if color is None:
color = matplotlib_to_rgba(combined[0])
if type(color) is str:
color = matplotlib_to_rgba(color)
env = get_openrave_env()
normal = average(rays, axis=0)
normal /= norm(normal)
section = [apex + ray * size / dot(normal, ray) for ray in rays]
handles = draw_polygon(points=section,
normal=normal,
combined=combined,
color=color)
edges = vstack([[apex, vertex] for vertex in section])
edge_color = array(color) * 0.7
edge_color[3] = 1.
handles.append(
env.drawlinelist(edges, linewidth=linewidth, colors=edge_color))
return handles
def __init__(self, path=None, xml=None, qd_lim=None):
"""
Create a new robot model.
INPUT:
- ``path`` -- path to the COLLADA model of the robot
- ``xml`` -- (optional) string in OpenRAVE XML format
- ``qd_lim`` -- maximum angular joint velocity (in [rad] / [s])
"""
assert path is not None or xml is not None
name = basename(splitext(path)[0])
if xml is None:
xml = Robot.__default_xml % (path, name)
env = get_openrave_env()
env.LoadData(xml)
rave = env.GetRobot(name)
nb_dofs = rave.GetDOF()
q_min, q_max = rave.GetDOFLimits()
rave.SetDOFVelocities([0] * nb_dofs)
rave.SetDOFVelocityLimits([1000.] * nb_dofs)
if qd_lim is None:
qd_lim = self.__default_qd_lim
qd_max = +qd_lim * ones(nb_dofs)
qd_min = -qd_lim * ones(nb_dofs)
self.has_free_flyer = False
self.ik = None # created by self.init_ik()
self.ik_process = None # created by self.init_ik()
self.ik_thread = None
self.is_visible = True
self.mass = sum([link.GetMass() for link in rave.GetLinks()])
self.nb_dofs = nb_dofs
self.q_max = q_max
self.q_max.flags.writeable = False
self.q_min = q_min
self.q_min.flags.writeable = False
self.qd_max = qd_max
self.qd_min = qd_min
self.qdd_max = None # set in child class
self.rave = rave
self.tau_max = None # set by hand in child robot class
self.transparency = 0. # initially opaque
def draw_arrow(p1, p2, color='r', linewidth=0.02):
"""
Draw an arrow between two points.
INPUT:
- ``p1`` -- 3D coordinates of the origin of the arrow
- ``p2`` -- 3D coordinates of the end of the arrow
- ``color`` -- (default: 'r') matplotlib color letter or RGB triplet
- ``linewidth`` -- thickness of force vector
OUTPUT:
And OpenRAVE handle. Must be stored in some variable, otherwise the drawn
object will vanish instantly.
"""
if type(color) is str:
color = matplotlib_to_rgb(color)
env = get_openrave_env()
return env.drawarrow(p1, p2, linewidth=linewidth, color=color)
def draw_line(p1, p2, color='g', linewidth=1.):
"""
Draw a line between points p1 and p2.
INPUT:
- ``p1`` -- one end of the line
- ``p2`` -- other end of the line
- ``color`` -- (default: 'g') matplotlib color letter or RGB triplet
- ``linewidth`` -- thickness of drawn line
OUTPUT:
And OpenRAVE handle. Must be stored in some variable, otherwise the drawn
object will vanish instantly.
"""
if type(color) is str:
color = matplotlib_to_rgb(color)
return get_openrave_env().drawlinelist(
array([p1, p2]), linewidth=linewidth, colors=color)
def draw_points(points, color='g', pointsize=0.05):
"""
Draw a list of points.
Parameters
----------
point : list of arrays
List of point coordinates in the world frame.
pointsize : scalar, optional
Radius of the drawn sphere in [m].
Returns
-------
handle : openravepy.GraphHandle
OpenRAVE graphical handle. Must be stored in some variable, otherwise
the drawn object will vanish instantly.
"""
if type(color) is str:
color = matplotlib_to_rgba(color, alpha=1.)
return get_openrave_env().plot3(
array(points), pointsize=pointsize, drawstyle=1,
colors=color)
def draw(self, apex, size=1., combined='g-#', color=None, linewidth=2):
"""
Draw cone with apex at a given world position.
INPUT:
- ``apex`` -- position of the apex of the cone in world coordinates
- ``size`` -- scale factor (default: 1.)
- ``combined`` -- drawing spec in matplotlib fashion (default: 'g-#')
- ``color`` -- color letter or RGBA tuple
- ``linewidth`` -- thickness of the edges of the cone
OUTPUT:
A list of OpenRAVE handles. Must be stored in some variable, otherwise
the drawn object will vanish instantly.
"""
assert len(apex) == 3, "apex is not a 3D point"
assert len(self.rays[0]) == 3, "only 3D cones can be drawn"
rays = self.rays
if color is None:
color = matplotlib_to_rgba(combined[0])
if type(color) is str:
color = matplotlib_to_rgba(color)
env = get_openrave_env()
normal = average(rays, axis=0)
normal /= norm(normal)
section = [apex + ray * size / dot(normal, ray) for ray in rays]
handles = draw_polygon(
points=section, normal=normal, combined=combined, color=color)
edges = vstack([[apex, vertex] for vertex in section])
edge_color = array(color) * 0.7
edge_color[3] = 1.
handles.append(env.drawlinelist(
edges, linewidth=linewidth, colors=edge_color))
return handles
def draw_force(p, f, scale=0.005, color='r', linewidth=0.015):
"""
Draw a force acting at a given point.
INPUT:
- ``p`` -- point where the force is acting
- ``f`` -- force vector
- ``color`` -- (default: 'r') matplotlib color letter or RGB triplet
- ``scale`` -- scaling factor between Euclidean and Force spaces
- ``linewidth`` -- thickness of force vector
OUTPUT:
And OpenRAVE handle. Must be stored in some variable, otherwise the drawn
object will vanish instantly.
"""
if type(color) is str:
color = matplotlib_to_rgb(color)
f_scale = scale * f
if dot(f_scale, f_scale) < 1e-6:
return None
return get_openrave_env().drawarrow(
p, p + f_scale, linewidth=linewidth, color=color)
def draw_points(points, color='g', pointsize=0.05):
if type(color) is str:
color = matplotlib_to_rgba(color, alpha=1.)
return get_openrave_env().plot3(
array(points), pointsize=pointsize, drawstyle=1,
colors=color)
def draw_polyhedron(points, combined='g-#', color=None, faces=None,
linewidth=1., pointsize=0.01, hull=None):
"""
Draw a polyhedron defined as the convex hull of a set of points.
INPUT:
- ``points`` -- list of 3D points
- ``combined`` -- (default: 'g-#') drawing spec in matplotlib fashion
- ``color`` -- color letter or RGBA tuple
- ``faces`` -- string indicating the faces of the polyhedron to draw
- ``hull`` -- used in the 2D case where the hull has zero volume
- ``linewidth`` -- line thickness in meters
- ``pointsize`` -- point size in meters
OUTPUT:
And OpenRAVE handle. Must be stored in some variable, otherwise the drawn
object will vanish instantly.
.. NOTE::
In the ``faces`` or ``combined`` strings, use '.' for vertices, '-' for
edges and '#' for facets.
"""
is_2d = hull is not None
if color is None:
color = combined[0]
if faces is None:
faces = combined[1:]
if type(color) is str:
color = matplotlib_to_rgba(color)
if hull is None:
try:
hull = ConvexHull(points)
except QhullError:
warn("QhullError: maybe polyhedron is empty?")
return []
vertices = array([points[i] for i in hull.vertices])
points = array(points)
color = array(color if color is not None else (0.0, 0.5, 0.0, 0.5))
handles = []
env = get_openrave_env()
if '-' in faces: # edges
edge_color = color * 0.7
edge_color[3] = 1.
edges = vstack([[points[i], points[j]]
for s in hull.simplices
for (i, j) in itertools.combinations(s, 2)])
edges = array(edges)
handles.append(env.drawlinelist(
edges, linewidth=linewidth, colors=edge_color))
if '#' in faces: # facets
if is_2d:
nv = len(vertices)
indices = array([(0, i, i + 1) for i in xrange(nv - 1)], int64)
handles.append(env.drawtrimesh(vertices, indices, colors=color))
else:
indices = array(hull.simplices, int64)
handles.append(env.drawtrimesh(points, indices, colors=color))
if '.' in faces: # vertices
color[:3] *= 0.75
color[3] = 1.
handles.append(env.plot3(
vertices, pointsize=pointsize, drawstyle=1, colors=color))
return handles
def remove(self):
"""Remove body from OpenRAVE environment."""
env = get_openrave_env()
with env:
env.Remove(self.rave)
def draw_polyhedron(points, combined='g-#', color=None, faces=None,
linewidth=1., pointsize=0.01, hull=None):
"""
Draw a polyhedron defined as the convex hull of a set of points.
Parameters
----------
points : list of arrays
List of 3D points in the world frame.
combined : string, optional
Drawing spec in matplotlib fashion. Default: 'g-#'.
color : char or triplet, optional
Color letter or RGB values, default is 'g' for green.
faces : string, optional
Faces of the polyhedron to draw. Use '.' for vertices, '-' for edges and
'#' for facets.
hull : scipy.spatial.ConvexHull
2D convex hull provided when drawing polygons, in which case the 3D hull
has zero volume.
linewidth : scalar
Thickness of drawn line.
pointsize : scalar
Vertex size.
Returns
-------
handles : list of openravepy.GraphHandle
OpenRAVE graphical handles. Must be stored in some variable, otherwise
the drawn object will vanish instantly.
Notes
-----
In the ``faces`` or ``combined`` strings, use '.' for vertices, '-' for
edges and '#' for facets.
"""
is_2d = hull is not None
if color is None:
color = combined[0]
if faces is None:
faces = combined[1:]
if type(color) is str:
color = matplotlib_to_rgba(color)
if hull is None:
try:
hull = ConvexHull(points)
except QhullError:
warn("QhullError: maybe polyhedron is empty?")
return []
vertices = array([points[i] for i in hull.vertices])
points = array(points)
color = array(color if color is not None else (0.0, 0.5, 0.0, 0.5))
handles = []
env = get_openrave_env()
if '-' in faces: # edges
edge_color = color * 0.7
edge_color[3] = 1.
edges = vstack([[points[i], points[j]]
for s in hull.simplices
for (i, j) in itertools.combinations(s, 2)])
edges = array(edges)
handles.append(env.drawlinelist(
edges, linewidth=linewidth, colors=edge_color))
if '#' in faces: # facets
if is_2d:
nv = len(vertices)
indices = array([(0, i, i + 1) for i in xrange(nv - 1)], int64)
handles.append(env.drawtrimesh(vertices, indices, colors=color))
else:
indices = array(hull.simplices, int64)
handles.append(env.drawtrimesh(points, indices, colors=color))
if '.' in faces: # vertices
color[:3] *= 0.75
color[3] = 1.
handles.append(env.plot3(
vertices, pointsize=pointsize, drawstyle=1, colors=color))
return handles