def initDisplay(self, loadModel): RobotWrapper.initDisplay(self, loadModel=loadModel) if loadModel and not hasattr(self, 'display'): RobotWrapper.initDisplay(self, loadModel=False) try: # self.viewer.gui.deleteNode('world/mobilebasis', True) self.viewer.gui.addBox('world/mobilebasis', .25, .25, .25, [.8, .2, .2, 1]) self.viewer.gui.addCylinder('world/mobilewheel1', .05, .45, [.1, .0, .0, 1]) self.viewer.gui.addCylinder('world/mobilewheel2', .05, .45, [.1, .0, .0, 1]) self.viewer.gui.setStaticTransform('world/mobilebasis', [.0, .0, .35, 1.0, .0, .0, .0]) self.viewer.gui.setStaticTransform( 'world/mobilewheel1', [+0.15, .0, .05, .7, .7, .0, .0]) self.viewer.gui.setStaticTransform( 'world/mobilewheel2', [-0.15, .0, .05, .7, .7, .0, .0]) self.viewer.gui.addXYZaxis('world/framebasis', [1., 0., 0., 1.], .03, .1) self.viewer.gui.addXYZaxis('world/frametool', [1., 0., 0., 1.], .03, .1) except: print("Error when adding 3d objects in the viewer ... ignore")
def __init__(self,*args): filename = args[0] if len(args) >= 2: mesh_dir = args[1] RobotWrapper.__init__(self,filename,mesh_dir,se3.JointModelFreeFlyer()) else: RobotWrapper.__init__(self,filename,se3.JointModelFreeFlyer()) # RobotWrapper.__init__(self,filename, root_joint='ff') self.q0 = np.matrix( [ 0.0, 0.0, 0.648702, 0.0, 0.0 , 0.0, 1.0, # Free flyer 0-6 0.0, 0.0, 0.0, 0.0, # CHEST HEAD 7-10 0.261799388, 0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.174532925, # LARM 11-17 0.261799388, -0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.174532925, # RARM 18-24 0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # LLEG 25-30 0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # RLEG 31-36 ] ).T self.ff = range(7) self.chest = range(7,9) self.head = range(9,11) self.l_arm = range(11,18) self.r_arm = range(18,25) self.l_leg = range(25,31) self.r_leg = range(31,37) self.opCorrespondances = { "lh": "LARM_JOINT5", "rh": "RARM_JOINT5", "lf": "LLEG_JOINT5", "rf": "RLEG_JOINT5", } for op,name in self.opCorrespondances.items(): idx = self.__dict__[op] = self.index(name)
def __init__(self, filename=MODELPATH[0] + 'baxter_description/urdf/baxter.urdf'): RobotWrapper.initFromURDF(self, filename, MODELPATH) self.Q_INIT = np.zeros(self.nq - NQ_OFFSET) self.q_def = np.zeros(self.nq) self.v_def = np.zeros(self.nv) # self.q0 = np.matrix( [ # 0.0, 0.0, 0.648702, 0.0, 0.0 , 0.0, 1.0, # Free flyer 0-6 # 0.0, 0.0, 0.0, 0.0, # CHEST HEAD 7-10 # 0.261799388, 0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.174532925, # LARM 11-17 # 0.261799388, -0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.174532925, # RARM 18-24 # 0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # LLEG 25-30 # 0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # RLEG 31-36 # ] ).T self.ff = list(range(7)) self.head = list(range(7, 7)) self.l_arm = list(range(8, 14)) self.r_arm = list(range(15, 21)) self.opCorrespondances = { "lh": "left_w2", "rh": "right_w2", } for op, name in list(self.opCorrespondances.items()): idx = self.__dict__[op] = self.index(name)
def com(self, q=None, v=None, a=None, update=True): if (update == False or q is None): return PinocchioRobotWrapper.com(self, q) if a is None: if v is None: return PinocchioRobotWrapper.com(self, q) return PinocchioRobotWrapper.com(self, q, v) return PinocchioRobotWrapper.com(self, q, v, a)
def loadRobot(M0, name): ''' This function load a UR5 robot n a new model, move the basis to placement <M0> and add the corresponding visuals in gepetto viewer with name prefix given by string <name>. It returns the robot wrapper (model,data). ''' robot = RobotWrapper(urdf, [PKG]) robot.model.jointPlacements[1] = M0 * robot.model.jointPlacements[1] robot.visual_model.geometryObjects[0].placement = M0 * robot.visual_model.geometryObjects[0].placement robot.visual_data.oMg[0] = M0 * robot.visual_data.oMg[0] robot.initDisplay(loadModel=True, viewerRootNodeName="world/" + name) return robot
def com(self, q=None, v=None, a=None, update_kinematics=True): if(update_kinematics==False or q is None): return PinocchioRobotWrapper.com(self, q); if q is not None: q_mat = np.asmatrix(q).reshape((self.model.nq,1)) if v is not None: v_mat = np.asmatrix(v).reshape((self.model.nv,1)) if a is None: if v is None: return PinocchioRobotWrapper.com(self, q_mat) return PinocchioRobotWrapper.com(self, q_mat, v_mat) return PinocchioRobotWrapper.com(self, q_mat, v_mat, np.asmatrix(a).reshape((self.model.nv,1)))
def display(self, q): RobotWrapper.display(self, q) M1 = self.data.oMi[1] self.viewer.gui.applyConfiguration('world/mobilebasis', se3ToXYZQUAT(M1)) self.viewer.gui.applyConfiguration('world/mobilewheel1', se3ToXYZQUAT(M1)) self.viewer.gui.applyConfiguration('world/mobilewheel2', se3ToXYZQUAT(M1)) self.viewer.gui.refresh() pinocchio.updateFramePlacements(self.model, self.data) self.viewer.gui.applyConfiguration('world/framebasis', se3ToXYZQUAT(self.data.oMf[-2])) self.viewer.gui.applyConfiguration('world/frametool', se3ToXYZQUAT(self.data.oMf[-1])) self.viewer.gui.refresh()
def display(self, q): RobotWrapper.display(self, q) M1 = self.data.oMi[1] self.viewer.gui.applyConfiguration('world/mobilebasis', pin.se3ToXYZQUATtuple(M1)) self.viewer.gui.applyConfiguration('world/mobilewheel1', pin.se3ToXYZQUATtuple(M1)) self.viewer.gui.applyConfiguration('world/mobilewheel2', pin.se3ToXYZQUATtuple(M1)) self.viewer.gui.refresh() pin.framesKinematics(self.model, self.data) self.viewer.gui.applyConfiguration('world/framebasis', pin.se3ToXYZQUATtuple(self.data.oMf[-2])) self.viewer.gui.applyConfiguration('world/frametool', pin.se3ToXYZQUATtuple(self.data.oMf[-1])) self.viewer.gui.refresh()
def display(self, q): RobotWrapper.display(self, q) M1 = self.data.oMi[1] self.viewer.gui.applyConfiguration('world/mobilebasis', M2gv(M1)) self.viewer.gui.applyConfiguration('world/mobilewheel1', M2gv(M1)) self.viewer.gui.applyConfiguration('world/mobilewheel2', M2gv(M1)) self.viewer.gui.refresh() se3.framesKinematics(self.model, self.data) self.viewer.gui.applyConfiguration('world/framebasis', M2gv(self.data.oMf[-2])) self.viewer.gui.applyConfiguration('world/frametool', M2gv(self.data.oMf[-1])) self.viewer.gui.refresh()
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 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 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 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 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 __init__(self,log="/tmp/log_pickup.txt"): self.wrapper = RobotWrapper("/local/mgeisert/models/ur5_quad.urdf") self.client=Client() self.client.gui.createWindow("w") self.client.gui.createScene("world") self.client.gui.addSceneToWindow("world",0L) self.client.gui.addURDF("world/ur5","/local/mgeisert/models/ur5_quad.urdf","/local/mgeisert/models/universal_robot/") self.client.gui.addSphere("world/sphere",0.07,[0.7,0.,0.,1.]) self.client.gui.createGroup("world/gripper") self.client.gui.addLandmark("world/gripper",0.1) self.wrapper.initDisplay("world/ur5") self.list = [] file = open(log, "r") configs = file.readlines() list1 = [] tmp3 = 0. for i in range(len(configs)): tmp = np.fromstring(configs[i][2:], dtype=float, sep="\t") tmp2 = [] if tmp[0]-tmp3>0.: tmp2.extend([tmp[0]-tmp3]) tmp2.extend(tmp[1:4]) x = Quaternion().fromRPY(tmp[4],tmp[5],tmp[6]) tmp2.extend(x.array[1:4]) tmp2.extend([x.array[0]]) tmp2.extend(tmp[7:13]) tmp2.append(tmp[0]) tmp3 = tmp[0] list1.append(tmp2) self.list.append(list1)
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 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 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 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 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 __init__(self, urdf, pkgs): RobotWrapper.__init__(self, urdf, pkgs, pinocchio.JointModelPlanar()) M0 = pinocchio.SE3(eye(3), np.matrix([0., 0., .6]).T) self.model.jointPlacements[2] = M0 * self.model.jointPlacements[2] self.visual_model.geometryObjects[0].placement = M0 * self.visual_model.geometryObjects[0].placement self.visual_data.oMg[0] = M0 * self.visual_data.oMg[0] # Placement of the "mobile" frame wrt basis center. basisMop = pinocchio.SE3(eye(3), np.matrix([.3, .0, .1]).T) self.model.addFrame(pinocchio.Frame('mobile', 1, 1, basisMop, pinocchio.FrameType.OP_FRAME)) # Placement of the tool frame wrt end effector frame (located at the center of the wrist) effMop = pinocchio.SE3(eye(3), np.matrix([.0, .08, .095]).T) self.model.addFrame(pinocchio.Frame('tool', 6, 6, effMop, pinocchio.FrameType.OP_FRAME)) # Create data again after setting frames self.data = self.model.createData()
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 __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 initDisplay(self, loadModel): RobotWrapper.initDisplay(self, loadModel=loadModel) if loadModel and not hasattr(self, 'display'): RobotWrapper.initDisplay(self, loadModel=False) try: # self.viewer.gui.deleteNode('world/mobilebasis', True) self.viewer.gui.addBox('world/mobilebasis', .25, .25, .25, [.8, .2, .2, 1]) self.viewer.gui.addCylinder('world/mobilewheel1', .05, .45, [.1, .0, .0, 1]) self.viewer.gui.addCylinder('world/mobilewheel2', .05, .45, [.1, .0, .0, 1]) self.viewer.gui.setStaticTransform('world/mobilebasis', [.0, .0, .35, 1.0, .0, .0, .0]) self.viewer.gui.setStaticTransform('world/mobilewheel1', [+0.15, .0, .05, .7, .7, .0, .0]) self.viewer.gui.setStaticTransform('world/mobilewheel2', [-0.15, .0, .05, .7, .7, .0, .0]) self.viewer.gui.addXYZaxis('world/framebasis', [1., 0., 0., 1.], .03, .1) self.viewer.gui.addXYZaxis('world/frametool', [1., 0., 0., 1.], .03, .1) except: print("Error when adding 3d objects in the viewer ... ignore")
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 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): RobotWrapper.__init__(self,filename) a=-0.001616164386097708 self.q0= np.matrix( [ 0, 0, 0.817, 0, 0, 0, 1, # Free flyer 0, 0.05, -0.40+a, 0.75, -0.35-a, -0.05, # left leg 0, -0.05, -0.40+a, 0.75, -0.35-a, 0.05, # right leg 0,0, # chest 0, 0.15, 0, 0.3, 0, 0.5, 0, # left arm 0, 0.15, 0, 0.3, 0, 0.5, 0, # right arm 0.0, 0.0 # head ] ).T self.opCorrespondances = { "lh": "arm_left_7_joint", "rh": "arm_right_7_joint", "rf": "leg_right_6_joint", "lf": "leg_left_6_joint", } for op,name in self.opCorrespondances.items(): idx = self.__dict__[op] = self.index(name)
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 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 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 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
'chest' : 'TrunkYaw'} _initialConfig = (0, 0, .840252, 0, 0, 0, # FF 0, 0, -0.3490658, 0.6981317, -0.3490658, 0, # LLEG 0, 0, -0.3490658, 0.6981317, -0.3490658, 0, # RLEG 0, # TRUNK 1.5, 0.6, -0.5, -1.05, -0.4, -0.3, -0.2, # LARM 0, 0, 0, 0, # HEAD 1.5, -0.6, 0.5, 1.05, -0.4, -0.3, -0.2, # RARM ) #----------------PINOCCHIO----------------------------# import pinocchio as se3 from pinocchio.robot_wrapper import RobotWrapper pinocchioRobot = RobotWrapper(_urdfPath, _urdfDir, se3.JointModelFreeFlyer()) pinocchioRobot.initDisplay(loadModel=True) #----------------ROBOT - DEVICE AND DYNAMICS----------# from dynamic_graph.sot.dynamics.humanoid_robot import HumanoidRobot robot = HumanoidRobot(_robotName, pinocchioRobot.model, pinocchioRobot.data, _initialConfig, _OperationalPointsMap) #-----------------------------------------------------# #----------------SOT (SOLVER)-------------------------# from dynamic_graph.sot.application.velocity.precomputed_tasks import initialize solver = initialize( robot ) #-----------------------------------------------------#
class PlayFromFile: def __init__(self,log="/tmp/log_pickup.txt"): self.wrapper = RobotWrapper("/local/mgeisert/models/ur5_quad.urdf") self.client=Client() self.client.gui.createWindow("w") self.client.gui.createScene("world") self.client.gui.addSceneToWindow("world",0L) self.client.gui.addURDF("world/ur5","/local/mgeisert/models/ur5_quad.urdf","/local/mgeisert/models/universal_robot/") self.client.gui.addSphere("world/sphere",0.07,[0.7,0.,0.,1.]) self.client.gui.createGroup("world/gripper") self.client.gui.addLandmark("world/gripper",0.1) self.wrapper.initDisplay("world/ur5") self.list = [] file = open(log, "r") configs = file.readlines() list1 = [] tmp3 = 0. for i in range(len(configs)): tmp = np.fromstring(configs[i][2:], dtype=float, sep="\t") tmp2 = [] if tmp[0]-tmp3>0.: tmp2.extend([tmp[0]-tmp3]) tmp2.extend(tmp[1:4]) x = Quaternion().fromRPY(tmp[4],tmp[5],tmp[6]) tmp2.extend(x.array[1:4]) tmp2.extend([x.array[0]]) tmp2.extend(tmp[7:13]) tmp2.append(tmp[0]) tmp3 = tmp[0] list1.append(tmp2) self.list.append(list1) def play(self, i=0, speed=1): for config in self.list[i]: q = np.matrix([[config[1]],[config[2]],[config[3]],[config[4]],[config[5]],[config[6]],[config[7]],[config[8]],[config[9]],[config[10]],[config[11]],[config[12]],[config[13]]]) self.wrapper.display(q) x = np.matrix([[0],[0.15],[0]]) x = self.wrapper.data.oMi[7]*x x = x.transpose() x = x.tolist()[0] print x quat = Quaternion(self.wrapper.data.oMi[7].rotation) x.extend(quat.array[0:4]) self.client.gui.applyConfiguration("world/gripper",x) if config[14]<3: self.client.gui.applyConfiguration("world/sphere",[2,-1,-1,1,0,0,0]) elif config[14]<7: self.client.gui.applyConfiguration("world/sphere",x) self.client.gui.refresh() time.sleep(config[0]*speed) def display(self,k, i=0): config = self.list[i][k] q = np.matrix([[config[1]],[config[2]],[config[3]],[config[4]],[config[5]],[config[6]],[config[7]],[config[8]],[config[9]],[config[10]],[config[11]],[config[12]],[config[13]]]) self.wrapper.display(q) def reload(self,file="/tmp/log_pickup.txt"): file = open(file, "r") configs = file.readlines() list1 = [] tmp3 = 0. for i in range(len(configs)): tmp = np.fromstring(configs[i][2:], dtype=float, sep="\t") tmp2 = [] if tmp[0]-tmp3>0.: tmp2.extend([tmp[0]-tmp3]) tmp2.extend(tmp[1:4]) x = Quaternion().fromRPY(tmp[4],tmp[5],tmp[6]) tmp2.extend(x.array[1:4]) tmp2.extend([x.array[0]]) tmp2.extend(tmp[7:13]) tmp2.append(tmp[0]) tmp3 = tmp[0] list1.append(tmp2) self.list.insert(0,list1)
0., 0. # Head ) #----------------------------------------------------------------------------- #---- ROBOT SPECIFICATIONS---------------------------------------------------- #----------------------------------------------------------------------------- #----------------------------------------------------------------------------- #---- DYN -------------------------------------------------------------------- #----------------------------------------------------------------------------- from pinocchio.robot_wrapper import RobotWrapper import pinocchio as pin from dynamic_graph.sot.dynamics_pinocchio import fromSotToPinocchio pinocchioRobot = RobotWrapper(urdfPath, urdfDir, pin.JointModelFreeFlyer()) pinocchioRobot.initDisplay(loadModel=True) pinocchioRobot.display(fromSotToPinocchio(initialConfig)) from dynamic_graph.sot.dynamics_pinocchio.humanoid_robot import HumanoidRobot robot = HumanoidRobot(robotName, pinocchioRobot.model, pinocchioRobot.data, initialConfig, OperationalPointsMap) # ------------------------------------------------------------------------------ # ---- Kinematic Stack of Tasks (SoT) ----------------------------------------- # ------------------------------------------------------------------------------ from dynamic_graph import plug from dynamic_graph.sot.core import SOT sot = SOT('sot')