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_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"]