Esempio n. 1
0
    def test_self_load(self):
        hint_list = [self.model_dir]

        model = pin.buildModelFromUrdf(self.model_path,
                                       pin.JointModelFreeFlyer())
        collision_model_ref = pin.buildGeomFromUrdf(model, self.model_path,
                                                    pin.GeometryType.COLLISION,
                                                    hint_list)

        collision_model_self = pin.GeometryModel()
        pin.buildGeomFromUrdf(model, self.model_path,
                              pin.GeometryType.COLLISION, collision_model_self,
                              hint_list)
        self.assertTrue(checkGeom(collision_model_ref, collision_model_self))

        collision_model_self = pin.GeometryModel()
        pin.buildGeomFromUrdf(model, self.model_path,
                              pin.GeometryType.COLLISION, collision_model_self,
                              self.model_dir)
        self.assertTrue(checkGeom(collision_model_ref, collision_model_self))

        hint_vec = pin.StdVec_StdString()
        hint_vec.append(self.model_dir)

        collision_model_self = pin.GeometryModel()
        pin.buildGeomFromUrdf(model, self.model_path,
                              pin.GeometryType.COLLISION, collision_model_self,
                              hint_vec)
        self.assertTrue(checkGeom(collision_model_ref, collision_model_self))
Esempio n. 2
0
def loadSoloLeg(solo8=True):
    if solo8:
        URDF_FILENAME = "solo.urdf"
        legMaxId = 4
    else:
        URDF_FILENAME = "solo12.urdf"
        legMaxId = 5
    SRDF_FILENAME = "solo.srdf"
    SRDF_SUBPATH = "/solo_description/srdf/" + SRDF_FILENAME
    URDF_SUBPATH = "/solo_description/robots/" + URDF_FILENAME
    modelPath = example_robot_data.getModelPath(URDF_SUBPATH)

    robot = example_robot_data.loadSolo(solo8)
    m1 = robot.model
    m2 = pinocchio.Model()
    for index, [j, M, name, parent, Y] in enumerate(
            zip(m1.joints, m1.jointPlacements, m1.names, m1.parents,
                m1.inertias)):
        if j.id < legMaxId:
            jointType = j.shortname()
            if jointType == 'JointModelFreeFlyer':
                # for the freeflyer we just take reduced mass and zero inertia
                jointType = 'JointModelPZ'
                Y.mass = Y.mass / 4
                Y.inertia = np.diag(1e-6 * np.ones(3))
                M = pinocchio.SE3.Identity()

            if index == 2:
                # start with the prismatic joint on the sliding guide
                M.translation = np.zeros(3)

            # 2D model, flatten y axis
            vector2d = M.translation
            vector2d[1] = 0.0
            M.translation = vector2d

            jid = m2.addJoint(parent, getattr(pinocchio, jointType)(), M, name)
            assert (jid == j.id)
            m2.appendBodyToJoint(jid, Y, pinocchio.SE3.Identity())

    for f in m1.frames:
        if f.parent < legMaxId:
            m2.addFrame(f)

    g2 = pinocchio.GeometryModel()
    for g in robot.visual_model.geometryObjects:
        if g.parentJoint < legMaxId:
            g2.addGeometryObject(g)

    robot.model = m2
    robot.data = m2.createData()
    robot.visual_model = g2
    robot.visual_data = pinocchio.GeometryData(g2)

    # Load SRDF file
    #q0 = example_robot_data.readParamsFromSrdf(robot.model, modelPath + SRDF_SUBPATH, False, False, 'standing')
    robot.q0 = np.zeros(m2.nq + 1)

    assert ((m2.rotorInertia[:m2.joints[1].nq] == 0.).all())
    return robot
    def __init__(self):
        super(TalosLegsLoader, self).__init__()
        legMaxId = 14
        m1 = self.robot.model
        m2 = pin.Model()
        for j, M, name, parent, Y in zip(m1.joints, m1.jointPlacements,
                                         m1.names, m1.parents, m1.inertias):
            if j.id < legMaxId:
                jid = m2.addJoint(parent,
                                  getattr(pin, j.shortname())(), M, name)
                upperPos = m2.upperPositionLimit
                lowerPos = m2.lowerPositionLimit
                effort = m2.effortLimit
                upperPos[m2.joints[jid].idx_q:m2.joints[jid].idx_q +
                         j.nq] = m1.upperPositionLimit[j.idx_q:j.idx_q + j.nq]
                lowerPos[m2.joints[jid].idx_q:m2.joints[jid].idx_q +
                         j.nq] = m1.lowerPositionLimit[j.idx_q:j.idx_q + j.nq]
                effort[m2.joints[jid].idx_v:m2.joints[jid].idx_v +
                       j.nv] = m1.effortLimit[j.idx_v:j.idx_v + j.nv]
                m2.upperPositionLimit = upperPos
                m2.lowerPositionLimit = lowerPos
                m2.effortLimit = effort
                assert jid == j.id
                m2.appendBodyToJoint(jid, Y, pin.SE3.Identity())

        upperPos = m2.upperPositionLimit
        upperPos[:7] = 1
        m2.upperPositionLimit = upperPos
        lowerPos = m2.lowerPositionLimit
        lowerPos[:7] = -1
        m2.lowerPositionLimit = lowerPos
        effort = m2.effortLimit
        effort[:6] = np.inf
        m2.effortLimit = effort

        # q2 = self.robot.q0[:19]
        for f in m1.frames:
            if f.parent < legMaxId:
                m2.addFrame(f)

        g2 = pin.GeometryModel()
        for g in self.robot.visual_model.geometryObjects:
            if g.parentJoint < 14:
                g2.addGeometryObject(g)

        self.robot.model = m2
        self.robot.data = m2.createData()
        self.robot.visual_model = g2
        # self.robot.q0=q2
        self.robot.visual_data = pin.GeometryData(g2)

        # Load SRDF file
        self.q0 = readParamsFromSrdf(self.robot.model, self.srdf_path,
                                     self.verbose, self.has_rotor_parameters,
                                     self.ref_posture)

        assert (m2.armature[:6] == 0.).all()
        # Add the free-flyer joint limits to the new model
        self.addFreeFlyerJointLimits()
