#!/usr/bin/python3
import argparse

from gepetto.corbaserver import start_server
import pinocchio

pinocchio.switchToNumpyArray()

parser = argparse.ArgumentParser(
    description="Visualize a robot in Gepetto-viewer, loaded with pinocchio"
)
parser.add_argument(
    "--package-dirs",
    type=str,
    default="",
    help="sequence of paths separated by : (so that $ROS_PACKAGE_PATH works)",
)
parser.add_argument("urdf_file", type=str, help="the urdf file")
parser.add_argument(
    "--collision",
    action="store_true",
    help="load the collision model instead of the visual",
)

args = parser.parse_args()

start_server()

robot = pinocchio.RobotWrapper.BuildFromURDF(
    args.urdf_file, package_dirs=args.package_dirs.split(":")
)
robot.initViewer()
robot.loadViewerModel()
if args.collision:
    robot.displayCollisions(True)
robot.display(pinocchio.neutral(robot.model))
