Robot Custom

import compas
from compas.robots import GithubPackageMeshLoader
from compas.robots import RobotModel
from compas_view2.app import App
from compas_view2.collections import Collection

import itertools

from compas.geometry import Transformation
from compas.geometry import Translation
from compas.geometry import Rotation
from compas.robots import Geometry
from math import radians, degrees

# github = GithubPackageMeshLoader('ros-industrial/abb', 'abb_irb6600_support', 'kinetic-devel')
# model = RobotModel.from_urdf_file(github.load_urdf('irb6640.urdf'))
# model.load_geometry(github)
model = RobotModel.ur5(True)

viewer = App(viewmode="lighted", enable_sceneform=True, enable_propertyform=True, enable_sidebar=True, width=2000, height=1000)

def create(link, parent=None, parent_joint=None):

    obj = None

    meshes = []

    for item in itertools.chain(link.visual):
        meshes.extend(Geometry._get_item_meshes(item))

    obj = parent.add(Collection(meshes), name=link.name, show_lines=False)

    if parent_joint:
        obj.matrix = Transformation.from_frame(parent_joint.origin).matrix

        @viewer.slider(title=parent_joint.name, minval=-180 , maxval=180)
        def rotate(angle):
            T = Translation.from_vector(obj.translation)
            R = Rotation.from_axis_and_angle(parent_joint.axis.vector, radians(angle))
            obj.matrix = (T * R).matrix
            viewer.view.update()

    for joint in link.joints:
        create(joint.child_link, parent = obj, parent_joint=joint)

create(model.root, viewer)

viewer.show()