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