コード例 #1
0
def add_snake(w,
              nbody,
              lengths=None,
              masses=None,
              gpos=None,
              gvel=None,
              is_fixed=True):
    """Add a snake-shape robot to the world.

    **Example:**

        >>> w = World()
        >>> add_snake(w, 3, lengths=[1.5, 1., 5.])

    """
    assert isinstance(w, World)
    if lengths is None:
        lengths = [.5] * nbody
    assert nbody == len(lengths)
    if masses is None:
        masses = [2.] * nbody
    assert nbody == len(masses)
    if gpos is None:
        gpos = [0.] * nbody
    assert nbody == len(gpos)
    if gvel is None:
        gvel = [0.] * nbody
    assert nbody == len(gvel)

    if is_fixed:
        frame = w.ground
    else:
        L = lengths[0] / 2.
        body = Body(mass=box([L, L, L], masses[0]))
        w.add_link(w.ground, FreeJoint(), body)
        frame = body

    for (length, mass, q, dq) in zip(lengths, masses, gpos, gvel):
        radius = length / 10.  #TODO use 5 instead of 10
        #angle = pi/2.
        angle = 0.  #TODO: remove, it is here for testing
        body = Body(
            mass=transport(cylinder(length, radius, mass),
                           dot(rotx(angle), transl(0., -length / 2., 0.))))
        joint = RzJoint(gpos=q, gvel=dq)
        w.add_link(frame, joint, body)
        frame = SubFrame(body, transl(0., length, 0.))
    w.register(frame)
    w.init()
コード例 #2
0
ファイル: simpleshapes.py プロジェクト: efim/arboris-python
def add_box(world, half_extents=(1., 1., 1.), mass=1., name='Box'):
    """Build a box robot.

    Example:

        >>> w = World()
        >>> add_box(w)
        >>> w.update_dynamic()

    """
    assert isinstance(world, World)
    box = Body(name=name, mass=massmatrix.box(half_extents, mass))
    world.add_link(world.ground, FreeJoint(), box)
    world.register(Box(box, half_extents))
    world.init()
コード例 #3
0
def add_table(world, hl=(.05, .05, .05), init_pos= eye(4), mass=1., name='wall'):
    """ Add a simple box to the world
    """
    from arboris.massmatrix import box
    from arboris.core import Body
    from arboris.joints import TzJoint
    ## Create the body
    M = box(hl, mass)
    bbox = Body(mass=M, name=name)
    world.register(Box(bbox, hl, name))

    ## Link the body to the world
    world.add_link(SubFrame(world.ground, init_pos), TzJoint(name=name+'_root'), bbox)

    world.init()
コード例 #4
0
def add_box(world, lengths=(1.,1.,1.), mass=1., name='Box'):
    """Build a box robot.

    Example:

        >>> w = World()
        >>> add_box(w)
        >>> w.update_dynamic()

    """
    assert isinstance(world, World)
    box = Body(
        name=name,
        mass=massmatrix.box(lengths, mass))
    world.add_link(world.ground, FreeJoint(), box)
    world.register(Box(box, lengths))
    world.init()
コード例 #5
0
ファイル: snake.py プロジェクト: sbarthelemy/arboris-python
def add_snake(w, nbody, lengths=None, masses=None, gpos=None, gvel=None,
        is_fixed=True):
    """Add a snake-shape robot to the world.

    **Example:**

        >>> w = World()
        >>> add_snake(w, 3, lengths=[1.5, 1., 5.])

    """
    assert isinstance(w, World)
    if lengths is None:
        lengths = [.5] * nbody
    assert nbody == len(lengths)
    if masses is None:
        masses = [2.] * nbody
    assert nbody == len(masses)
    if gpos is None:
        gpos = [0.] * nbody
    assert nbody == len(gpos)
    if gvel is None:
        gvel = [0.] * nbody
    assert nbody == len(gvel)

    if is_fixed:
        frame = w.ground
    else:
        L = lengths[0]/2.
        body = Body(mass=box([L, L, L], masses[0]))
        w.add_link(w.ground, FreeJoint(), body)
        frame = body

    for (length, mass, q, dq) in zip(lengths, masses, gpos, gvel):
        radius = length/10. #TODO use 5 instead of 10
        #angle = pi/2.
        angle = 0. #TODO: remove, it is here for testing
        body = Body(mass=transport(cylinder(length, radius, mass),
                                   dot(rotx(angle),
                                       transl(0., -length/2., 0.))))
        joint = RzJoint(gpos=q, gvel=dq)
        w.add_link(frame, joint, body)
        frame = SubFrame(body, transl(0., length, 0.))
    w.register(frame)
    w.init()
