Unverified Commit 6bde5477 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #1530 from jcarpent/topic/viewer

Add support of Convex within MeshCat + improve GeometryObject bindings
parents 1c1e7a93 c0e0c397
//
// Copyright (c) 2017-2020 CNRS INRIA
// Copyright (c) 2017-2021 CNRS INRIA
//
#ifndef __pinocchio_python_geometry_object_hpp__
......@@ -46,11 +46,14 @@ namespace pinocchio
bp::args("self","otherGeometryObject"),
"Copy constructor"
))
.def_readwrite("meshScale",&GeometryObject::meshScale,
.add_property("meshScale",
bp::make_getter(&GeometryObject::meshScale,
bp::return_internal_reference<>()),
bp::make_setter(&GeometryObject::meshScale),
"Scaling parameter of the mesh.")
.add_property("meshColor",
bp::make_getter(&GeometryObject::meshColor,
bp::return_value_policy<bp::return_by_value>()),
bp::return_internal_reference<>()),
bp::make_setter(&GeometryObject::meshColor),
"Color rgba of the mesh.")
.def_readwrite("geometry", &GeometryObject::geometry,
......
......@@ -6,6 +6,7 @@ from . import BaseVisualizer
import os
import warnings
import numpy as np
from distutils.version import LooseVersion
try:
import hppfcl
......@@ -24,21 +25,34 @@ def isMesh(geometry_object):
return False
def loadBVH(bvh):
def loadMesh(mesh):
import meshcat.geometry as mg
num_vertices = bvh.num_vertices
num_tris = bvh.num_tris
vertices = np.empty((num_vertices,3))
faces = np.empty((num_tris,3),dtype=int)
if isinstance(mesh,hppfcl.BVHModelBase):
num_vertices = mesh.num_vertices
num_tris = mesh.num_tris
call_triangles = mesh.tri_indices
call_vertices = mesh.vertices
elif isinstance(mesh,hppfcl.Convex):
num_vertices = mesh.num_points
num_tris = mesh.num_polygons
call_triangles = mesh.polygons
call_vertices = mesh.points
faces = np.empty((num_tris,3),dtype=int)
for k in range(num_tris):
tri = bvh.tri_indices(k)
tri = call_triangles(k)
faces[k] = [tri[i] for i in range(3)]
for k in range(num_vertices):
vert = bvh.vertices(k)
vertices[k] = vert
if LooseVersion(hppfcl.__version__) >= LooseVersion("1.7.7"):
vertices = call_vertices()
else:
vertices = np.empty((num_vertices,3))
for k in range(num_vertices):
vertices[k] = call_vertices(k)
vertices = vertices.astype(np.float32)
if num_tris > 0:
......@@ -137,6 +151,8 @@ class MeshcatVisualizer(BaseVisualizer):
obj = meshcat.geometry.Box(npToTuple(2. * geom.halfSide))
elif isinstance(geom, hppfcl.Sphere):
obj = meshcat.geometry.Sphere(geom.radius)
elif isinstance(geom, hppfcl.ConvexBase):
obj = loadMesh(geom)
else:
msg = "Unsupported geometry type for %s (%s)" % (geometry_object.name, type(geom) )
warnings.warn(msg, category=UserWarning, stacklevel=2)
......@@ -183,7 +199,7 @@ class MeshcatVisualizer(BaseVisualizer):
obj = self.loadMesh(geometry_object)
is_mesh = True
elif WITH_HPP_FCL_BINDINGS and isinstance(geometry_object.geometry, hppfcl.BVHModelBase):
obj = loadBVH(geometry_object.geometry)
obj = loadMesh(geometry_object.geometry)
else:
msg = "The geometry object named " + geometry_object.name + " is not supported by Pinocchio/MeshCat for vizualization."
warnings.warn(msg, category=UserWarning, stacklevel=2)
......
......@@ -45,6 +45,17 @@ viz.display(q0)
viz.displayCollisions(True)
viz.displayVisuals(False)
mesh = visual_model.geometryObjects[0].geometry
mesh.buildConvexRepresentation(True)
convex = mesh.convex
if convex is not None:
placement = pin.SE3.Identity()
placement.translation[0] = 2.
geometry = pin.GeometryObject("convex",0,convex,placement)
geometry.meshColor = np.ones((4))
visual_model.addGeometryObject(geometry)
# Display another robot.
viz2 = MeshcatVisualizer(model, collision_model, visual_model)
viz2.initViewer(viz.viewer)
......
......@@ -226,7 +226,7 @@ PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
const std::string & meshPath = "",
const Eigen::Vector3d & meshScale = Eigen::Vector3d::Ones(),
const bool overrideMaterial = false,
const Eigen::Vector4d & meshColor = Eigen::Vector4d::Zero(),
const Eigen::Vector4d & meshColor = Eigen::Vector4d::Ones(),
const std::string & meshTexturePath = "")
: name(name)
, parentFrame(std::numeric_limits<FrameIndex>::max())
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment