Unverified Commit 4c834f20 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #1472 from jcarpent/topic/python

meshcat: fix loading of BVH model
parents 6f977ef6 2f2b9370
Pipeline #15222 passed with stage
in 147 minutes and 22 seconds
......@@ -13,6 +13,16 @@ try:
except:
WITH_HPP_FCL_BINDINGS = False
def isMesh(geometry_object):
""" Check whether the geometry object contains a Mesh supported by MeshCat """
if geometry_object.meshPath == "":
return False
_, file_extension = os.path.splitext(geometry_object.meshPath)
if file_extension.lower() in [".dae", ".obj", ".stl"]:
return True
return False
def loadBVH(bvh):
import meshcat.geometry as mg
......@@ -165,13 +175,19 @@ class MeshcatVisualizer(BaseVisualizer):
viewer_name = self.getViewerNodeName(geometry_object, geometry_type)
is_mesh = False
try:
if WITH_HPP_FCL_BINDINGS and isinstance(geometry_object.geometry, hppfcl.ShapeBase):
obj = self.loadPrimitive(geometry_object)
elif isMesh(geometry_object):
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)
else:
obj = self.loadMesh(geometry_object)
msg = "The geometry object named " + geometry_object.name + " is not supported by Pinocchio/MeshCat for vizualization."
warnings.warn(msg, category=UserWarning, stacklevel=2)
return
if obj is None:
return
except Exception as e:
......@@ -195,6 +211,10 @@ class MeshcatVisualizer(BaseVisualizer):
material.opacity = float(meshColor[3])
self.viewer[viewer_name].set_object(obj, material)
if is_mesh: # Apply the scaling
scale = list(np.asarray(geometry_object.meshScale).flatten())
self.viewer[viewer_name].set_property("scale",scale)
def loadViewerModel(self, rootNodeName="pinocchio", color = None):
"""Load the robot in a MeshCat viewer.
Parameters:
......@@ -241,14 +261,19 @@ class MeshcatVisualizer(BaseVisualizer):
pin.updateGeometryPlacements(self.model, self.data, self.visual_model, self.visual_data)
for visual in self.visual_model.geometryObjects:
visual_name = self.getViewerNodeName(visual,pin.GeometryType.VISUAL)
# Get mesh pose.
M = self.visual_data.oMg[self.visual_model.getGeometryId(visual.name)]
# Manage scaling
scale = np.asarray(visual.meshScale).flatten()
S = np.diag(np.concatenate((scale,[1.0])))
T = np.array(M.homogeneous).dot(S)
# Manage scaling: force scaling even if this should be normally handled by MeshCat (but there is a bug here)
if isMesh(visual):
scale = np.asarray(visual.meshScale).flatten()
S = np.diag(np.concatenate((scale,[1.0])))
T = np.array(M.homogeneous).dot(S)
else:
T = M.homogeneous
# Update viewer configuration.
self.viewer[self.getViewerNodeName(visual,pin.GeometryType.VISUAL)].set_transform(T)
self.viewer[visual_name].set_transform(T)
def displayCollisions(self,visibility):
"""Set whether to display collision objects or not.
......
......@@ -360,7 +360,7 @@ namespace pinocchio
FrameIndex frame_id;
UrdfGeomVisitorBase::Frame frame = visitor.getBodyFrame (link_name, frame_id);
SE3 body_placement = frame.placement;
const SE3 & body_placement = frame.placement;
std::size_t objectId = 0;
for (typename VectorSharedT::const_iterator i = geometries_array.begin();i != geometries_array.end(); ++i)
......@@ -391,7 +391,7 @@ namespace pinocchio
std::string meshTexturePath;
bool overrideMaterial = getVisualMaterial<GeometryType>((*i), meshTexturePath, meshColor, package_dirs);
SE3 geomPlacement = body_placement * convertFromUrdf((*i)->origin);
const SE3 geomPlacement = body_placement * convertFromUrdf((*i)->origin);
std::ostringstream geometry_object_suffix;
geometry_object_suffix << "_" << objectId;
const std::string & geometry_object_name = std::string(link_name + geometry_object_suffix.str());
......
Markdown is supported
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