Skip to content
Snippets Groups Projects
Select Git revision
  • 8d8c4806e2fd5ad077736902b99bd3bdbbd855b6
  • master default
  • multipleinheritance
3 results

ComplexElement.html

Blame
    • gepetto_ex.py 2.06 KiB
    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    15
    16
    17
    18
    19
    20
    21
    22
    23
    24
    25
    26
    27
    28
    29
    30
    31
    32
    33
    34
    35
    36
    37
    38
    39
    40
    41
    42
    43
    44
    45
    46
    47
    48
    49
    50
    51
    52
    53
    54
    55
    56
    57
    58
    59
    60 
    

    

    

    # TODO:
# - get python gepetto viz to work
# - put that into robot manager
# - delete this file afterward

import pinocchio as pin
import numpy as np
import sys
import os
from os.path import dirname, join, abspath
import time
 
from pinocchio.visualize import GepettoVisualizer
import gepetto.corbaserver
gepetto.corbaserver.start_server()
time.sleep(3)
 
# 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 = "/home/gospodar/lund/praxis/ur_driver_humble_ws/src/Universal_Robots_ROS2_Description/meshes/ur5e/"
#urdf_filename = "talos_reduced.urdf"
#urdf_model_path = join(join(model_path,"talos_data/robots"),urdf_filename)
urdf_model_path = "/home/gospodar/lund/praxis/ur_driver_humble_ws/src/Universal_Robots_ROS2_Description/urdf/ur5e.urdf"
package_dir = "/home/gospodar/lund/praxis/ur_driver_humble_ws/src/Universal_Robots_ROS2_Description/meshes/ur5e/" 
 
#model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_model_path, mesh_dir, pin.JointModelFreeFlyer())
#model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_model_path, mesh_dir, pin.JointModelFreeFlyer())
#model = pin.Model()
model = pin.buildModelFromUrdf(urdf_model_path)
visual_model = pin.buildGeomFromUrdf(model, urdf_model_path, pin.GeometryType.VISUAL)
collision_model = pin.buildGeomFromUrdf(model, urdf_model_path, pin.GeometryType.COLLISION)
viz = GepettoVisualizer(model, collision_model, visual_model)
viz.initViewer()
viz.loadViewerModel()

NQ, NV = model.nq, model.nv

# Display a robot configuration.
q0 = pin.neutral(model)
viz.display(q0)
data = pin.Data(model)

#for i in range(10000):
#    q0[0] += 0.001
#    pin.forwardKinematics(model, data, q0)
#    viz.display(q0)
 
# Display another robot.
#viz2 = GepettoVisualizer(model, collision_model, visual_model)
#viz2.initViewer(viz.viewer)
#viz2.loadViewerModel(rootNodeName = "pinocchio2")
#q = q0.copy()
#q[1] = 1.0
#viz2.display(q)