Exemplo n.º 1
0
    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")
Exemplo n.º 2
0
    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)
Exemplo n.º 4
0
 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)
Exemplo n.º 5
0
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
Exemplo n.º 6
0
 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)))
Exemplo n.º 7
0
    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()
Exemplo n.º 8
0
    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()
Exemplo n.º 9
0
    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()
Exemplo n.º 10
0
    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()
Exemplo n.º 11
0
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
Exemplo n.º 12
0
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
Exemplo n.º 13
0
    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)
Exemplo n.º 14
0
 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
Exemplo n.º 15
0
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())
Exemplo n.º 16
0
 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)
Exemplo n.º 17
0
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])
Exemplo n.º 18
0
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
Exemplo n.º 19
0
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
Exemplo n.º 20
0
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
Exemplo n.º 21
0
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)
Exemplo n.º 22
0
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
Exemplo n.º 23
0
    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()
Exemplo n.º 24
0
 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())
Exemplo n.º 25
0
 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
Exemplo n.º 26
0
    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")
Exemplo n.º 27
0
 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
Exemplo n.º 28
0
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
Exemplo n.º 29
0
 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)
Exemplo n.º 30
0
    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)
Exemplo n.º 31
0
    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'
Exemplo n.º 32
0
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
Exemplo n.º 33
0
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
Exemplo n.º 34
0
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
Exemplo n.º 35
0
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
Exemplo n.º 36
0
                         '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 )
#-----------------------------------------------------#
Exemplo n.º 37
0
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')