def main():
fGraph = build_graph()
def get_info(x):
temp = x.split()
return tuple([int(temp[0]), int(temp[1]), float(temp[2])])
motion_idx = [None, None]
with open('mg.txt', 'r') as f:
s = f.read().splitlines(False)
total_num = int(s[0])
motion_info = list(map(get_info, s[1:]))
pydart.init()
world = [
pydart.World(1. / 1200., "../DartDeep/data/woody_with_ground_v2.xml")
for _ in range(2)
]
# viewer settings
rd_contact_positions = [None]
rd_contact_forces = [None]
viewer = hsv.hpSimpleViewer(rect=(0, 0, 1200, 800), viewForceWnd=False)
viewer.doc.addRenderer('motionModel[0]',
yr.DartRenderer(world[0], (255, 240, 255)))
viewer.doc.addRenderer('motionModel[1]', yr.DartRenderer(world[1]))
motion_state = [0]
def callback_0(_):
motion_state[0] = 0
def callback_1(_):
motion_state[0] = 1
def callback_2(_):
motion_state[0] = 2
viewer.objectInfoWnd.addBtn('0', callback_0)
viewer.objectInfoWnd.addBtn('1', callback_1)
viewer.objectInfoWnd.addBtn('2', callback_2)
def postCallback(frame):
for i in range(2):
world[i].skeletons[1].set_positions(
fGraph.motion_frames.get_q(motion_info[frame][i]))
print(frame, motion_info[frame])
viewer.setPreFrameCallback_Always(postCallback)
viewer.setMaxFrame(total_num - 1)
viewer.startTimer(1. / 30.)
viewer.show()
Fl.run()
def __init__(self,
model_path,
frame_skip,
observation_size,
action_bounds,
dt=0.002):
pydart.init()
print('pydart initialization OK')
if model_path.startswith("/"):
fullpath = model_path
else:
fullpath = os.path.join(os.path.dirname(__file__), "assets",
model_path)
if not path.exists(fullpath):
raise IOError("File %s does not exist" % fullpath)
self.dart_world = pydart.World(dt, fullpath)
self.robot_skeleton = self.dart_world.skeletons[
-1] # assume that the skeleton of interest is always the last one
for jt in range(0, len(self.robot_skeleton.joints)):
if self.robot_skeleton.joints[jt].has_position_limit(0):
self.robot_skeleton.joints[jt].set_position_limit_enforced(
True)
self.frame_skip = frame_skip
self.viewer = None
observation, _reward, done, _info = self._step(
np.zeros(len(action_bounds[0])))
assert not done
self.obs_dim = observation_size
self.act_dim = len(action_bounds[0])
self.action_space = spaces.Box(action_bounds[1], action_bounds[0])
self.track_skeleton_id = -1 # track the last skeleton's com by default
high = np.inf * np.ones(self.obs_dim)
low = -high
self.observation_space = spaces.Dict({
"observation":
spaces.Box(low, high),
"mass":
spaces.Box(np.array([0]), np.array([10]))
})
# self.observation_space = spaces.Box(low, high)
self._seed()
# self.viewer = None
self.metadata = {
'render.modes': ['human', 'rgb_array'],
'video.frames_per_second': int(np.round(1.0 / self.dt))
}
def _create_dart_objs(model_path, env):
full_path = os.path.join(os.path.dirname(__file__), 'assets', model_path)
if not os.path.exists(full_path):
raise IOError("File {} does not exist".format(full_path))
dart_world = pydart.World(env.dart_world.dt, full_path)
robot = dart_world.skeletons[-1] # assume the skeleton of interest is always the last one!
for jt in range(0, len(robot.joints)):
for dof in range(len(robot.joints[jt].dofs)):
if robot.joints[jt].has_position_limit(dof):
robot.joints[jt].set_position_limit_enforced(True)
return dart_world, robot
def __init__(self, folder):
pydart.init()
self.world = pydart.World(1./1200., "../data/cmu_with_ground.xml")
self.model = self.world.skeletons[1]
self.ik = DartIk(self.model)
self.controller = RNNController(folder)
self.all_angles = [[] for i in range(93)]
viewer = SimpleViewer(rect=[0, 0, 1280+300, 720+1+55], viewForceWnd=False)
self.viewer = viewer
# viewer.record(True)
viewer.record(False)
viewer.setMaxFrame(10000)
self.isFirst = True
self.lines = None
viewer.motionViewWnd.glWindow.set_mouse_pick(True)
def callback_btn(ptr):
self.controller.reset()
viewer.objectInfoWnd.addBtn('reset', callback_btn)
self.rc = yr.RenderContext()
self.rd_target_position = [None]
self.rd_frames = [None]
viewer.doc.addRenderer('contact', yr.PointsRenderer(self.rd_target_position, (0, 255, 0), save_state=False))
viewer.doc.addRenderer('MotionModel', yr.DartRenderer(self.world, (150,150,255), yr.POLYGON_FILL, save_state=False))
viewer.doc.addRenderer('rd_frames', yr.FramesRenderer(self.rd_frames))
def extraDrawCallback():
self.rd_target_position[0] = self.viewer.motionViewWnd.glWindow.pickPoint
self.step_model()
del self.rd_frames[:]
self.rd_frames.append(self.model.body(0).world_transform())
# for i in range(3):
# print(self.model.body(0).world_transform()[:3, i])
viewer.setExtraDrawCallback(extraDrawCallback)
viewer.startTimer(1. / 30.)
viewer.show()
# viewer.play()
Fl.run()
def __init__(
self,
dt=0.002,
control_frequency=20,
optimizeMu=False,
TorqueLimits=2.5,
includeIMU=False,
rollout_length=2.0,
):
# Initialize Pydart2 and other parameters here
# This only does DARWIN-OP2 for now
self.dt = dt
self.control_frequency = control_frequency
self.optimizeMu = optimizeMu
self.TorqueLimits = TorqueLimits
self.includeIMU = includeIMU
self.rollout_length = rollout_length
self.DarwinWorld = pydart.World(self.dt)
self.DarwinWorld.add_skeleton('ground1.urdf')
self.DarwinWorld.add_skeleton('robotis_op2.urdf')
self.skel = self.DarwinWorld.skeletons[-1] # last skel
self.DarwinWorld.set_gravity([0., 0., -9.81])
self.ndofs = self.skel.ndofs
self.preverror = np.zeros(self.ndofs, )
self.edot = np.zeros(self.ndofs, )
self.error = []
for i in range(self.skel.ndofs):
j = self.skel.dof(i)
j.set_damping_coefficient(0.275)
# Enforce Joint Limits
for joint in range(0, len(self.skel.joints)):
for dof in range(len(self.skel.joints[joint].dofs)):
if self.skel.joints[joint].has_position_limit(dof):
self.skel.joints[joint].set_position_limit_enforced(True)
for body in self.skel.bodynodes:
if body.name == "base_link":
body.set_mass(0.001)
if body.name == "MP_PMDCAMBOARD":
body.set_mass(0.001)
shape_map['Neck1'] = ('sphere', 0.1)
shape_map['LeftFoot'] = ('box', [0.1, 0.09, 0.225])
shape_map['RightFoot'] = ('box', [0.1, 0.09, 0.225])
shape_map['LeftHand'] = ('sphere', 0.05)
shape_map['RightHand'] = ('sphere', 0.05)
BvhSkelToDartSkel(motion[0].skeleton, massMap, shape_map).save('test.xml')
from fltk import *
from PyCommon.modules.GUI.hpSimpleViewer import hpSimpleViewer as SimpleViewer
from PyCommon.modules.Renderer import ysRenderer as yr
import pydart2 as pydart
pydart.init()
world = pydart.World(1. / 1200., "test.xml")
# model = world.skeletons[1]
posture = ym.JointPosture(motion[0].skeleton)
posture.initLocalRs()
posture.translateByOffset([0.02642508, 1.02154927, 1.09827205])
posture.updateGlobalT()
viewer = SimpleViewer(rect=(0, 0, 1200, 800), viewForceWnd=False)
viewer.setMaxFrame(100)
rd_target_position = [None]
points = [None]
viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion))
viewer.doc.addRenderer(
def __init__(self, env_name='walk'):
self.skel_file_name = '../data/cmu_with_ground.xml'
self.world = pydart.World(1. / 1200., self.skel_file_name)
self.world.control_skel = self.world.skeletons[1]
self.skel = self.world.skeletons[1]
self.Kp, self.Kd = 400., 40.
self.pdc = PDController(self.skel, self.world.time_step(), 400., 40.)
self.env_name = env_name
self.ref_world = pydart.World(1. / 1200., self.skel_file_name)
self.ref_skel = self.ref_world.skeletons[1]
self.step_per_frame = 40
self.rnn = RNNController(env_name)
self.RNN_MOTION_SCALE = 0.01
self.rnn_joint_list = [
"Head", "Hips", "LHipJoint", "LeftArm", "LeftFoot", "LeftForeArm",
"LeftHand", "LeftLeg", "LeftShoulder", "LeftToeBase", "LeftUpLeg",
"LowerBack", "Neck", "Neck1", "RHipJoint", "RightArm", "RightFoot",
"RightForeArm", "RightHand", "RightLeg", "RightShoulder",
"RightToeBase", "RightUpLeg", "Spine", "Spine1"
]
self.rnn_target_update_prob = 1. / 150.
self.ik = DartIk(self.ref_skel)
self.rsi = True
self.w_g = 0.3
self.w_p = 0.65 * .7
self.w_v = 0.1 * .7
self.w_e = 0.15 * .7
self.w_c = 0.1 * .7
self.exp_g = 2.5
self.exp_p = 2.
self.exp_v = 0.1
self.exp_e = 40.
self.exp_c = 10.
self.body_num = self.skel.num_bodynodes()
self.idx_e = [
self.skel.bodynode_index('LeftFoot'),
self.skel.bodynode_index('RightFoot'),
self.skel.bodynode_index('LeftHand'),
self.skel.bodynode_index('RightHand')
]
self.body_e = list(map(self.skel.body, self.idx_e))
self.ref_body_e = list(map(self.ref_skel.body, self.idx_e))
self.time_offset = 0.
self.step_num = 0
# state_num = 2 + (3*3 + 4) * self.body_num
state_num = 2 + 2 * 3 * self.body_num + 3 * (self.skel.num_dofs() - 3)
action_num = self.skel.num_dofs() - 6
state_high = np.array([np.finfo(np.float32).max] * state_num)
action_high = np.array([pi * 10. / 2.] * action_num)
self.action_space = gym.spaces.Box(-action_high,
action_high,
dtype=np.float32)
self.observation_space = gym.spaces.Box(-state_high,
state_high,
dtype=np.float32)
self.viewer = None
self.phase_frame = 0
self.goal = np.zeros(2)
self.goal_prev = self.goal.copy()
self.goal_in_world_frame = np.zeros(3)
self.first = True
self.prev_ref_q = self.ref_skel.positions()
self.prev_ref_dq = np.zeros(self.ref_skel.num_dofs())
self.prev_ref_p_e = np.asarray([
body.world_transform()[:3, 3] for body in self.ref_body_e
]).flatten()
self.prev_ref_com = self.ref_skel.com()
def __init__(self):
self.world = pydart.World(1./1200., "../data/woody_with_ground.xml")
self.world.control_skel = self.world.skeletons[1]
self.skel = self.world.skeletons[1]
self.pdc = PDController(self.skel, self.world.time_step(), 400., 40.)
self.Kp, self.Kd = 400., 40.
self.ik = DartIk(self.skel)
self.ref_motions = list() # type: list[ym.Motion]
self.ref_motions.append(yf.readBvhFile("../data/woody_walk_normal.bvh")[40:])
self.ref_motions.append(yf.readBvhFile("../data/wd2_WalkForwardVFast00.bvh"))
self.ref_motions.append(yf.readBvhFile("../data/wd2_WalkForwardSlow01.bvh"))
self.ref_motions.append(yf.readBvhFile("../data/walk_right_45degree.bvh")[21:134])
self.ref_motions.append(yf.readBvhFile("../data/walk_left_45degree.bvh")[26:141])
# self.ref_motions.append(yf.readBvhFile("../data/walk_left_90degree.bvh")[21:153])
# self.ref_motions.append(yf.readBvhFile('../data/wd2_WalkSukiko00.bvh'))
# self.ref_motions.append(yf.readBvhFile("../data/wd2_jump0.bvh")[164:280])
# self.ref_motions.append(yf.readBvhFile("../data/wd2_u-turn.bvh")[25:214])
# self.ref_motions[-1].translateByOffset([0., 0.03, 0.])
self.motion_num = len(self.ref_motions)
self.reward_weights_by_fps = [self.ref_motions[0].fps / self.ref_motions[i].fps for i in range(self.motion_num)]
self.ref_motion = self.ref_motions[0]
self.ref_world = pydart.World(1./1200., "../data/woody_with_ground.xml")
self.ref_skel = self.ref_world.skeletons[1]
self.step_per_frame = None
self.rsi = True
self.specified_motion_num = 0
self.is_motion_specified = False
# self.w_g = 0.3
# self.w_p = 0.65 * .7
# self.w_v = 0.1 * .7
# self.w_e = 0.15 * .7
# self.w_c = 0.1 * .7
self.w_p = 0.65
self.w_v = 0.1
self.w_e = 0.15
self.w_c = 0.1
self.exp_g = 2.5
self.exp_p = 2.
self.exp_v = 0.1
self.exp_e = 40.
self.exp_c = 10.
self.body_num = self.skel.num_bodynodes()
self.idx_e = [self.skel.bodynode_index('LeftFoot'), self.skel.bodynode_index('RightFoot'),
self.skel.bodynode_index('LeftForeArm'), self.skel.bodynode_index('RightForeArm')]
self.body_e = list(map(self.skel.body, self.idx_e))
self.ref_body_e = list(map(self.ref_skel.body, self.idx_e))
self.motion_len = len(self.ref_motion)
self.goals_in_world_frame = list()
self.goal = np.zeros(2)
self.goals = list()
self.goal_window = 45
state_num = 3*self.goal_window + (3*3 + 4) * self.body_num
# action_num = self.skel.num_dofs() - 6
action_num = self.skel.num_dofs()
state_high = np.array([np.finfo(np.float32).max] * state_num)
action_high = np.array([pi*2.] * action_num)
self.action_space = gym.spaces.Box(-action_high, action_high, dtype=np.float32)
self.observation_space = gym.spaces.Box(-state_high, state_high, dtype=np.float32)
self.viewer = None
self.phase_frame = 0
self.prev_ref_q = np.zeros(self.ref_skel.num_dofs())
self.prev_ref_dq = np.zeros(self.ref_skel.num_dofs())
self.prev_ref_p_e_hat = np.asarray([body.world_transform()[:3, 3] for body in self.ref_body_e]).flatten()
self.prev_ref_com = self.ref_skel.com()
# setting for reward
self.reward_joint = list()
self.reward_joint.append('j_RightUpLeg')
self.reward_joint.append('j_RightLeg')
self.reward_joint.append('j_LeftUpLeg')
self.reward_joint.append('j_LeftLeg')
self.reward_joint.append('j_Spine')
self.reward_joint.append('j_Spine1')
self.reward_joint.append('j_RightArm')
self.reward_joint.append('j_RightForeArm')
self.reward_joint.append('j_LeftArm')
self.reward_joint.append('j_LeftForeArm')
def __init__(self, env_name='walk'):
self.world = pydart.World(1. / 1200.,
"../data/woody_with_ground_v2.xml")
self.world.control_skel = self.world.skeletons[1]
self.skel = self.world.skeletons[1]
self.Kp, self.Kd = 400., 40.
self.pdc = PDController(self.skel, self.world.time_step(), 400., 40.)
self.env_name = env_name
self.ref_motion = None # type: ym.Motion
if env_name == 'walk':
self.ref_motion = yf.readBvhFile(
"../data/woody_walk_normal.bvh")[40:]
elif env_name == 'spiral_walk':
self.ref_motion = yf.readBvhFile(
"../data/wd2_spiral_walk_normal05.bvh")
elif env_name == 'walk_spin':
self.ref_motion = yf.readBvhFile(
"../data/wd2_2foot_walk_turn2.bvh")
elif env_name == 'jump':
self.ref_motion = yf.readBvhFile("../data/wd2_jump0.bvh")[164:280]
elif env_name == 'walk_fast':
self.ref_motion = yf.readBvhFile(
"../data/wd2_WalkForwardVFast00.bvh")
elif env_name == 'walk_left_90':
self.ref_motion = yf.readBvhFile("../data/walk_left_90degree.bvh")
elif env_name == 'walk_left_45':
self.ref_motion = yf.readBvhFile("../data/walk_left_45degree.bvh")
elif env_name == 'walk_pick':
self.ref_motion = yf.readBvhFile("../data/wd2_pick_walk_1.bvh")
elif env_name == 'walk_u_turn':
self.ref_motion = yf.readBvhFile("../data/wd2_u-turn.bvh")[25:214]
self.ref_motion.translateByOffset([0., 0.03, 0.])
elif env_name == 'jump_whole':
self.ref_motion = yf.readBvhFile("../data/wd2_jump0.bvh")[315:966]
elif env_name == 'walk_u_turn_whole':
self.ref_motion = yf.readBvhFile("../data/wd2_u-turn.bvh")
self.ref_motion.translateByOffset([0., 0.03, 0.])
self.ref_world = pydart.World(1. / 1200.,
"../data/woody_with_ground.xml")
self.ref_skel = self.ref_world.skeletons[1]
# self.step_per_frame = round((1./self.world.time_step()) / self.ref_motion.fps)
self.step_per_frame = 40
self.rsi = True
self.w_p = 0.65
self.w_v = 0.1
self.w_e = 0.15
self.w_c = 0.1
self.exp_p = 2.
self.exp_v = 0.1
self.exp_e = 40.
self.exp_c = 10.
self.body_num = self.skel.num_bodynodes()
self.idx_e = [
self.skel.bodynode_index('LeftFoot'),
self.skel.bodynode_index('RightFoot'),
self.skel.bodynode_index('LeftForeArm'),
self.skel.bodynode_index('RightForeArm')
]
self.body_e = list(map(self.skel.body, self.idx_e))
self.ref_body_e = list(map(self.ref_skel.body, self.idx_e))
self.motion_len = len(self.ref_motion)
self.motion_time = len(self.ref_motion) / self.ref_motion.fps
self.time_offset = 0.
state_num = 1 + (3 * 3 + 4) * self.body_num
action_num = self.skel.num_dofs() - 6
state_high = np.array([np.finfo(np.float32).max] * state_num)
action_high = np.array([pi * 10. / 2.] * action_num)
self.action_space = gym.spaces.Box(-action_high,
action_high,
dtype=np.float32)
self.observation_space = gym.spaces.Box(-state_high,
state_high,
dtype=np.float32)
self.viewer = None
self.phase_frame = 0
# max training time
self.training_time = 3.
self.evaluation = False
self.visualize = False
self.visualize_select_motion = 0
def __init__(self, env_name='walk', dart_skel_file='../data/wd2_seg.xml'):
self.dart_skel_file = dart_skel_file
self.world = pydart.World(1. / 1200., self.dart_skel_file)
self.world.control_skel = self.world.skeletons[1]
self.world.disable_recording()
self.world.collision_auto_update = False
self.skel = self.world.skeletons[1]
self.Kp, self.Kd = 400., 40.
self.env_name = env_name
self.ref_motion = None # type: ym.Motion
motion_name = None
if env_name == 'walk':
motion_name = "../data/segfoot_wd2_WalkForwardNormal00.bvh"
elif env_name == 'walk_repeated':
motion_name = "../data/segfoot_wd2_WalkForwardNormal00_REPEATED.bvh"
elif env_name == 'walk_fast':
motion_name = "../data/segfoot_wd2_WalkForwardVFast00.bvh"
elif env_name == 'walk_sukiko':
motion_name = '../data/segfoot_wd2_WalkSukiko00.bvh'
elif env_name == 'walk_u_turn':
motion_name = '../data/segfoot_wd2_u-turn_edit.bvh'
elif env_name == '1foot_contact_run':
motion_name = '../data/segfoot_wd2_1foot_contact_run2_edit.bvh'
elif env_name == 'round_girl':
motion_name = '../data/segfoot_wd2_boxing_round_round_girl1_edit.bvh'
elif env_name == 'fast_2foot_hop':
motion_name = '../data/segfoot_wd2_fast_2foot_hop_edit.bvh'
elif env_name == 'slow_2foot_hop':
motion_name = '../data/segfoot_wd2_slow_2foot_hop_edit.bvh'
elif env_name == 'long_broad_jump':
motion_name = '../data/segfoot_wd2_long_broad_jump_edit.bvh'
elif env_name == 'short_broad_jump':
motion_name = '../data/segfoot_wd2_short_broad_jump_edit.bvh'
elif env_name == 'n_kick':
motion_name = '../data/segfoot_wd2_n_kick_edit.bvh'
elif env_name == 'jump':
motion_name = '../data/wd2_jump0_edited.bvh'
SEGMENT_FOOT_MAG = 0.01
SEGMENT_FOOT_RAD = 0.008
bvh = yf.readBvhFileAsBvh(motion_name)
current_path = os.path.dirname(os.path.abspath(__file__))
partBvhFilePath = current_path + '/../../PyCommon/modules/samples/'
partBvhFilePath = partBvhFilePath + 'foot_model_real_joint_01.bvh'
partBvh = yf.readBvhFileAsBvh(partBvhFilePath)
partSkeleton = partBvh.toJointSkeleton(1., False)
bvh.replaceJointFromBvh('RightFoot', partBvh, SEGMENT_FOOT_MAG)
partBvh = yf.readBvhFileAsBvh(partBvhFilePath)
partBvh.mirror('YZ')
bvh.replaceJointFromBvh('LeftFoot', partBvh, SEGMENT_FOOT_MAG)
self.ref_motion = bvh.toJointMotion(1., False) # type: ym.JointMotion
if env_name == 'walk':
# self.ref_motion = self.ref_motion[20:]
self.ref_motion.translateByOffset([0., -0.03, 0.])
if env_name == 'walk_repeated':
# self.ref_motion = self.ref_motion[20:]
self.ref_motion.translateByOffset([0., -0.03, 0.])
elif env_name == 'jump':
self.ref_motion = self.ref_motion[164:280]
self.ref_motion.translateByOffset([0., -0.06, 0.])
elif env_name == 'walk_u_turn':
self.ref_motion = self.ref_motion[25:214]
self.ref_motion.translateByOffset([0., -0.09, 0.])
elif env_name == 'jump_whole':
self.ref_motion = self.ref_motion[315:966]
elif env_name == 'walk_u_turn_whole':
self.ref_motion.translateByOffset([0., 0.03, 0.])
elif env_name == '1foot_contact_run':
self.ref_motion.translateByOffset([0., -0.09, 0.])
elif env_name == 'round_girl':
self.ref_motion = self.ref_motion[505:658]
self.ref_motion.translateByOffset([0., -0.01, 0.])
elif env_name == 'fast_2foot_hop':
self.ref_motion.translateByOffset([0., -0.09, 0.])
elif env_name == 'slow_2foot_hop':
self.ref_motion.translateByOffset([0., -0.09, 0.])
elif env_name == 'long_broad_jump':
self.ref_motion.translateByOffset([0., -0.09, 0.])
elif env_name == 'short_broad_jump':
self.ref_motion.translateByOffset([0., -0.09, 0.])
elif env_name == 'n_kick':
self.ref_motion.translateByOffset([0., -0.09, 0.])
self.ref_world = pydart.World(1. / 1200., "../data/wd2_seg.xml")
self.ref_skel = self.ref_world.skeletons[1]
self.step_per_frame = round(
(1. / self.world.time_step()) / self.ref_motion.fps)
# self.step_per_frame = 40
fix_motion_data_by_foot(self.ref_motion, self.ref_skel,
SEGMENT_FOOT_RAD)
self.rsi = True
self.rsi_num = None
# self.w_p = 0.65
self.w_p = 0.50
self.w_v = 0.1
self.w_e = 0.10
self.w_c = 0.1
self.w_t = 0.1
self.w_y_ankle = 0.1
self.exp_p = 2.
self.exp_v = 0.1
self.exp_e = 40.
self.exp_c = 10.
self.exp_t = 20.
self.exp_y_ankle = 5.
# soohwan style
# self.w_p = 0.15
# self.w_v = 0.05
# self.w_c = 0.4
# self.w_c_v = 0.05
# self.w_e = 0.2
# self.w_e_ori = 0.05
#
# self.exp_p = 2.
# self.exp_v = 20.
# self.exp_c = .3
# self.exp_c_v = 2.
# self.exp_e = 2.
# self.exp_e_ori = 2.
self.body_num = self.skel.num_bodynodes()
self.idx_e = [
self.skel.bodynode_index('LeftFoot'),
self.skel.bodynode_index('RightFoot'),
self.skel.bodynode_index('LeftForeArm'),
self.skel.bodynode_index('RightForeArm')
]
self.ref_body_e = list(map(self.ref_skel.body, self.idx_e))
self.ankle_joint_names = ['j_LeftFoot', 'j_RightFoot']
self.motion_len = len(self.ref_motion)
self.motion_time = len(self.ref_motion) / self.ref_motion.fps
self.time_offset = 0.
self.viewer = None
self.phase_frame = 0
self.prev_ref_q = np.zeros(self.ref_skel.num_dofs())
self.prev_ref_dq = np.zeros(self.ref_skel.num_dofs())
self.prev_ref_p_e_hat = np.asarray([
body.world_transform()[:3, 3] for body in self.ref_body_e
]).flatten()
self.prev_ref_p_e_ori_hat = [
body.world_transform()[:3, :3] for body in self.ref_body_e
]
self.prev_ref_com = self.ref_skel.com()
self.prev_ref_com_vel = self.ref_skel.com_velocity()
self.prev_ref_com_spatial_vel = self.ref_skel.com_spatial_velocity()
self.prev_ref_torso_ori = self.ref_skel.body(
'Spine').world_transform()[:3, :3]
self.prev_ref_ankle_joint_y_pos = np.asarray([
joint.get_world_frame_after_transform()[1, 3] for joint in [
self.ref_skel.joint(joint_name)
for joint_name in self.ankle_joint_names
]
]).flatten()
# setting for reward
self.reward_joint = list()
self.reward_joint.append('j_Hips')
self.reward_joint.append('j_RightUpLeg')
self.reward_joint.append('j_RightLeg')
self.reward_joint.append('j_LeftUpLeg')
self.reward_joint.append('j_LeftLeg')
self.reward_joint.append('j_Spine')
self.reward_joint.append('j_Spine1')
self.reward_joint.append('j_RightArm')
self.reward_joint.append('j_RightForeArm')
self.reward_joint.append('j_LeftArm')
self.reward_joint.append('j_LeftForeArm')
# setting for pd gain
self.foot_joint = list()
self.foot_joint.append('j_RightFoot')
self.foot_joint.append('j_RightFoot_foot_0_0')
self.foot_joint.append('j_RightFoot_foot_0_0_0')
self.foot_joint.append('j_RightFoot_foot_0_1_0')
self.foot_joint.append('j_RightFoot_foot_1_0')
self.foot_joint.append('j_LeftFoot')
self.foot_joint.append('j_LeftFoot_foot_0_0')
self.foot_joint.append('j_LeftFoot_foot_0_0_0')
self.foot_joint.append('j_LeftFoot_foot_0_1_0')
self.foot_joint.append('j_LeftFoot_foot_1_0')
state_num = 1 + (3 * 3 + 4) * self.body_num
action_num = self.skel.num_dofs() - 6 + len(self.foot_joint)
state_high = np.array([np.finfo(np.float32).max] * state_num)
action_high = np.array([pi * 10.] * action_num)
self.action_space = gym.spaces.Box(-action_high,
action_high,
dtype=np.float32)
self.observation_space = gym.spaces.Box(-state_high,
state_high,
dtype=np.float32)
self.force_done = False
self.applied_force = np.zeros(3)
self.applied_force_offset = np.zeros(3)
import pydart2 as pydart
if __name__ == '__main__':
print('Hello, PyDART!')
pydart.init()
print('pydart initialization OK')
world = pydart.World(0.0005, './data/skel/cubes.skel')
print('pydart create_world OK')
while world.t < 2.0:
if world.nframes % 100 == 0:
skel = world.skeletons[-1]
print("%.4fs: The last cube COM = %s" % (world.t, str(skel.C)))
world.step()
import OpenGL.GL as GL
import OpenGL.GLU as GLU
import OpenGL.GLUT as GLUT
from pydart2.gui.opengl.renderer import Renderer
from pydart2.gui.trackball import Trackball
from pydart2.gui.glut.window import GLUTWindow
stop = False
if __name__ == '__main__':
print('Hello, PyDART!')
pydart.init()
print('pydart initialization OK')
world = pydart.World(0.0005, 'examples/data/skel/arti_data.skel')
#world.set_gravity([0, -9.8, 0])
print('pydart create_world OK')
# pydart.gui.viewer.launch(world)
# skel = world.skeletons[-1]
# skel.friction = 0.9
#skel.q = (np.random.rand(skel.ndofs))
#skel.set_forces(np.array([0, 0, 0, 100, 0, 0]))
#skel.set_accelerations([0, 5, 0])
#print(skel.tau)
#print(skel.friction)
# skel.controller = ApplyForce(skel)
win = GLUTWindow(world, None)
# win.scene.set_camera(None)
win.scene.add_camera(
Trackball(rot=[-0.152, 0.045, -0.002, 0.987], trans=[0, 0.2, -.500]),
heels = ['h_heel_left', 'h_heel_right']
vf = self.jt.apply(heels, [0, -700, 0])
tau += vf
# Make sure the first six are zero
tau[:6] = 0
return tau
if __name__ == '__main__':
print('Example: bipedJump')
pydart.init()
print('pydart initialization OK')
world = pydart.World(0.0005, './data/skel/fullbody1.skel')
print('pydart create_world OK')
# Initialize the pose
skel = world.skeletons[1]
q = skel.q
q[(2, 4, 5)] = [0.02 * np.pi, -0.02, 0]
skel.set_positions(q)
print('skeleton position OK')
# Initialize the controller
skel.set_controller(Controller(skel, world.dt))
print('create controller OK')
pydart.gui.viewer.launch(world)
def compute(self):
q = self.skel.q
dq = np.zeros(self.skel.ndofs,)
print("self",self.skel.q)
#
self.torques = -0*np.ones(self.skel.ndofs,)
return self.torques
pydart.init()
world = pydart.World(0.001,'reacher2d.skel')#,''walker2d.skel
world.g = [0,0,9.81]
skel = world.skeletons[0]
q = skel.q
q[0] = 0#np.pi/4
q[1] = np.pi/2
#print("q",q)
for body in skel.bodynodes:
print("body",body)
zero_vector = np.zeros((1, self.robot.ndofs))
P = np.concatenate((P, zero_vector), axis=1)
C = constant.T.dot(constant)
# return [newJacobian, newJacobian_dot, Q, P, C]
return [self.taskWeight * Q, self.taskWeight * P, self.taskWeight * C]
if __name__ == "__main__":
print('Hello, PyDART!')
pydart.init()
test_world = pydart.World(1.0 / 2000.0, "../data/skel/two_cubes.skel")
test_robot = test_world.add_skeleton("../data/KR5/KR5_sixx_R650.urdf")
cv_3 = np.array([[50., 25., 1.], [59., 12., 3.], [50., 10., 5.],
[57., 2., 8.], [40., 4., 10.], [40., 14., 12.]]).T
timeKnots = [0.1, 0.2, 0.4, 0.7, 2.0, 3.0]
#test_desiredPosition = array([0.1, 0.2, 0.3]).reshape((3,1))
taskWeight = 1000
test_task = trajectoryTask(test_robot, cv_3, timeKnots, taskWeight)
[Q, P, C] = test_task.calcMatricies()
def __init__(self, env_name='walk'):
self.world = pydart.World(1. / 1800., "../data/wd2_without_ground.xml")
# self.world.control_skel = self.world.skeletons[1]
# self.skel = self.world.skeletons[1]
self.world.control_skel = self.world.skeletons[0]
self.skel = self.world.skeletons[0]
self.Kp, self.Kd = 400., 40.
self.env_name = env_name
self.ref_motion = None # type: ym.Motion
motion_name = None
if env_name == 'walk':
motion_name = "../data/segfoot_wd2_WalkForwardNormal00.bvh"
elif env_name == 'walk_fast':
motion_name = "../data/segfoot_wd2_WalkForwardVFast00.bvh"
elif env_name == 'walk_sukiko':
motion_name = '../data/segfoot_wd2_WalkSukiko00.bvh'
SEGMENT_FOOT_MAG = 0.01
SEGMENT_FOOT_RAD = 0.008
bvh = yf.readBvhFileAsBvh(motion_name)
current_path = os.path.dirname(os.path.abspath(__file__))
partBvhFilePath = current_path + '/../../PyCommon/modules/samples/'
partBvhFilePath = partBvhFilePath + 'foot_model_real_joint_01.bvh'
partBvh = yf.readBvhFileAsBvh(partBvhFilePath)
partSkeleton = partBvh.toJointSkeleton(1., False)
bvh.replaceJointFromBvh('RightFoot', partBvh, SEGMENT_FOOT_MAG)
partBvh = yf.readBvhFileAsBvh(partBvhFilePath)
partBvh.mirror('YZ')
bvh.replaceJointFromBvh('LeftFoot', partBvh, SEGMENT_FOOT_MAG)
self.ref_motion = bvh.toJointMotion(1., False) # type: ym.JointMotion
if env_name == 'walk':
# self.ref_motion = self.ref_motion[20:]
self.ref_motion.translateByOffset([0., -0.03, 0.])
elif env_name == 'jump':
self.ref_motion = self.ref_motion[164:280]
elif env_name == 'walk_u_turn':
self.ref_motion = self.ref_motion[25:214]
self.ref_motion.translateByOffset([0., 0.03, 0.])
elif env_name == 'jump_whole':
self.ref_motion = self.ref_motion[315:966]
elif env_name == 'walk_u_turn_whole':
self.ref_motion.translateByOffset([0., 0.03, 0.])
self.ref_world = pydart.World(1. / 1200.,
"../data/wd2_without_ground.xml")
# self.ref_skel = self.ref_world.skeletons[1]
self.ref_skel = self.ref_world.skeletons[0]
# self.step_per_frame = round((1./self.world.time_step()) / self.ref_motion.fps)
self.step_per_frame = 40
self.rsi = True
self.w_p = 0.65
self.w_v = 0.1
self.w_e = 0.15
self.w_c = 0.1
self.exp_p = 2.
self.exp_v = 0.1
self.exp_e = 40.
self.exp_c = 10.
self.body_num = self.skel.num_bodynodes()
self.idx_e = [
self.skel.bodynode_index('LeftFoot'),
self.skel.bodynode_index('RightFoot'),
self.skel.bodynode_index('LeftForeArm'),
self.skel.bodynode_index('RightForeArm')
]
self.body_e = list(map(self.skel.body, self.idx_e))
self.ref_body_e = list(map(self.ref_skel.body, self.idx_e))
self.motion_len = len(self.ref_motion)
self.motion_time = len(self.ref_motion) / self.ref_motion.fps
self.time_offset = 0.
self.viewer = None
self.phase_frame = 0
self.prev_ref_q = np.zeros(self.ref_skel.num_dofs())
self.prev_ref_dq = np.zeros(self.ref_skel.num_dofs())
self.prev_ref_p_e_hat = np.asarray([
body.world_transform()[:3, 3] for body in self.ref_body_e
]).flatten()
self.prev_ref_com = self.ref_skel.com()
# setting for reward
self.reward_joint = list()
self.reward_joint.append('j_Hips')
self.reward_joint.append('j_RightUpLeg')
self.reward_joint.append('j_RightLeg')
self.reward_joint.append('j_LeftUpLeg')
self.reward_joint.append('j_LeftLeg')
self.reward_joint.append('j_Spine')
self.reward_joint.append('j_Spine1')
self.reward_joint.append('j_RightArm')
self.reward_joint.append('j_RightForeArm')
self.reward_joint.append('j_LeftArm')
self.reward_joint.append('j_LeftForeArm')
# setting for pd gain
self.foot_joint = list()
self.foot_joint.append('j_RightFoot')
self.foot_joint.append('j_RightFoot_foot_0_0')
self.foot_joint.append('j_RightFoot_foot_0_0_0')
self.foot_joint.append('j_RightFoot_foot_0_1_0')
self.foot_joint.append('j_RightFoot_foot_1_0')
self.foot_joint.append('j_LeftFoot')
self.foot_joint.append('j_LeftFoot_foot_0_0')
self.foot_joint.append('j_LeftFoot_foot_0_0_0')
self.foot_joint.append('j_LeftFoot_foot_0_1_0')
self.foot_joint.append('j_LeftFoot_foot_1_0')
state_num = 1 + (3 * 3 + 4) * self.body_num
action_num = self.skel.num_dofs() - 6 + len(self.foot_joint)
state_high = np.array([np.finfo(np.float32).max] * state_num)
action_high = np.array([pi * 10.] * action_num)
self.action_space = gym.spaces.Box(-action_high,
action_high,
dtype=np.float32)
self.observation_space = gym.spaces.Box(-state_high,
state_high,
dtype=np.float32)
def __init__(self, rnn_len):
skel_file = '../data/cmu_with_ground.xml'
self.world = pydart.World(1./1200., skel_file)
self.world.control_skel = self.world.skeletons[1]
self.skel = self.world.skeletons[1]
self.Kp, self.Kd = 800., 40.
self.pdc = PDController(self.skel, self.world.time_step(), 400., 40.)
self.ref_motion = yf.readBvhFile("../data/cmu_tpose.bvh")[:rnn_len]
self.ref_motion_len = rnn_len
self.ref_world = pydart.World(1./1200., skel_file)
self.ref_skel = self.ref_world.skeletons[1]
self.step_per_frame = 40
self.rsi = True
# self.w_g = 0.3
# self.w_p = 0.65 * .7
# self.w_v = 0.1 * .7
# self.w_e = 0.15 * .7
# self.w_c = 0.1 * .7
self.w_p = 0.65
self.w_v = 0.1
self.w_e = 0.15
self.w_c = 0.1
self.exp_g = 2.5
self.exp_p = 2.
self.exp_v = 0.1
self.exp_e = 40.
self.exp_c = 10.
self.body_num = self.skel.num_bodynodes()
self.idx_e = [self.skel.bodynode_index('LeftFoot'), self.skel.bodynode_index('RightFoot'),
self.skel.bodynode_index('LeftHand'), self.skel.bodynode_index('RightHand')]
self.body_e = list(map(self.skel.body, self.idx_e))
self.ref_body_e = list(map(self.ref_skel.body, self.idx_e))
self.motion_len = len(self.ref_motion)
self.motion_time = len(self.ref_motion) / self.ref_motion.fps
state_num = 2 + (3*3 + 4) * self.body_num + (3 + 4) * self.body_num
action_num = self.skel.num_dofs() - 6
state_high = np.array([np.finfo(np.float32).max] * state_num)
action_high = np.array([pi*10./2.] * action_num)
self.action_space = gym.spaces.Box(-action_high, action_high, dtype=np.float32)
self.observation_space = gym.spaces.Box(-state_high, state_high, dtype=np.float32)
self.viewer = None
self.phase_frame = 0
self.goals_in_world_frame = list()
self.goal = np.zeros(2)
self.prev_ref_q = np.zeros(self.ref_skel.num_dofs())
self.prev_ref_dq = np.zeros(self.ref_skel.num_dofs())
self.prev_ref_p_e_hat = np.asarray([body.world_transform()[:3, 3] for body in self.ref_body_e]).flatten()
self.prev_ref_com = self.ref_skel.com()
self.prev_goal = np.zeros(2)
self.frame = frame
threading.Thread.__init__(self)
#plt.show()
def run(self):
self.app.MainLoop()
def stop(self):
self.frame.Close()
skel_path = "/home/qfei/dart/data/sdf/atlas/"
if __name__ == '__main__':
pydart.init()
world = pydart.World(1 / 1000)
ground = world.add_skeleton(skel_path + "ground.urdf")
atlas = world.add_skeleton(skel_path + "atlas_v3_no_head_soft_feet.sdf")
skel = world.skeletons[1]
q = skel.q
q[0] = -0.5 * np.pi
q[4] = q[4] + 0.01
#print(len(q))
#input(len(q))
skel.set_positions(q)
controller = SC.Controller(skel, world)
controller.update(None)
""" Add damping force to the skeleton """
def __init__(self, skel):
self.skel = skel
def compute(self):
damping = -0.01 * self.skel.dq
damping[1::3] *= 0.1
return damping
if __name__ == '__main__':
import pydart2 as pydart
pydart.init(verbose=True)
print('pydart initialization OK')
world = pydart.World(0.0002, './data/skel/chain.skel')
print('pydart create_world OK')
skel = world.skeletons[0]
skel.q = (np.random.rand(skel.ndofs) - 0.5)
print('init pose = %s' % skel.q)
skel.controller = DampingController(skel)
pydart.gui.viewer.launch(world)
# # Or, you can manually create the window...
# win = pydart.gui.viewer.PydartWindow(world)
# win.camera_event(1)
# win.run_application()
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
import pydart2 as pydart
import numpy as np
from pydart2.gui.trackball import Trackball
from static_window import *
pydart.init()
print('pydart initialization OK')
world = pydart.World(1.0 / 500, "./assets/hopper_box.skel")
robot = world.skeletons[-1]
world.set_gravity([0.0, -10.0, 0.0])
world.set_collision_detector(world.BULLET_COLLISION_DETECTOR)
print('pydart create_world OK')
# floor_name = "./assets/ground.urdf"
# floor = world.add_skeleton(floor_name)
#
# filename = "./assets/hopper_my_box.urdf"
# robot = world.add_skeleton(filename)
# print('pydart add_skeleton OK')
# https://github.com/dartsim/dart/blob/v6.3.0/dart/constraint/ContactConstraint.cpp
for i in range(0, len(world.skeletons[0].bodynodes)):
world.skeletons[0].bodynodes[i].set_friction_coeff(1.0)
def init_sim(self):
self.querystep = 1
self.frameskip = 30
self.sim = pydart.World(1/STEP_PER_SEC)
self.sim.set_recording(False)
self.ground = self.sim.add_skeleton(skel_path+"ground.urdf")
self.model = self.sim.add_skeleton(skel_path+"atlas_v3_no_head.sdf")
self.data = None
self.viewer = None
self.skel = self.sim.skeletons[1]
q = self.skel.q
q[0] = -0.5*np.pi
q[4] = q[4]
q[7] = q[7] + 0.2
q[16] = q[16]+0.2
#q[29] = q[29]+0.3
# q[28] = q[28]-0.1
#q[3] = q[3]+1
#x방향으로 진행
self.initPos = q
self.skel.set_positions(q)
self.controller = SC.Controller(self.skel, self.sim)
app = wx.App(0)
frame = wx.Frame(None, -1, size=(1200,960))
#self.gui = Cgui.dartGui(frame,self.sim,self.controller)
self.gui = ModuleTest_drawMesh_new.dartGui(frame,self.sim,self.controller)
frame.Show(True)
self.guiThread = wxPythonThread(app,frame)
##gym Setting##
#lowhigh = np.array([-300,300])
#self.action_space = spaces.Box(low = 0, high = 1.5, shape=(5,))
#마지막2개, desiredSpeed
observation_spaces = np.concatenate([self.sim.skeletons[1].q[:4],self.sim.skeletons[1].q[6:],self.sim.skeletons[1].dq,[int(self.controller.mCurrentStateMachine.mCurrentState.mName),0]])
observation_spaces = np.zeros(len(observation_spaces))
#observation_spaces[0:]=500
self.observation_space = spaces.Box(observation_spaces, -observation_spaces)
#print(observation_spaces)
#input()
#print(self.sim.skeletons[1].com())
#plt.ion()
#input("init")
#self.plt_xAxis = np.array([0])
#self.plt_yAxis = np.array([0])
#self.draw_plot(self.plt_xAxis,self.plt_yAxis)
#plt.plot(self.plt_xAxis,self.plt_yAxis)
#plt.show()
self.step_counter = 0
self.r_foot = self.skel.body("r_foot")
self.l_foot = self.skel.body("l_foot")
self.l_uleg = self.skel.body("l_uleg")
self.r_uleg = self.skel.body("r_uleg")
self.l_lleg = self.skel.body("l_lleg")
self.r_lleg = self.skel.body("r_lleg")
self.l_leg_hpy = self.skel.joint("l_leg_hpy")
self.r_leg_hpy = self.skel.joint("r_leg_hpy")
self.pelvisX = self.skel.joint("back_bkx")
self.pelvisY = self.skel.joint("back_bky")
self.pelvisZ = self.skel.joint("back_bkz")
self.pelvis = self.skel.body("pelvis")
self.XveloQueue = CircularQueue(self.frameskip*2)
self.ZveloQueue = CircularQueue(self.frameskip*2)
self.actionSteps = 0
self.episodeTotalReward = 0
self.isrender = False
self.p_targetspeed = 0
self.targetspeed = DESIRED_MAX_SPEED
self.desiredSpeed = 0
self.step_per_sec = STEP_PER_SEC
self.mlinearActionRatio = 0
return damping
if __name__ == '__main__':
import pydart2 as pydart
from pydart2 import skeleton_builder
pydart.init(verbose=True)
print('pydart initialization OK')
#pydart.world__addCapsule()
#skel_empty = skeleton_builder.SkeletonBuilder("new human")
#world = pydart.World(0.0002, './data/skel/chain.skel')
world = pydart.World(0.0002, "EMPTY")
world.set_gravity([0, 0, 9.81])
print('pydart create_world OK')
#pydart.world__addCapsule()
skel = world.skeletons[0]
#skel_q_init = np.random.rand(skel.ndofs) - 0.5
skel_q_init = skel.ndofs * [0]
skel_q_init[3] = np.pi / 2
print skel_q_init
skel.set_positions(skel_q_init)
#print skel.root_bodynode()
#print skel.name
#from pydart2 import bodynode
#bn = bodynode.BodyNode(skel, 8)
self.skel = skel
self.target = None
self.Kp = np.array([0.0] * 6 + [400.0] * (self.skel.ndofs - 6))
self.Kd = np.array([0.0] * 6 + [40.0] * (self.skel.ndofs - 6))
def compute(self):
return -self.Kp * (self.skel.q - self.target) - self.Kd * self.skel.dq
if __name__ == '__main__':
print('Example: Simple Tracking')
pydart.init()
print('pydart initialization OK')
world = pydart.World(0.0005)
world.set_gravity([0.0, 0.0, 0.0])
print('World OK')
skel = world.add_skeleton('./data/sdf/atlas/atlas_v3_no_head.sdf')
skel.set_root_joint_to_trans_and_euler()
print('Skeleton = ' + str(skel))
# Set joint damping
for dof in skel.dofs:
dof.set_damping_coefficient(80.0)
# Set target pose
target = skel.positions()
target[("l_arm_shy", "r_arm_shy")] = -1.0, 1.0
target[("l_leg_hpy", "r_leg_hpy")] = -1.0, -1.0
def __init__(self,
env_name,
num_slaves=1,
eval_print=True,
eval_log=True,
visualize_only=False):
np.random.seed(seed=int(time.time()))
self.env_name = env_name
self.rnn_len = 500
self.env = HpDartEnv(self.rnn_len)
self.num_slaves = num_slaves
self.num_state = self.env.observation_space.shape[0]
self.num_action = self.env.action_space.shape[0]
self.num_epochs = 10
self.num_evaluation = 0
self.num_training = 0
self.gamma = 0.99
self.lb = 0.95
self.clip_ratio = 0.2
# self.buffer_size = 2048
# self.batch_size = 128
self.buffer_size = 16384
self.batch_size = 1024
self.replay_buffer = ReplayBuffer(10000)
self.total_episodes = []
self.model = Model(self.num_state, self.num_action,
(256, 256, 128)).float()
self.optimizer = optim.Adam(self.model.parameters(), lr=1E-4)
# self.optimizer = optim.Adam(self.model.parameters(), lr=7E-4)
self.w_entropy = 0.0
self.saved = False
self.save_directory = self.env_name + '_' + 'model_' + time.strftime(
"%m%d%H%M") + '/'
if not self.saved and not os.path.exists(
self.save_directory) and not visualize_only:
os.makedirs(self.save_directory)
self.saved = True
self.log_file = None
if not visualize_only:
self.log_file = open(self.save_directory + 'log.txt', 'w')
self.eval_print = eval_print
self.eval_log = eval_log
self.state_sender = [] # type: list[Connection]
self.result_sender = [] # type: list[Connection]
self.state_receiver = [] # type: list[Connection]
self.result_receiver = [] # type: list[Connection]
self.action_sender = [] # type: list[Connection]
self.reset_sender = [] # type: list[Connection]
self.motion_sender = [] # type: list[Connection]
self.envs = [] # type: list[Process]
self.ik_world = pydart.World(1. / 1200., '../data/cmu_with_ground.xml')
self.ik_skel = self.ik_world.skeletons[1]
self.rnn = None # type: RNNController
self.RNN_MOTION_SCALE = 0.01
self.rnn_joint_list = [
"Head", "Hips", "LHipJoint", "LeftArm", "LeftFoot", "LeftForeArm",
"LeftHand", "LeftLeg", "LeftShoulder", "LeftToeBase", "LeftUpLeg",
"LowerBack", "Neck", "Neck1", "RHipJoint", "RightArm", "RightFoot",
"RightForeArm", "RightHand", "RightLeg", "RightShoulder",
"RightToeBase", "RightUpLeg", "Spine", "Spine1"
]
self.rnn_target_update_prob = 2. / self.rnn_len
self.ik = DartIk(self.ik_skel)
self.qs = list()
self.replace_motion_num = self.rnn_len // 2
self.goal_in_world_frame = np.array((5., 0., 0.))
self.init_envs()
# for evaluation
self.last_reward = 0.
self.last_steps = 0
def __init__(self):
joint_names = [
'pelvis', 'leftThigh', 'rightThigh', 'spine', 'leftCalf',
'rightCalf', 'spine1', 'leftFoot', 'rightFoot', 'spine2', 'neck',
'leftShoulder', 'rightShoulder', 'head', 'leftUpperArm',
'rightUpperArm', 'leftForeArm', 'rightForeArm', 'leftHand',
'rightHand'
]
red_parent_ref = [
-1, 0, 0, 0, 1, 2, 3, 4, 5, 6, 9, 9, 9, 10, 11, 12, 14, 15, 16, 17
]
radii = [
0.12769461096148343, 0.09182758062402535, 0.09183365264505983,
0.128195873394974, 0.054201506359127107, 0.054204782651247056,
0.11441021953758712, 0.05134816175061338, 0.051367681280828095,
0.12840909729388708, 0.08568376456832136, 0.09205466746236975,
0.09207195048860886, 0.09417674072728775, 0.04450030029080607,
0.04453954596015603, 0.03869968571382823, 0.03875571491372652,
0.02922814811888941, 0.0297973538063161
]
radii_med = [
0.1443921929584865, 0.09879753554393726, 0.09880958937889255,
0.14038170079741627, 0.05892499163491047, 0.05891232648197925,
0.13581042138687494, 0.04932037409824811, 0.049343780055406926,
0.1396929637576165, 0.0861958797422231, 0.09078340056685348,
0.09077927107473645, 0.09547074699086751, 0.05319511539915989,
0.0532042033919998, 0.041963113363422504, 0.04198389621382932,
0.02922814811888941, 0.0297973538063161
]
lengths = [
0.1331743261250832, 0.25341930101960214, 0.2534154318813515,
0.0726371962432468, 0.49113402834862235, 0.4911362895591663,
0.11898002646639905, 0.1501895966849679, 0.15021745070394746,
0.01754202138831551, 0.13059433732982026, 0.020496847079791814,
0.020512735422737065, 0.12692070216147233, 0.2900605373659263,
0.29364024694729224, 0.28099209587280943, 0.2945346732747498,
0.1390784801895961, 0.13718088501877865
]
lengths_med = [
0.11221012660894233, 0.21838745393591003, 0.21838652247014215,
0.04009886721901377, 0.4404043065827791, 0.4404005018107258,
0.03352836109003122, 0.14624878028636093, 0.14626475496405048,
0.09201591904274159, 0.10279722790210173, 0.03407651369086422,
0.03408094322130216, 0.11318665800279451, 0.25693470642127425,
0.2624116078945138, 0.26616023158348917, 0.2692486717111876,
0.1390784801895961, 0.13718088501877865
]
initial_rots = [[0., 0., 1.57079633], [0., 0., 3.14159265],
[0., 0., 3.14159265], [0., 0., 1.57079633],
[0., 0., 3.14159265], [0., 0., 3.14159265],
[0., 0., 1.57079633], [1.57079633, 0., 0.],
[1.57079633, 0., 0.], [0., 0., 1.57079633],
[0., 0., 0.], [0., 0., -1.57079633],
[0., 0., 1.57079633], [0., 0., 0.],
[0., 0., -1.57079633], [0., 0., 1.57079633],
[0., 0., -1.57079633], [0., 0., 1.57079633],
[0., 0., -1.57079633], [0., 0., 1.57079633]]
cap_offsets = [
[0.0, -0.04, 0.0],
[0.011169376037587928, -0.15284656946574843, 0.012903663900451865],
[
-0.008170654460734582, -0.14484365810893893,
0.009715733426505214
],
[-0.004528416367088686, 0.013605244090683206, 0.04478019087771228],
[
-0.010037273572433072, -0.23605176764955693,
-0.016171902309391355
],
[0.01309094764543177, -0.2308922510404945, -0.016202019999160024],
[
-0.00591252018941266, -0.01317568744365618,
-0.002010005222645302
],
[0.01094751596360935, -0.03146133114580548, 0.07573397320647444],
[
-0.012013303281514093, -0.033379105586368994,
0.07790374457555227
],
[
-0.0035521415314855153, 0.036178626564513906,
-0.0033071152353626437
],
[0.009242849296194584, 0.03218511724478738, 0.020464375843508388],
[0.07173393305388479, 0.03415988444216093, -0.021207023426983884],
[-0.07011977756584435, 0.03500467757275714, -0.01431098898502919],
[
0.0017877181471058897, -0.003604552239007318,
-0.013366571797805468
],
[0.1670164481950132, -0.01047385968513933, -0.00955946729325003],
[
-0.17489038653427352, -0.012404766368258179,
-0.01493462681539015
],
[0.13686313856829088, 0.012178408710815582, 0.009027286895816854],
[-0.14869936653600047, 0.012743380671612937, 0.010315014780206785],
[0.07295133648723572, 0.003332175704076991, 0.00477568727285764],
[
-0.06900674634442043, 0.0053072850629911975,
0.005648618042937342
]
]
joint_locs = [
[0.048575008136347936, 0.024119849828711848, 0.0],
[0.05603581582384387, -0.08435470924277266, -0.02125171045905388],
[
-0.056734287288310335, -0.09246840627063796,
-0.019865012003098714
],
[0.00478469354216688, 0.12540467056158064, -0.029426528753042178],
[0.05438855330810292, -0.43938729822764777, 0.010090597202908305],
[-0.05687942203278569, -0.4329298092709282, -0.002264318594297732],
[0.004335110280848386, 0.1424258678301017, 0.032243119346393],
[
-0.013017916423372161, -0.47630985208241616,
-0.03525983240121225
],
[0.01879643192824705, -0.47063802124032617, -0.031908165182390605],
[
-0.0037562386956846213, 0.057957653859279396,
-0.0026084214232746233
],
[
-0.012891345804552689, 0.23382254793308366,
-0.024546227595192645
],
[0.06810633105445495, 0.12066967354888963, -0.025617194319459552],
[-0.08312624481554046, 0.1213346306423565, -0.02646990522515696],
[0.010160433355839844, 0.11431142528985228, 0.06561347872079941],
[0.11340919931011699, 0.04872322320654052, -0.014010241744952277],
[
-0.10906087070470047, 0.050948984849806256,
-0.0034195487756392123
],
[0.2909446510941778, -0.020225692509760418, -0.016255943269017036],
[-0.2940904813191163, -0.024448930412666936, -0.02096928534590224],
[0.281254400830233, 0.011761597131565299, -0.011719547351094269],
[-0.2957180548455181, 0.014828733728343285, -0.00917540965007943],
[0.08686301274055253, -0.010243204490181151, -0.01588782879746012],
[
-0.09127255488333685, -0.008439567905351025,
-0.010547811064669982
]
]
########################################## SET UP DART ###########################################
pydart.init(verbose=True)
self.world = pydart.World(0.002, "EMPTY") #0.003, .0002
self.world.set_gravity([0, 0, GRAVITY]) #([0, 0, -9.81])
self.world.set_collision_detector(detector_type=2)
################## CREATE SKELETON FOR HUMAN, ADD TO WORLD, AND SET PARAMETERS ###################
self.world.add_empty_skeleton(_skel_name="human")
for count in range(NUM_CAPSULES):
cap_rad = radii[count]
cap_len = lengths[count]
cap_init_rot = initial_rots[count]
cap_offset = cap_offsets[count]
joint_loc = joint_locs[count]
if count == 0:
self.world.add_capsule(parent=int(red_parent_ref[count]),
radius=cap_rad,
length=cap_len,
cap_rot=cap_init_rot,
cap_offset=cap_offset,
joint_loc=joint_loc,
joint_type="FREE",
joint_name=joint_names[count])
elif count == 4 or count == 5:
self.world.add_capsule(parent=int(red_parent_ref[count]),
radius=cap_rad,
length=cap_len,
cap_rot=cap_init_rot,
cap_offset=cap_offset,
joint_loc=joint_loc,
joint_type="REVOLUTE_X",
joint_name=joint_names[count])
elif count == 16 or count == 17:
self.world.add_capsule(parent=int(red_parent_ref[count]),
radius=cap_rad,
length=cap_len,
cap_rot=cap_init_rot,
cap_offset=cap_offset,
joint_loc=joint_loc,
joint_type="REVOLUTE_Y",
joint_name=joint_names[count])
else:
self.world.add_capsule(parent=int(red_parent_ref[count]),
radius=cap_rad,
length=cap_len,
cap_rot=cap_init_rot,
cap_offset=cap_offset,
joint_loc=joint_loc,
joint_type="BALL",
joint_name=joint_names[count])
skel = self.world.add_built_skeleton(_skel_id=0, _skel_name="human")
skel.set_self_collision_check(True)
skel.set_adjacent_body_check(False)
skel.set_collision_filter(True)
#set parameters
#[-1.32726466 0.31681432 0.04725983 - 1.29570571 - 0.60504953 - 0.43173041]
skel = self.assign_initial_joint_angles(skel)
skel = self.assign_joint_rest_and_stiffness(skel)
skel = self.assign_joint_limits_and_damping(skel)
skel = self.assign_body_masses_and_inertia(skel, initial_rots, radii,
lengths, radii_med,
lengths_med)
#self.world.test_filter()
########################### CREATE SKELETON FOR FLOOR AND ADD TO WORLD ###########################
self.world.add_empty_skeleton(_skel_name="floor")
self.world.add_weld_box(
width=10.0,
length=10.0,
height=0.2,
joint_loc=[
0.0, 0.0, -STARTING_HEIGHT / DART_TO_FLEX_CONV / 2 - 0.05 - 2.0
],
box_rot=[0.0, 0.0, 0.0],
joint_name="floor") # -0.05
skel_floor = self.world.add_built_skeleton(_skel_id=1,
_skel_name="floor")
#now setup the open GL window
self.title = "GLUT Window"
self.window_size = (1280, 720)
self.scene = OpenGLScene(*self.window_size)
self.mouseLastPos = None
self.is_simulating = False
self.is_animating = False
self.frame_index = 0
self.capture_index = 0
self.force_application_count = 0
self.count = 0
def hard_reset(self):
self.world = pydart.World(1. / 1200., "../data/wd2_seg.xml")
self.world.control_skel = self.world.skeletons[1]
self.skel = self.world.skeletons[1]
return self.reset()
def hard_reset(self):
self.world = pydart.World(1. / 1200., self.dart_skel_file)
self.world.control_skel = self.world.skeletons[1]
self.skel = self.world.skeletons[1]
return self.reset()
return (amplitude * np.sin(period * self.skel.world.t + phase))
def compute(self):
self.target = self.update_target_poses()
# print("Position: ", self.skel.q)
# print("Xpos: ", self.skel.q['joint_2_pos_x'])
return -self.Kp * (self.skel.q - self.target) - self.Kd * self.skel.dq
if __name__ == '__main__':
print('Hello, PyDART!')
pydart.init()
print('pydart initialization OK')
world = pydart.World(0.0002, './data/skel/spider.skel')
try:
world.set_collision_detector(2)
except Exception as e:
print(
'Does not have ODE collision detector, reverted to bullet collision detector'
)
world.set_collision_detector(2)
print('pydart create_world OK')
spiderSkel = world.skeletons[1]
# print(quadripedSkel.q)
# bodynodes = inchwormSkel.bodynodes
# for node in bodynodes:
# print("friction: ", node.friction_coeff())
controller = Controller(spiderSkel)
def __init__(self, model_paths, frame_skip, observation_size, action_bounds, \
dt=0.002, obs_type="parameter", action_type="continuous", visualize=True, disableViewer=False,\
screen_width=80, screen_height=45):
assert obs_type in ('parameter', 'image')
assert action_type in ("continuous", "discrete")
print('pydart initialization OK')
self.viewer = None
if len(model_paths) < 1:
raise StandardError("At least one model file is needed.")
if isinstance(model_paths, str):
model_paths = [model_paths]
# convert everything to fullpath
full_paths = []
for model_path in model_paths:
if model_path.startswith("/"):
fullpath = model_path
else:
fullpath = os.path.join(os.path.dirname(__file__), "assets",
model_path)
if not path.exists(fullpath):
raise IOError("File %s does not exist" % fullpath)
full_paths.append(fullpath)
if full_paths[0][-5:] == '.skel':
self.dart_world = pydart.World(dt, full_paths[0])
else:
self.dart_world = pydart.World(dt)
for fullpath in full_paths:
self.dart_world.add_skeleton(fullpath)
self.robot_skeleton = self.dart_world.skeletons[
-1] # assume that the skeleton of interest is always the last one
for jt in range(0, len(self.robot_skeleton.joints)):
for dof in range(len(self.robot_skeleton.joints[jt].dofs)):
if self.robot_skeleton.joints[jt].has_position_limit(dof):
self.robot_skeleton.joints[jt].set_position_limit_enforced(
True)
self._obs_type = obs_type
self.frame_skip = frame_skip
self.visualize = visualize #Show the window or not
self.disableViewer = disableViewer
# random perturbation
self.add_perturbation = False
self.perturbation_parameters = [
0.05, 5, 2
] # probability, magnitude, bodyid, duration
self.perturbation_duration = 40
self.perturb_force = np.array([0, 0, 0])
#assert not done
self.obs_dim = observation_size
self.act_dim = len(action_bounds[0])
# for discrete instances, action_space should be defined in the subclass
if action_type == "continuous":
self.action_space = spaces.Box(action_bounds[1], action_bounds[0])
self.track_skeleton_id = -1 # track the last skeleton's com by default
# initialize the viewer, get the window size
# initial here instead of in _render
# in image learning
self.screen_width = screen_width
self.screen_height = screen_height
self._get_viewer()
# Give different observation space for different kind of envs
if self._obs_type == 'parameter':
high = np.inf * np.ones(self.obs_dim)
low = -high
self.observation_space = spaces.Box(low, high)
elif self._obs_type == 'image':
# Change to grayscale image later
self.observation_space = spaces.Box(low=0,
high=255,
shape=(self.screen_width,
self.screen_height))
else:
raise error.Error('Unrecognized observation type: {}'.format(
self._obs_type))
self._seed()
#self.viewer = None
self.metadata = {
'render.modes': ['human', 'rgb_array'],
'video.frames_per_second': int(np.round(1.0 / self.dt))
}
# limitations under the License.
import sys
import pydart2 as pydart
# if len(sys.argv) != 2:
# print("Usage: inspect_skeleton.py [*.urdf/*.sdf]")
# exit(0)
skel_path = "assets/Hopper/hopper_link1_1.urdf"
print("skeleton path = %s" % skel_path)
pydart.init()
print("pydart init OK")
world = pydart.World(1.0 / 1000.0)
print("World init OK")
world.g = [0.0, 0.0, -9.8]
print("gravity = %s" % str(world.g))
skel = world.add_skeleton(skel_path)
print("Skeleton add OK")
print('----------------------------------------')
print('[Basic information]')
print('\tmass = %.6f' % skel.m)
print('\t# DoFs = %s' % skel.ndofs)
print('[BodyNode]')
