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]')