def loadDoublePendulum():
URDF_FILENAME = "double_pendulum.urdf"
URDF_SUBPATH = "/double_pendulum_description/urdf/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH,
[getVisualPath(modelPath)])
return robot
def loadRomeo():
URDF_FILENAME = "romeo.urdf"
URDF_SUBPATH = "/romeo_description/urdf/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
return RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH,
[getVisualPath(modelPath)],
pinocchio.JointModelFreeFlyer())
def loadIris():
URDF_FILENAME = "iris_simple.urdf"
URDF_SUBPATH = "/iris_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath],
pinocchio.JointModelFreeFlyer())
return robot
def __init__(self):
urdf_path = join(self.path, self.urdf_subpath, self.urdf_filename)
self.model_path = getModelPath(urdf_path, self.verbose)
self.urdf_path = join(self.model_path, urdf_path)
self.robot = RobotWrapper.BuildFromURDF(self.urdf_path, [join(self.model_path, '../..')],
pin.JointModelFreeFlyer() if self.free_flyer else None)
if self.srdf_filename:
self.srdf_path = join(self.model_path, self.path, self.srdf_subpath, self.srdf_filename)
self.robot.q0 = readParamsFromSrdf(self.robot.model, self.srdf_path, self.verbose,
self.has_rotor_parameters, self.ref_posture)
if pin.WITH_HPP_FCL and pin.WITH_HPP_FCL_BINDINGS:
# Add all collision pairs
self.robot.collision_model.addAllCollisionPairs()
# Remove collision pairs per SRDF
pin.removeCollisionPairs(self.robot.model, self.robot.collision_model, self.srdf_path, False)
# Recreate collision data since the collision pairs changed
self.robot.collision_data = self.robot.collision_model.createData()
else:
self.srdf_path = None
self.robot.q0 = pin.neutral(self.robot.model)
if self.free_flyer:
self.addFreeFlyerJointLimits()
def computeWrench(cs, Robot, dt):
rp = RosPack()
urdf = rp.get_path(
Robot.packageName
) + '/urdf/' + Robot.urdfName + Robot.urdfSuffix + '.urdf'
#srdf = "package://" + package + '/srdf/' + cfg.Robot.urdfName+cfg.Robot.srdfSuffix + '.srdf'
robot = RobotWrapper.BuildFromURDF(urdf, pin.StdVec_StdString(),
pin.JointModelFreeFlyer(), False)
model = robot.model
data = robot.data
q_t = cs.concatenateQtrajectories()
dq_t = cs.concatenateQtrajectories()
ddq_t = cs.concatenateQtrajectories()
t = q_t.min()
for phase in cs.contactPhases:
phase.wrench_t = None
t = phase.timeInitial
while t <= phase.timeFinal:
pin.rnea(model, data, phase.q_t(t), phase.dq_t(t), phase.ddq_t(t))
pcom, vcom, acom = robot.com(phase.q_t(t), phase.dq_t(t),
phase.ddq_t(t))
# FIXME : why do I need to call com to have the correct values in data ??
phi0 = data.oMi[1].act(pin.Force(data.tau[:6]))
if phase.wrench_t is None:
phase.wrench_t = piecewise(
polynomial(phi0.vector.reshape(-1, 1), t, t))
else:
phase.wrench_t.append(phi0.vector, t)
t += dt
if phase.timeFinal - dt / 2. <= t <= phase.timeFinal + dt / 2.:
t = phase.timeFinal
def loadUR(robot=5, limited=False, gripper=False):
assert (not (gripper and (robot == 10 or limited)))
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])
def buildRobotWrapper(cls):
# Rebuild the robot wrapper instead of using the existing model to
# also load the visuals.
robot = RobotWrapper.BuildFromURDF(cls.urdf_path, cls.meshes_path)
robot.model.rotorInertia[1:] = cls.motor_inertia
robot.model.rotorGearRatio[1:] = cls.motor_gear_ration
return robot
def loadHector():
URDF_FILENAME = "quadrotor_base.urdf"
URDF_SUBPATH = "/hector_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath],
pinocchio.JointModelFreeFlyer())
return robot
def initialize(self, planner_setting):
package_dirs = [
os.path.dirname(
os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
]
urdf = str(
os.path.dirname(
os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) +
'/urdf/quadruped.urdf')
self.robot = RobotWrapper.BuildFromURDF(
urdf,
package_dirs=package_dirs,
root_joint=pin.JointModelFreeFlyer())
self.robot.data = self.robot.model.createData()
self.initDisplay(loadModel=True)
self.robot.viewer.gui.addFloor('world/floor')
self.z_floor = planner_setting.get(PlannerDoubleParam_FloorHeight)
self.robot.viewer.gui.applyConfiguration(
'world/floor', [0.0, 0.0, self.z_floor, 0.0, 0.0, 0.0, 1.0])
self.robot.viewer.gui.refresh()
self.robot.q = self.robot.model.referenceConfigurations.copy()
self.robot.dq = zero(self.robot.model.nv)
self.robot.q[7:] = np.transpose(
np.matrix(
planner_setting.get(
PlannerVectorParam_KinematicDefaultJointPositions)))
pin.forwardKinematics(self.robot.model, self.robot.data, self.robot.q)
self.robot.display(self.robot.q)
def loadANYmal(withArm=None):
if withArm is None:
URDF_FILENAME = "anymal.urdf"
SRDF_FILENAME = "anymal.srdf"
REF_POSTURE = "standing"
elif withArm == "kinova":
URDF_FILENAME = "anymal-kinova.urdf"
SRDF_FILENAME = "anymal-kinova.srdf"
REF_POSTURE = "standing_with_arm_up"
URDF_SUBPATH = "/anymal_b_simple_description/robots/" + URDF_FILENAME
SRDF_SUBPATH = "/anymal_b_simple_description/srdf/" + SRDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH,
[getVisualPath(modelPath)],
pinocchio.JointModelFreeFlyer())
# Load SRDF file
robot.q0 = readParamsFromSrdf(robot.model,
modelPath + SRDF_SUBPATH,
False,
False,
referencePose=REF_POSTURE)
# Add the free-flyer joint limits
addFreeFlyerJointLimits(robot.model)
return robot
def main(robotpkg_prefix, robot):
pkg = Path(robotpkg_prefix) / 'share/example-robot-data'
urdf = ROBOTS[robot]
robot = RobotWrapper.BuildFromURDF(str(pkg / urdf), [str(pkg)],
pin.JointModelFreeFlyer())
robot.initDisplay()
robot.loadDisplayModel("pinocchio")
robot.display(robot.q0)
def loadPanda():
URDF_FILENAME = "panda.urdf"
URDF_SUBPATH = "/panda_description/urdf/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH,
[getVisualPath(modelPath)])
return robot
def on_connect(self, sid, environ):
self.user_sids.add(sid)
robot = RobotWrapper.BuildFromURDF(self.filename)
self.robot_map = self.robot_map.set(sid, robot)
self.q_map = self.q_map.set(sid, np.zeros(6))
F = cuda.device_array_like(np.zeros(tsdf_vol_params.shape))
W = cuda.device_array_like(np.zeros(tsdf_vol_params.shape))
self.tsdf_map = self.tsdf_map.set(sid, (F, W))
self.sync_vars_map = self.sync_vars_map.set(sid, dict())
def loadModels(robotname, robot_urdf_file):
"""This function create a robot model and its data by inputting a URDF file that describes the robot.
Input: robotname: directory containing model of robot
robot_urdf_file: URDF file of robot
Output: robot: robot model and data created by Pinocchio"""
pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))),
"models")
model_path = join(pinocchio_model_dir, "others/robots")
mesh_dir = model_path
urdf_filename = robot_urdf_file
urdf_dir = robotname + "/urdf"
urdf_model_path = join(join(model_path, urdf_dir), urdf_filename)
if not isFext:
robot = RobotWrapper.BuildFromURDF(urdf_model_path, mesh_dir)
else:
robot = RobotWrapper.BuildFromURDF(urdf_model_path, mesh_dir,
pin.JointModelFreeFlyer())
return robot
def __init__(self):
current_dir = os.path.dirname(__file__)
robot_dir = os.path.join(current_dir, "assets/")
urdf_path = os.path.join(current_dir, "assets/poppy_ergo_jr.urdf")
self.wrapper = RobotWrapper.BuildFromURDF(urdf_path,
package_dirs=[robot_dir])
self.dummy_data = self.wrapper.data.copy()
self._set_collision_pairs(self.wrapper)
# do not count the universe as a joint
self.joints_shape = (self.wrapper.model.njoints - 1, 3)
def __init__(self):
filename = str(os.path.dirname(os.path.abspath(__file__)))
pkg = filename + '/../../Model'
urdf = pkg + '/jet_description/urdf/dyros_jet_robot.urdf'
self.robot = RobotWrapper.BuildFromURDF(urdf, [
pkg,
])
self.srdf = pkg + '/srdf/dyros_jet_robot.srdf'
self.model = self.robot.model
self.data = self.robot.data
def __init__(self):
self.PKG = os.path.dirname(os.path.abspath(__file__)) + '/../../../resources/urdfs/anymal/'
self.URDF = self.PKG + 'urdf/anymal_boxy.urdf'
if self.PKG is None:
self.robot = RobotWrapper.BuildFromURDF(self.URDF)
else:
self.robot = RobotWrapper.BuildFromURDF(self.URDF, [self.PKG])
self.robot = RobotWrapper.BuildFromURDF(self.URDF, [self.PKG])
self.model = self.robot.model
self.data = self.robot.data
self.LF_foot_jac = None
self.LH_foot_jac = None
self.RF_foot_jac = None
self.RH_foot_jac = None
self.urdf_foot_name_lf = 'LF_FOOT'
self.urdf_foot_name_lh = 'LH_FOOT'
self.urdf_foot_name_rf = 'RF_FOOT'
self.urdf_foot_name_rh = 'RH_FOOT'
self.ik_success = False
def loadUR(robot=5, limited=False, gripper=False):
assert (not (gripper and (robot == 10 or limited)))
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)
model = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, ['/opt/openrobots/share/'])
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 __init__(self):
self.PKG = os.path.dirname(
os.path.abspath(__file__)) + '/../../resources/urdfs/hyqreal/'
self.URDF = self.PKG + 'urdf/hyqreal.urdf'
if self.PKG is None:
self.robot = RobotWrapper.BuildFromURDF(self.URDF)
else:
self.robot = RobotWrapper.BuildFromURDF(self.URDF, [self.PKG])
self.model = self.robot.model
self.data = self.robot.data
self.LF_foot_jac = None
self.LH_foot_jac = None
self.RF_foot_jac = None
self.RH_foot_jac = None
self.urdf_foot_name_lf = 'lf_foot'
self.urdf_foot_name_lh = 'lh_foot'
self.urdf_foot_name_rf = 'rf_foot'
self.urdf_foot_name_rh = 'rh_foot'
def loadHyQ(modelPath=None):
URDF_FILENAME = "hyq_no_sensors.urdf"
URDF_SUBPATH = "/hyq_description/robots/" + URDF_FILENAME
if modelPath is None:
modelPath = getModelPath(URDF_SUBPATH)
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
# TODO define default position inside srdf
robot.q0.flat[7:] = [-0.2, 0.75, -1.5, -0.2, -0.75, 1.5, -0.2, 0.75, -1.5, -0.2, -0.75, 1.5]
robot.q0[2] = 0.57750958
robot.model.referenceConfigurations["half_sitting"] = robot.q0
return robot
def loadTiagoNoHand():
URDF_FILENAME = "tiago_no_hand.urdf"
# SRDF_FILENAME = "tiago.srdf"
# SRDF_SUBPATH = "/tiago_description/srdf/" + SRDF_FILENAME
URDF_SUBPATH = "/tiago_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
# Load SRDF file
# readParamsFromSrdf(robot, modelPath+SRDF_SUBPATH, False)
return robot
def loadKinova():
URDF_FILENAME = "kinova.urdf"
SRDF_FILENAME = "kinova.srdf"
SRDF_SUBPATH = "/kinova_description/srdf/" + SRDF_FILENAME
URDF_SUBPATH = "/kinova_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
# Load SRDF file
readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False, "arm_up")
return robot
def loadTiago():
URDF_FILENAME = "tiago.urdf"
# SRDF_FILENAME = "tiago.srdf"
# SRDF_SUBPATH = "/tiago_description/srdf/" + SRDF_FILENAME
URDF_SUBPATH = "/tiago_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH,
[getVisualPath(modelPath)])
# Load SRDF file
# robot.q0 = readParamsFromSrdf(robot.model, modelPath+SRDF_SUBPATH, False)
return robot
def loadTalosArm():
URDF_FILENAME = "talos_left_arm.urdf"
URDF_SUBPATH = "/talos_data/robots/" + URDF_FILENAME
SRDF_FILENAME = "talos.srdf"
SRDF_SUBPATH = "/talos_data/srdf/" + SRDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
# Load SRDF file
readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
return robot
def loadHyQ():
URDF_FILENAME = "hyq_no_sensors.urdf"
SRDF_FILENAME = "hyq.srdf"
SRDF_SUBPATH = "/hyq_description/srdf/" + SRDF_FILENAME
URDF_SUBPATH = "/hyq_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
# Load SRDF file
readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
# Add the free-flyer joint limits
addFreeFlyerJointLimits(robot)
return robot
def loadTalos():
URDF_FILENAME = "talos_reduced.urdf"
SRDF_FILENAME = "talos.srdf"
SRDF_SUBPATH = "/talos_data/srdf/" + SRDF_FILENAME
URDF_SUBPATH = "/talos_data/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
# Load SRDF file
readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
assert ((robot.model.armature[:6] == 0.).all())
# Add the free-flyer joint limits
addFreeFlyerJointLimits(robot)
return robot
def loadTalosArm(modelPath='/opt/openrobots/share', freeFloating=False):
URDF_FILENAME = "talos_left_arm.urdf"
URDF_SUBPATH = "/talos_data/robots/" + URDF_FILENAME
robot = RobotWrapper.BuildFromURDF(
modelPath + URDF_SUBPATH, [modelPath],
pinocchio.JointModelFreeFlyer() if freeFloating else None)
if freeFloating:
u = robot.model.upperPositionLimit
u[:7] = 1
robot.model.upperPositionLimit = u
l = robot.model.lowerPositionLimit
l[:7] = -1
robot.model.lowerPositionLimit = l
return robot
def loadSolo(solo=True):
if solo:
URDF_FILENAME = "solo.urdf"
else:
URDF_FILENAME = "solo12.urdf"
SRDF_FILENAME = "solo.srdf"
SRDF_SUBPATH = "/solo_description/srdf/" + SRDF_FILENAME
URDF_SUBPATH = "/solo_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
# Load SRDF file
readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False, "standing")
# Add the free-flyer joint limits
addFreeFlyerJointLimits(robot)
return robot
def export(cs_com, cs_wb):
rp = RosPack()
urdf = rp.get_path(cfg.Robot.packageName) + '/urdf/' + cfg.Robot.urdfName + cfg.Robot.urdfSuffix + '.urdf'
if cfg.WB_VERBOSE:
print("load robot : ", urdf)
#srdf = "package://" + package + '/srdf/' + cfg.Robot.urdfName+cfg.Robot.srdfSuffix + '.srdf'
robot = RobotWrapper.BuildFromURDF(urdf, pin.StdVec_StdString(), pin.JointModelFreeFlyer(), False)
if cfg.WB_VERBOSE:
print("robot loaded in export OpenHRP")
results = computeWaistData(robot, res)
path = cfg.EXPORT_PATH + "/openHRP/" + cfg.DEMO_NAME
if not os.path.exists(path):
os.makedirs(path)
q_openhrp_l = generateOpenHRPMotion(results, path, cfg.DEMO_NAME, useRefZMP=cfg.openHRP_useZMPref)
writeKinematicsData(results, path, cfg.DEMO_NAME)
def loadICub(reduced=True):
if reduced:
URDF_FILENAME = "icub_reduced.urdf"
else:
URDF_FILENAME = "icub.urdf"
SRDF_FILENAME = "icub.srdf"
SRDF_SUBPATH = "/icub_description/srdf/" + SRDF_FILENAME
URDF_SUBPATH = "/icub_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
# Load SRDF file
readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
# Add the free-flyer joint limits
addFreeFlyerJointLimits(robot)
return robot
