예제 #1
0
    def __init__(self):
        self.show_impulse = False
        # self.name = 'GP_--N_handfree_naive_clean'
        # self.name = 'Atlas_##_--N_naive_clean'
        self.name = 'Atlas_##--N'
        # self.name = 'BioloidGP_--N'
        # self.name = 'BioloidGP_--N_from_one_foot'

        # Init api
        pydart.init()
        self.world = pydart.create_world(1.0 / 2000.0)
        self.world.add_skeleton(config.DATA_PATH + "sdf/ground.urdf",
                                control=False)
        if 'GP' in self.name:
            self.world.add_skeleton(config.DATA_PATH +
                                    "urdf/BioloidGP/BioloidGP.URDF")
            self.world.skel.set_joint_damping(0.15)
        # self.world.add_skeleton(config.DATA_PATH +
        #                         "urdf/atlas/atlas_v3_no_head.urdf")
        else:
            self.world.add_skeleton(config.DATA_PATH +
                                    "urdf/atlas/atlas_v3_no_head.urdf")
            self.world.skel.set_joint_damping(0.15)

        self.skel = self.world.skel  # shortcut for the control skeleton

        for i, dof in enumerate(self.skel.dofs):
            print i, dof, 'efforts:', self.skel.tau_lo[i], self.skel.tau_hi[i]

        # Configure the scene
        self.cfg = scene.configure.Configure(self)
        self.name = self.name.replace('--', '%.1f' % self.cfg.f_mag)
        if '##' in self.name and self.cfg.tag is not None:
            self.name = self.name.replace('##', self.cfg.tag)

        self.prob = problem.Problem(self, self.cfg.name)
        self.history = History(self)
        self.impulse_live_renderer = ImpulseLiveRenderer(self)

        # # ### Now, configure the controllers
        self.tip_controller = model.controller.Controller(self.skel, self.prob)
        self.tip_controller.pd.set_pd_params(self.name)
        self.event_handler = events.Handler()

        # Reset to the initial state
        self.reset()

        print 'skel.m = ', self.skel.m
        print 'skel.approx_inertia_x = ', self.skel.approx_inertia_x()
        print 'skel.q = ', self.skel.q

        self.rc = scene.range_checker.RangeChecker(self)
        self.rc.check_all()

        # Abstract model
        self.abstract_tip = abstract.dynamic.DynamicTIP(self.prob, self.rc)

        # For handle callbacks properly..
        self.history.clear()
        self.history.push()
예제 #2
0
    def __init__(self, step_activation=None):
        self.prefix = ''
        self.postfix = ''
        self.logger = logging.getLogger(__name__)
        logger = self.logger
        # Init pydart
        pydart.init()
        logger.info('pydart initialization OK')

        # Create world
        self.world = pydart.create_world(1.0 / 1000.0)
        self.world.add_skeleton("./data/sdf/ground.urdf", control=False)
        self.world.add_skeleton("./data/urdf/BioloidGP/BioloidGP.URDF")
        logger.info('pydart create_world OK: dt = %f' % self.world.dt)

        # Configure human
        self.skel = self.world.skels[1]
        for i, body in enumerate(self.skel.bodies):
            self.logger.info('%d: %s' % (i, body.name))
        for i, dof in enumerate(self.skel.dofs):
            self.logger.info('%d: %s' % (i, dof.name))

        # Create the controller
        self.skel.controller = Controller(self,
                                          self.skel,
                                          self.world.dt)

        # Create the motion
        self.motion = Motion(self.skel)

        logger.info('set the initial pose OK')
        self.reset()
예제 #3
0
파일: main.py 프로젝트: jslee02/pydart
import sys
import pydart
import numpy as np
import controller
print('Example: bipedStand')


pydart.init()
print('pydart initialization OK')

data_dir = pydart.misc.example_data_dir(__file__)
print('data_dir = ' + data_dir)

world = pydart.create_world(1.0 / 2000.0, data_dir + '/skel/fullbody1.skel')
# world = pydart.create_world(1.0 / 2000.0,
#                             data_dir + '/skel/soft_fullbody.skel')
print('pydart create_world OK')

skel = world.skels[1]

# Initialize the pose. q is an instance of SkelVector.
q = skel.q
q["j_pelvis_pos_y"] = -0.05
q["j_pelvis_rot_y"] = -0.2
q["j_thigh_left_z", "j_shin_left", "j_heel_left_1"] = 0.15, -0.4, 0.25
q["j_thigh_right_z", "j_shin_right", "j_heel_right_1"] = 0.15, -0.4, 0.25
q["j_abdomen_2"] = 0.0
skel.set_positions(q)
print('skeleton position OK')

# Initialize the controller
예제 #4
0
파일: main.py 프로젝트: jslee02/pydart
        self.skel = skel

    def compute(self):
        damping = -0.01 * self.skel.qdot
        for i in range(1, self.skel.ndofs, 3):
            damping[i] *= 0.1
        return damping


