def test_multi_load(self):
hint_list = [self.model_dir, "wrong/hint"]
expected_collision_path = os.path.join(
self.model_dir,
'romeo_description/meshes/V1/collision/LHipPitch.dae')
expected_visual_path = os.path.join(
self.model_dir, 'romeo_description/meshes/V1/visual/LHipPitch.dae')
model = pin.buildModelFromUrdf(self.model_path,
pin.JointModelFreeFlyer())
collision_model = pin.buildGeomFromUrdf(model, self.model_path,
pin.GeometryType.COLLISION,
hint_list)
col = collision_model.geometryObjects[1]
self.assertEqual(col.meshPath, expected_collision_path)
visual_model = pin.buildGeomFromUrdf(model, self.model_path,
pin.GeometryType.VISUAL,
hint_list)
vis = visual_model.geometryObjects[1]
self.assertEqual(vis.meshPath, expected_visual_path)
model_2, collision_model_2, visual_model_2 = pin.buildModelsFromUrdf(
self.model_path, hint_list, pin.JointModelFreeFlyer())
self.assertEqual(model, model_2)
col_2 = collision_model_2.geometryObjects[1]
self.assertEqual(col_2.meshPath, expected_collision_path)
vis_2 = visual_model_2.geometryObjects[1]
self.assertEqual(vis_2.meshPath, expected_visual_path)
model_c, collision_model_c = pin.buildModelsFromUrdf(
self.model_path,
hint_list,
pin.JointModelFreeFlyer(),
geometry_types=pin.GeometryType.COLLISION)
self.assertEqual(model, model_c)
col_c = collision_model_c.geometryObjects[1]
self.assertEqual(col_c.meshPath, expected_collision_path)
model_v, visual_model_v = pin.buildModelsFromUrdf(
self.model_path,
hint_list,
pin.JointModelFreeFlyer(),
geometry_types=pin.GeometryType.VISUAL)
self.assertEqual(model, model_v)
vis_v = visual_model_v.geometryObjects[1]
self.assertEqual(vis_v.meshPath, expected_visual_path)
def _build_from_urdf(self, model_wrapper, urdf_path, package_path):
model = model_wrapper.model
geom_model = model_wrapper.geom_model
pin.buildModelFromUrdf(urdf_path, model)
pin.buildGeomFromUrdf(
model, urdf_path, pin.GeometryType.COLLISION, geom_model, package_path,
)
# viz_model = pin.buildGeomFromUrdf(
# model, urdf_path, pin.GeometryType.VISUAL, [packages_path],
# )
# ! keep this line here for body dimensions to be defined (otherwize = -inf)
model_data = model.createData()
geom_model_data = geom_model.createData()
def test_self_load(self):
hint_list = [self.model_dir]
model = pin.buildModelFromUrdf(self.model_path,
pin.JointModelFreeFlyer())
collision_model_ref = pin.buildGeomFromUrdf(model, self.model_path,
pin.GeometryType.COLLISION,
hint_list)
collision_model_self = pin.GeometryModel()
pin.buildGeomFromUrdf(model, self.model_path,
pin.GeometryType.COLLISION, collision_model_self,
hint_list)
self.assertTrue(checkGeom(collision_model_ref, collision_model_self))
collision_model_self = pin.GeometryModel()
pin.buildGeomFromUrdf(model, self.model_path,
pin.GeometryType.COLLISION, collision_model_self,
self.model_dir)
self.assertTrue(checkGeom(collision_model_ref, collision_model_self))
hint_vec = pin.StdVec_StdString()
hint_vec.append(self.model_dir)
collision_model_self = pin.GeometryModel()
pin.buildGeomFromUrdf(model, self.model_path,
pin.GeometryType.COLLISION, collision_model_self,
hint_vec)
self.assertTrue(checkGeom(collision_model_ref, collision_model_self))
def test_load(self):
hint_list = [self.model_dir, "wrong/hint"]
expected_mesh_path = os.path.join(self.model_dir,'romeo_description/meshes/V1/collision/LHipPitch.dae')
model = pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer())
collision_model = pin.buildGeomFromUrdf(model, self.model_path, pin.GeometryType.COLLISION, hint_list)
col = collision_model.geometryObjects[1]
self.assertEqual(col.meshPath, expected_mesh_path)
def test_load(self):
current_file = os.path.dirname(os.path.abspath(__file__))
model_dir = os.path.abspath(os.path.join(current_file, '../../models/romeo'))
model_path = os.path.abspath(os.path.join(model_dir, 'romeo_description/urdf/romeo.urdf'))
expected_mesh_path = os.path.join(model_dir,'romeo_description/meshes/V1/collision/LHipPitch.dae')
hint_list = [model_dir, "wrong/hint"]
model = pin.buildModelFromUrdf(model_path, pin.JointModelFreeFlyer())
collision_model = pin.buildGeomFromUrdf(model, model_path, pin.utils.fromListToVectorOfString(hint_list), pin.GeometryType.COLLISION)
col = collision_model.geometryObjects[1]
self.assertTrue(col.meshPath == expected_mesh_path)
def test_load(self):
current_file = os.path.dirname(os.path.abspath(__file__))
model_dir = os.path.abspath(os.path.join(current_file, '../../models/romeo'))
model_path = os.path.abspath(os.path.join(model_dir, 'romeo_description/urdf/romeo.urdf'))
expected_mesh_path = os.path.join(model_dir,'romeo_description/meshes/V1/collision/LHipPitch.dae')
hint_list = [model_dir, "wrong/hint"]
model = pin.buildModelFromUrdf(model_path, pin.JointModelFreeFlyer())
collision_model = pin.buildGeomFromUrdf(model, model_path, pin.utils.fromListToVectorOfString(hint_list), pin.GeometryType.COLLISION)
col = collision_model.geometryObjects[1]
self.assertTrue(col.meshPath == expected_mesh_path)
def test_deprecated_signatures(self):
model = pin.buildModelFromUrdf(self.model_path,
pin.JointModelFreeFlyer())
hint_list = [self.model_dir, "wrong/hint"]
collision_model = pin.buildGeomFromUrdf(model, self.model_path,
hint_list,
pin.GeometryType.COLLISION)
hint_vec = pin.StdVec_StdString()
hint_vec.append(self.model_dir)
collision_model = pin.buildGeomFromUrdf(model, self.model_path,
hint_vec,
pin.GeometryType.COLLISION)
collision_model = pin.buildGeomFromUrdf(model, self.model_path,
self.model_dir,
pin.GeometryType.COLLISION)
if pin.WITH_HPP_FCL_BINDINGS:
collision_model = pin.buildGeomFromUrdf(model, self.model_path,
hint_list,
pin.GeometryType.COLLISION,
pin.hppfcl.MeshLoader())
collision_model = pin.buildGeomFromUrdf(model, self.model_path,
hint_vec,
pin.GeometryType.COLLISION,
pin.hppfcl.MeshLoader())
collision_model = pin.buildGeomFromUrdf(model, self.model_path,
self.model_dir,
pin.GeometryType.COLLISION,
pin.hppfcl.MeshLoader())
import os
from os.path import dirname, join, abspath
pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))),"models")
model_path = join(pinocchio_model_dir,"others/robots")
mesh_dir = model_path
urdf_filename = "romeo_small.urdf"
urdf_model_path = join(join(model_path,"romeo_description/urdf"),urdf_filename)
# Load model
model = pin.buildModelFromUrdf(urdf_model_path,pin.JointModelFreeFlyer())
# Load collision geometries
geom_model = pin.buildGeomFromUrdf(model,urdf_model_path,model_path,pin.GeometryType.COLLISION)
# Add collisition pairs
geom_model.addAllCollisionPairs()
print("num collision pairs - initial:",len(geom_model.collisionPairs))
# Remove collision pairs listed in the SRDF file
srdf_filename = "romeo.srdf"
srdf_model_path = model_path + "/romeo_description/srdf/" + srdf_filename
pin.removeCollisionPairs(model,geom_model,srdf_model_path)
print("num collision pairs - final:",len(geom_model.collisionPairs))
# Load reference configuration
pin.loadReferenceConfigurations(model,srdf_model_path)
def __init__(self,
robot,
use_theoretical_model=False,
mesh_root_path=None,
urdf_rgba=None,
lock=None,
backend=None,
robot_name="robot",
window_name='jiminy',
scene_name='world'):
"""
@brief Constructor.
@param robot The jiminy.Robot to display.
@param use_theoretical_model Whether to use the theoretical (rigid) model or the flexible model for this robot.
@param mesh_root_path Optional, path to the folder containing the URDF meshes.
@param urdf_rgba Color to use to display this robot (rgba).
- @param lock Custom threading.Lock
- Optional: Only required for parallel rendering using multiprocessing.
It is required since some backends does not support multiple
simultaneous connections (e.g. corbasever).
@param backend The name of the desired backend to use for rendering.
Optional, either 'gepetto-gui' or 'meshcat' ('panda3d' available soon).
@param robot_name Unique robot name, to identify each robot in the viewer.
@param scene_name Scene name, used only when gepetto-gui is used as backend.
@param window_name Window name, used only when gepetto-gui is used as backend.
Note that it is not allowed to be equal to the window name.
"""
# Backup some user arguments
self.urdf_path = robot.urdf_path
self.robot_name = robot_name
self.scene_name = scene_name
self.window_name = window_name
self.use_theoretical_model = use_theoretical_model
self._lock = lock if lock is not None else Viewer._lock
if self.scene_name == self.window_name:
raise ValueError(
"Please, choose a different name for the scene and the window."
)
# Extract the right Pinocchio model
if self.use_theoretical_model:
self.pinocchio_model = robot.pinocchio_model_th
self.pinocchio_data = robot.pinocchio_data_th
else:
self.pinocchio_model = robot.pinocchio_model
self.pinocchio_data = robot.pinocchio_data
# Select the desired backend
if backend is None:
if Viewer.backend is None:
if Viewer._is_notebook() or (not 'gepetto-gui'
in backends_available):
backend = 'meshcat'
else:
backend = 'gepetto-gui'
else:
backend = Viewer.backend
else:
if not backend in backends_available:
raise ValueError("%s backend not available." % backend)
# Update the backend currently running, if any
if (Viewer.backend != backend) and \
(Viewer._backend_obj is not None or \
Viewer._backend_proc is not None):
Viewer.close()
print("Different backend already running. Closing it...")
Viewer.backend = backend
# Check if the backend is still available, if any
if Viewer._backend_obj is not None and Viewer._backend_proc is not None:
is_backend_running = True
if Viewer._backend_proc.poll() is not None:
is_backend_running = False
if (Viewer.backend == 'gepetto-gui'):
from omniORB.CORBA import TRANSIENT as gepetto_server_error
try:
Viewer._backend_obj.gui.refresh()
except gepetto_server_error:
is_backend_running = False
if not is_backend_running:
Viewer._backend_obj = None
Viewer._backend_proc = None
Viewer._backend_exception = None
else:
is_backend_running = False
# Access the current backend or create one if none is available
try:
if (Viewer.backend == 'gepetto-gui'):
import omniORB
Viewer._backend_exception = omniORB.CORBA.COMM_FAILURE
if Viewer._backend_obj is None:
Viewer._backend_obj, Viewer._backend_proc = \
Viewer._get_gepetto_client(True)
is_backend_running = Viewer._backend_proc is None
if Viewer._backend_obj is not None:
self._client = Viewer._backend_obj.gui
else:
raise RuntimeError("Impossible to open Gepetto-viewer.")
if not scene_name in self._client.getSceneList():
self._client.createSceneWithFloor(scene_name)
if not window_name in self._client.getWindowList():
self._window_id = self._client.createWindow(window_name)
self._client.addSceneToWindow(scene_name, self._window_id)
self._client.createGroup(scene_name + '/' + scene_name)
self._client.addLandmark(scene_name + '/' + scene_name,
0.1)
else:
self._window_id = int(
np.where([
name == window_name
for name in self._client.getWindowList()
])[0][0])
# Set the default camera pose if the viewer is not running before
if not is_backend_running:
self.setCameraTransform(
translation=DEFAULT_CAMERA_XYZRPY_OFFSET_GEPETTO[:3],
rotation=DEFAULT_CAMERA_XYZRPY_OFFSET_GEPETTO[3:])
else:
from pinocchio.visualize import MeshcatVisualizer
from pinocchio.shortcuts import createDatas
if Viewer._backend_obj is None:
Viewer._create_meshcat_backend()
if Viewer._is_notebook():
Viewer.display_jupyter_cell()
else:
Viewer._backend_obj.open()
self._client = MeshcatVisualizer(self.pinocchio_model, None,
None)
self._client.viewer = Viewer._backend_obj
except:
raise RuntimeError("Impossible to load backend.")
# Create a RobotWrapper
root_path = mesh_root_path if mesh_root_path is not None else os.environ.get(
'JIMINY_MESH_PATH', [])
if (Viewer.backend == 'gepetto-gui'):
Viewer._delete_gepetto_nodes_viewer(scene_name + '/' +
self.robot_name)
if (urdf_rgba is not None):
alpha = urdf_rgba[3]
self.urdf_path = Viewer._get_colorized_urdf(
self.urdf_path, urdf_rgba[:3], root_path)
else:
alpha = 1.0
collision_model = pin.buildGeomFromUrdf(self.pinocchio_model,
self.urdf_path, root_path,
pin.GeometryType.COLLISION)
visual_model = pin.buildGeomFromUrdf(self.pinocchio_model,
self.urdf_path, root_path,
pin.GeometryType.VISUAL)
self._rb = RobotWrapper(model=self.pinocchio_model,
collision_model=collision_model,
visual_model=visual_model)
if not self.use_theoretical_model:
self._rb.data = robot.pinocchio_data
self.pinocchio_data = self._rb.data
# Load robot in the backend viewer
if (Viewer.backend == 'gepetto-gui'):
self._rb.initViewer(windowName=window_name,
sceneName=scene_name,
loadModel=False)
self._rb.loadViewerModel(self.robot_name)
self._client.setFloatProperty(scene_name + '/' + self.robot_name,
'Transparency', 1 - alpha)
else:
self._client.collision_model = collision_model
self._client.visual_model = visual_model
self._client.data, self._client.collision_data, self._client.visual_data = \
createDatas(self.pinocchio_model, collision_model, visual_model)
self._client.loadViewerModel(rootNodeName=self.robot_name,
color=urdf_rgba)
self._rb.viz = self._client
import pinocchio, os
urdfP = "package://example-robot-data/robots/ur_description/urdf/ur5_robot.urdf"
rosPaths = os.environ["ROS_PACKAGE_PATH"].split(':')
for p in rosPaths:
urdf = p + "/" + urdfP[len("package://"):]
if os.path.isfile(urdf):
model = pinocchio.buildModelFromUrdf(urdf)
visual = pinocchio.buildGeomFromUrdf(model, urdf, pinocchio.VISUAL,
rosPaths)
from PythonQt.QtPinocchio import FrameTreeWidget
frameTree = FrameTreeWidget()
frameTree.loadModel(model, visual)
frameTree.initViewer("ur5")
dock = frameTree.putInsideDockWidget()
from pinocchio.romeo_wrapper import RomeoWrapper import os current_path = os.getcwd() # The model of Romeo is contained in the path PINOCCHIO_GIT_REPOSITORY/models/romeo model_path = current_path + "/" + "../../models/romeo" mesh_dir = model_path urdf_filename = "romeo_small.urdf" urdf_model_path = model_path + "/romeo_description/urdf/" + urdf_filename # Load model model = pin.buildModelFromUrdf(urdf_model_path,pin.JointModelFreeFlyer()) # Load collision geometries geom_model = pin.buildGeomFromUrdf(model,urdf_model_path,[model_path],pin.GeometryType.COLLISION) # Add collisition pairs geom_model.addAllCollisionPairs() # Remove collision pairs listed in the SRDF file srdf_filename = "romeo_small.srdf" srdf_model_path = model_path + "/romeo_description/srdf/" + srdf_filename pin.removeCollisionPairs(model,geom_model,srdf_model_path) # Load reference configuration pin.loadReferenceConfigurations(model,srdf_model_path) # Retrieve the half sitting position from the SRDF file q = model.referenceConfigurations["half_sitting"]
