# Load the URDF model.
# 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, "example-robot-data/robots")
mesh_dir = pinocchio_model_dir
urdf_filename = "talos_reduced.urdf"
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)
# Initialize the viewer.
try:
viz.initViewer()
except ImportError as err:
print(
"Error while initializing the viewer. It seems you should install gepetto-viewer"
)
print(err)
sys.exit(0)
try:
viz.loadViewerModel("pinocchio")
except AttributeError as err:
print(
"Error while loading the viewer model. It seems you should start gepetto-viewer"
)
print(err)
sys.exit(0)
placement = pin.SE3(np.eye(3), np.array([i, 0, 0]))
geom_obj = pin.GeometryObject("obj{}".format(i), 0, 0, geom, placement)
color = np.random.uniform(0, 1, 4)
color[3] = 1
geom_obj.meshColor = color
geom_model.addGeometryObject(geom_obj)
viz = GepettoVisualizer(
model=model,
collision_model=geom_model,
visual_model=geom_model,
)
# Initialize the viewer.
try:
viz.initViewer()
except ImportError as error:
print(
"Error while initializing the viewer. It seems you should install gepetto-viewer"
)
print(error)
sys.exit(0)
try:
viz.loadViewerModel("shapes")
except AttributeError as error:
print(
"Error while loading the viewer model. It seems you should start gepetto-viewer"
)
print(error)
sys.exit(0)
# Load the URDF model.
# Conversion with str seems to be necessary when executing this file with ipython
current_path = str(os.path.dirname(os.path.abspath(__file__)))
model_path = str(
os.path.abspath(os.path.join(current_path, '../../models/romeo')))
mesh_dir = model_path
urdf_model_path = str(
os.path.abspath(
os.path.join(model_path, 'romeo_description/urdf/romeo_small.urdf')))
model, collision_model, visual_model = pin.buildModelsFromUrdf(
urdf_model_path, mesh_dir, pin.JointModelFreeFlyer())
display = GepettoVisualizer(model, collision_model, visual_model)
# Initialize the display.
display.initViewer()
display.loadViewerModel("pinocchio")
# Display a robot configuration.
q0 = np.matrix([
0,
0,
0.840252,
0,
0,
0,
1, # Free flyer
0,
0,
-0.3490658,
0.6981317,