示例#1
0
文件: common.py 项目: salini/LQPctrl
def create_icub_and_init(chair=False, gravity=False):
    ## CREATE THE WORLD
    w = World()
    w._up[:] = [0, 0, 1]
    icub.add(w, create_shapes=False)
    w.register(Plane(w.ground, (0, 0, 1, 0), "floor"))

    if chair is True:
        w.register(Sphere(w.getbodies()['l_hip_2'], .0325, name='l_gluteal'))
        w.register(Sphere(w.getbodies()['r_hip_2'], .0325, name='r_gluteal'))
        w.register(
            Box(SubFrame(w.ground, transl(.2, 0, 0.26)), (.075, .1, .02),
                name='chair'))
        w.register(
            Box(SubFrame(w.ground, transl(.255, 0, 0.36)), (.02, .1, .1),
                name='chair_back'))

    ## INIT
    joints = w.getjoints()
    joints['root'].gpos[0:3, 3] = [0, 0, .598]
    joints['l_shoulder_roll'].gpos[:] = pi / 8
    joints['r_shoulder_roll'].gpos[:] = pi / 8
    joints['l_elbow_pitch'].gpos[:] = pi / 8
    joints['r_elbow_pitch'].gpos[:] = pi / 8
    joints['l_knee'].gpos[:] = -0.1
    joints['r_knee'].gpos[:] = -0.1
    joints['l_ankle_pitch'].gpos[:] = -0.1
    joints['r_ankle_pitch'].gpos[:] = -0.1

    shapes = w.getshapes()
    floor_const = [
        SoftFingerContact((shapes[s], shapes['floor']), 1.5, name=s)
        for s in ['lf1', 'lf2', 'lf3', 'lf4', 'rf1', 'rf2', 'rf3', 'rf4']
    ]
    for c in floor_const:
        w.register(c)

    if chair is True:
        chair_const = [
            SoftFingerContact((shapes[s], shapes['chair']), 1.5, name=s)
            for s in ['l_gluteal', 'r_gluteal']
        ]
        for c in chair_const:
            w.register(c)

    w.update_dynamic()

    ## CTRL
    if gravity:
        w.register(WeightController())

    return w
示例#2
0
 def runTest(self):
     b0 = Body(mass=eye(6))
     w = World()
     w.add_link(w.ground, FreeJoint(), b0)
     w.init()
     ctrl = WeightController()
     w.register(ctrl)
     c0 = BallAndSocketConstraint(frames=(w.ground, b0))
     w.register(c0)
     w.init()
     w.update_dynamic()
     dt = 0.001
     w.update_controllers(dt)
     w.update_constraints(dt)
     self.assertListsAlmostEqual(c0._force, [0., 9.81, 0.])
     w.integrate(dt)
     w.update_dynamic()
     self.assertListsAlmostEqual(
         b0.pose, [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0],
                   [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]])
示例#3
0
def create_3r_and_init(gpos=(0,0,0), gvel=(0,0,0), gravity=False):
    ## CREATE THE WORLD
    w = World()
    simplearm.add_simplearm(w, with_shapes=True)

    ## INIT
    joints = w.getjoints()
    joints["Shoulder"].gpos[:] = gpos[0]
    joints["Elbow"].gpos[:]    = gpos[1]
    joints["Wrist"].gpos[:]    = gpos[2]
    joints["Shoulder"].gvel[:] = gvel[0]
    joints["Elbow"].gvel[:]    = gvel[1]
    joints["Wrist"].gvel[:]    = gvel[2]

    w.update_dynamic()

    ## CTRL
    if gravity:
        w.register(WeightController())

    return w
示例#4
0
 def setUp(self):
     """
     Ensure multi-dof Rzyx behaves like the combination of Rz, Ry, Rx.
     """
     world = World()
     self.Ba = Body()
     Rzyx = RzRyRxJoint()
     world.add_link(world.ground, Rzyx, self.Ba)
     Bzy = Body(name='zy')
     Byx = Body(name='yx')
     self.Bb = Body()
     Rz = RzJoint()
     world.add_link(world.ground, Rz, Bzy)
     Ry = RyJoint()
     world.add_link(Bzy, Ry, Byx)
     Rx = RxJoint()
     world.add_link(Byx, Rx, self.Bb)
     world.init()
     (az, ay, ax) = (3.14/6, 3.14/4, 3.14/3)
     Rzyx.gpos[:] = (az, ay, ax)
     (Rz.gpos[0], Ry.gpos[0], Rx.gpos[0]) = (az, ay, ax)
     world.update_dynamic()
示例#5
0
#!/usr/bin/env python

"""
This example show how to add mesh from a collada file to another,
in order to display complex meshes during visualization.
"""

from arboris.core import World
from arboris.robots import simplearm
from arboris.homogeneousmatrix import transl

##### Create world
w = World()

simplearm.add_simplearm(w, with_shapes=True)


##### Init
joints = w.getjoints()

for i in range(3):
    joints[i].gpos[:] = 0.5

w.update_dynamic()


##### Add ctrl
from arboris.controllers import WeightController

w.register(WeightController())
#!/usr/bin/python
#coding=utf-8
#author=joseph salini
"""This tutorial presents the structure of Arboris-python.

It presents the World, Body, Joint, SubFrame and Constraint classes.
"""

##### About the World ##########################################################

from arboris.core import World
from arboris.robots import simplearm

w = World()  # create an instance of world.
simplearm.add_simplearm(w)  # add a simple arm robot

# The world is composed of ...
bodies = w.getbodies()  # ... named list of bodies
joints = w.getjoints()  # ... named list of joints
frames = w.getframes()  # ... named list of frames (including bodies)
shapes = w.getshapes()  # ... named list of shapes
consts = w.getconstraints()  # ... named list of constraints
ctrls = w.getcontrollers()  # ... named list of controllers
# And that's all!
print("bodies", bodies)
print("joints", joints)

# These lists are named, so instances can be retrieved as follows:
body_Arm = bodies["Arm"]
joint_Shoulder = joints["Shoulder"]
print("Arm", body_Arm)