pydart.init()
print('pydart initialization OK')

data_dir = pydart.misc.example_data_dir(__file__)
print('data_dir = ' + data_dir)

world = pydart.create_world(1.0 / 2000.0, data_dir + '/skel/chain.skel')
print('pydart create_world OK')

skel = world.skels[0]
skel.q = (np.random.rand(skel.ndofs) - 0.5)
print('init pose = %s' % skel.q)
skel.controller = DampingController(skel)

if 'qt' in sys.argv:
    tb = pydart.qtgui.Trackball(phi=-0.0, theta=0.0, zoom=1.0,
                                rot=[-0.02, -0.71, -0.02, 0.71],
                                trans=[0.02, 0.09, -1.0])
    pydart.qtgui.run(title='rigidChain', simulation=world, trackball=tb)
else:
    pydart.glutgui.run(title='rigidChain', simulation=world)
예제 #5
0
파일: main.py 프로젝트: hpgit/HumanFoot
import sys
import pydart
print('Example: softBodies')

pydart.init()
print('pydart initialization OK')

data_dir = pydart.misc.example_data_dir(__file__)
print('data_dir = ' + data_dir)

world = pydart.create_world(1.0 / 2000.0, data_dir + '/skel/softBodies.skel')
print('pydart create_world OK')

if 'qt' in sys.argv:
    tb = pydart.qtgui.Trackball(phi=-1.5,
                                theta=-23.3,
                                zoom=1.0,
                                rot=[-0.20, 0.04, -0.01, 0.98],
                                trans=[0.07, 0.03, -2.00])
    pydart.qtgui.run(title='softBodies', simulation=world, trackball=tb)
else:
    pydart.glutgui.run(title='softBodies',
                       simulation=world,
                       trans=[0, -0.1, -1.4])
예제 #6
0
파일: main.py 프로젝트: pombredanne/pydart
import pydart

print("Hello, PyDART!")

pydart.init()
print("pydart initialization OK")

data_dir = pydart.misc.example_data_dir(__file__)
print("data_dir = " + data_dir)

world = pydart.create_world(1.0 / 2000.0, data_dir + "/skel/cubes.skel")
print("pydart create_world OK")

for i, skel in enumerate(world.skels):
    print("Skeleton %d. nDofs = %d" % (i, len(skel.dofs)))

while world.t < 2.0:
    if world.nframes % 100 == 0:
        print("%.4fs: The third cube pos = %s" % (world.t, world.skels[3].C))
    world.step()
예제 #7
0
파일: main.py 프로젝트: hpgit/HumanFoot
        self.skel = skel

    def compute(self):
        damping = -0.01 * self.skel.qdot
        for i in range(1, self.skel.ndofs, 3):
            damping[i] *= 0.1
        return damping


pydart.init()
print('pydart initialization OK')

data_dir = pydart.misc.example_data_dir(__file__)
print('data_dir = ' + data_dir)

world = pydart.create_world(1.0 / 2000.0, data_dir + '/skel/chain.skel')
print('pydart create_world OK')

skel = world.skels[0]
skel.q = (np.random.rand(skel.ndofs) - 0.5)
print('init pose = %s' % skel.q)
skel.controller = DampingController(skel)

if 'qt' in sys.argv:
    tb = pydart.qtgui.Trackball(phi=-0.0,
                                theta=0.0,
                                zoom=1.0,
                                rot=[-0.02, -0.71, -0.02, 0.71],
                                trans=[0.02, 0.09, -1.0])
    pydart.qtgui.run(title='rigidChain', simulation=world, trackball=tb)
else:
예제 #8
0
import sys
import pydart
import numpy as np
import controller
print('Example: bipedStand')

pydart.init()
print('pydart initialization OK')

data_dir = pydart.misc.example_data_dir(__file__)
print('data_dir = ' + data_dir)

world = pydart.create_world(1.0 / 2000.0)
world.add_skeleton(data_dir + '/sdf/atlas/ground.urdf')
world.add_skeleton(data_dir + '/sdf/atlas/atlas_v3_no_head_soft_feet.sdf')
print('pydart create_world OK')

skel = world.skels[1]

# Initialize the pose. q is an instance of SkelVector.
q = skel.q
q[0] = -0.5 * np.pi
# q["j_pelvis_rot_y"] = -0.2
# q["j_thigh_left_z", "j_shin_left", "j_heel_left_1"] = 0.15, -0.4, 0.25
# q["j_thigh_right_z", "j_shin_right", "j_heel_right_1"] = 0.15, -0.4, 0.25
# q["j_abdomen_2"] = 0.0
q['l_arm_shx'] = -0.5 * np.pi
q['r_arm_shx'] = 0.5 * np.pi
skel.set_positions(q)

for i, body in enumerate(skel.bodies):
예제 #9
0
파일: main.py 프로젝트: labiybafakh/pydart
import sys
import pydart
import numpy as np
import controller
print('Example: bipedStand')

pydart.init()
print('pydart initialization OK')

data_dir = pydart.misc.example_data_dir(__file__)
print('data_dir = ' + data_dir)

world = pydart.create_world(1.0 / 2000.0, data_dir + '/skel/fullbody1.skel')
# world = pydart.create_world(1.0 / 2000.0,
#                             data_dir + '/skel/soft_fullbody.skel')
print('pydart create_world OK')

skel = world.skels[1]

# Initialize the pose. q is an instance of SkelVector.
q = skel.q
q["j_pelvis_pos_y"] = -0.05
q["j_pelvis_rot_y"] = -0.2
q["j_thigh_left_z", "j_shin_left", "j_heel_left_1"] = 0.15, -0.4, 0.25
q["j_thigh_right_z", "j_shin_right", "j_heel_right_1"] = 0.15, -0.4, 0.25
q["j_abdomen_2"] = 0.0
skel.set_positions(q)
print('skeleton position OK')

# Initialize the controller
skel.controller = controller.Controller(skel, world.dt)
예제 #10
0
파일: main.py 프로젝트: jslee02/pydart
import sys
import pydart
print('Example: softBodies')

pydart.init()
print('pydart initialization OK')

data_dir = pydart.misc.example_data_dir(__file__)
print('data_dir = ' + data_dir)

world = pydart.create_world(1.0 / 2000.0, data_dir + '/skel/softBodies.skel')
print('pydart create_world OK')

if 'qt' in sys.argv:
    tb = pydart.qtgui.Trackball(phi=-1.5, theta=-23.3, zoom=1.0,
                                rot=[-0.20, 0.04, -0.01, 0.98],
                                trans=[0.07, 0.03, -2.00])
    pydart.qtgui.run(title='softBodies', simulation=world, trackball=tb)
else:
    pydart.glutgui.run(title='softBodies', simulation=world,
                       trans=[0, -0.1, -1.4])
예제 #11
0
파일: main.py 프로젝트: pombredanne/pydart
import sys
import pydart
import numpy as np
print('Example: bipedStand')


pydart.init()
print('pydart initialization OK')

data_dir = pydart.misc.example_data_dir(__file__)
print('data_dir = ' + data_dir)

world = pydart.create_world(1.0 / 2000.0)
world.add_skeleton(data_dir + '/sdf/atlas/ground.urdf')
world.add_skeleton(data_dir + '/vsk/Yunseong.vsk')
print('pydart create_world OK')

skel = world.skels[1]

# Initialize the pose. q is an instance of SkelVector.
q = skel.q
q[4] = 0.05
q[39] = -1.5
q[55] = -q[39]
skel.set_positions(q)

for i, body in enumerate(skel.bodies):
    print i, body.name, body.num_markers()
for i, dof in enumerate(skel.dofs):
    print i, dof.name
for i, m in enumerate(skel.markers):
예제 #12
0
def renderTimer(timer):
    glutPostRedisplay()
    glutTimerFunc(20, renderTimer, 1)


###############################################################################
# Begin main function

# Initialize PyDart
pydart.init()
# world = pydart.create_world(1.0 / 2000.0)
# world.add_skeleton(DATA_PATH + "sdf/ground.urdf", control=False)
# # world.add_skeleton(DATA_PATH + "urdf/BioloidGP/BioloidGP.URDF")
# world.add_skeleton(DATA_PATH + "urdf/atlas/atlas_v3_no_head.urdf")
world = pydart.create_world(1.0 / 2000.0, DATA_PATH + "skel/fullbody1.skel")
world.skels[-1].set_joint_damping(0.15)

# Set the skeleton pose.
q = np.zeros(world.skels[-1].ndofs)
q[0] = -0.45 * math.pi
# q[4] = 0.221
q[4] = 1.25
q[5] = q[4]
world.skels[-1].q = q

# Create a pd controller
pd = PDController(world.skels[-1], 500.0, 25.0)
pd.target = world.skels[-1].q

# Init glut
예제 #13
0
rd_cForcesControl = None
rd_cPositionsControl = None

rd_ForceControl = None
rd_ForceDes = None
rd_Position = None
rd_PositionDes = None

rd_jointPos = None

viewer = None

pydart.init()
data_dir = os.path.dirname(__file__)
print('data_dir = ' + data_dir)
dartWorld = pydart.create_world(1.0 / 1800.0, data_dir + '/test.xml')
dartWorld.test()
q = dartWorld.skels[1].q
q['j_Hips_pos_y'] = 0.
q['j_Hips_pos_x'] = 0.
q['j_Hips_pos_z'] = 0.
dartWorld.skels[1].set_positions(q)
dartWorld.skels[1].dof
dartWorld.skels[1].controller = PDController(dartWorld.skels[1], dartWorld.dt)
# pydart.glutgui.run(title='bipedStand', simulation=dartWorld, trans=[0, 0, -3])


def init():
    global motion
    global mcfg
    global wcfg