コード例 #6
0
def add_table(world,
              hl=(.05, .05, .05),
              init_pos=eye(4),
              mass=1.,
              name='wall'):
    """ Add a simple box to the world
    """
    from arboris.massmatrix import box
    from arboris.core import Body
    from arboris.joints import TzJoint
    ## Create the body
    M = box(hl, mass)
    bbox = Body(mass=M, name=name)
    world.register(Box(bbox, hl, name))

    ## Link the body to the world
    world.add_link(SubFrame(world.ground, init_pos),
                   TzJoint(name=name + '_root'), bbox)

    world.init()
コード例 #7
0
def add_robot(w, gpos, gvel, is_fixed=True):
    assert isinstance(w, World)
    
    
    if is_fixed:
        frame = w.ground
    else:
        L = lengths[0]/2.
        body = Body(mass=box([L, L, L], masses[0]))
        w.add_link(w.ground, FreeJoint(), body)
        frame = body
        
        
    for (length, mass, q, dq) in zip(lengths, masses, gpos, gvel):
        radius = length/10.
        M = transport(cylinder(length, radius, mass), 
                      transl(0., -length/2., 0.))
        body = Body(mass=M)
        joint = RzJoint(gpos=q, gvel=dq)
        w.add_link(frame, joint, body)
        frame = SubFrame(body, transl(0., length, 0.))
    w.register(frame)
    w.init()
コード例 #8
0
ファイル: test_weight.py プロジェクト: badbane/arboris-python
        plot(self.timeline, self.height,
             self.timeline, self.get_theoric())
        legend(('simulated', 'theoric'))

w = ObservableWorld()

if True:
    from arboris.homogeneousmatrix import transl
    H_bc = transl(1,1,1)
else:
    H_bc = eye(4)
lengths = (1.,1.,1.)
mass = 1.
body = Body(
        name='box_body',
        mass=massmatrix.transport(massmatrix.box(lengths, mass), H_bc))
subframe = SubFrame(body, H_bc, name="box_com")

if True:
    twist_c = array([0.,0.,0.,0.,0.,0.])
else:
    twist_c = array([1,1,1,0,0,0.])
twist_b = dot(homogeneousmatrix.adjoint(H_bc), twist_c)
freejoint = FreeJoint(gpos=homogeneousmatrix.inv(H_bc), gvel=twist_b)
w.add_link(w.ground, freejoint, body)
w.register(Box(subframe, lengths))


