Verified Commit b4d7a8b6 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

models: fix model mesh path

parent 02fb4e29
......@@ -8,8 +8,8 @@ from os.path import *
# Load UR robot arm
# This path refers to Pinocchio source code but you can define your own directory here.
pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models")
model_path = pinocchio_model_dir + '/others/robots'
mesh_dir = model_path
model_path = pinocchio_model_dir + '/example-robot-data/robots'
mesh_dir = pinocchio_model_dir
# You should change here to set up your own URDF file
urdf_filename = model_path + '/ur_description/urdf/ur5_robot.urdf'
model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_filename, mesh_dir)
......
......@@ -17,7 +17,7 @@ int main(int /*argc*/, char ** /*argv*/)
const std::string robots_model_path = PINOCCHIO_MODEL_DIR;
// You should change here to set up your own URDF file
const std::string urdf_filename = robots_model_path + std::string("/example-robot-data/robots/talos_data/urdf/talos_reduced.urdf");
const std::string urdf_filename = robots_model_path + std::string("/example-robot-data/robots/talos_data/robots/talos_reduced.urdf");
// You should change here to set up your own SRDF file
const std::string srdf_filename = robots_model_path + std::string("/example-robot-data/robots/talos_data/srdf/talos.srdf");
......
......@@ -6,8 +6,8 @@ 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
model_path = join(pinocchio_model_dir,"example-robot-data/robots")
mesh_dir = pinocchio_model_dir
urdf_filename = "romeo_small.urdf"
urdf_model_path = join(join(model_path,"romeo_description/urdf"),urdf_filename)
......@@ -15,7 +15,7 @@ urdf_model_path = join(join(model_path,"romeo_description/urdf"),urdf_filename)
model = pin.buildModelFromUrdf(urdf_model_path,pin.JointModelFreeFlyer())
# Load collision geometries
geom_model = pin.buildGeomFromUrdf(model,urdf_model_path,model_path,pin.GeometryType.COLLISION)
geom_model = pin.buildGeomFromUrdf(model,urdf_model_path,mesh_dir,pin.GeometryType.COLLISION)
# Add collisition pairs
geom_model.addAllCollisionPairs()
......
......@@ -5,8 +5,8 @@ from os.path import dirname, join, abspath
# This path refers to Pinocchio source code but you can define your own directory here.
pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models")
model_path = join(pinocchio_model_dir,"others/robots") if len(argv)<2 else argv[1]
mesh_dir = model_path
model_path = join(pinocchio_model_dir,"example-robot-data/robots") if len(argv)<2 else argv[1]
mesh_dir = pinocchio_model_dir
urdf_model_path = join(model_path,"ur_description/urdf/ur5_robot.urdf")
# Load the urdf model
......
......@@ -13,10 +13,10 @@ from pinocchio.visualize import GepettoVisualizer
# Conversion with str seems to be necessary when executing this file with ipython
pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))),"models")
model_path = join(pinocchio_model_dir,"others/robots")
mesh_dir = model_path
model_path = join(pinocchio_model_dir,"example-robot-data/robots")
mesh_dir = pinocchio_model_dir
urdf_filename = "talos_reduced.urdf"
urdf_model_path = join(join(model_path,"talos_data/urdf"),urdf_filename)
urdf_model_path = join(join(model_path,"talos_data/robots"),urdf_filename)
model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_model_path, mesh_dir, pin.JointModelFreeFlyer())
viz = GepettoVisualizer(model, collision_model, visual_model)
......
......@@ -14,8 +14,8 @@ from pinocchio.visualize import MeshcatVisualizer
# Conversion with str seems to be necessary when executing this file with ipython
pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))),"models")
model_path = join(pinocchio_model_dir,"others/robots")
mesh_dir = model_path
model_path = join(pinocchio_model_dir,"example-robot-data/robots")
mesh_dir = pinocchio_model_dir
urdf_filename = "romeo_small.urdf"
urdf_model_path = join(join(model_path,"romeo_description/urdf"),urdf_filename)
......
......@@ -14,10 +14,10 @@ from pinocchio.visualize import MeshcatVisualizer
# Conversion with str seems to be necessary when executing this file with ipython
pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))),"models")
model_path = join(pinocchio_model_dir,"others/robots")
mesh_dir = model_path
model_path = join(pinocchio_model_dir,"example-robot-data/robots")
mesh_dir = pinocchio_model_dir
urdf_filename = "talos_reduced.urdf"
urdf_model_path = join(join(model_path,"talos_data/urdf"),urdf_filename)
urdf_model_path = join(join(model_path,"talos_data/robots"),urdf_filename)
model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_model_path, mesh_dir, pin.JointModelFreeFlyer())
......
......@@ -6,7 +6,7 @@ from os.path import dirname, join, abspath
pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models")
# You should change here to set up your own URDF file or just pass it as an argument of this example.
urdf_filename = pinocchio_model_dir + '/others/robots/ur_description/urdf/ur5_robot.urdf' if len(argv)<2 else argv[1]
urdf_filename = pinocchio_model_dir + '/example-robot-data/robots/ur_description/urdf/ur5_robot.urdf' if len(argv)<2 else argv[1]
# Load the urdf model
model = pinocchio.buildModelFromUrdf(urdf_filename)
......
......@@ -29,10 +29,10 @@ if len(argv)>1:
# Conversion with str seems to be necessary when executing this file with ipython
pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))),"models")
model_path = join(pinocchio_model_dir,"others/robots")
mesh_dir = model_path
model_path = join(pinocchio_model_dir,"example-robot-data/robots")
mesh_dir = pinocchio_model_dir
urdf_filename = "talos_reduced.urdf"
urdf_model_path = join(join(model_path,"talos_data/urdf"),urdf_filename)
urdf_model_path = join(join(model_path,"talos_data/robots"),urdf_filename)
robot = RobotWrapper.BuildFromURDF(urdf_model_path, mesh_dir, pin.JointModelFreeFlyer())
......
......@@ -10,11 +10,12 @@ class TestGeometryObjectUrdfBindings(unittest.TestCase):
def setUp(self):
self.current_file = os.path.dirname(str(os.path.abspath(__file__)))
self.model_dir = os.path.abspath(os.path.join(self.current_file, "../../models/others/robots"))
self.mesh_path = os.path.abspath(os.path.join(self.current_file, "../../models"))
self.model_dir = os.path.abspath(os.path.join(self.current_file, "../../models/example-robot-data/robots"))
self.model_path = os.path.abspath(os.path.join(self.model_dir, "romeo_description/urdf/romeo.urdf"))
def test_load(self):
hint_list = [self.model_dir, "wrong/hint"]
hint_list = [self.mesh_path, "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())
......@@ -24,7 +25,7 @@ class TestGeometryObjectUrdfBindings(unittest.TestCase):
self.assertEqual(os.path.normpath(col.meshPath), os.path.normpath(expected_mesh_path))
def test_self_load(self):
hint_list = [self.model_dir]
hint_list = [self.mesh_path]
model = pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer())
collision_model_ref = pin.buildGeomFromUrdf(model, self.model_path, pin.GeometryType.COLLISION, hint_list)
......@@ -34,11 +35,11 @@ class TestGeometryObjectUrdfBindings(unittest.TestCase):
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)
pin.buildGeomFromUrdf(model, self.model_path, pin.GeometryType.COLLISION, collision_model_self, self.mesh_path)
self.assertTrue(checkGeom(collision_model_ref, collision_model_self))
hint_vec = pin.StdVec_StdString()
hint_vec.append(self.model_dir)
hint_vec.append(self.mesh_path)
collision_model_self = pin.GeometryModel()
pin.buildGeomFromUrdf(model, self.model_path, pin.GeometryType.COLLISION, collision_model_self, hint_vec)
......@@ -46,7 +47,7 @@ class TestGeometryObjectUrdfBindings(unittest.TestCase):
def test_multi_load(self):
hint_list = [self.model_dir, "wrong/hint"]
hint_list = [self.mesh_path, "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')
......@@ -87,19 +88,19 @@ class TestGeometryObjectUrdfBindings(unittest.TestCase):
def test_deprecated_signatures(self):
model = pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer())
hint_list = [self.model_dir, "wrong/hint"]
hint_list = [self.mesh_path, "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)
hint_vec.append(self.mesh_path)
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)
collision_model = pin.buildGeomFromUrdf(model, self.model_path, self.mesh_path, 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())
collision_model = pin.buildGeomFromUrdf(model, self.model_path, self.mesh_path, pin.GeometryType.COLLISION, pin.hppfcl.MeshLoader())
if __name__ == '__main__':
unittest.main()
......@@ -7,7 +7,7 @@ class TestGeometryObjectUrdfBindings(unittest.TestCase):
def setUp(self):
self.current_file = os.path.dirname(str(os.path.abspath(__file__)))
self.model_dir = os.path.abspath(os.path.join(self.current_file, "../../models/others/robots"))
self.model_dir = os.path.abspath(os.path.join(self.current_file, "../../models/example-robot-data/robots"))
self.model_path = os.path.abspath(os.path.join(self.model_dir, "romeo_description/urdf/romeo.urdf"))
def test_load(self):
......
......@@ -23,7 +23,7 @@ BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
BOOST_AUTO_TEST_CASE ( build_model )
{
const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
const std::string dir = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots");
const std::string dir = PINOCCHIO_MODEL_DIR;
pinocchio::Model model;
pinocchio::urdf::buildModel(filename, model);
......@@ -147,7 +147,7 @@ BOOST_AUTO_TEST_CASE ( build_model_from_UDRFTree )
BOOST_AUTO_TEST_CASE ( build_model_with_joint )
{
const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
const std::string dir = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots");
const std::string dir = PINOCCHIO_MODEL_DIR;
pinocchio::Model model;
pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model);
......
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