Esempio n. 4
0
def loadTalosLegs():
    robot = 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)

    legMaxId = 14
    m1 = robot.model
    m2 = pinocchio.Model()
    for j, M, name, parent, Y in zip(m1.joints, m1.jointPlacements, m1.names,
                                     m1.parents, m1.inertias):
        if j.id < legMaxId:
            jid = m2.addJoint(parent,
                              getattr(pinocchio, j.shortname())(), M, name)
            up = m2.upperPositionLimit
            down = m2.lowerPositionLimit
            up[m2.joints[jid].idx_q:m2.joints[jid].idx_q +
               j.nq] = m1.upperPositionLimit[j.idx_q:j.idx_q + j.nq]
            down[m2.joints[jid].idx_q:m2.joints[jid].idx_q +
                 j.nq] = m1.lowerPositionLimit[j.idx_q:j.idx_q + j.nq]
            m2.upperPositionLimit = up
            m2.lowerPositionLimit = down
            assert (jid == j.id)
            m2.appendBodyToJoint(jid, Y, pinocchio.SE3.Identity())

    u = m2.upperPositionLimit
    u[:7] = 1
    m2.upperPositionLimit = u
    limit = m2.lowerPositionLimit
    limit[:7] = -1
    m2.lowerPositionLimit = limit

    # q2 = robot.q0[:19]
    for f in m1.frames:
        if f.parent < legMaxId:
            m2.addFrame(f)

    g2 = pinocchio.GeometryModel()
    for g in robot.visual_model.geometryObjects:
        if g.parentJoint < 14:
            g2.addGeometryObject(g)

    robot.model = m2
    robot.data = m2.createData()
    robot.visual_model = g2
    # robot.q0=q2
    robot.visual_data = pinocchio.GeometryData(g2)

    # Load SRDF file
    robot.q0 = np.array(np.resize(robot.q0, robot.model.nq)).T
    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)

    assert ((m2.armature[:6] == 0.).all())
    # Add the free-flyer joint limits
    addFreeFlyerJointLimits(robot)
    return robot