weightc = WeightController(w)       
w.register(weightc)
obs = TrajLog(w.getframes()['box_com'], w)
コード例 #9
0
def add(w, is_fixed=False, create_shapes=True, create_contacts=True):
    """
    construction of the icub robot for arboris-python:
    Kinematics data are from: http://eris.liralab.it/wiki/ICubForwardKinematics
    Inertia comes from the Icub.cpp used in the iCub_SIM program
    Some data are not well explained, or are badly defined
    """

    bodies_data = get_bodies_data()
    bodies_shapes_data = get_bodies_shapes_data()
    joints_data = get_joints_data()
    shapes_data = get_contact_data()

    ## bodies creation
    bodies = {}
    for name, data in bodies_data.items():
        bodies[name] = Body(name=name)
        mass = zeros((6, 6))
        for dims, m, H in data:  # get dims, mass and transformation from data
            sf = SubFrame(bodies[name], H)
            if len(dims) == 3:  # check the type of shape: len =3: box
                M = box(dims, m)
            elif len(dims) == 2:  #  len =2:cylinder,
                M = cylinder(dims[0], dims[1], m)
            elif len(dims) == 1:  #  len =1:sphere,
                M = sphere(dims[0], m)
            else:
                raise ValueError
            mass += transport(M, inv(H))  # add the mass of the shape to
        bodies[name].mass = mass  # the total mass

    ## check if iCub has its waist fixed on the structure (the ground)
    if is_fixed:
        bodies['waist'] = w.ground
    else:
        w.add_link(w.ground, FreeJoint(name='root'), bodies['waist'])

    ## joints creation
    for name, data in joints_data.items():
        parent, Hp_l, child = data
        w.add_link(SubFrame(bodies[parent], Hp_l), RzJoint(name=name),
                   bodies[child])

    if create_shapes is True:
        ## body shapes creations
        for name, data in bodies_shapes_data.items():
            for dims, H in data:  # get dims, mass and transformation from data
                sf = SubFrame(bodies[name], H)
                if len(dims) == 3:  # check the type of shape: len =3: box
                    sh = Box(sf, dims, name + ".body_shape")
                elif len(dims) == 2:  #  len =2:cylinder,
                    sh = Cylinder(sf, dims[0], dims[1], name + ".body_shape")
                elif len(dims) == 1:  #  len =1:sphere,
                    sh = Sphere(sf, dims[0], name + ".body_shape")
                else:
                    raise ValueError
                w.register(sh)

    if create_contacts is True:
        ## contact shapes creation
        for name, data in shapes_data.items():
            parent, dims, Hpf = data
            sf = SubFrame(bodies[parent], Hpf, name=name)
            if len(dims) == 3:  # check the type of shape: len =3: box
                sh = Box(sf, dims, name=name)
            elif len(dims) == 2:  #  len =2:cylinder,
                sh = Cylinder(sf, dims[0], dims[1], name=name)
            elif len(dims) == 1:  #  len =1:sphere,
                sh = Sphere(sf, dims[0], name=name)
            else:
                sh = Point(sf, name=name)
            w.register(sh)

    w.init()
コード例 #10
0
ファイル: test_weight.py プロジェクト: bssrdf/arboris-python
    def plot_height(self):
        plot(self.timeline, self.height, self.timeline, self.get_theoric())
        legend(('simulated', 'theoric'))


w = World()

if True:
    from arboris.homogeneousmatrix import transl
    H_bc = transl(1, 1, 1)
else:
    H_bc = eye(4)
half_extents = (.5, .5, .5)
mass = 1.
body = Body(name='box_body',
            mass=massmatrix.transport(massmatrix.box(half_extents, mass),
                                      H_bc))
subframe = SubFrame(body, H_bc, name="box_com")

if True:
    twist_c = array([0., 0., 0., 0., 0., 0.])
else:
    twist_c = array([1, 1, 1, 0, 0, 0.])
twist_b = dot(homogeneousmatrix.adjoint(H_bc), twist_c)
freejoint = FreeJoint(gpos=homogeneousmatrix.inv(H_bc), gvel=twist_b)
w.add_link(w.ground, freejoint, body)
w.register(Box(subframe, half_extents))

weightc = WeightController()
w.register(weightc)
obs = TrajLog(w.getframes()['box_com'], w)
コード例 #11
0
        plot(self.timeline, self.height,
             self.timeline, self.get_theoric())
        legend(('simulated', 'theoric'))

w = World()

if True:
    from arboris.homogeneousmatrix import transl
    H_bc = transl(1,1,1)
else:
    H_bc = eye(4)
half_extents = (.5,.5,.5)
mass = 1.
body = Body(
        name='box_body',
        mass=massmatrix.transport(massmatrix.box(half_extents, mass), H_bc))
subframe = SubFrame(body, H_bc, name="box_com")

if True:
    twist_c = array([0.,0.,0.,0.,0.,0.])
else:
    twist_c = array([1,1,1,0,0,0.])
twist_b = dot(homogeneousmatrix.adjoint(H_bc), twist_c)
freejoint = FreeJoint(gpos=homogeneousmatrix.inv(H_bc), gvel=twist_b)
w.add_link(w.ground, freejoint, body)
w.register(Box(subframe, half_extents))


