diff --git a/python/example_robot_data/robots_loader.py b/python/example_robot_data/robots_loader.py index 8d36a81502d12923fee441f0ac9d593df46d74d6..3a537972154fe1799a508b8ef366706422643280 100644 --- a/python/example_robot_data/robots_loader.py +++ b/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():