Esempio n. 5
0
def createPendulum(nbJoint, length=1.0, mass=1.0, viewer=None):
    '''
    Creates the Pinocchio kinematic <rmodel> and visuals <gmodel> models for 
    a N-pendulum.

    @param nbJoint: number of joints <N> of the N-pendulum.
    @param length: length of each arm of the pendulum.
    @param mass: mass of each arm of the pendulum.
    @param viewer: gepetto-viewer CORBA client. If not None, then creates the geometries
    in the viewer.
    '''
    rmodel = pio.Model()
    gmodel = pio.GeometryModel()

    color = [red, green, blue, transparency] = [1, 1, 0.78, 1.0]
    colorred = [1.0, 0.0, 0.0, 1.0]

    radius = 0.1 * length

    prefix = ''
    jointId = 0
    jointPlacement = pio.SE3.Identity()
    inertia = pio.Inertia(mass,
                          np.matrix([0.0, 0.0, length / 2]).T,
                          mass / 5 * np.diagflat([1e-2, length**2, 1e-2]))
    viewerPrefix = getViewerPrefix(viewer, ["world", "pinocchio", "visuals"])

    for i in range(nbJoint):
        istr = str(i)
        name = prefix + "joint" + istr
        jointName, bodyName = [name + "_joint", name + "_body"]
        jointId = rmodel.addJoint(jointId, pio.JointModelRY(), jointPlacement,
                                  jointName)
        rmodel.appendBodyToJoint(jointId, inertia, pio.SE3.Identity())

        if viewer is not None:
            viewer.addSphere(viewerPrefix + jointName, 1.5 * radius, colorred)
            viewer.addCapsule(viewerPrefix + bodyName, radius, .8 * length,
                              color)
        gmodel.addGeometryObject(
            Capsule(jointName, jointId, pio.SE3.Identity(), 1.5 * radius, 0.0))
        gmodel.addGeometryObject(
            Capsule(bodyName, jointId,
                    pio.SE3(eye(3),
                            np.matrix([0., 0., length / 2]).T), radius,
                    0.8 * length))
        jointPlacement = pio.SE3(eye(3), np.matrix([0.0, 0.0, length]).T)

    rmodel.addFrame(
        pio.Frame('tip', jointId, 0, jointPlacement, pio.FrameType.OP_FRAME))

    rmodel.upperPositionLimit = np.zeros(nbJoint) + 2 * np.pi
    rmodel.lowerPositionLimit = np.zeros(nbJoint) - 2 * np.pi
    rmodel.velocityLimit = np.zeros(nbJoint) + 5.0

    return rmodel, gmodel
Esempio n. 6
0
 def __init__(self, model=None, geom_model=None):
     if model is None:
         model = pin.Model()
     if geom_model is None:
         geom_model = pin.GeometryModel()
     self._model = model
     self._geom_model = geom_model
     self._data = None
     self._geom_data = None
     self._clip_bounds = (None, None)
Esempio n. 7
0
    def __init__(self):
        self.viewer = GepettoViewerServer()
        #self.visuals = []
        self.model = pio.Model()
        self.gmodel = pio.GeometryModel()

        self.createHand()
        self.addCollisionPairs()

        self.data = self.model.createData()
        self.gdata = pio.GeometryData(self.gmodel)
        self.gdata.collisionRequest.enable_contact=True
        
        self.q0 = zero(self.model.nq)
        self.q0[0] = np.pi
        self.q0[-2] = -np.pi/3
        self.q0[2:-4] = -np.pi/6
        self.v0 = zero(self.model.nv)
        self.collisionPairs = []
Esempio n. 8
0
def loadTalosLegs(modelPath='/opt/openrobots/share/example-robot-data'):
    from pinocchio import JointModelFreeFlyer, JointModelRX, JointModelRY, JointModelRZ
    robot = loadTalos(modelPath=modelPath)
    SRDF_FILENAME = "talos.srdf"
    SRDF_SUBPATH = "/talos_data/srdf/" + SRDF_FILENAME
    legMaxId = 14

    m1 = robot.model
    m2 = pinocchio.Model()
    for j, M, name, parent, Y in zip(m1.joints, m1.jointPlacements, m1.names,
                                     m1.parents, m1.inertias):
        if j.id < legMaxId:
            jid = m2.addJoint(parent, locals()[j.shortname()](), M, name)
            assert (jid == j.id)
            m2.appendBodyToJoint(jid, Y, pinocchio.SE3.Identity())
    m2.upperPositionLimit = np.matrix([1.] * 19).T
    m2.lowerPositionLimit = np.matrix([-1.] * 19).T
    #q2 = robot.q0[:19]
    for f in m1.frames:
        if f.parent < legMaxId:
            m2.addFrame(f)

    g2 = pinocchio.GeometryModel()
    for g in robot.visual_model.geometryObjects:
        if g.parentJoint < 14:
            g2.addGeometryObject(g)

    robot.model = m2
    robot.data = m2.createData()
    robot.visual_model = g2
    # robot.q0=q2
    robot.visual_data = pinocchio.GeometryData(g2)

    # Load SRDF file
    pinocchio.getNeutralConfiguration(m2, modelPath + SRDF_SUBPATH, False)
    pinocchio.loadRotorParameters(m2, modelPath + SRDF_SUBPATH, False)
    m2.armature = \
        np.multiply(m2.rotorInertia.flat, np.square(m2.rotorGearRatio.flat))
    assert ((m2.armature[:6] == 0.).all())
    robot.q0 = m2.neutralConfiguration.copy()
    return robot
Esempio n. 9
0
import sys
import numpy as np
import pinocchio as pin
try:
    import hppfcl
except ImportError:
    print("This example requires hppfcl")
    sys.exit(0)
from pinocchio.visualize import GepettoVisualizer

pin.switchToNumpyArray()

model = pin.Model()

geom_model = pin.GeometryModel()
geometries = [
    hppfcl.Capsule(0.1, 0.8),
    hppfcl.Sphere(0.5),
    hppfcl.Box(1, 1, 1),
    hppfcl.Cylinder(0.1, 1.0),
    hppfcl.Cone(0.5, 1.0),
]
for i, geom in enumerate(geometries):
    placement = pin.SE3(np.eye(3), np.array([i, 0, 0]))
    geom_obj = pin.GeometryObject("obj{}".format(i), 0, 0, geom, placement)
    color = np.random.uniform(0, 1, 4)
    color[3] = 1
    geom_obj.meshColor = color
    geom_model.addGeometryObject(geom_obj)

viz = GepettoVisualizer(
Esempio n. 10
0
def loadTalos(legs=False, arm=False):
    URDF_FILENAME = "talos_left_arm.urdf" if arm else "talos_reduced.urdf"
    SRDF_FILENAME = "talos.srdf"

    robot = robot_loader('talos_data', URDF_FILENAME, SRDF_FILENAME, free_flyer=not arm)

    assert (robot.model.armature[:6] == 0.).all()

    if legs:
        legMaxId = 14
        m1 = robot.model
        m2 = pin.Model()
        for j, M, name, parent, Y in zip(m1.joints, m1.jointPlacements, m1.names, m1.parents, m1.inertias):
            if j.id < legMaxId:
                jid = m2.addJoint(parent, getattr(pin, j.shortname())(), M, name)
                upperPos = m2.upperPositionLimit
                lowerPos = m2.lowerPositionLimit
                effort = m2.effortLimit
                upperPos[m2.joints[jid].idx_q:m2.joints[jid].idx_q + j.nq] = m1.upperPositionLimit[j.idx_q:j.idx_q +
                                                                                                   j.nq]
                lowerPos[m2.joints[jid].idx_q:m2.joints[jid].idx_q + j.nq] = m1.lowerPositionLimit[j.idx_q:j.idx_q +
                                                                                                   j.nq]
                effort[m2.joints[jid].idx_v:m2.joints[jid].idx_v + j.nv] = m1.effortLimit[j.idx_v:j.idx_v + j.nv]
                m2.upperPositionLimit = upperPos
                m2.lowerPositionLimit = lowerPos
                m2.effortLimit = effort
                assert jid == j.id
                m2.appendBodyToJoint(jid, Y, pin.SE3.Identity())

        upperPos = m2.upperPositionLimit
        upperPos[:7] = 1
        m2.upperPositionLimit = upperPos
        lowerPos = m2.lowerPositionLimit
        lowerPos[:7] = -1
        m2.lowerPositionLimit = lowerPos
        effort = m2.effortLimit
        effort[:6] = np.inf
        m2.effortLimit = effort

        # q2 = robot.q0[:19]
        for f in m1.frames:
            if f.parent < legMaxId:
                m2.addFrame(f)

        g2 = pin.GeometryModel()
        for g in robot.visual_model.geometryObjects:
            if g.parentJoint < 14:
                g2.addGeometryObject(g)

        robot.model = m2
        robot.data = m2.createData()
        robot.visual_model = g2
        # robot.q0=q2
        robot.visual_data = pin.GeometryData(g2)

        # Load SRDF file
        robot.q0 = robot.q0[:robot.model.nq]
        model_path = getModelPath(join('talos_data/robots', URDF_FILENAME))
        robot.q0 = readParamsFromSrdf(robot.model, join(model_path, 'talos_data/srdf', SRDF_FILENAME), False)

        assert (m2.armature[:6] == 0.).all()
        # Add the free-flyer joint limits to the new model
        addFreeFlyerJointLimits(robot.model)

    return robot