weightc = WeightController()
w.register(weightc)
obs = TrajLog(w.getframes()['box_com'], w)
コード例 #12
0
lin_velocity = [.4,.5,.6]
twist = np.array(rot_velocity + lin_velocity) # rotation velocity first
print("twist", twist)



##### About mass matrix ########################################################

import arboris.massmatrix as Mm


# mass matrix expressed in principal inertia frame, length in "m", mass in "kg"
M_com = Mm.sphere(radius=.1, mass=10)
M_com = Mm.ellipsoid(radii=(.1, .2, .3), mass=10)
M_com = Mm.cylinder(length=.1, radius=.2, mass=10)  # revolution axis along z
M_com = Mm.box(half_extents=(.1, .2, .3), mass=10)



isMm = Mm.ismassmatrix(M_com)       # check validity of mass matrix
print("is mass matrix?", isMm)


H_com_a= Hg.transl(.4,.5,.6)        # translation from {a} (new frame) to {com}
M_a = Mm.transport(M_com, H_com_a)


H_a_com = Mm.principalframe(M_a)    # return principal inertia frame from {a}
print("H_com_a * H_a_com\n", np.dot(H_com_a, H_a_com))

コード例 #13
0
ファイル: icub.py プロジェクト: mitkof6/arboris-python
def add(w, is_fixed=False, create_shapes=True, create_contacts=True):
    """
    construction of the icub robot for arboris-python:
    Kinematics data are from: http://eris.liralab.it/wiki/ICubForwardKinematics
    Inertia comes from the Icub.cpp used in the iCub_SIM program
    Some data are not well explained, or are badly defined
    """

    bodies_data = get_bodies_data()
    bodies_shapes_data = get_bodies_shapes_data()
    joints_data = get_joints_data()
    shapes_data = get_contact_data()

    ## bodies creation
    bodies = {}
    for name, data in bodies_data.items():
        bodies[name] = Body(name=name)
        mass = zeros((6, 6))
        for dims, m, H in data: # get dims, mass and transformation from data
            sf = SubFrame(bodies[name], H)
            if len(dims) == 3:      # check the type of shape: len =3: box
                M  = box(dims, m)
            elif len(dims) == 2:                            #  len =2:cylinder,
                M  = cylinder(dims[0], dims[1], m)
            elif len(dims) == 1:                            #  len =1:sphere,
                M  = sphere(dims[0], m)
            else:
                raise ValueError
            mass += transport(M, inv(H))    # add the mass of the shape to
        bodies[name].mass = mass            # the total mass

    ## check if iCub has its waist fixed on the structure (the ground)
    if is_fixed:
        bodies['waist'] = w.ground
    else:
        w.add_link(w.ground, FreeJoint(name='root'), bodies['waist'])

    ## joints creation
    for name, data in joints_data.items():
        parent, Hp_l, child = data
        w.add_link(SubFrame(bodies[parent], Hp_l),
                   RzJoint(name=name),
                   bodies[child])


    if create_shapes is True:
        ## body shapes creations
        for name, data in bodies_shapes_data.items():
            for dims, H in data: # get dims, mass and transformation from data
                sf = SubFrame(bodies[name], H)
                if len(dims) == 3:      # check the type of shape: len =3: box
                    sh = Box(sf, dims, name+".body_shape")
                elif len(dims) == 2:                            #  len =2:cylinder,
                    sh = Cylinder(sf, dims[0], dims[1], name+".body_shape")
                elif len(dims) == 1:                            #  len =1:sphere,
                    sh = Sphere(sf, dims[0], name+".body_shape")
                else:
                    raise ValueError
                w.register(sh)

    if create_contacts is True:
        ## contact shapes creation
        for name, data in shapes_data.items():
            parent, dims, Hpf = data
            sf = SubFrame(bodies[parent], Hpf, name=name)
            if len(dims) == 3:      # check the type of shape: len =3: box
                sh = Box(sf, dims, name=name)
            elif len(dims) == 2:                            #  len =2:cylinder,
                sh = Cylinder(sf, dims[0], dims[1], name=name)
            elif len(dims) == 1:                            #  len =1:sphere,
                sh = Sphere(sf, dims[0], name=name)
            else:
                sh = Point(sf, name=name)
            w.register(sh)

    w.init()