include ur srdf into loadUR

python/example_robot_data/robots_loader.py

@@ -2,7 +2,6 @@ import sys
 from os.path import dirname, exists, join
 
 import numpy as np
-
 import pinocchio
 from pinocchio.robot_wrapper import RobotWrapper
 
@@ -35,7 +34,8 @@ def readParamsFromSrdf(robot, SRDF_PATH, verbose, has_rotor_parameters=True, ref
         pinocchio.loadRotorParameters(rmodel, SRDF_PATH, verbose)
     rmodel.armature = np.multiply(rmodel.rotorInertia.flat, np.square(rmodel.rotorGearRatio.flat))
     pinocchio.loadReferenceConfigurations(rmodel, SRDF_PATH, verbose)
-    robot.q0.flat[:] = rmodel.referenceConfigurations[referencePose].copy()
+    if referencePose is not None:
+        robot.q0.flat[:] = rmodel.referenceConfigurations[referencePose].copy()
     return
 
 
@@ -252,7 +252,12 @@ def loadUR(robot=5, limited=False, gripper=False):
     URDF_FILENAME = "ur%i%s_%s.urdf" % (robot, "_joint_limited" if limited else '', 'gripper' if gripper else 'robot')
     URDF_SUBPATH = "/ur_description/urdf/" + URDF_FILENAME
     modelPath = getModelPath(URDF_SUBPATH)
-    return RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
+    model = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
+    if robot == 5 or robot == 3 and gripper:
+        SRDF_FILENAME = "ur%i%s.srdf" % (robot, '_gripper' if gripper else '')
+        SRDF_SUBPATH = "/ur_description/srdf/" + SRDF_FILENAME
+        readParamsFromSrdf(model, modelPath + SRDF_SUBPATH, False, False, None)
+    return model
 
 
 def loadHector():