Commit 3028f018 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

include ur srdf into loadUR

parent edb456b0
Pipeline #7404 failed with stage
in 5 minutes and 33 seconds
......@@ -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():
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment