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()
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()
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
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)
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])
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()
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:
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):
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)
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])
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):
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
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
