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 main(): MOTION_ONLY = False pydart.init() env_name = 'multi' ppo = PPO(env_name, 1, visualize_only=True) if not MOTION_ONLY: ppo.LoadModel('model/' + env_name + '.pt') ppo.env.specify_motion_num(1) ppo.env.Resets(False) # viewer settings rd_contact_positions = [None] rd_contact_forces = [None] dart_world = ppo.env.world viewer = hsv.hpSimpleViewer(rect=(0, 0, 1200, 800), viewForceWnd=False) viewer.doc.addRenderer( 'MotionModel', yr.DartRenderer(ppo.env.ref_world, (150, 150, 255), yr.POLYGON_FILL)) viewer.doc.addRenderer( 'controlModel', yr.DartRenderer(dart_world, (255, 240, 255), yr.POLYGON_FILL)) viewer.doc.addRenderer( 'contact', yr.VectorsRenderer(rd_contact_forces, rd_contact_positions, (255, 0, 0))) def preCallback(frame): ppo.env.ref_skel.set_positions(ppo.env.ref_motion.get_q(frame)) def simulateCallback(frame): state = ppo.env.GetState(0) action_dist, _ = ppo.model(torch.tensor(state.reshape(1, -1)).float()) action = action_dist.loc.detach().numpy() res = ppo.env.Steps(action) # contact rendering contacts = ppo.env.world.collision_result.contacts del rd_contact_forces[:] del rd_contact_positions[:] for contact in contacts: rd_contact_forces.append(contact.f / 1000.) rd_contact_positions.append(contact.p) if MOTION_ONLY: viewer.setPreFrameCallback_Always(preCallback) else: viewer.setSimulateCallback(simulateCallback) viewer.setMaxFrame(len(ppo.env.ref_motion) - 1) viewer.startTimer(1 / ppo.env.ref_motion.fps) viewer.show() Fl.run()
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, 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 main(): from time import strftime pydart.init() args = common_arg_parser().parse_args() logger.configure(dir='./log'+strftime("%Y%m%d%H%M")+'/') model, env = train(num_timesteps=10000000, seed=args.seed) logger.log("Running trained model") obs = np.zeros((env.num_envs,) + env.observation_space.shape) obs[:] = env.reset() while True: actions = model.step(obs) res = env.step(actions) obs[:] = res[0] done = res[2] if done[0]: break
def __init__(self, runtime, hyperParam=None): pydart.init() #print('pydart initialization OK') self.runtime = runtime self.world = trainingWorldR650.trainingWorldR650(step=0.1, setGravity=False) #print('pydart create_world OK') self.skel = self.world.skeletons[ -1] #print(world.skeletons[-1]): [Skeleton(2): KR5sixxR650WP_description] self.hyperParams = random.random((5, 3)) self.fitness = -1
def main(): from fltk import Fl from PyCommon.modules.GUI import hpSimpleViewer as hsv from PyCommon.modules.Renderer import ysRenderer as yr pydart.init() args = mujoco_arg_parser().parse_args() logger.configure(dir="./run/") model, env = load(args.env, num_timesteps=10000000, seed=args.seed) logger.log("Running trained model") obs = np.zeros((env.num_envs, ) + env.observation_space.shape) obs[:] = env.reset() env.venv.envs[0].env.rsi = False dart_world = env.venv.envs[0].env.world viewer = hsv.hpSimpleViewer(viewForceWnd=False) viewer.doc.addRenderer( 'controlModel', yr.DartRenderer(dart_world, (150, 150, 255), yr.POLYGON_FILL)) def simulateCallback(frame): actions = model.step(obs) res = env.step(actions[0]) obs[:] = res[0] done = res[2] # if done[0]: # break # env.render() viewer.setSimulateCallback(simulateCallback) viewer.setMaxFrame(3000) viewer.startTimer(1 / 30.) viewer.show() Fl.run()
class DampingController(object): """ 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)
import tensorflow as tf from DartDeep.dart_env_v2_1 import HpDartEnv from DartDeep.tf.ppo_tf import * from DartDeep.tf.worker import Worker import pydart2 tf.reset_default_graph() global_episodes = tf.Variable(0, dtype=tf.int32, name='global_episodes', trainable=False) summary_writer = tf.summary.FileWriter('./summary_log/' + 'Walking') pydart2.init() env = HpDartEnv() chief = Worker('Walking', env, summary_writer, global_episodes, visualize=False, gamma=0.95, batch_size=128, a_lr=5e-5, c_lr=1e-2) with tf.Session() as sess: saver = tf.train.Saver(max_to_keep=5) sess.run(tf.global_variables_initializer()) chief.ppo.load_model(sess, saver) chief.process(sess, saver)
def init_world(self): pydart.init(verbose=False) print('pydart initialization OK') self.world = MyWorld(self.dt)
def main(): # np.set_printoptions(precision=4, linewidth=200) np.set_printoptions(precision=4, linewidth=1000, threshold=np.inf) #motion, mcfg, wcfg, stepsPerFrame, config = mit.create_vchain_5() motion, mcfg, wcfg, stepsPerFrame, config, frame_rate= mit.create_biped() #motion, mcfg, wcfg, stepsPerFrame, config = mit.create_jump_biped() frame_step_size = 1./frame_rate pydart.init() dartModel = cdm.DartModel(wcfg, motion[0], mcfg, DART_CONTACT_ON) dartMotionModel = cdm.DartModel(wcfg, motion[0], mcfg, DART_CONTACT_ON) # wcfg.lockingVel = 0.01 time_step = dartModel.world.time_step() # dartModel.initializeForwardDynamics() # dartModel.initializeHybridDynamics() #controlToMotionOffset = (1.5, -0.02, 0) controlToMotionOffset = (1.5, 0, 0) # dartModel.translateByOffset(controlToMotionOffset) totalDOF = dartModel.getTotalDOF() DOFs = dartModel.getDOFs() # parameter Kt = config['Kt']; Dt = config['Dt'] # tracking gain Kl = config['Kl']; Dl = config['Dl'] # linear balance gain Kh = config['Kh']; Dh = config['Dh'] # angular balance gain Ks = config['Ks']; Ds = config['Ds'] # penalty force spring gain Bt = config['Bt'] Bl = config['Bl'] Bh = config['Bh'] w = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap']) supL = motion[0].skeleton.getJointIndex(config['supLink1']) supR = motion[0].skeleton.getJointIndex(config['supLink2']) selectedBody = motion[0].skeleton.getJointIndex(config['end']) constBody = motion[0].skeleton.getJointIndex('RightFoot') # momentum matrix linkMasses = dartModel.getBodyMasses() print([body.name for body in dartModel.skeleton.bodynodes]) print(linkMasses) totalMass = dartModel.getTotalMass() TO = ymt.make_TO(linkMasses) dTO = ymt.make_dTO(len(linkMasses)) # optimization problem = yac.LSE(totalDOF, 12) #a_sup = (0,0,0, 0,0,0) #ori #a_sup = (0,0,0, 0,0,0) #L a_supL = (0,0,0, 0,0,0) a_supR = (0,0,0, 0,0,0) a_sup_2 = (0,0,0, 0,0,0, 0,0,0, 0,0,0) CP_old = [mm.v3(0.,0.,0.)] CP_des = [None] dCP_des = [np.zeros(3)] # penalty method # bodyIDsToCheck = range(dartModel.getBodyNum()) bodyIDsToCheck = [supL, supR] #mus = [1.]*len(bodyIDsToCheck) mus = [.5]*len(bodyIDsToCheck) # flat data structure ddth_des_flat = ype.makeFlatList(totalDOF) dth_flat = ype.makeFlatList(totalDOF) ddth_sol = ype.makeNestedList(DOFs) # viewer rd_footCenter = [None] rd_footCenterL = [None] rd_footCenterR = [None] rd_CM_plane = [None] rd_CM = [None] rd_CP = [None] rd_CP_des = [None] rd_dL_des_plane = [None] rd_dH_des = [None] rd_grf_des = [None] rd_exf_des = [None] rd_exfen_des = [None] rd_root_des = [None] rd_CF = [None] rd_CF_pos = [None] rootPos = [None] selectedBodyId = [selectedBody] extraForce = [None] extraForcePos = [None] rightFootVectorX = [None] rightFootVectorY = [None] rightFootVectorZ = [None] rightFootPos = [None] rightVectorX = [None] rightVectorY = [None] rightVectorZ = [None] rightPos = [None] viewer = hsv.hpSimpleViewer(viewForceWnd=False) # viewer.record(False) # viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,255,255), yr.LINK_BONE)) viewer.doc.addObject('motion', motion) viewer.doc.addRenderer('motionModel', yr.DartModelRenderer(dartMotionModel, (150,150,255), yr.POLYGON_FILL)) viewer.doc.addRenderer('controlModel', yr.DartRenderer(dartModel.world, (255,240,255), yr.POLYGON_FILL)) viewer.doc.addRenderer('rd_footCenter', yr.PointsRenderer(rd_footCenter)) viewer.doc.addRenderer('rd_CM_plane', yr.PointsRenderer(rd_CM_plane, (255,255,0))) viewer.doc.addRenderer('rd_CP', yr.PointsRenderer(rd_CP, (0,255,0))) viewer.doc.addRenderer('rd_CP_des', yr.PointsRenderer(rd_CP_des, (255,0,255))) viewer.doc.addRenderer('rd_dL_des_plane', yr.VectorsRenderer(rd_dL_des_plane, rd_CM, (255,255,0))) viewer.doc.addRenderer('rd_dH_des', yr.VectorsRenderer(rd_dH_des, rd_CM, (0,255,0))) #viewer.doc.addRenderer('rd_grf_des', yr.ForcesRenderer(rd_grf_des, rd_CP_des, (0,255,0), .001)) viewer.doc.addRenderer('rd_CF', yr.VectorsRenderer(rd_CF, rd_CF_pos, (255,0,0))) viewer.doc.addRenderer('extraForce', yr.VectorsRenderer(rd_exf_des, extraForcePos, (0,255,0))) viewer.doc.addRenderer('extraForceEnable', yr.VectorsRenderer(rd_exfen_des, extraForcePos, (255,0,0))) #viewer.doc.addRenderer('right_foot_oriX', yr.VectorsRenderer(rightFootVectorX, rightFootPos, (255,0,0))) #viewer.doc.addRenderer('right_foot_oriY', yr.VectorsRenderer(rightFootVectorY, rightFootPos, (0,255,0))) #viewer.doc.addRenderer('right_foot_oriZ', yr.VectorsRenderer(rightFootVectorZ, rightFootPos, (0,0,255))) viewer.doc.addRenderer('right_oriX', yr.VectorsRenderer(rightVectorX, rightPos, (255,0,0))) viewer.doc.addRenderer('right_oriY', yr.VectorsRenderer(rightVectorY, rightPos, (0,255,0))) viewer.doc.addRenderer('right_oriZ', yr.VectorsRenderer(rightVectorZ, rightPos, (0,0,255))) #success!! #initKt = 50 #initKl = 10.1 #initKh = 3.1 #initBl = .1 #initBh = .1 #initSupKt = 21.6 #initFm = 100.0 #success!! -- 2015.2.12. double stance #initKt = 50 #initKl = 37.1 #initKh = 41.8 #initBl = .1 #initBh = .13 #initSupKt = 21.6 #initFm = 165.0 #single stance #initKt = 25 #initKl = 80.1 #initKh = 10.8 #initBl = .1 #initBh = .13 #initSupKt = 21.6 #initFm = 50.0 #single stance -> double stance #initKt = 25 #initKl = 60. #initKh = 20. #initBl = .1 #initBh = .13 #initSupKt = 21.6 #initFm = 50.0 initKt = 25. # initKl = 11. # initKh = 22. initKl = 100. initKh = 100. initBl = .1 initBh = .13 initSupKt = 17. # initSupKt = 2.5 initFm = 50.0 initComX = 0. initComY = 0. initComZ = 0. viewer.objectInfoWnd.add1DSlider("Kt", 0., 300., 1., initKt) viewer.objectInfoWnd.add1DSlider("Kl", 0., 300., 1., initKl) viewer.objectInfoWnd.add1DSlider("Kh", 0., 300., 1., initKh) viewer.objectInfoWnd.add1DSlider("Bl", 0., 1., .001, initBl) viewer.objectInfoWnd.add1DSlider("Bh", 0., 1., .001, initBh) viewer.objectInfoWnd.add1DSlider("SupKt", 0., 100., 0.1, initSupKt) viewer.objectInfoWnd.add1DSlider("Fm", 0., 1000., 10., initFm) viewer.objectInfoWnd.add1DSlider("com X offset", -1., 1., 0.01, initComX) viewer.objectInfoWnd.add1DSlider("com Y offset", -1., 1., 0.01, initComY) viewer.objectInfoWnd.add1DSlider("com Z offset", -1., 1., 0.01, initComZ) viewer.force_on = False def viewer_SetForceState(object): viewer.force_on = True def viewer_GetForceState(): return viewer.force_on def viewer_ResetForceState(): viewer.force_on = False viewer.objectInfoWnd.addBtn('Force on', viewer_SetForceState) viewer_ResetForceState() offset = 60 viewer.objectInfoWnd.begin() viewer.objectInfoWnd.labelForceX = Fl_Value_Input(20, 30+offset*9, 40, 20, 'X') viewer.objectInfoWnd.labelForceX.value(0) viewer.objectInfoWnd.labelForceY = Fl_Value_Input(80, 30+offset*9, 40, 20, 'Y') viewer.objectInfoWnd.labelForceY.value(0) viewer.objectInfoWnd.labelForceZ = Fl_Value_Input(140, 30+offset*9, 40, 20, 'Z') viewer.objectInfoWnd.labelForceZ.value(1) viewer.objectInfoWnd.labelForceDur = Fl_Value_Input(220, 30+offset*9, 40, 20, 'Dur') viewer.objectInfoWnd.labelForceDur.value(0.1) viewer.objectInfoWnd.end() #self.sliderFm = Fl_Hor_Nice_Slider(10, 42+offset*6, 250, 10) pdcontroller = PDController(dartModel, dartModel.skeleton, wcfg.timeStep, Kt, Dt) def getParamVal(paramname): return viewer.objectInfoWnd.getVal(paramname) def getParamVals(paramnames): return (getParamVal(name) for name in paramnames) ik_solver = hikd.numIkSolver(dartMotionModel) body_num = dartModel.getBodyNum() # dJsys = np.zeros((6*body_num, totalDOF)) # dJsupL = np.zeros((6, totalDOF)) # dJsupR = np.zeros((6, totalDOF)) # Jpre = [np.zeros((6*body_num, totalDOF)), np.zeros((6, totalDOF)), np.zeros((6, totalDOF))] extendedFootName = ['Foot'] lIDdic = {'Left'+name: motion[0].skeleton.getJointIndex('Left'+name) for name in extendedFootName} rIDdic = {'Right'+name: motion[0].skeleton.getJointIndex('Right'+name) for name in extendedFootName} lIDlist = [motion[0].skeleton.getJointIndex('Left'+name) for name in extendedFootName] rIDlist = [motion[0].skeleton.getJointIndex('Right'+name) for name in extendedFootName] bodyIDs, contactPositions, contactPositionLocals, contactForces = [], [], [], [] ################################### # simulate ################################### def simulateCallback(frame): # print() # print(dartModel.getJointVelocityGlobal(0)) # print(dartModel.getDOFVelocities()[0]) # print(dartModel.get_dq()[:6]) dartMotionModel.update(motion[frame]) global g_initFlag global forceShowTime global preFootCenter global maxContactChangeCount global contactChangeCount global contact global contactChangeType # print('contactstate:', contact, contactChangeCount) Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals(['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt']) Dt = 2.*(Kt**.5) Dl = (Kl**.5) Dh = (Kh**.5) dt_sup = 2.*(kt_sup**.5) # Dt = .2*(Kt**.5) # Dl = .2*(Kl**.5) # Dh = .2*(Kh**.5) # dt_sup = .2*(kt_sup**.5) pdcontroller.setKpKd(Kt, Dt) footHeight = dartModel.getBody(supL).shapenodes[0].shape.size()[1]/2. doubleTosingleOffset = 0.15 singleTodoubleOffset = 0.30 #doubleTosingleOffset = 0.09 doubleTosingleVelOffset = 0.0 com_offset_x, com_offset_y, com_offset_z = getParamVals(['com X offset', 'com Y offset', 'com Z offset']) footOffset = np.array((com_offset_x, com_offset_y, com_offset_z)) des_com = dartMotionModel.getCOM() + footOffset footCenterL = dartMotionModel.getBodyPositionGlobal(supL) footCenterR = dartMotionModel.getBodyPositionGlobal(supR) footBodyOriL = dartMotionModel.getBodyOrientationGlobal(supL) footBodyOriR = dartMotionModel.getBodyOrientationGlobal(supR) torso_pos = dartMotionModel.getBodyPositionGlobal(4) torso_ori = dartMotionModel.getBodyOrientationGlobal(4) # tracking # th_r = motion.getDOFPositions(frame) th_r = dartMotionModel.getDOFPositions() th = dartModel.getDOFPositions() th_r_flat = dartMotionModel.get_q() # dth_r = motion.getDOFVelocities(frame) dth = dartModel.getDOFVelocities() # ddth_r = motion.getDOFAccelerations(frame) # ddth_des = yct.getDesiredDOFAccelerations(th_r, th, None, dth, None, Kt, Dt) dth_flat = dartModel.get_dq() # dth_flat = np.concatenate(dth) # ddth_des_flat = pdcontroller.compute(dartMotionModel.get_q()) # ddth_des_flat = pdcontroller.compute(th_r) ddth_des_flat = pdcontroller.compute_flat(th_r_flat) # ype.flatten(ddth_des, ddth_des_flat) # ype.flatten(dth, dth_flat) ################################################# # jacobian ################################################# contact_des_ids = [dartModel.skeleton.bodynode_index("LeftFoot")] contact_ids = list() # temp idx for balancing contact_ids.extend(contact_des_ids) contact_joint_ori = list(map(dartModel.getJointOrientationGlobal, contact_ids)) contact_joint_pos = list(map(dartModel.getJointPositionGlobal, contact_ids)) contact_body_ori = list(map(dartModel.getBodyOrientationGlobal, contact_ids)) contact_body_pos = list(map(dartModel.getBodyPositionGlobal, contact_ids)) contact_body_vel = list(map(dartModel.getBodyVelocityGlobal, contact_ids)) contact_body_angvel = list(map(dartModel.getBodyAngVelocityGlobal, contact_ids)) ref_joint_ori = list(map(motion[frame].getJointOrientationGlobal, contact_ids)) ref_joint_pos = list(map(motion[frame].getJointPositionGlobal, contact_ids)) ref_joint_vel = [motion.getJointVelocityGlobal(joint_idx, frame) for joint_idx in contact_ids] ref_joint_angvel = [motion.getJointAngVelocityGlobal(joint_idx, frame) for joint_idx in contact_ids] ref_body_ori = list(map(dartMotionModel.getBodyOrientationGlobal, contact_ids)) ref_body_pos = list(map(dartMotionModel.getBodyPositionGlobal, contact_ids)) for idx in range(len(ref_body_pos)): ref_body_pos[idx] = dartModel.skeleton.body("RightFoot").shapenodes[0].shape.size()[1]/2. # ref_body_vel = list(map(controlModel.getBodyVelocityGlobal, contact_ids)) ref_body_angvel = [motion.getJointAngVelocityGlobal(joint_idx, frame) for joint_idx in contact_ids] ref_body_vel = [ref_joint_vel[i] + np.cross(ref_joint_angvel[i], ref_body_pos[i] - ref_joint_pos[i]) for i in range(len(ref_joint_vel))] is_contact = [1] * len(contact_ids) contact_right = len(set(contact_des_ids).intersection(rIDlist)) > 0 contact_left = len(set(contact_des_ids).intersection(lIDlist)) > 0 footOriL = dartModel.getJointOrientationGlobal(supL) footOriR = dartModel.getJointOrientationGlobal(supR) footCenterL = dartModel.getBodyPositionGlobal(supL) footCenterR = dartModel.getBodyPositionGlobal(supR) footBodyOriL = dartModel.getBodyOrientationGlobal(supL) footBodyOriR = dartModel.getBodyOrientationGlobal(supR) footBodyVelL = dartModel.getBodyVelocityGlobal(supL) footBodyVelR = dartModel.getBodyVelocityGlobal(supR) footBodyAngVelL = dartModel.getBodyAngVelocityGlobal(supL) footBodyAngVelR = dartModel.getBodyAngVelocityGlobal(supR) refFootL = dartMotionModel.getBodyPositionGlobal(supL) refFootR = dartMotionModel.getBodyPositionGlobal(supR) # refFootAngVelL = motion.getJointAngVelocityGlobal(supL, frame) # refFootAngVelR = motion.getJointAngVelocityGlobal(supR, frame) refFootAngVelL = np.zeros(3) refFootAngVelR = np.zeros(3) refFootJointVelR = motion.getJointVelocityGlobal(supR, frame) refFootJointAngVelR = motion.getJointAngVelocityGlobal(supR, frame) refFootJointR = motion.getJointPositionGlobal(supR, frame) # refFootVelR = refFootJointVelR + np.cross(refFootJointAngVelR, (refFootR-refFootJointR)) refFootVelR = np.zeros(3) refFootJointVelL = motion.getJointVelocityGlobal(supL, frame) refFootJointAngVelL = motion.getJointAngVelocityGlobal(supL, frame) refFootJointL = motion.getJointPositionGlobal(supL, frame) # refFootVelL = refFootJointVelL + np.cross(refFootJointAngVelL, (refFootL-refFootJointL)) refFootVelL = np.zeros(3) contactR = 1 contactL = 1 # contMotionOffset = th[0][0] - th_r[0][0] contMotionOffset = dartModel.getBodyPositionGlobal(0) - dartMotionModel.getBodyPositionGlobal(0) contMotionOffset = np.zeros(3) linkPositions = dartModel.getBodyPositionsGlobal() linkVelocities = dartModel.getBodyVelocitiesGlobal() linkAngVelocities = dartModel.getBodyAngVelocitiesGlobal() linkInertias = dartModel.getBodyInertiasGlobal() CM = dartModel.skeleton.com() dCM = dartModel.skeleton.com_velocity() CM_plane = copy.copy(CM) CM_plane[1]=0. dCM_plane = copy.copy(dCM) dCM_plane[1]=0. P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM, linkInertias) dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses, linkVelocities, dCM, linkAngVelocities, linkInertias) #calculate jacobian body_num = dartModel.getBodyNum() Jsys = np.zeros((6*body_num, totalDOF)) dJsys = np.zeros((6*body_num, totalDOF)) Jsys_, dJsysdq = compute_J_dJdq(dartModel.skeleton) # dJsys = np.zeros((6*body_num, totalDOF)) for i in range(dartModel.getBodyNum()): Jsys[6*i:6*i+6, :] = dartModel.getBody(i).world_jacobian()[range(-3, 3), :] dJsys[6*i:6*i+6, :] = dartModel.getBody(i).world_jacobian_classic_deriv()[range(-3, 3), :] dJsysdq = np.dot(dJsys, dartModel.skeleton.dq) # print(Jsys_ - Jsys) # print(Jsys_.dot(dth_flat)) # print(Jsys.dot(dth_flat)) # print(dartModel.getBody(0).world_linear_velocity()) # print(np.dot(Jsys[:3, :3], Jsys[0:3, 3:6].T)) print('dq', np.asarray(dartModel.skeleton.dq)[6:9]) print('joint vel', dartModel.skeleton.joint(1).velocity()) # print('bjoint', mm.exp(dartModel.skeleton.q[6:9]).dot(get_bjoint_jacobian(dartModel.skeleton.q[6:9]).dot(np.asarray(dartModel.skeleton.dq)[6:9]))) # print('not bjoint', mm.exp(dartModel.skeleton.q[6:9]).dot(np.asarray(dartModel.skeleton.dq)[6:9])) print('frombody', dartModel.getJointOrientationGlobal(1).T.dot(dartModel.getJointAngVelocityGlobal(1) - dartModel.getJointAngVelocityGlobal(0))) print('ddq', np.asarray(dartModel.skeleton.ddq)[6:9]) # print('bjoint', mm.exp(dartModel.skeleton.q[6:9]).dot(get_bjoint_jacobian(dartModel.skeleton.q[6:9]).dot(np.asarray(dartModel.skeleton.dq)[6:9]))) # print('not bjoint', mm.exp(dartModel.skeleton.q[6:9]).dot(np.asarray(dartModel.skeleton.dq)[6:9])) bodybody = dartModel.skeleton.body(1) joint_trans = dartModel.skeleton.joint(1).get_world_frame_after_transform() joint_pos = bodybody.to_local(joint_trans[:3, 3]) print('com spati', dartModel.getJointOrientationGlobal(1).T.dot( dartModel.skeleton.body(1).world_angular_acceleration() - dartModel.skeleton.body(0).world_angular_acceleration())) J_contacts = [] # type: list[np.ndarray] dJ_contacts = [] # type: list[np.ndarray] for contact_id in contact_ids: J_contacts.append(Jsys[6*contact_id:6*contact_id + 6, :]) dJ_contacts.append(dJsysdq[6*contact_id:6*contact_id + 6]) #calculate footCenter footCenter = .5 * (footCenterL + footCenterR) + footOffset if contact == 2: footCenter = footCenterL.copy() + footOffset #elif contact == 1 or footCenterL[1] > doubleTosingleOffset/2: if contact == 1: footCenter = footCenterR.copy() + footOffset footCenter[1] = 0. if contactChangeCount > 0 and contactChangeType == 'StoD': # change footcenter gradually footCenter = preFootCenter + (maxContactChangeCount - contactChangeCount)*(footCenter-preFootCenter)/maxContactChangeCount preFootCenter = footCenter.copy() # linear momentum # CM_ref_plane = footCenter # dL_des_plane = Kl*totalMass*(CM_ref_plane - CM_plane) - Dl*totalMass*dCM_plane # dL_des_plane[1] = 0. CM_ref_plane = footCenter CM_ref_plane[1] = dartMotionModel.skeleton.com()[1] dL_des_plane = Kl*totalMass*(CM_ref_plane - CM) - Dl*totalMass*dCM # dL_des_plane[1] = 0. # angular momentum CP_ref = footCenter CP = yrp.getCP(contactPositions, contactForces) if CP_old[0] is None or CP is None: dCP = None else: dCP = (CP - CP_old[0])/frame_step_size CP_old[0] = CP CP_des[0] = None if CP is not None and dCP is not None: ddCP_des = Kh*(CP_ref - CP) - Dh*(dCP) CP_des[0] = CP + dCP * frame_step_size + .5 * ddCP_des*(frame_step_size**2) dH_des = mm.cross(CP_des[0] - CM, dL_des_plane + totalMass*mm.s2v(wcfg.gravity)) else: dH_des = None # set up equality constraint a_oris = list(map(mm.logSO3, [mm.getSO3FromVectors(np.dot(contact_body_ori[i], mm.unitY()), mm.unitY()) for i in range(len(contact_body_ori))])) KT_SUP = np.diag([kt_sup/10., kt_sup, kt_sup/10.]) a_sups = [np.append(np.dot(KT_SUP, (ref_body_pos[i] - contact_body_pos[i] + contMotionOffset)) - dt_sup * contact_body_vel[i], kt_sup*a_oris[i] - dt_sup * contact_body_angvel[i]) for i in range(len(a_oris))] # print(a_sups) # print(np.asarray(dartModel.skeleton.dq)[0:3]) # print(dartModel.getJointAngVelocityGlobal(0)) # print(dartModel.getJointAngVelocityLocal(0)) # momentum matrix RS = np.dot(P, Jsys) R, S = np.vsplit(RS, 2) # rs = np.dot((np.dot(dP, Jsys) + np.dot(P, dJsys)), dth_flat) rs = np.dot(dP, np.dot(Jsys, dth_flat)) + np.dot(P, dJsysdq) r_bias, s_bias = np.hsplit(rs, 2) ####################################################### # optimization ####################################################### #if contact == 2 and footCenterR[1] > doubleTosingleOffset/2: if contact == 2: config['weightMap']['RightUpLeg'] = .8 config['weightMap']['RightLeg'] = .8 config['weightMap']['RightFoot'] = .8 else: config['weightMap']['RightUpLeg'] = .1 config['weightMap']['RightLeg'] = .25 config['weightMap']['RightFoot'] = .2 #if contact == 1 and footCenterL[1] > doubleTosingleOffset/2: if contact == 1: config['weightMap']['LeftUpLeg'] = .8 config['weightMap']['LeftLeg'] = .8 config['weightMap']['LeftFoot'] = .8 else: config['weightMap']['LeftUpLeg'] = .1 config['weightMap']['LeftLeg'] = .25 config['weightMap']['LeftFoot'] = .2 # print('vel2', np.dot(dartModel.getJointOrientationGlobal(0).T, dartModel.skeleton.body(0).world_linear_velocity())) w = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap']) #if contact == 2: #mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1, q_bias1) mot.addTrackingTerms(problem, totalDOF, Bt, w, ddth_des_flat) mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias) if dH_des is not None: mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias) if True: for c_idx in range(len(contact_ids)): mot.addConstraint2(problem, totalDOF, J_contacts[c_idx], dJ_contacts[c_idx], a_sups[c_idx]) if contactChangeCount > 0: contactChangeCount -= 1 if contactChangeCount == 0: maxContactChangeCount = 30 contactChangeType = 0 r = problem.solve() problem.clear() ddth_sol_flat = np.asarray(r['x']) # ype.nested(r['x'], ddth_sol) # ddth_sol[:6] = np.zeros(6) rootPos[0] = dartModel.getBodyPositionGlobal(selectedBody) localPos = [[0, 0, 0]] inv_h = 1./time_step _bodyIDs, _contactPositions, _contactPositionLocals, _contactForces = [], [], [], [] for i in range(stepsPerFrame): # apply penalty force _ddq, _tau, _bodyIDs, _contactPositions, _contactPositionLocals, _contactForces = hqp.calc_QP(dartModel.skeleton, ddth_sol_flat, inv_h) # _bodyIDs, _contactPositions, _contactPositionLocals, _contactForces = dartModel.calcPenaltyForce(bodyIDsToCheck,mus, Ks, Ds) dartModel.applyPenaltyForce(_bodyIDs, _contactPositionLocals, _contactForces) dartModel.skeleton.set_forces(_tau) # dartModel.setDOFAccelerations(ddth_sol) if forceShowTime > viewer.objectInfoWnd.labelForceDur.value(): forceShowTime = 0 viewer_ResetForceState() forceforce = np.array([viewer.objectInfoWnd.labelForceX.value(), viewer.objectInfoWnd.labelForceY.value(), viewer.objectInfoWnd.labelForceZ.value()]) extraForce[0] = getParamVal('Fm') * mm.normalize2(forceforce) if viewer_GetForceState(): forceShowTime += wcfg.timeStep dartModel.applyPenaltyForce(selectedBodyId, localPos, extraForce) dartModel.step() del bodyIDs[:] del contactPositions[:] del contactPositionLocals[:] del contactForces[:] bodyIDs.extend(_bodyIDs) contactPositions.extend(_contactPositions) contactPositionLocals.extend(_contactPositionLocals) contactForces.extend(_contactForces) # rendering rightFootVectorX[0] = np.dot(footOriL, np.array([.1, 0, 0])) rightFootVectorY[0] = np.dot(footOriL, np.array([0, .1, 0])) rightFootVectorZ[0] = np.dot(footOriL, np.array([0, 0,.1])) rightFootPos[0] = footCenterL rightVectorX[0] = np.dot(footBodyOriL, np.array([.1,0,0])) rightVectorY[0] = np.dot(footBodyOriL, np.array([0,.1,0])) rightVectorZ[0] = np.dot(footBodyOriL, np.array([0,0,.1])) rightPos[0] = footCenterL + np.array([.1,0,0]) rd_footCenter[0] = footCenter rd_footCenterL[0] = footCenterL rd_footCenterR[0] = footCenterR rd_CM[0] = CM rd_CM_plane[0] = CM.copy() rd_CM_plane[0][1] = 0. if CP is not None and dCP is not None: rd_CP[0] = CP rd_CP_des[0] = CP_des[0] rd_dL_des_plane[0] = [dL_des_plane[0]/100, dL_des_plane[1]/100, dL_des_plane[2]/100] rd_dH_des[0] = dH_des rd_grf_des[0] = dL_des_plane - totalMass*mm.s2v(wcfg.gravity) rd_root_des[0] = rootPos[0] del rd_CF[:] del rd_CF_pos[:] for i in range(len(contactPositions)): rd_CF.append( contactForces[i]/100) rd_CF_pos.append(contactPositions[i].copy()) if viewer_GetForceState(): rd_exfen_des[0] = [extraForce[0][0]/100, extraForce[0][1]/100, extraForce[0][2]/100] rd_exf_des[0] = [0,0,0] else: rd_exf_des[0] = [extraForce[0][0]/100, extraForce[0][1]/100, extraForce[0][2]/100] rd_exfen_des[0] = [0,0,0] extraForcePos[0] = dartModel.getBodyPositionGlobal(selectedBody) viewer.setSimulateCallback(simulateCallback) viewer.startTimer(1/30.) viewer.show() Fl.run()
def main2(): import PyCommon.modules.Motion.ysMotionAnalysis as yma pydart.init() world = [ pydart.World(1. / 1200., "../DartDeep/data/woody_with_ground_v2.xml") for _ in range(2) ] motion_files = ['wd2_jump0.bvh'] motion_files = ['woody_walk_normal.bvh'] motion_files = ['walk_left_90degree.bvh'] motion = yf.readBvhFileAsBvh(motion_files[0]).toPmLinearMotion(1., False) # 70, 112, 156 right heel strike mg = hmg.MotionGraph() mg.add_motion(motion) mg.build() total_num = len(mg.transition_processed) # 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) hRef = 0.1 vRef = 0.4 lc = yma.getElementContactStates(motion, 'LeftFoot', hRef, vRef) rc = yma.getElementContactStates(motion, 'RightFoot', hRef, vRef) lindex = motion[0].skeleton.getElementIndex('LeftFoot') rindex = motion[0].skeleton.getElementIndex('RightFoot') def postCallback(frame): transition = mg.transition_processed[frame] world[0].skeletons[1].set_positions( motion.get_q(transition.motion_from_idx)) world[1].skeletons[1].set_positions( motion.get_q(transition.motion_to_idx)) print(frame, transition.motion_from_idx, transition.motion_to_idx, transition.dist) print(motion.getPosition(lindex)) print(lc[transition.motion_from_idx], rc[transition.motion_from_idx]) print(lc[transition.motion_to_idx], rc[transition.motion_to_idx]) viewer.setPreFrameCallback_Always(postCallback) viewer.setMaxFrame(total_num - 1) viewer.startTimer(1. / 30.) viewer.show() all_node_set = set() dot = Digraph() for i in range(len(mg.transition_processed)): all_node_set.add(mg.transition_processed[i].motion_from_idx) all_node_set.add(mg.transition_processed[i].motion_to_idx) all_node_list = list(all_node_set) all_node_list.sort() for i in all_node_list: dot.node(str(i)) all_edge_set = set() for i in range(len(all_node_list) - 1): all_edge_set.add((all_node_list[i], all_node_list[i + 1])) for i in range(len(mg.transition_processed)): all_edge_set.add((mg.transition_processed[i].motion_from_idx, mg.transition_processed[i].motion_to_idx)) for i in list(all_edge_set): dot.edge(str(i[0]), str(i[1])) # print(dot.source) dot.render('test', view=True) dot.pipe() Fl.run()
def main(fm=50., fv=np.array([0., 0., 50.]), ts=2., te=2.1): np.set_printoptions(precision=4, linewidth=200) # np.set_printoptions(precision=4, linewidth=1000, threshold=np.inf) motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped() frame_step_size = 1. / frame_rate pydart.init() dartModel = cdm.DartModel(wcfg, motion[0], mcfg, DART_CONTACT_ON) dartMotionModel = cdm.DartModel(wcfg, motion[0], mcfg, DART_CONTACT_ON) # wcfg.lockingVel = 0.01 dartModel.initializeHybridDynamics() #controlToMotionOffset = (1.5, -0.02, 0) controlToMotionOffset = (1.5, 0, 0) dartModel.translateByOffset(controlToMotionOffset) totalDOF = dartModel.getTotalDOF() DOFs = dartModel.getDOFs() # parameter Kt = 25. Dt = 5. Ks = config['Ks'] Ds = config['Ds'] w = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap']) supL = motion[0].skeleton.getJointIndex(config['supLink1']) supR = motion[0].skeleton.getJointIndex(config['supLink2']) selectedBody = motion[0].skeleton.getJointIndex(config['end']) constBody = motion[0].skeleton.getJointIndex('RightFoot') # momentum matrix linkMasses = dartModel.getBodyMasses() totalMass = dartModel.getTotalMass() dartModel.world.time() # penalty method # bodyIDsToCheck = range(dartModel.getBodyNum()) bodyIDsToCheck = [supL, supR] #mus = [1.]*len(bodyIDsToCheck) mus = [.5] * len(bodyIDsToCheck) # viewer selectedBodyId = [selectedBody] extraForce = [None] extraForcePos = [None] pdcontroller = PDController(dartModel, dartModel.skeleton, wcfg.timeStep, Kt, Dt) ################################### #simulate ################################### def simulateCallback(frame): dartMotionModel.update(motion[frame]) # tracking # th_r = motion.getDOFPositions(frame) th_r = dartMotionModel.getDOFPositions() th = dartModel.getDOFPositions() th_r_flat = dartMotionModel.get_q() dth_flat = dartModel.get_dq() ddth_des_flat = pdcontroller.compute_flat(th_r_flat) localPos = [[0, 0, 0]] for i in range(stepsPerFrame): # apply penalty force if not DART_CONTACT_ON: bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds) dartModel.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces) # dartModel.skeleton.set_accelerations(ddth_sol) dartModel.skeleton.set_accelerations(ddth_des_flat) # dartModel.skeleton.set_forces(np.zeros(totalDOF)) extraForce[0] = fm * fv if ts <= dartModel.world.time() <= te: dartModel.applyPenaltyForce(selectedBodyId, localPos, extraForce) dartModel.step() if DART_CONTACT_ON: bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.get_dart_contact_info( ) else: bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds)
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 __init__(self, render, STARTING_HEIGHT, input_flex_radius, input_flex_length, input_flex_mass, shiftSIDE=0.0, shiftUD=0.0): model_path = '/home/henry/git/SMPL_python_v.1.0.0/smpl/models/basicModel_f_lbs_10_207_0_v1.0.0.pkl' m = load_model(model_path) regs = np.load( '/home/henry/git/smplify_public/code/models/regressors_locked_normalized_male.npz' ) length_regs = regs['betas2lens'] rad_regs = regs['betas2rads'] betas = m.betas capsules_median = get_capsules(m, betas * 0, length_regs, rad_regs) capsules = get_capsules(m, betas, length_regs, rad_regs) joint_names = joint2name initial_rots = rots0 self.num_steps = 10000 self.render_dart = render self.ct = 0 self.num_dart_steps = 4 self.has_reset_velocity1 = False self.has_reset_velocity2 = False joint_ref = list(m.kintree_table[1]) #joints parent_ref = list(m.kintree_table[0]) #parent of each joint parent_ref[0] = -1 self.capsules = capsules pydart.init(verbose=True) print('pydart initialization OK') self.world = pydart.World( 0.0103 / 4, "EMPTY" ) #0.002 is what works well. 0.0103 is when the velocity aligns. thus flex is 0.0103/0.0020 = 5.15x more fast than dart self.world.set_gravity([0, 0, GRAVITY]) #([0, 0, -9.81]) self.world.set_collision_detector(detector_type=2) self.world.add_empty_skeleton(_skel_name="human") self.force_dir_list_prev = [[], [], [], [], [], [], [], [], [], [], [], [], [], [], [], [], [], [], [], []] self.pmat_idx_list_prev = [[], [], [], [], [], [], [], [], [], [], [], [], [], [], [], [], [], [], [], []] self.force_loc_list_prev = [[], [], [], [], [], [], [], [], [], [], [], [], [], [], [], [], [], [], [], []] joint_root_loc = np.asarray(np.transpose(capsules[0].t)[0]) self.step_num = 0 joint_locs = [] capsule_locs = [] joint_locs_abs = [] joint_locs_trans_abs = [] capsule_locs_abs = [] mJ = np.asarray(m.J) mJ_transformed = np.asarray(m.J_transformed) shift = [shiftSIDE, shiftUD, 0.0] red_joint_ref = joint_ref[0:20] #joints red_parent_ref = parent_ref[0:20] #parent of each joint red_parent_ref[10] = 9 #fix neck red_parent_ref[11] = 9 #fix l inner shoulder red_parent_ref[13] = 10 #fix head red_parent_ref[14] = 11 #fix l outer shoulder red_parent_ref[15] = 12 #fix r outer shoulder red_parent_ref[16] = 14 #fix l elbow red_parent_ref[17] = 15 #fix r elbow head_ref = [10, 13] leg_cap_ref = [1, 2, 4, 5] foot_ref = [7, 8] l_arm_ref = [11, 14, 16, 18] r_arm_ref = [12, 15, 17, 19] self.red_joint_ref = [joint_ref[0]] self.red_parent_ref = red_parent_ref #make lists of the locations of the joint locations and the smplify capsule initial ends for i in range(np.shape(mJ)[0]): if i == 0: joint_locs.append(list(mJ[0, :] - mJ[0, :] + shift)) joint_locs_abs.append(list(mJ[0, :] - mJ[0, :])) joint_locs_trans_abs.append( list(mJ_transformed[0, :] - mJ_transformed[0, :])) if i < 20: capsule_locs.append( list( np.asarray(np.transpose(capsules[i].t)[0]) - joint_root_loc)) capsule_locs_abs.append( list( np.asarray(np.transpose(capsules[i].t)[0]) - joint_root_loc)) print(capsule_locs_abs, "caps locs abs") else: joint_locs.append(list(mJ[i, :] - mJ[parent_ref[i], :])) joint_locs_abs.append(list(mJ[i, :] - mJ[0, :])) joint_locs_trans_abs.append( list(mJ_transformed[i, :] - mJ_transformed[0, :])) if i < 20: capsule_locs.append( list( np.asarray(np.transpose(capsules[i].t)[0]) - np.asarray( np.transpose(capsules[red_parent_ref[i]].t)[0]) )) capsule_locs_abs.append( list( np.asarray(np.transpose(capsules[i].t)[0]) - joint_root_loc)) capsule_locs_abs[i][0] += np.abs( float(capsules[0].length[0])) / 2 if i in [ 1, 2 ]: #shift over the legs relative to where the pelvis mid capsule is capsule_locs[i][0] += np.abs( float(capsules[0].length[0])) / 2 if i in [ 3, 6, 9 ]: #shift over the torso segments relative to their length and their parents length to match the mid capsule capsule_locs[i][0] -= ( np.abs(float(capsules[i].length[0])) - np.abs(float( capsules[red_parent_ref[i]].length[0]))) / 2 if i in [ 10, 11, 12 ]: #shift over the inner shoulders and neck to match the middle of the top spine capsule capsule_locs[i][0] += np.abs( float(capsules[red_parent_ref[i]].length[0])) / 2 if i in [ 3, 6, 9 ]: #shift over everything in the abs list to match the root capsule_locs_abs[i][0] -= np.abs( float(capsules[i].length[0])) / 2 del (joint_locs[10]) del (joint_locs[10]) del (joint_locs_abs[10]) del (joint_locs_abs[10]) self.joint_locs = joint_locs count = 0 root_joint_type = "FREE" self.cap_offsets = [] self.cap_init_rots = [] lowest_points = [] for capsule in capsules: print "************* Capsule No.", count, joint_names[ count], " joint ref: ", red_joint_ref[ count], " parent_ref: ", red_parent_ref[ count], " ****************" cap_rad = input_flex_radius cap_len = input_flex_length cap_init_rot = list(np.asarray(initial_rots[count])) joint_loc = joint_locs[count] joint_loc_abs = joint_locs_abs[count] capsule_loc = capsule_locs[count] capsule_loc_abs = capsule_locs_abs[count] cap_offset = [0., 0., 0.] if count in leg_cap_ref: cap_offset[1] = -cap_len / 2 if count in foot_ref: cap_offset[2] = cap_len / 2 if count in l_arm_ref: cap_offset[0] = cap_len / 2 if count in r_arm_ref: cap_offset[0] = -cap_len / 2 #if count in head_ref: cap_offset[1] = cap_len/2 cap_offset[0] += capsule_loc_abs[0] - joint_loc_abs[0] cap_offset[1] += capsule_loc_abs[1] - joint_loc_abs[1] - .04 cap_offset[2] += capsule_loc_abs[2] - joint_loc_abs[2] self.cap_offsets.append(np.asarray(cap_offset)) self.cap_init_rots.append(np.asarray(cap_init_rot)) 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=root_joint_type, 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]) lowest_points.append( np.asarray(joint_locs_trans_abs)[count, 2] - np.abs(float(input_flex_radius))) count += 1 break #print "pelvis cap", #print np.asarray(joint_locs_trans_abs)[:, 2] self.STARTING_HEIGHT = STARTING_HEIGHT - 0.0508 #add a floor-STARTING_HEIGHT / DART_TO_FLEX_CONV self.world.add_weld_box( width=10.0, length=10.0, height=0.2, joint_loc=[ 0.0, 0.0, -self.STARTING_HEIGHT / DART_TO_FLEX_CONV / 2 - 0.05 ], box_rot=[0.0, 0.0, 0.0], joint_name="floor") #-0.05 skel = self.world.add_built_skeleton(_skel_id=0, _skel_name="human") skel.set_self_collision_check(True) #weight the capsules appropriately volume = [] volume_median = [] for body_ct in range(NUM_CAPSULES): #give the capsules a weight propertional to their volume cap_rad = input_flex_radius cap_len = input_flex_length cap_rad_median = np.abs(float(capsules_median[body_ct].rad[0])) cap_len_median = np.abs(float(capsules_median[body_ct].length[0])) volume.append(np.pi * np.square(cap_rad) * (cap_rad * 4 / 3 + cap_len)) volume_median.append(np.pi * np.square(cap_rad_median) * (cap_rad_median * 4 / 3 + cap_len_median)) skel.bodynodes[0].set_mass(input_flex_mass) body_mass = 0.0 #set the mass moment of inertia matrices for body_ct in range(NUM_CAPSULES): radius = input_flex_radius length = input_flex_length radius2 = radius * radius length2 = length * length mass = skel.bodynodes[body_ct].m cap_init_rot = list(np.asarray(initial_rots[body_ct])) volumeCylinder = np.pi * radius2 * length volumeSphere = np.pi * radius * radius * radius * 4 / 3 density = mass / (volumeCylinder + volumeSphere) massCylinder = density * volumeCylinder massSphere = density * volumeSphere Ixx = massCylinder * (length2 / 12.0 + radius2 / 4.0) + massSphere * ( length2 + (3.0 / 8.0) * length * radius + (2.0 / 5.0) * radius2) Izz = massCylinder * (radius2 / 2.0) + massSphere * ( (2.0 / 5.0) * radius2) RotMatInit = LibDartSkel().eulerAnglesToRotationMatrix( [np.pi / 2, 0.0, 0.0]) RotMat = LibDartSkel().eulerAnglesToRotationMatrix(cap_init_rot) I = np.matmul(np.matmul(RotMatInit, RotMat), np.asarray([Ixx, Izz, Ixx])) Ixx = np.abs(I[0]) Iyy = np.abs(I[1]) Izz = np.abs(I[2]) #print body_ct, I skel.bodynodes[body_ct].set_inertia_entries(Ixx, Iyy, Izz) body_mass += skel.bodynodes[body_ct].m break print "Body mass is: ", body_mass, "kg" self.body_node = 9 #need to solve for the body node that corresponds to a force using flex. self.force = np.asarray([0.0, 100.0, 100.0]) self.offset_from_centroid = np.asarray([-0.15, 0.0, 0.0]) self.pmat_red_all = np.load( "/home/henry/git/volumetric_pose_gen/data/pmat_red.npy") self.force_dir_red_dart_all = np.load( "/home/henry/git/volumetric_pose_gen/data/force_dir_red.npy") for element in range(len(self.force_dir_red_dart_all)): self.force_dir_red_dart_all[element] = (np.multiply( np.asarray(self.force_dir_red_dart_all[element]), np.expand_dims(np.asarray(self.pmat_red_all[element]), axis=1))) self.force_loc_red_dart_all = np.load( "/home/henry/git/volumetric_pose_gen/data/force_loc_red.npy" ).tolist() self.nearest_capsule_list_all = np.load( "/home/henry/git/volumetric_pose_gen/data/nearest_capsule.npy" ).tolist() print('init pose = %s' % skel.q) skel.controller = DampingController(skel) #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 self.zi = [] self.b = [] self.a = [] for i in range(60): b, a = signal.butter(1, 0.05, analog=False) self.b.append(b) self.a.append(a) self.zi.append(signal.lfilter_zi(self.b[-1], self.a[-1])) self.ziF = [] self.bF = [] self.aF = [] for i in range(3): b, a = signal.butter(1, 0.05, analog=False) self.bF.append(b) self.aF.append(a) self.ziF.append(signal.lfilter_zi(self.bF[-1], self.aF[-1]))
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()
def main(): np.set_printoptions(precision=4, linewidth=200) # np.set_printoptions(precision=4, linewidth=1000, threshold=np.inf) pydart.init() dartModel = cdm.DartModel(None, None, None, None, 'cart_pole_blade.skel') dartMotionModel = cdm.DartModel(None, None, None, None, 'cart_pole_blade.skel') footIdlist = list( dartMotionModel.skeleton.body('h_' + name).index_in_skeleton() for name in ['blade_left', 'blade_right']) up_vec_in_each_link = dict() for foot_id in footIdlist: up_vec_in_each_link[ foot_id] = dartMotionModel.getBodyOrientationGlobal(foot_id)[1, :] pelvis_pos = dartMotionModel.skeleton.dof_indices( (["j_pelvis_pos_x", 'j_pelvis_pos_y', 'j_pelvis_pos_z'])) pelvis_x = dartMotionModel.skeleton.dof_indices((["j_pelvis_rot_x"])) pelvis = dartMotionModel.skeleton.dof_indices( (["j_pelvis_rot_y", "j_pelvis_rot_z"])) upper_body = dartMotionModel.skeleton.dof_indices( ["j_abdomen_1", "j_abdomen_2"]) right_leg = dartMotionModel.skeleton.dof_indices([ "j_thigh_right_x", "j_thigh_right_y", "j_thigh_right_z", "j_shin_right" ]) left_leg = dartMotionModel.skeleton.dof_indices( ["j_thigh_left_x", "j_thigh_left_y", "j_thigh_left_z", "j_shin_left"]) arms = dartMotionModel.skeleton.dof_indices( ["j_bicep_left_x", "j_bicep_right_x"]) foot = dartMotionModel.skeleton.dof_indices( ["j_heel_left_1", "j_heel_left_2", "j_heel_right_1", "j_heel_right_2"]) leg_y = dartMotionModel.skeleton.dof_indices( ["j_thigh_right_y", "j_thigh_left_y"]) INIT_ANGLE = 0.09 s0q = np.zeros(dartMotionModel.skeleton.ndofs) s0q[pelvis_pos] = 0., .92, 0. # s0q[pelvis] = 0., -0. # s0q[upper_body] = 0.3, -0. s0q[right_leg] = -0., -0., 0.9, -1.5 s0q[left_leg] = -INIT_ANGLE, 0., 0.0, 0.0 s0q[foot] = 0., INIT_ANGLE, 0., 0. # s0q[right_leg] = -0., -0., 0.2, -.4 # s0q[left_leg] = -0., 0., 0.2, -.4 # s0q[foot] = 0.2, 0., 0.2, 0. # s0q[leg_y] = -0.785, 0.785 s0q[arms] = 1.5, -1.5 dartModel.set_q(s0q) dartMotionModel.set_q(s0q) frame_step_size = 1. / 40. stepsPerFrame = 25 time_step = dartModel.world.time_step() # wcfg.lockingVel = 0.01 # dartModel.initializeHybridDynamics() #controlToMotionOffset = (1.5, -0.02, 0) controlToMotionOffset = (0, 0, 2.0) dartModel.translateByOffset(controlToMotionOffset) totalDOF = dartModel.getTotalDOF() DOFs = dartModel.getDOFs() # parameter Kt = 25. Dt = 2. * (Kt**.5) Kl = 100. Dl = 2. * (Kt**.5) Kh = 100. Dh = 2. * (Kt**.5) Ks = 20000. Ds = 2. * (Kt**.5) Bt = 1. Bl = 0.1 Bh = 0.13 supL = dartModel.skeleton.body('h_blade_left').index_in_skeleton() supR = dartModel.skeleton.body('h_blade_right').index_in_skeleton() selectedBody = dartModel.skeleton.body('h_head').index_in_skeleton() # momentum matrix linkMasses = dartModel.getBodyMasses() print([body.name for body in dartModel.skeleton.bodynodes]) print(linkMasses) totalMass = dartModel.getTotalMass() TO = ymt.make_TO(linkMasses) dTO = ymt.make_dTO(len(linkMasses)) # optimization problem = yac.LSE(totalDOF, 12) #a_sup = (0,0,0, 0,0,0) #ori #a_sup = (0,0,0, 0,0,0) #L a_supL = (0, 0, 0, 0, 0, 0) a_supR = (0, 0, 0, 0, 0, 0) a_sup_2 = (0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0) CP_old = [mm.v3(0., 0., 0.)] CP_des = [None] dCP_des = [np.zeros(3)] # penalty method # bodyIDsToCheck = range(dartModel.getBodyNum()) bodyIDsToCheck = [supL, supR] #mus = [1.]*len(bodyIDsToCheck) mus = [.5] * len(bodyIDsToCheck) # flat data structure # ddth_des_flat = ype.makeFlatList(totalDOF) # dth_flat = ype.makeFlatList(totalDOF) # ddth_sol = ype.makeNestedList(DOFs) config = dict() config['weightMap'] = { 'j_scapula_left': .2, 'j_bicep_left': .2, 'j_forearm_left': .2, 'j_hand_left': .2, 'j_scapula_right': .2, 'j_bicep_right': .2, 'j_forearm_right': .2, 'j_hand_right': .2, 'j_abdomen': .6, 'j_spine': .6, 'j_head': .6, 'j_heel_right': .2, 'j_heel_left': .2, 'j_pelvis': 0.5, 'j_thigh_left': 5., 'j_shin_left': .5, 'j_thigh_right': 5., 'j_shin_right': .5 } # viewer rd_footCenter = [None] rd_footCenterL = [None] rd_footCenterR = [None] rd_CM_plane = [None] rd_CM = [None] rd_CP = [None] rd_CP_des = [None] rd_dL_des_plane = [None] rd_dH_des = [None] rd_grf_des = [None] rd_exf_des = [None] rd_exfen_des = [None] rd_root_des = [None] rd_CF = [None] rd_CF_pos = [None] rootPos = [None] selectedBodyId = [selectedBody] extraForce = [None] extraForcePos = [None] rightFootVectorX = [None] rightFootVectorY = [None] rightFootVectorZ = [None] rightFootPos = [None] rightVectorX = [None] rightVectorY = [None] rightVectorZ = [None] rightPos = [None] # viewer = ysv.SimpleViewer() viewer = hsv.hpSimpleViewer(viewForceWnd=False) viewer.setMaxFrame(1000) #viewer.record(False) # viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,255,255), yr.LINK_BONE)) # viewer.doc.addObject('motion', motion) # viewer.doc.addRenderer('motionModel', cvr.VpModelRenderer(motionModel, (150,150,255), yr.POLYGON_FILL)) # viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_LINE)) #viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_FILL)) viewer.doc.addRenderer( 'motionModel', yr.DartRenderer(dartMotionModel.world, (255, 240, 255), yr.POLYGON_FILL)) viewer.doc.addRenderer( 'controlModel', yr.DartRenderer(dartModel.world, (150, 150, 255), yr.POLYGON_FILL)) viewer.doc.addRenderer('rd_footCenter', yr.PointsRenderer(rd_footCenter)) viewer.doc.addRenderer('rd_CM_plane', yr.PointsRenderer(rd_CM_plane, (255, 255, 0))) viewer.doc.addRenderer('rd_CP', yr.PointsRenderer(rd_CP, (0, 255, 0))) viewer.doc.addRenderer('rd_CP_des', yr.PointsRenderer(rd_CP_des, (255, 0, 255))) viewer.doc.addRenderer( 'rd_dL_des_plane', yr.VectorsRenderer(rd_dL_des_plane, rd_CM, (255, 255, 0))) viewer.doc.addRenderer('rd_dH_des', yr.VectorsRenderer(rd_dH_des, rd_CM, (0, 255, 0))) #viewer.doc.addRenderer('rd_grf_des', yr.ForcesRenderer(rd_grf_des, rd_CP_des, (0,255,0), .001)) viewer.doc.addRenderer('rd_CF', yr.VectorsRenderer(rd_CF, rd_CF_pos, (255, 0, 0))) viewer.doc.addRenderer( 'extraForce', yr.VectorsRenderer(rd_exf_des, extraForcePos, (0, 255, 0))) viewer.doc.addRenderer( 'extraForceEnable', yr.VectorsRenderer(rd_exfen_des, extraForcePos, (255, 0, 0))) #viewer.doc.addRenderer('right_foot_oriX', yr.VectorsRenderer(rightFootVectorX, rightFootPos, (255,0,0))) #viewer.doc.addRenderer('right_foot_oriY', yr.VectorsRenderer(rightFootVectorY, rightFootPos, (0,255,0))) #viewer.doc.addRenderer('right_foot_oriZ', yr.VectorsRenderer(rightFootVectorZ, rightFootPos, (0,0,255))) #viewer.doc.addRenderer('right_oriX', yr.VectorsRenderer(rightVectorX, rightPos, (255,0,0))) #viewer.doc.addRenderer('right_oriY', yr.VectorsRenderer(rightVectorY, rightPos, (0,255,0))) #viewer.doc.addRenderer('right_oriZ', yr.VectorsRenderer(rightVectorZ, rightPos, (0,0,255))) #success!! #initKt = 50 #initKl = 10.1 #initKh = 3.1 #initBl = .1 #initBh = .1 #initSupKt = 21.6 #initFm = 100.0 #success!! -- 2015.2.12. double stance #initKt = 50 #initKl = 37.1 #initKh = 41.8 #initBl = .1 #initBh = .13 #initSupKt = 21.6 #initFm = 165.0 #single stance #initKt = 25 #initKl = 80.1 #initKh = 10.8 #initBl = .1 #initBh = .13 #initSupKt = 21.6 #initFm = 50.0 #single stance -> double stance #initKt = 25 #initKl = 60. #initKh = 20. #initBl = .1 #initBh = .13 #initSupKt = 21.6 #initFm = 50.0 initKt = 25. # initKl = 11. # initKh = 22. initKl = 100. initKh = 100. initBl = .1 initBh = .13 initSupKt = 17. # initSupKt = 2.5 initFm = 50.0 initComX = 0. initComY = 0. initComZ = 0. viewer.objectInfoWnd.add1DSlider("Kt", 0., 300., 1., initKt) viewer.objectInfoWnd.add1DSlider("Kl", 0., 300., 1., initKl) viewer.objectInfoWnd.add1DSlider("Kh", 0., 300., 1., initKh) viewer.objectInfoWnd.add1DSlider("Bl", 0., 1., .001, initBl) viewer.objectInfoWnd.add1DSlider("Bh", 0., 1., .001, initBh) viewer.objectInfoWnd.add1DSlider("SupKt", 0., 100., 0.1, initSupKt) viewer.objectInfoWnd.add1DSlider("Fm", 0., 1000., 10., initFm) viewer.objectInfoWnd.add1DSlider("com X offset", -1., 1., 0.01, initComX) viewer.objectInfoWnd.add1DSlider("com Y offset", -1., 1., 0.01, initComY) viewer.objectInfoWnd.add1DSlider("com Z offset", -1., 1., 0.01, initComZ) viewer.force_on = False def viewer_SetForceState(object): viewer.force_on = True def viewer_GetForceState(): return viewer.force_on def viewer_ResetForceState(): viewer.force_on = False viewer.objectInfoWnd.addBtn('Force on', viewer_SetForceState) viewer_ResetForceState() offset = 60 viewer.objectInfoWnd.begin() viewer.objectInfoWnd.labelForceX = Fl_Value_Input(20, 30 + offset * 9, 40, 20, 'X') viewer.objectInfoWnd.labelForceX.value(0) viewer.objectInfoWnd.labelForceY = Fl_Value_Input(80, 30 + offset * 9, 40, 20, 'Y') viewer.objectInfoWnd.labelForceY.value(0) viewer.objectInfoWnd.labelForceZ = Fl_Value_Input(140, 30 + offset * 9, 40, 20, 'Z') viewer.objectInfoWnd.labelForceZ.value(1) viewer.objectInfoWnd.labelForceDur = Fl_Value_Input( 220, 30 + offset * 9, 40, 20, 'Dur') viewer.objectInfoWnd.labelForceDur.value(0.1) viewer.objectInfoWnd.end() #self.sliderFm = Fl_Hor_Nice_Slider(10, 42+offset*6, 250, 10) pdcontroller = PDController(dartModel, dartModel.skeleton, dartModel.world.time_step(), Kt, Dt) def getParamVal(paramname): return viewer.objectInfoWnd.getVal(paramname) def getParamVals(paramnames): return (getParamVal(name) for name in paramnames) ik_solver = hikd.numIkSolver(dartMotionModel) body_num = dartModel.getBodyNum() # dJsys = np.zeros((6*body_num, totalDOF)) # dJsupL = np.zeros((6, totalDOF)) # dJsupR = np.zeros((6, totalDOF)) # Jpre = [np.zeros((6*body_num, totalDOF)), np.zeros((6, totalDOF)), np.zeros((6, totalDOF))] ################################### #simulate ################################### bodyIDs, contactPositions, contactPositionLocals, contactForces = [], [], [], [] def simulateCallback(frame): # print() # print(dartModel.getJointVelocityGlobal(0)) # print(dartModel.getDOFVelocities()[0]) # print(dartModel.get_dq()[:6]) # dartMotionModel.update(motion[frame]) global g_initFlag global forceShowTime global preFootCenter global maxContactChangeCount global contactChangeCount global contact global contactChangeType # print('contactstate:', contact, contactChangeCount) Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals( ['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt']) Dt = 2. * (Kt**.5) Dl = (Kl**.5) Dh = (Kh**.5) dt_sup = 2. * (kt_sup**.5) # Dt = .2*(Kt**.5) # Dl = .2*(Kl**.5) # Dh = .2*(Kh**.5) # dt_sup = .2*(kt_sup**.5) pdcontroller.setKpKd(Kt, Dt) footHeight = dartModel.getBody(supL).shapenodes[0].shape.size()[1] / 2. doubleTosingleOffset = 0.15 singleTodoubleOffset = 0.30 com_offset_x, com_offset_y, com_offset_z = getParamVals( ['com X offset', 'com Y offset', 'com Z offset']) footOffset = np.array((com_offset_x, com_offset_y, com_offset_z)) # tracking # th_r = motion.getDOFPositions(frame) th_r = dartMotionModel.getDOFPositions() th = dartModel.getDOFPositions() th_r_flat = dartMotionModel.get_q() # dth_r = motion.getDOFVelocities(frame) # dth = dartModel.getDOFVelocities() # ddth_r = motion.getDOFAccelerations(frame) # ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt) dth_flat = dartModel.get_dq() # dth_flat = np.concatenate(dth) # ddth_des_flat = pdcontroller.compute(dartMotionModel.get_q()) # ddth_des_flat = pdcontroller.compute(th_r) ddth_des_flat = pdcontroller.compute_flat(th_r_flat) # ype.flatten(ddth_des, ddth_des_flat) # ype.flatten(dth, dth_flat) print(dartModel.skeleton.get_spd_tau(th_r_flat, Kt, Dt)) ################################################# # jacobian ################################################# footOriL = dartModel.getJointOrientationGlobal(supL) footOriR = dartModel.getJointOrientationGlobal(supR) footCenterL = dartModel.getBodyPositionGlobal(supL) footCenterR = dartModel.getBodyPositionGlobal(supR) footBodyOriL = dartModel.getBodyOrientationGlobal(supL) footBodyOriR = dartModel.getBodyOrientationGlobal(supR) footBodyVelL = dartModel.getBodyVelocityGlobal(supL) footBodyVelR = dartModel.getBodyVelocityGlobal(supR) footBodyAngVelL = dartModel.getBodyAngVelocityGlobal(supL) footBodyAngVelR = dartModel.getBodyAngVelocityGlobal(supR) refFootL = dartMotionModel.getBodyPositionGlobal(supL) refFootR = dartMotionModel.getBodyPositionGlobal(supR) # refFootAngVelL = motion.getJointAngVelocityGlobal(supL, frame) # refFootAngVelR = motion.getJointAngVelocityGlobal(supR, frame) refFootAngVelL = np.zeros(3) refFootAngVelR = np.zeros(3) refFootVelR = np.zeros(3) refFootVelL = np.zeros(3) contactR = 1 contactL = 1 if refFootVelR[1] < 0 and refFootVelR[1] * frame_step_size + refFootR[ 1] > singleTodoubleOffset: contactR = 0 if refFootVelL[1] < 0 and refFootVelL[1] * frame_step_size + refFootL[ 1] > singleTodoubleOffset: contactL = 0 if refFootVelR[1] > 0 and refFootVelR[1] * frame_step_size + refFootR[ 1] > doubleTosingleOffset: contactR = 0 if refFootVelL[1] > 0 and refFootVelL[1] * frame_step_size + refFootL[ 1] > doubleTosingleOffset: contactL = 0 # contactR = 0 # contMotionOffset = th[0][0] - th_r[0][0] # contMotionOffset = dartModel.getBodyPositionGlobal(0) - dartMotionModel.getBodyPositionGlobal(0) contMotionOffset = controlToMotionOffset linkPositions = dartModel.getBodyPositionsGlobal() linkVelocities = dartModel.getBodyVelocitiesGlobal() linkAngVelocities = dartModel.getBodyAngVelocitiesGlobal() linkInertias = dartModel.getBodyInertiasGlobal() CM = dartModel.skeleton.com() dCM = dartModel.skeleton.com_velocity() CM_plane = copy.copy(CM) CM_plane[1] = 0. dCM_plane = copy.copy(dCM) dCM_plane[1] = 0. P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM, linkInertias) dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses, linkVelocities, dCM, linkAngVelocities, linkInertias) #calculate contact state #if g_initFlag == 1 and contact == 1 and refFootR[1] < doubleTosingleOffset and footCenterR[1] < 0.08: if g_initFlag == 1: #contact state # 0: flying 1: right only 2: left only 3: double #if contact == 2 and refFootR[1] < doubleTosingleOffset: if contact == 2 and contactR == 1: contact = 3 maxContactChangeCount += 30 contactChangeCount += maxContactChangeCount contactChangeType = 'StoD' #elif contact == 3 and refFootL[1] < doubleTosingleOffset: elif contact == 1 and contactL == 1: contact = 3 maxContactChangeCount += 30 contactChangeCount += maxContactChangeCount contactChangeType = 'StoD' #elif contact == 3 and refFootR[1] > doubleTosingleOffset: elif contact == 3 and contactR == 0: contact = 2 contactChangeCount += maxContactChangeCount contactChangeType = 'DtoS' #elif contact == 3 and refFootL[1] > doubleTosingleOffset: elif contact == 3 and contactL == 0: contact = 1 contactChangeCount += maxContactChangeCount contactChangeType = 'DtoS' else: contact = 0 #if refFootR[1] < doubleTosingleOffset: if contactR == 1: contact += 1 #if refFootL[1] < doubleTosingleOffset: if contactL == 1: contact += 2 #initialization if g_initFlag == 0: softConstPoint = footCenterR.copy() footCenter = footCenterL + (footCenterR - footCenterL) / 2.0 footCenter[1] = 0. preFootCenter = footCenter.copy() #footToBodyFootRotL = np.dot(np.transpose(footOriL), footBodyOriL) #footToBodyFootRotR = np.dot(np.transpose(footOriR), footBodyOriR) # if refFootR[1] < doubleTosingleOffset: # contact +=1 # if refFootL[1] < doubleTosingleOffset: # contact +=2 if refFootR[1] < footHeight: contact += 1 if refFootL[1] < footHeight: contact += 2 g_initFlag = 1 contact = 2 # contact = 1 + 2 # calculate jacobian body_num = dartModel.getBodyNum() Jsys = np.zeros((6 * body_num, totalDOF)) dJsys = np.zeros((6 * body_num, totalDOF)) for i in range(dartModel.getBodyNum()): Jsys[6 * i:6 * i + 6, :] = dartModel.getBody(i).world_jacobian()[range(-3, 3), :] dJsys[6 * i:6 * i + 6, :] = dartModel.getBody( i).world_jacobian_classic_deriv()[range(-3, 3), :] JsupL = dartModel.getBody(supL).world_jacobian()[range(-3, 3), :] dJsupL = dartModel.getBody(supL).world_jacobian_classic_deriv()[ range(-3, 3), :] JsupR = dartModel.getBody(supR).world_jacobian()[range(-3, 3), :] dJsupR = dartModel.getBody(supR).world_jacobian_classic_deriv()[ range(-3, 3), :] # calculate footCenter footCenter = .5 * (footCenterL + footCenterR) + footOffset if contact == 2: footCenter = footCenterL.copy() + footOffset if contact == 1: footCenter = footCenterR.copy() + footOffset footCenter[1] = 0. footCenter[0] += 0.02 preFootCenter = footCenter.copy() # linear momentum # CM_ref_plane = footCenter.copy() # CM_ref_plane += np.array([0., 0.9, 0.]) # dL_des_plane = Kl*totalMass*(CM_ref_plane - CM_plane) - Dl*totalMass*dCM_plane # dL_des_plane[1] = 0. kl = np.diagflat([Kl * 5., Kl, Kl * 5.]) dl = np.diagflat([2.2 * Dl, Dl, 2.2 * Dl]) CM_ref = footCenter.copy() CM_ref[1] = dartMotionModel.getCOM()[1] - 0.1 # CM_ref += np.array((0., com_offset_y, 0.)) # dL_des_plane = Kl*totalMass*(CM_ref - CM) - Dl*totalMass*dCM dL_des_plane = kl.dot(totalMass * (CM_ref - CM)) - dl.dot(totalMass * dCM) # angular momentum CP_ref = footCenter CP = yrp.getCP(contactPositions, contactForces) if CP_old[0] is None or CP is None: dCP = None else: dCP = (CP - CP_old[0]) / frame_step_size CP_old[0] = CP CP_des[0] = None # if CP_des[0] is None: # CP_des[0] = footCenter if CP is not None and dCP is not None: ddCP_des = Kh * (CP_ref - CP) - Dh * (dCP) CP_des[0] = CP + dCP * frame_step_size + .5 * ddCP_des * ( frame_step_size**2) dH_des = np.cross( CP_des[0] - CM, dL_des_plane - totalMass * mm.s2v(dartModel.world.gravity())) # dH_des = np.cross(footCenter - CM, dL_des_plane - totalMass*mm.s2v(dartModel.world.gravity())) # H = np.dot(P, np.dot(Jsys, dth_flat)) # dH_des = -Kh * H[3:] else: dH_des = None # set up equality constraint a_oriL = mm.logSO3( mm.getSO3FromVectors(np.dot(footBodyOriL, np.array([0, 1, 0])), np.array([0, 1, 0]))) a_oriR = mm.logSO3( mm.getSO3FromVectors(np.dot(footBodyOriR, np.array([0, 1, 0])), np.array([0, 1, 0]))) footErrorL = refFootL.copy() footErrorL[1] = dartModel.getBody( supL).shapenodes[0].shape.size()[1] / 2. footErrorL += -footCenterL + contMotionOffset footErrorR = refFootR.copy() footErrorR[1] = dartModel.getBody( supR).shapenodes[0].shape.size()[1] / 2. footErrorR += -footCenterR + contMotionOffset a_supL = np.append( kt_sup * footErrorL + dt_sup * (refFootVelL - footBodyVelL), kt_sup * a_oriL + dt_sup * (refFootAngVelL - footBodyAngVelL)) a_supR = np.append( kt_sup * footErrorR + dt_sup * (refFootVelR - footBodyVelR), kt_sup * a_oriR + dt_sup * (refFootAngVelR - footBodyAngVelR)) # momentum matrix RS = np.dot(P, Jsys) R, S = np.vsplit(RS, 2) rs = np.dot((np.dot(dP, Jsys) + np.dot(P, dJsys)), dth_flat) r_bias, s_bias = np.hsplit(rs, 2) ####################################################### # optimization ####################################################### if LEG_FLEXIBLE: if contact == 2: config['weightMap']['j_thigh_right'] = .8 config['weightMap']['j_shin_right'] = .8 config['weightMap']['j_heel_right'] = .8 else: config['weightMap']['j_thigh_right'] = .1 config['weightMap']['j_shin_right'] = .25 config['weightMap']['j_heel_right'] = .2 if contact == 1: config['weightMap']['j_thigh_left'] = .8 config['weightMap']['j_shin_left'] = .8 config['weightMap']['j_heel_left'] = .8 else: config['weightMap']['j_thigh_left'] = .1 config['weightMap']['j_shin_left'] = .25 config['weightMap']['j_heel_left'] = .2 w = mot.getTrackingWeightDart(DOFs, dartModel.skeleton, config['weightMap']) mot.addTrackingTerms(problem, totalDOF, Bt, w, ddth_des_flat) mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias) if dH_des is not None: mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias) if contact & 1: mot.addConstraint(problem, totalDOF, JsupR, dJsupR, dth_flat, a_supR) if contact & 2: mot.addConstraint(problem, totalDOF, JsupL, dJsupL, dth_flat, a_supL) if contactChangeCount > 0: contactChangeCount -= 1 if contactChangeCount == 0: maxContactChangeCount = 30 contactChangeType = 0 r = problem.solve() problem.clear() # ype.nested(r['x'], ddth_sol) ddth_sol = np.asarray(r['x']) # ddth_sol[:6] = np.zeros(6) if dH_des is None: ddth_sol = ddth_des_flat rootPos[0] = dartModel.getBodyPositionGlobal(selectedBody) localPos = [[0, 0, 0]] inv_h = 1. / time_step _bodyIDs, _contactPositions, _contactPositionLocals, _contactForces = [], [], [], [] for iii in range(stepsPerFrame): _ddq, _tau, _bodyIDs, _contactPositions, _contactPositionLocals, _contactForces = hqp.calc_QP( dartModel.skeleton, ddth_sol, inv_h) # _ddq, _tau, _bodyIDs, _contactPositions, _contactPositionLocals, _contactForces = hqp.calc_QP(dartModel.skeleton, ddth_des_flat, inv_h) # print(frame, i, tau) dartModel.applyPenaltyForce(_bodyIDs, _contactPositionLocals, _contactForces) dartModel.skeleton.set_forces(_tau) if forceShowTime > viewer.objectInfoWnd.labelForceDur.value(): forceShowTime = 0 viewer_ResetForceState() forceforce = np.array([ viewer.objectInfoWnd.labelForceX.value(), viewer.objectInfoWnd.labelForceY.value(), viewer.objectInfoWnd.labelForceZ.value() ]) extraForce[0] = getParamVal('Fm') * mm.normalize2(forceforce) if viewer_GetForceState(): forceShowTime += time_step dartModel.applyPenaltyForce(selectedBodyId, localPos, extraForce) dartModel.step() del bodyIDs[:] del contactPositions[:] del contactPositions[:] del contactPositionLocals[:] del contactForces[:] bodyIDs.extend(_bodyIDs) contactPositions.extend(_contactPositions) contactPositionLocals.extend(_contactPositionLocals) contactForces.extend(_contactForces) # rendering rightFootVectorX[0] = np.dot(footOriL, np.array([.1, 0, 0])) rightFootVectorY[0] = np.dot(footOriL, np.array([0, .1, 0])) rightFootVectorZ[0] = np.dot(footOriL, np.array([0, 0, .1])) rightFootPos[0] = footCenterL rightVectorX[0] = np.dot(footBodyOriL, np.array([.1, 0, 0])) rightVectorY[0] = np.dot(footBodyOriL, np.array([0, .1, 0])) rightVectorZ[0] = np.dot(footBodyOriL, np.array([0, 0, .1])) rightPos[0] = footCenterL + np.array([.1, 0, 0]) rd_footCenter[0] = footCenter rd_footCenterL[0] = footCenterL rd_footCenterR[0] = footCenterR rd_CM[0] = CM rd_CM_plane[0] = CM.copy() rd_CM_plane[0][1] = 0. if CP is not None and dCP is not None: rd_CP[0] = CP rd_CP_des[0] = CP_des[0] rd_dL_des_plane[0] = [ dL_des_plane[0] / 100, dL_des_plane[1] / 100, dL_des_plane[2] / 100 ] rd_dH_des[0] = dH_des rd_grf_des[0] = dL_des_plane - totalMass * mm.s2v( dartModel.world.gravity()) rd_root_des[0] = rootPos[0] del rd_CF[:] del rd_CF_pos[:] for i in range(len(contactPositions)): rd_CF.append(contactForces[i] / 100) rd_CF_pos.append(contactPositions[i].copy()) if viewer_GetForceState(): rd_exfen_des[0] = [ extraForce[0][0] / 100, extraForce[0][1] / 100, extraForce[0][2] / 100 ] rd_exf_des[0] = [0, 0, 0] else: rd_exf_des[0] = [ extraForce[0][0] / 100, extraForce[0][1] / 100, extraForce[0][2] / 100 ] rd_exfen_des[0] = [0, 0, 0] extraForcePos[0] = dartModel.getBodyPositionGlobal(selectedBody) viewer.setSimulateCallback(simulateCallback) viewer.startTimer(1 / 30.) viewer.show() Fl.run()
def setUp(self): pydart.init(verbose=False)
def main(): MOTION_ONLY = False pydart.init() env_name = 'multi' ppo = PPO_MULTI(env_name, 0, visualize_only=True) if not MOTION_ONLY: ppo.LoadModel('model/param.pt') ppo.env.specify_motion_num(5) ppo.env.Resets(False) # viewer settings rd_contact_positions = [None] rd_contact_forces = [None] rd_targets = [None] dart_world = ppo.env.world viewer = hsv.hpSimpleViewer(rect=(0, 0, 1200, 800), viewForceWnd=False) # viewer = hsv.hpSimpleViewer(rect=[0, 0, 960+300, 1+1080+55], viewForceWnd=False) viewer.doc.addRenderer( 'MotionModel', yr.DartRenderer(ppo.env.ref_world, (150, 150, 255), yr.POLYGON_FILL)) viewer.doc.addRenderer( 'controlModel', yr.DartRenderer(dart_world, (255, 240, 255), yr.POLYGON_FILL)) viewer.doc.addRenderer( 'contact', yr.VectorsRenderer(rd_contact_forces, rd_contact_positions, (255, 0, 0))) viewer.doc.addRenderer('targets', yr.PointsRenderer(rd_targets, color=(0, 0, 0))) viewer.setMaxFrame(3000) cameraTargets = [None] * (viewer.getMaxFrame() + 1) def preCallback(frame): ppo.env.ref_skel.set_positions(ppo.env.ref_motion.get_q(frame)) def simulateCallback(frame): state = ppo.env.GetState(0) action_dist, _ = ppo.model(state.reshape(1, -1)) action = action_dist.loc.detach().numpy() res = ppo.env.Steps(action) del rd_targets[:] rd_targets.extend(ppo.env.goals_in_world_frame) # res = ppo.env.step_after_training(action[0]) if res[2]: print(frame, 'Done') ppo.env.reset() # contact rendering contacts = ppo.env.world.collision_result.contacts del rd_contact_forces[:] del rd_contact_positions[:] for contact in contacts: rd_contact_forces.append(contact.f / 1000.) rd_contact_positions.append(contact.p) def postFrameCallback_Always(frame): if cameraTargets[frame] is None: cameraTargets[frame] = np.asarray( dart_world.skel.body(0).to_world()) viewer.setCameraTarget(cameraTargets[frame]) if MOTION_ONLY: viewer.setPreFrameCallback_Always(preCallback) else: viewer.setSimulateCallback(simulateCallback) viewer.setPostFrameCallback_Always(postFrameCallback_Always) viewer.startTimer(1 / ppo.env.ref_motion.fps) viewer.show() Fl.run()
def setUp(self): pydart.init(verbose=False)
def main(): np.set_printoptions(precision=4, linewidth=200) # np.set_printoptions(precision=4, linewidth=1000, threshold=np.inf) pydart.init() dartModel = cdm.DartModel(None, None, None, None, 'cart_pole_blade.skel') dartMotionModel = cdm.DartModel(None, None, None, None, 'cart_pole_blade.skel') footIdlist = list( dartMotionModel.skeleton.body('h_' + name).index_in_skeleton() for name in ['blade_left', 'blade_right']) up_vec_in_each_link = dict() for foot_id in footIdlist: up_vec_in_each_link[ foot_id] = dartMotionModel.getBodyOrientationGlobal(foot_id)[1, :] pelvis_pos = dartMotionModel.skeleton.dof_indices( (["j_pelvis_pos_x", 'j_pelvis_pos_y', 'j_pelvis_pos_z'])) pelvis_x = dartMotionModel.skeleton.dof_indices((["j_pelvis_rot_x"])) pelvis = dartMotionModel.skeleton.dof_indices( (["j_pelvis_rot_y", "j_pelvis_rot_z"])) upper_body = dartMotionModel.skeleton.dof_indices( ["j_abdomen_1", "j_abdomen_2"]) right_leg = dartMotionModel.skeleton.dof_indices([ "j_thigh_right_x", "j_thigh_right_y", "j_thigh_right_z", "j_shin_right" ]) left_leg = dartMotionModel.skeleton.dof_indices( ["j_thigh_left_x", "j_thigh_left_y", "j_thigh_left_z", "j_shin_left"]) arms = dartMotionModel.skeleton.dof_indices( ["j_bicep_left_x", "j_bicep_right_x"]) foot = dartMotionModel.skeleton.dof_indices( ["j_heel_left_1", "j_heel_left_2", "j_heel_right_1", "j_heel_right_2"]) leg_y = dartMotionModel.skeleton.dof_indices( ["j_thigh_right_y", "j_thigh_left_y"]) s0q = np.zeros(dartMotionModel.skeleton.ndofs) s0q[pelvis_pos] = 0., .95, 0. # s0q[pelvis] = 0., -0. # s0q[upper_body] = 0.3, -0. s0q[right_leg] = -0., -0., 0.9, -1.5 # s0q[left_leg] = 0., 0., 0.0, -0.1 # s0q[leg_y] = -0.785, 0.785 s0q[arms] = 1.5, -1.5 dartModel.set_q(s0q) dartMotionModel.set_q(s0q) frame_step_size = 1. / 40. stepsPerFrame = 25 time_step = dartModel.world.time_step() # wcfg.lockingVel = 0.01 # dartModel.initializeHybridDynamics() #controlToMotionOffset = (1.5, -0.02, 0) controlToMotionOffset = (0, 0, 2.0) dartModel.translateByOffset(controlToMotionOffset) totalDOF = dartModel.getTotalDOF() DOFs = dartModel.getDOFs() # parameter Kt = 25. Dt = 2. * (Kt**.5) Kl = 100. Dl = 2. * (Kt**.5) Kh = 100. Dh = 2. * (Kt**.5) Ks = 20000. Ds = 2. * (Kt**.5) Bt = 1. Bl = 0.1 Bh = 0.13 supL = dartModel.skeleton.body('h_blade_left').index_in_skeleton() supR = dartModel.skeleton.body('h_blade_right').index_in_skeleton() selectedBody = dartModel.skeleton.body('h_head').index_in_skeleton() # momentum matrix linkMasses = dartModel.getBodyMasses() print([body.name for body in dartModel.skeleton.bodynodes]) print(linkMasses) totalMass = dartModel.getTotalMass() TO = ymt.make_TO(linkMasses) dTO = ymt.make_dTO(len(linkMasses)) # optimization problem = yac.LSE(totalDOF, 12) #a_sup = (0,0,0, 0,0,0) #ori #a_sup = (0,0,0, 0,0,0) #L a_supL = (0, 0, 0, 0, 0, 0) a_supR = (0, 0, 0, 0, 0, 0) a_sup_2 = (0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0) CP_old = [mm.v3(0., 0., 0.)] CP_des = [None] dCP_des = [np.zeros(3)] # penalty method # bodyIDsToCheck = range(dartModel.getBodyNum()) bodyIDsToCheck = [supL, supR] #mus = [1.]*len(bodyIDsToCheck) mus = [.5] * len(bodyIDsToCheck) # flat data structure # ddth_des_flat = ype.makeFlatList(totalDOF) # dth_flat = ype.makeFlatList(totalDOF) # ddth_sol = ype.makeNestedList(DOFs) config = dict() config['weightMap'] = { 'j_scapula_left': .2, 'j_bicep_left': .2, 'j_forearm_left': .2, 'j_hand_left': .2, 'j_scapula_right': .2, 'j_bicep_right': .2, 'j_forearm_right': .2, 'j_hand_right': .2, 'j_abdomen': .6, 'j_spine': .6, 'j_head': .6, 'j_heel_right': .2, 'j_heel_left': .2, 'j_pelvis': 0.5, 'j_thigh_left': .1, 'j_shin_left': .3, 'j_thigh_right': .1, 'j_shin_right': .3 } # viewer rd_footCenter = [None] rd_footCenterL = [None] rd_footCenterR = [None] rd_CM_plane = [None] rd_CM = [None] rd_CP = [None] rd_CP_des = [None] rd_dL_des_plane = [None] rd_dH_des = [None] rd_grf_des = [None] rd_exf_des = [None] rd_exfen_des = [None] rd_root_des = [None] rd_CF = [None] rd_CF_pos = [None] rootPos = [None] selectedBodyId = [selectedBody] extraForce = [None] extraForcePos = [None] rightFootVectorX = [None] rightFootVectorY = [None] rightFootVectorZ = [None] rightFootPos = [None] rightVectorX = [None] rightVectorY = [None] rightVectorZ = [None] rightPos = [None] # viewer = ysv.SimpleViewer() viewer = hsv.hpSimpleViewer(viewForceWnd=False) viewer.setMaxFrame(1000) #viewer.record(False) # viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,255,255), yr.LINK_BONE)) # viewer.doc.addObject('motion', motion) # viewer.doc.addRenderer('motionModel', cvr.VpModelRenderer(motionModel, (150,150,255), yr.POLYGON_FILL)) # viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_LINE)) #viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_FILL)) viewer.doc.addRenderer( 'motionModel', yr.DartRenderer(dartMotionModel.world, (255, 240, 255), yr.POLYGON_FILL)) viewer.doc.addRenderer( 'controlModel', yr.DartRenderer(dartModel.world, (150, 150, 255), yr.POLYGON_FILL)) viewer.doc.addRenderer('rd_footCenter', yr.PointsRenderer(rd_footCenter)) viewer.doc.addRenderer('rd_CM_plane', yr.PointsRenderer(rd_CM_plane, (255, 255, 0))) viewer.doc.addRenderer('rd_CP', yr.PointsRenderer(rd_CP, (0, 255, 0))) viewer.doc.addRenderer('rd_CP_des', yr.PointsRenderer(rd_CP_des, (255, 0, 255))) viewer.doc.addRenderer( 'rd_dL_des_plane', yr.VectorsRenderer(rd_dL_des_plane, rd_CM, (255, 255, 0))) viewer.doc.addRenderer('rd_dH_des', yr.VectorsRenderer(rd_dH_des, rd_CM, (0, 255, 0))) #viewer.doc.addRenderer('rd_grf_des', yr.ForcesRenderer(rd_grf_des, rd_CP_des, (0,255,0), .001)) viewer.doc.addRenderer('rd_CF', yr.VectorsRenderer(rd_CF, rd_CF_pos, (255, 0, 0))) viewer.doc.addRenderer( 'extraForce', yr.VectorsRenderer(rd_exf_des, extraForcePos, (0, 255, 0))) viewer.doc.addRenderer( 'extraForceEnable', yr.VectorsRenderer(rd_exfen_des, extraForcePos, (255, 0, 0))) #viewer.doc.addRenderer('right_foot_oriX', yr.VectorsRenderer(rightFootVectorX, rightFootPos, (255,0,0))) #viewer.doc.addRenderer('right_foot_oriY', yr.VectorsRenderer(rightFootVectorY, rightFootPos, (0,255,0))) #viewer.doc.addRenderer('right_foot_oriZ', yr.VectorsRenderer(rightFootVectorZ, rightFootPos, (0,0,255))) #viewer.doc.addRenderer('right_oriX', yr.VectorsRenderer(rightVectorX, rightPos, (255,0,0))) #viewer.doc.addRenderer('right_oriY', yr.VectorsRenderer(rightVectorY, rightPos, (0,255,0))) #viewer.doc.addRenderer('right_oriZ', yr.VectorsRenderer(rightVectorZ, rightPos, (0,0,255))) #success!! #initKt = 50 #initKl = 10.1 #initKh = 3.1 #initBl = .1 #initBh = .1 #initSupKt = 21.6 #initFm = 100.0 #success!! -- 2015.2.12. double stance #initKt = 50 #initKl = 37.1 #initKh = 41.8 #initBl = .1 #initBh = .13 #initSupKt = 21.6 #initFm = 165.0 #single stance #initKt = 25 #initKl = 80.1 #initKh = 10.8 #initBl = .1 #initBh = .13 #initSupKt = 21.6 #initFm = 50.0 #single stance -> double stance #initKt = 25 #initKl = 60. #initKh = 20. #initBl = .1 #initBh = .13 #initSupKt = 21.6 #initFm = 50.0 initKt = 25. # initKl = 11. # initKh = 22. initKl = 100. initKh = 100. initBl = .1 initBh = .13 initSupKt = 17. # initSupKt = 2.5 initFm = 50.0 initComX = 0. initComY = 0. initComZ = 0. viewer.objectInfoWnd.add1DSlider("Kt", 0., 300., 1., initKt) viewer.objectInfoWnd.add1DSlider("Kl", 0., 300., 1., initKl) viewer.objectInfoWnd.add1DSlider("Kh", 0., 300., 1., initKh) viewer.objectInfoWnd.add1DSlider("Bl", 0., 1., .001, initBl) viewer.objectInfoWnd.add1DSlider("Bh", 0., 1., .001, initBh) viewer.objectInfoWnd.add1DSlider("SupKt", 0., 100., 0.1, initSupKt) viewer.objectInfoWnd.add1DSlider("Fm", 0., 1000., 10., initFm) viewer.objectInfoWnd.add1DSlider("com X offset", -1., 1., 0.01, initComX) viewer.objectInfoWnd.add1DSlider("com Y offset", -1., 1., 0.01, initComY) viewer.objectInfoWnd.add1DSlider("com Z offset", -1., 1., 0.01, initComZ) viewer.force_on = False def viewer_SetForceState(object): viewer.force_on = True def viewer_GetForceState(): return viewer.force_on def viewer_ResetForceState(): viewer.force_on = False viewer.objectInfoWnd.addBtn('Force on', viewer_SetForceState) viewer_ResetForceState() offset = 60 viewer.objectInfoWnd.begin() viewer.objectInfoWnd.labelForceX = Fl_Value_Input(20, 30 + offset * 9, 40, 20, 'X') viewer.objectInfoWnd.labelForceX.value(0) viewer.objectInfoWnd.labelForceY = Fl_Value_Input(80, 30 + offset * 9, 40, 20, 'Y') viewer.objectInfoWnd.labelForceY.value(0) viewer.objectInfoWnd.labelForceZ = Fl_Value_Input(140, 30 + offset * 9, 40, 20, 'Z') viewer.objectInfoWnd.labelForceZ.value(1) viewer.objectInfoWnd.labelForceDur = Fl_Value_Input( 220, 30 + offset * 9, 40, 20, 'Dur') viewer.objectInfoWnd.labelForceDur.value(0.1) viewer.objectInfoWnd.end() #self.sliderFm = Fl_Hor_Nice_Slider(10, 42+offset*6, 250, 10) pdcontroller = PDController(dartModel, dartModel.skeleton, dartModel.world.time_step(), Kt, Dt) def getParamVal(paramname): return viewer.objectInfoWnd.getVal(paramname) def getParamVals(paramnames): return (getParamVal(name) for name in paramnames) ik_solver = hikd.numIkSolver(dartMotionModel) body_num = dartModel.getBodyNum() # dJsys = np.zeros((6*body_num, totalDOF)) # dJsupL = np.zeros((6, totalDOF)) # dJsupR = np.zeros((6, totalDOF)) # Jpre = [np.zeros((6*body_num, totalDOF)), np.zeros((6, totalDOF)), np.zeros((6, totalDOF))] l_idx = [dartModel.skeleton.body("h_blade_left").index_in_skeleton()] r_idx = [dartModel.skeleton.body("h_blade_right").index_in_skeleton()] up_vec_in_each_link = {supL: mm.unitY(), supR: mm.unitY()} mbc = DartMomentumBalanceController(dartModel.skeleton, dartMotionModel.skeleton, config['weightMap'], up_vec_in_each_link) ################################### #simulate ################################### def simulateCallback(frame): # print() # print(dartModel.getJointVelocityGlobal(0)) # print(dartModel.getDOFVelocities()[0]) # print(dartModel.get_dq()[:6]) # dartMotionModel.update(motion[frame]) global g_initFlag global forceShowTime global preFootCenter global maxContactChangeCount global contactChangeCount global contact global contactChangeType # print('contactstate:', contact, contactChangeCount) Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals( ['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt']) mbc.set_parameters(Kt, Kl, Kh, Bl, Bh, kt_sup) pdcontroller.setKpKd(Kt, Dt) footHeight = dartModel.getBody(supL).shapenodes[0].shape.size()[1] / 2. doubleTosingleOffset = 0.15 singleTodoubleOffset = 0.30 com_offset_x, com_offset_y, com_offset_z = getParamVals( ['com X offset', 'com Y offset', 'com Z offset']) footOffset = np.array((com_offset_x, com_offset_y, com_offset_z)) # tracking th_r_flat = dartMotionModel.get_q() dth_flat = dartModel.get_dq() ddth_des_flat = pdcontroller.compute_flat(th_r_flat) bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds) CP = yrp.getCP(contactPositions, contactForces) ddth_sol = mbc.solve( ddth_des_flat, # [dartModel.skeleton.body('h_blade_left').index_in_skeleton(), dartModel.skeleton.body('h_blade_right').index_in_skeleton()], [dartModel.skeleton.body('h_blade_left').index_in_skeleton()], footOffset + np.array([0.9]), r_idx, l_idx, CP, None) localPos = np.zeros(3) inv_h = 1. / time_step for i in range(stepsPerFrame): ddq, tau, bodyIDs, contactPositions, contactPositionLocals, contactForces = hqp.calc_QP( dartModel.skeleton, ddth_sol, inv_h) # ddq, tau, bodyIDs, contactPositions, contactPositionLocals, contactForces = hqp.calc_QP(dartModel.skeleton, ddth_des_flat, inv_h) # print(frame, i, tau) dartModel.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces) dartModel.skeleton.set_forces(tau) if forceShowTime > viewer.objectInfoWnd.labelForceDur.value(): forceShowTime = 0 viewer_ResetForceState() forceforce = np.array([ viewer.objectInfoWnd.labelForceX.value(), viewer.objectInfoWnd.labelForceY.value(), viewer.objectInfoWnd.labelForceZ.value() ]) extraForce[0] = getParamVal('Fm') * mm.normalize2(forceforce) if viewer_GetForceState(): forceShowTime += time_step dartModel.applyPenaltyForce(selectedBodyId, localPos, extraForce) dartModel.step() # rendering rd_CM[0] = dartModel.skeleton.com() rd_CM_plane[0] = dartModel.skeleton.com().copy() rd_CM_plane[0][1] = 0. rd_root_des[0] = rootPos[0] del rd_CF[:] del rd_CF_pos[:] for i in range(len(contactPositions)): rd_CF.append(contactForces[i] / 100) rd_CF_pos.append(contactPositions[i].copy()) if viewer_GetForceState(): rd_exfen_des[0] = [ extraForce[0][0] / 100, extraForce[0][1] / 100, extraForce[0][2] / 100 ] rd_exf_des[0] = [0, 0, 0] else: rd_exf_des[0] = [ extraForce[0][0] / 100, extraForce[0][1] / 100, extraForce[0][2] / 100 ] rd_exfen_des[0] = [0, 0, 0] extraForcePos[0] = dartModel.getBodyPositionGlobal(selectedBody) viewer.setSimulateCallback(simulateCallback) viewer.startTimer(1 / 30.) viewer.show() Fl.run()
def main(): CURRENT_CHECK = False RSI = True MOTION_ONLY = False SKELETON_ON = False CAMERA_TRACKING = True PD_PLOT = False PUSH = False pydart.init() env_name = 'walk' env_name = 'walk_repeated' # env_name = 'walk_fast' # env_name = 'walk_sukiko' # env_name = 'walk_u_turn' # env_name = '1foot_contact_run' # env_name = 'round_girl' # env_name = 'fast_2foot_hop' # env_name = 'slow_2foot_hop' # env_name = 'long_broad_jump' # env_name = 'short_broad_jump' # env_name = 'n_kick' # env_name = 'jump' gain_p0 = [] gain_p1 = [] gain_p2 = [] gain_p3 = [] gain_p4 = [] MA_DUR = 5 ma_gain_p0 = [] ma_gain_p1 = [] ma_gain_p2 = [] ma_gain_p3 = [] ma_gain_p4 = [] rfoot_contact_ranges = [] lfoot_contact_ranges = [] ppo = PPO(env_name, 0, visualize_only=True) if not MOTION_ONLY and not CURRENT_CHECK: ppo.LoadModel('model/' + env_name + '.pt') # ppo.LoadModel('model/' + 'param' + '.pt') # ppo.LoadModel('model_test/' + env_name + '.pt') # ppo.LoadModel('model_test/' + 'param' + '.pt') elif not MOTION_ONLY and CURRENT_CHECK: env_model_dir = [] for dir_name in sorted(os.listdir()): if env_name in dir_name: env_model_dir.append(dir_name) pt_names = os.listdir(env_model_dir[-1]) pt_names.pop(pt_names.index('log.txt')) pt_names.sort(key=lambda f: int(os.path.splitext(f)[0])) pt_names.append('1171.pt') ppo.LoadModel(env_model_dir[-1] + '/' + pt_names[-1]) print(env_model_dir[-1] + '/' + pt_names[-1]) ppo.env.dart_skel_file = '../data/wd2_seg_rough_terrain.xml' ppo.env.rsi_num = 560 ppo.env.hard_reset() ppo.env.Resets(RSI) ppo.env.ref_skel.set_positions( ppo.env.ref_motion.get_q(ppo.env.phase_frame)) # viewer settings rd_contact_positions = [None] rd_contact_forces = [None] rd_exf = [None] rd_exf_pos = [None] dart_world = ppo.env.world skel = dart_world.skeletons[1] viewer_w, viewer_h = 960, 1080 viewer = hsv.hpSimpleViewer(rect=(0, 0, viewer_w + 300, 1 + viewer_h + 55), viewForceWnd=False) # viewer.doc.addRenderer('MotionModel', yr.DartRenderer(ppo.env.ref_world, (150,150,255), yr.POLYGON_FILL)) control_model_renderer = None if not MOTION_ONLY: control_model_renderer = yr.DartRenderer(dart_world, (255, 240, 255), yr.POLYGON_FILL) viewer.doc.addRenderer('controlModel', control_model_renderer) for shape_idx in range( ppo.env.world.skeletons[0].body(0).num_shapenodes() // 2): control_model_renderer.geom_colors[0][0][shape_idx] = (128, 128, 128) # viewer.doc.addRenderer('contact', yr.VectorsRenderer(rd_contact_forces, rd_contact_positions, (255,0,0))) viewer.doc.addRenderer( 'extraForceEnable', yr.WideArrowRenderer(rd_exf, rd_exf_pos, (255, 0, 0), lineWidth=.05, fromPoint=False)) def makeEmptyBasicSkeletonTransformDict(init=None): Ts = dict() Ts['pelvis'] = init Ts['spine_ribs'] = init Ts['head'] = init Ts['thigh_R'] = init Ts['shin_R'] = init Ts['foot_heel_R'] = init Ts['foot_R'] = init Ts['heel_R'] = init Ts['outside_metatarsal_R'] = init Ts['outside_phalanges_R'] = init Ts['inside_metatarsal_R'] = init Ts['inside_phalanges_R'] = init Ts['upper_limb_R'] = init Ts['lower_limb_R'] = init Ts['thigh_L'] = init Ts['shin_L'] = init Ts['foot_heel_L'] = init Ts['foot_L'] = init Ts['heel_L'] = init Ts['outside_metatarsal_L'] = init Ts['outside_phalanges_L'] = init Ts['inside_metatarsal_L'] = init Ts['inside_phalanges_L'] = init Ts['upper_limb_L'] = init Ts['lower_limb_L'] = init return Ts skeleton_renderer = None if SKELETON_ON: # skeleton_renderer = yr.BasicSkeletonRenderer(makeEmptyBasicSkeletonTransformDict(np.eye(4)), offset_Y=-0.08) # skeleton_renderer = yr.BasicSkeletonRenderer(makeEmptyBasicSkeletonTransformDict(np.eye(4)), color=(230, 230, 230), offset_draw=(0.8, -0.02, 0.)) skeleton_renderer = yr.BasicSkeletonRenderer( makeEmptyBasicSkeletonTransformDict(np.eye(4)), color=(230, 230, 230), offset_draw=(0., -0.0, 0.)) viewer.doc.addRenderer('skeleton', skeleton_renderer) def simulateCallback(frame): state = ppo.env.GetState(0) action_dist, v = ppo.model(torch.tensor(state.reshape(1, -1)).float()) action = action_dist.loc.detach().numpy() value = v.detach().numpy() if PUSH and 110 < frame < 123: ppo.env.applied_force = np.array([0., 0., 50.]) ppo.env.applied_force_offset = np.array([-0.11, 0., 0.]) del rd_exf[:] del rd_exf_pos[:] rd_exf.append(ppo.env.applied_force / 100.) rd_exf_pos.append( ppo.env.skel.body('Spine').to_world( ppo.env.applied_force_offset)) elif PUSH and 195 < frame < 208: ppo.env.applied_force = np.array([0., 0., 80.]) ppo.env.applied_force_offset = np.array([-0.11, 0., 0.]) del rd_exf[:] del rd_exf_pos[:] rd_exf.append(ppo.env.applied_force / 100.) rd_exf_pos.append( ppo.env.skel.body('Spine').to_world( ppo.env.applied_force_offset)) elif PUSH and 281 < frame < 294: ppo.env.applied_force = np.array([0., 0., 100.]) ppo.env.applied_force_offset = np.array([-0.11, 0., 0.]) del rd_exf[:] del rd_exf_pos[:] rd_exf.append(ppo.env.applied_force / 100.) rd_exf_pos.append( ppo.env.skel.body('Spine').to_world( ppo.env.applied_force_offset)) else: del rd_exf[:] del rd_exf_pos[:] ppo.env.applied_force = np.zeros(3) res = ppo.env.Steps(action) # for gain plotting gain_p0.append(res[3]['kp'][ppo.env.skel.dof_index('j_RightFoot_x')]) gain_p1.append( res[3]['kp'][ppo.env.skel.dof_index('j_RightFoot_foot_0_0_x')]) gain_p2.append( res[3]['kp'][ppo.env.skel.dof_index('j_RightFoot_foot_0_0_0_x')]) gain_p3.append( res[3]['kp'][ppo.env.skel.dof_index('j_RightFoot_foot_0_1_0_x')]) gain_p4.append( res[3]['kp'][ppo.env.skel.dof_index('j_RightFoot_foot_1_0_x')]) ma_gain_p0.append( sum(gain_p0[-MA_DUR:]) / MA_DUR if len(gain_p0) >= MA_DUR else sum(gain_p0) / len(gain_p0)) ma_gain_p1.append( sum(gain_p1[-MA_DUR:]) / MA_DUR if len(gain_p1) >= MA_DUR else sum(gain_p1) / len(gain_p1)) ma_gain_p2.append( sum(gain_p2[-MA_DUR:]) / MA_DUR if len(gain_p2) >= MA_DUR else sum(gain_p2) / len(gain_p2)) ma_gain_p3.append( sum(gain_p3[-MA_DUR:]) / MA_DUR if len(gain_p3) >= MA_DUR else sum(gain_p3) / len(gain_p3)) ma_gain_p4.append( sum(gain_p4[-MA_DUR:]) / MA_DUR if len(gain_p4) >= MA_DUR else sum(gain_p4) / len(gain_p4)) if any([ 'RightFoot' in body.name for body in ppo.env.world.collision_result.contacted_bodies ]): if rfoot_contact_ranges and rfoot_contact_ranges[-1][ 1] == frame - 1: rfoot_contact_ranges[-1][1] = frame else: rfoot_contact_ranges.append([frame, frame]) # res = ppo.env.Steps(np.zeros_like(action)) ppo.env.world.collision_result.update() # print(frame, ppo.env.Ref_skel.current_frame, ppo.env.world.time()*ppo.env.ref_motion.fps) # print(frame, res[0][0]) # if res[0][0] > 0.46: # ppo.env.continue_from_now_by_phase(0.2) # print(frame, ' '.join(["{:0.1f}".format(400. * exp(log(400.) * rate/10.)) for rate in action[0][ppo.env.skel.ndofs-6:]])) if res[2]: print(frame, 'Done') ppo.env.reset() contacts = ppo.env.world.collision_result.contacts # contact body rendering if control_model_renderer is not None: skel_idx = dart_world.skel.id for body_idx in range( dart_world.skeletons[skel_idx].num_bodynodes()): for shape_idx in range(dart_world.skeletons[skel_idx].body( body_idx).num_shapenodes()): control_model_renderer.geom_colors[skel_idx][body_idx][ shape_idx] = control_model_renderer.totalColor for contact in contacts: body_idx, geom_idx = ( contact.bodynode_id1, contact.shape_id1 ) if dart_world.skeletons[contact.skel_id1].body( contact.bodynode_id1).name != 'ground' else ( contact.bodynode_id2, contact.shape_id2) body = dart_world.skeletons[skel_idx].body(body_idx) visual = sum(shapenode.has_visual_aspect() for shapenode in body.shapenodes) collision = sum(shapenode.has_collision_aspect() for shapenode in body.shapenodes) if visual == collision: control_model_renderer.geom_colors[skel_idx][body_idx][ geom_idx - visual] = (255, 0, 0) else: control_model_renderer.geom_colors[skel_idx][body_idx][ (geom_idx - visual) // 2] = (255, 0, 0) if PD_PLOT: fig = plt.figure(1) plt.clf() fig.add_subplot(5, 1, 1) for rfoot_contact_range in rfoot_contact_ranges: plt.axvspan(rfoot_contact_range[0], rfoot_contact_range[1], facecolor='0.5', alpha=0.3) plt.ylabel('ankle') plt.plot(range(len(ma_gain_p0)), ma_gain_p0) fig.add_subplot(5, 1, 2) for rfoot_contact_range in rfoot_contact_ranges: plt.axvspan(rfoot_contact_range[0], rfoot_contact_range[1], facecolor='0.5', alpha=0.3) plt.ylabel('talus') plt.plot(range(len(ma_gain_p1)), ma_gain_p1) fig.add_subplot(5, 1, 3) for rfoot_contact_range in rfoot_contact_ranges: plt.axvspan(rfoot_contact_range[0], rfoot_contact_range[1], facecolor='0.5', alpha=0.3) plt.ylabel('thumb') plt.plot(range(len(ma_gain_p2)), ma_gain_p2) fig.add_subplot(5, 1, 4) for rfoot_contact_range in rfoot_contact_ranges: plt.axvspan(rfoot_contact_range[0], rfoot_contact_range[1], facecolor='0.5', alpha=0.3) plt.ylabel('phalange') plt.plot(range(len(ma_gain_p3)), ma_gain_p3) fig.add_subplot(5, 1, 5) for rfoot_contact_range in rfoot_contact_ranges: plt.axvspan(rfoot_contact_range[0], rfoot_contact_range[1], facecolor='0.5', alpha=0.3) plt.ylabel('heel') plt.plot(range(len(ma_gain_p4)), ma_gain_p4) plt.show() plt.pause(0.001) # contact rendering contacts = ppo.env.world.collision_result.contacts del rd_contact_forces[:] del rd_contact_positions[:] for contact in contacts: rd_contact_forces.append(contact.f / 1000.) rd_contact_positions.append(contact.p) # render skeleton if SKELETON_ON: Ts = dict() Ts['pelvis'] = skel.joint('j_Hips').get_local_transform() Ts['thigh_R'] = skel.joint('j_RightUpLeg').get_local_transform() Ts['shin_R'] = skel.joint('j_RightLeg').get_local_transform() Ts['foot_R'] = skel.joint('j_RightFoot').get_local_transform() Ts['foot_heel_R'] = skel.joint('j_RightFoot').get_local_transform() Ts['heel_R'] = np.eye(4) Ts['outside_metatarsal_R'] = skel.joint( 'j_RightFoot_foot_0_0').get_local_transform() Ts['outside_phalanges_R'] = skel.joint( 'j_RightFoot_foot_0_0_0').get_local_transform() # Ts['inside_metatarsal_R'] = controlModel.getJointTransform(idDic['RightFoot_foot_0_1']) Ts['inside_metatarsal_R'] = np.eye(4) Ts['inside_phalanges_R'] = skel.joint( 'j_RightFoot_foot_0_1_0').get_local_transform() Ts['spine_ribs'] = skel.joint('j_Spine').get_local_transform() Ts['head'] = skel.joint('j_Spine1').get_local_transform() Ts['upper_limb_R'] = skel.joint('j_RightArm').get_local_transform() # Ts['upper_limb_R'] = np.dot(skel.joint('j_RightArm').get_local_transform(), mm.SO3ToSE3(mm.rotX(pi/6.))) Ts['lower_limb_R'] = skel.joint( 'j_RightForeArm').get_local_transform() # Ts['lower_limb_R'] = np.dot(skel.joint('j_RightForeArm').get_local_transform(), mm.SO3ToSE3(mm.rotY(-pi/6.))) Ts['thigh_L'] = skel.joint('j_LeftUpLeg').get_local_transform() Ts['shin_L'] = skel.joint('j_LeftLeg').get_local_transform() Ts['foot_L'] = skel.joint('j_LeftFoot').get_local_transform() Ts['foot_heel_L'] = skel.joint('j_LeftFoot').get_local_transform() Ts['heel_L'] = np.eye(4) Ts['outside_metatarsal_L'] = skel.joint( 'j_LeftFoot_foot_0_0').get_local_transform() Ts['outside_phalanges_L'] = skel.joint( 'j_LeftFoot_foot_0_0_0').get_local_transform() # Ts['inside_metatarsal_L'] = controlModel.getJointTransform(idDic['LeftFoot_foot_0_1']) Ts['inside_metatarsal_L'] = np.eye(4) Ts['inside_phalanges_L'] = skel.joint( 'j_LeftFoot_foot_0_1_0').get_local_transform() Ts['upper_limb_L'] = skel.joint('j_LeftArm').get_local_transform() # Ts['upper_limb_L'] = np.dot(skel.joint('j_LeftArm').get_local_transform(), mm.SO3ToSE3(mm.rotX(pi/6.))) Ts['lower_limb_L'] = skel.joint( 'j_LeftForeArm').get_local_transform() # Ts['lower_limb_L'] = np.dot(skel.joint('j_LeftForeArm').get_local_transform(), mm.SO3ToSE3(mm.rotY(pi/6.))) skeleton_renderer.appendFrameState(Ts) if MOTION_ONLY: viewer.setMaxFrame(len(ppo.env.ref_motion) - 1) if CAMERA_TRACKING: cameraTargets = [None] * (viewer.getMaxFrame() + 1) def postCallback(frame): ppo.env.ref_skel.set_positions(ppo.env.ref_motion.get_q(frame - 1)) ppo.env.ref_motion.frame = frame - 1 if CAMERA_TRACKING: if cameraTargets[frame] is None: cameraTargets[frame] = ppo.env.ref_skel.body(0).com() viewer.setCameraTarget(cameraTargets[frame]) viewer.setPostFrameCallback_Always(postCallback) else: viewer.setSimulateCallback(simulateCallback) viewer.setMaxFrame(3000) if CAMERA_TRACKING: cameraTargets = [None] * (viewer.getMaxFrame() + 1) def postFrameCallback_Always(frame): if CAMERA_TRACKING: if cameraTargets[frame] is None: cameraTargets[frame] = ppo.env.skel.body(0).com() viewer.setCameraTarget(cameraTargets[frame]) viewer.setPostFrameCallback_Always(postFrameCallback_Always) viewer.startTimer(1. / 30.) viewer.show() Fl.run()
def init_dart(self): pydart.init()
def main(): np.set_printoptions(precision=4, linewidth=200) # np.set_printoptions(precision=4, linewidth=1000, threshold=np.inf) #motion, mcfg, wcfg, stepsPerFrame, config = mit.create_vchain_5() motion, mcfg, wcfg, stepsPerFrame, config, frame_rate= mit.create_biped() #motion, mcfg, wcfg, stepsPerFrame, config = mit.create_jump_biped() frame_step_size = 1./frame_rate pydart.init() dartModel = cdm.DartModel(wcfg, motion[0], mcfg, DART_CONTACT_ON) dartMotionModel = cdm.DartModel(wcfg, motion[0], mcfg, DART_CONTACT_ON) # wcfg.lockingVel = 0.01 # dartModel.initializeHybridDynamics() if not DART_CONTACT_ON: dartModel.initContactPoint() #controlToMotionOffset = (1.5, -0.02, 0) # controlToMotionOffset = (1.5, 0, 0) # dartModel.translateByOffset(controlToMotionOffset) totalDOF = dartModel.getTotalDOF() DOFs = dartModel.getDOFs() # parameter Kt = config['Kt']; Dt = config['Dt'] # tracking gain Kl = config['Kl']; Dl = config['Dl'] # linear balance gain Kh = config['Kh']; Dh = config['Dh'] # angular balance gain Ks = config['Ks']; Ds = config['Ds'] # penalty force spring gain Bt = config['Bt'] Bl = config['Bl'] Bh = config['Bh'] w = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap']) supL = motion[0].skeleton.getJointIndex(config['supLink1']) supR = motion[0].skeleton.getJointIndex(config['supLink2']) selectedBody = motion[0].skeleton.getJointIndex(config['end']) constBody = motion[0].skeleton.getJointIndex('RightFoot') # momentum matrix linkMasses = dartModel.getBodyMasses() totalMass = dartModel.getTotalMass() TO = ymt.make_TO(linkMasses) dTO = ymt.make_dTO(len(linkMasses)) # optimization problem = yac.LSE(totalDOF, 12) #a_sup = (0,0,0, 0,0,0) #ori #a_sup = (0,0,0, 0,0,0) #L a_supL = (0,0,0, 0,0,0) a_supR = (0,0,0, 0,0,0) a_sup_2 = (0,0,0, 0,0,0, 0,0,0, 0,0,0) CP_old = [mm.v3(0.,0.,0.)] CP_des = [None] dCP_des = [np.zeros(3)] # penalty method # bodyIDsToCheck = range(dartModel.getBodyNum()) bodyIDsToCheck = [supL, supR] #mus = [1.]*len(bodyIDsToCheck) mus = [.5]*len(bodyIDsToCheck) # flat data structure # ddth_des_flat = ype.makeFlatList(totalDOF) # dth_flat = ype.makeFlatList(totalDOF) # ddth_sol = ype.makeNestedList(DOFs) # viewer rd_footCenter = [None] rd_footCenterL = [None] rd_footCenterR = [None] rd_CM_plane = [None] rd_CM = [None] rd_CP = [None] rd_CP_des = [None] rd_dL_des_plane = [None] rd_dH_des = [None] rd_grf_des = [None] rd_exf_des = [None] rd_exfen_des = [None] rd_root_des = [None] rd_CF = [None] rd_CF_pos = [None] rootPos = [None] selectedBodyId = [selectedBody] extraForce = [None] extraForcePos = [None] rightFootVectorX = [None] rightFootVectorY = [None] rightFootVectorZ = [None] rightFootPos = [None] rightVectorX = [None] rightVectorY = [None] rightVectorZ = [None] rightPos = [None] # viewer = ysv.SimpleViewer() viewer = hsv.hpSimpleViewer(viewForceWnd=False) #viewer.record(False) viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,255,255), yr.LINK_BONE)) viewer.doc.addObject('motion', motion) # viewer.doc.addRenderer('motionModel', cvr.VpModelRenderer(motionModel, (150,150,255), yr.POLYGON_FILL)) # viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_LINE)) #viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_FILL)) # viewer.doc.addRenderer('motionModel', yr.DartModelRenderer(dartMotionModel, (150,150,255), yr.POLYGON_FILL)) # viewer.doc.addRenderer('controlModel', yr.DartModelRenderer(dartModel, (255,240,255), yr.POLYGON_FILL)) viewer.doc.addRenderer('controlModel', yr.DartRenderer(dartModel.world, (150,150,255), yr.POLYGON_FILL)) viewer.doc.addRenderer('rd_footCenter', yr.PointsRenderer(rd_footCenter)) viewer.doc.addRenderer('rd_CM_plane', yr.PointsRenderer(rd_CM_plane, (255,255,0))) viewer.doc.addRenderer('rd_CP', yr.PointsRenderer(rd_CP, (0,255,0))) viewer.doc.addRenderer('rd_CP_des', yr.PointsRenderer(rd_CP_des, (255,0,255))) viewer.doc.addRenderer('rd_dL_des_plane', yr.VectorsRenderer(rd_dL_des_plane, rd_CM, (255,255,0))) viewer.doc.addRenderer('rd_dH_des', yr.VectorsRenderer(rd_dH_des, rd_CM, (0,255,0))) #viewer.doc.addRenderer('rd_grf_des', yr.ForcesRenderer(rd_grf_des, rd_CP_des, (0,255,0), .001)) viewer.doc.addRenderer('rd_CF', yr.VectorsRenderer(rd_CF, rd_CF_pos, (255,0,0))) viewer.doc.addRenderer('extraForce', yr.VectorsRenderer(rd_exf_des, extraForcePos, (0,255,0))) viewer.doc.addRenderer('extraForceEnable', yr.VectorsRenderer(rd_exfen_des, extraForcePos, (255,0,0))) #viewer.doc.addRenderer('right_foot_oriX', yr.VectorsRenderer(rightFootVectorX, rightFootPos, (255,0,0))) #viewer.doc.addRenderer('right_foot_oriY', yr.VectorsRenderer(rightFootVectorY, rightFootPos, (0,255,0))) #viewer.doc.addRenderer('right_foot_oriZ', yr.VectorsRenderer(rightFootVectorZ, rightFootPos, (0,0,255))) #viewer.doc.addRenderer('right_oriX', yr.VectorsRenderer(rightVectorX, rightPos, (255,0,0))) #viewer.doc.addRenderer('right_oriY', yr.VectorsRenderer(rightVectorY, rightPos, (0,255,0))) #viewer.doc.addRenderer('right_oriZ', yr.VectorsRenderer(rightVectorZ, rightPos, (0,0,255))) #success!! #initKt = 50 #initKl = 10.1 #initKh = 3.1 #initBl = .1 #initBh = .1 #initSupKt = 21.6 #initFm = 100.0 #success!! -- 2015.2.12. double stance #initKt = 50 #initKl = 37.1 #initKh = 41.8 #initBl = .1 #initBh = .13 #initSupKt = 21.6 #initFm = 165.0 #single stance #initKt = 25 #initKl = 80.1 #initKh = 10.8 #initBl = .1 #initBh = .13 #initSupKt = 21.6 #initFm = 50.0 #single stance -> double stance #initKt = 25 #initKl = 60. #initKh = 20. #initBl = .1 #initBh = .13 #initSupKt = 21.6 #initFm = 50.0 initKt = 25. # initKl = 11. # initKh = 22. initKl = 100. initKh = 100. initBl = .1 initBh = .13 initSupKt = 17. # initSupKt = 2.5 initFm = 50.0 initComX = 0. initComY = 0. initComZ = 0. viewer.objectInfoWnd.add1DSlider("Kt", 0., 300., 1., initKt) viewer.objectInfoWnd.add1DSlider("Kl", 0., 300., 1., initKl) viewer.objectInfoWnd.add1DSlider("Kh", 0., 300., 1., initKh) viewer.objectInfoWnd.add1DSlider("Bl", 0., 1., .001, initBl) viewer.objectInfoWnd.add1DSlider("Bh", 0., 1., .001, initBh) viewer.objectInfoWnd.add1DSlider("SupKt", 0., 100., 0.1, initSupKt) viewer.objectInfoWnd.add1DSlider("Fm", 0., 1000., 10., initFm) viewer.objectInfoWnd.add1DSlider("com X offset", -1., 1., 0.01, initComX) viewer.objectInfoWnd.add1DSlider("com Y offset", -1., 1., 0.01, initComY) viewer.objectInfoWnd.add1DSlider("com Z offset", -1., 1., 0.01, initComZ) viewer.force_on = False def viewer_SetForceState(object): viewer.force_on = True def viewer_GetForceState(): return viewer.force_on def viewer_ResetForceState(): viewer.force_on = False viewer.reset = False def viewer_reset(object): viewer.reset = True viewer.objectInfoWnd.addBtn('Force on', viewer_SetForceState) viewer.objectInfoWnd.addBtn('reset', viewer_reset) viewer_ResetForceState() offset = 60 viewer.objectInfoWnd.begin() viewer.objectInfoWnd.labelForceX = Fl_Value_Input(20, 30+offset*9, 40, 20, 'X') viewer.objectInfoWnd.labelForceX.value(0) viewer.objectInfoWnd.labelForceY = Fl_Value_Input(80, 30+offset*9, 40, 20, 'Y') viewer.objectInfoWnd.labelForceY.value(0) viewer.objectInfoWnd.labelForceZ = Fl_Value_Input(140, 30+offset*9, 40, 20, 'Z') viewer.objectInfoWnd.labelForceZ.value(1) viewer.objectInfoWnd.labelForceDur = Fl_Value_Input(220, 30+offset*9, 40, 20, 'Dur') viewer.objectInfoWnd.labelForceDur.value(0.1) viewer.objectInfoWnd.end() #self.sliderFm = Fl_Hor_Nice_Slider(10, 42+offset*6, 250, 10) pdcontroller = PDController(dartModel, dartModel.skeleton, wcfg.timeStep, Kt, Dt, config['PDweightMap']) def getParamVal(paramname): return viewer.objectInfoWnd.getVal(paramname) def getParamVals(paramnames): return (getParamVal(name) for name in paramnames) ik_solver = hikd.numIkSolver(dartMotionModel) body_num = dartModel.getBodyNum() # dJsys = np.zeros((6*body_num, totalDOF)) # dJsupL = np.zeros((6, totalDOF)) # dJsupR = np.zeros((6, totalDOF)) # Jpre = [np.zeros((6*body_num, totalDOF)), np.zeros((6, totalDOF)), np.zeros((6, totalDOF))] ################################### #simulate ################################### def simulateCallback(frame): print(frame) # print() # print(dartModel.getJointVelocityGlobal(0)) # print(dartModel.getDOFVelocities()[0]) # print(dartModel.get_dq()[:6]) dartMotionModel.update(motion[frame]) global g_initFlag global forceShowTime global preFootCenter global maxContactChangeCount global contactChangeCount global contact global contactChangeType # print('contactstate:', contact, contactChangeCount) Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals(['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt']) # Dt = 2.*(Kt**.5) Dt = Kt/100. Dl = (Kl**.5) Dh = (Kh**.5) dt_sup = 2.*(kt_sup**.5) # Dt = .2*(Kt**.5) # Dl = .2*(Kl**.5) # Dh = .2*(Kh**.5) # dt_sup = .2*(kt_sup**.5) pdcontroller.setKpKd(Kt, Dt) footHeight = dartModel.getBody(supL).shapenodes[0].shape.size()[1]/2. doubleTosingleOffset = 0.15 singleTodoubleOffset = 0.30 #doubleTosingleOffset = 0.09 doubleTosingleVelOffset = 0.0 com_offset_x, com_offset_y, com_offset_z = getParamVals(['com X offset', 'com Y offset', 'com Z offset']) footOffset = np.array((com_offset_x, com_offset_y, com_offset_z)) des_com = dartMotionModel.getCOM() + footOffset footCenterL = dartMotionModel.getBodyPositionGlobal(supL) footCenterR = dartMotionModel.getBodyPositionGlobal(supR) footBodyOriL = dartMotionModel.getBodyOrientationGlobal(supL) footBodyOriR = dartMotionModel.getBodyOrientationGlobal(supR) torso_pos = dartMotionModel.getBodyPositionGlobal(4) torso_ori = dartMotionModel.getBodyOrientationGlobal(4) # ik_solver.setInitPose(motion[frame]) # ik_solver.addConstraints(supL, np.zeros(3), footCenterL, footBodyOriL, (True, True, True, True)) # ik_solver.addConstraints(supR, np.zeros(3), footCenterR, footBodyOriR, (True, True, True, True)) # ik_solver.addConstraints(4, np.zeros(3), torso_pos, torso_ori, (False, False, False, True)) # ik_solver.solve(des_com) # ik_solver.clear() # tracking # th_r = motion.getDOFPositions(frame) th_r = dartMotionModel.getDOFPositions() th = dartModel.getDOFPositions() th_r_flat = dartMotionModel.get_q() # dth_r = motion.getDOFVelocities(frame) # dth = dartModel.getDOFVelocities() # ddth_r = motion.getDOFAccelerations(frame) # ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt) dth_flat = dartModel.get_dq() # dth_flat = np.concatenate(dth) # ddth_des_flat = pdcontroller.compute(dartMotionModel.get_q()) # ddth_des_flat = pdcontroller.compute(th_r) ddth_des_flat = pdcontroller.compute_flat(th_r_flat) # ype.flatten(ddth_des, ddth_des_flat) # ype.flatten(dth, dth_flat) ################################################# # jacobian ################################################# footOriL = dartModel.getJointOrientationGlobal(supL) footOriR = dartModel.getJointOrientationGlobal(supR) footCenterL = dartModel.getBodyPositionGlobal(supL) footCenterR = dartModel.getBodyPositionGlobal(supR) footBodyOriL = dartModel.getBodyOrientationGlobal(supL) footBodyOriR = dartModel.getBodyOrientationGlobal(supR) footBodyVelL = dartModel.getBodyVelocityGlobal(supL) footBodyVelR = dartModel.getBodyVelocityGlobal(supR) footBodyAngVelL = dartModel.getBodyAngVelocityGlobal(supL) footBodyAngVelR = dartModel.getBodyAngVelocityGlobal(supR) refFootL = dartMotionModel.getBodyPositionGlobal(supL) refFootR = dartMotionModel.getBodyPositionGlobal(supR) # refFootAngVelL = motion.getJointAngVelocityGlobal(supL, frame) # refFootAngVelR = motion.getJointAngVelocityGlobal(supR, frame) refFootAngVelL = np.zeros(3) refFootAngVelR = np.zeros(3) refFootJointVelR = motion.getJointVelocityGlobal(supR, frame) refFootJointAngVelR = motion.getJointAngVelocityGlobal(supR, frame) refFootJointR = motion.getJointPositionGlobal(supR, frame) # refFootVelR = refFootJointVelR + np.cross(refFootJointAngVelR, (refFootR-refFootJointR)) refFootVelR = np.zeros(3) refFootJointVelL = motion.getJointVelocityGlobal(supL, frame) refFootJointAngVelL = motion.getJointAngVelocityGlobal(supL, frame) refFootJointL = motion.getJointPositionGlobal(supL, frame) # refFootVelL = refFootJointVelL + np.cross(refFootJointAngVelL, (refFootL-refFootJointL)) refFootVelL = np.zeros(3) contactR = 1 contactL = 1 if refFootVelR[1] < 0 and refFootVelR[1]*frame_step_size + refFootR[1] > singleTodoubleOffset: contactR = 0 if refFootVelL[1] < 0 and refFootVelL[1]*frame_step_size + refFootL[1] > singleTodoubleOffset: contactL = 0 if refFootVelR[1] > 0 and refFootVelR[1]*frame_step_size + refFootR[1] > doubleTosingleOffset: contactR = 0 if refFootVelL[1] > 0 and refFootVelL[1]*frame_step_size + refFootL[1] > doubleTosingleOffset: contactL = 0 # contactR = 0 # contMotionOffset = th[0][0] - th_r[0][0] contMotionOffset = dartModel.getBodyPositionGlobal(0) - dartMotionModel.getBodyPositionGlobal(0) linkPositions = dartModel.getBodyPositionsGlobal() linkVelocities = dartModel.getBodyVelocitiesGlobal() linkAngVelocities = dartModel.getBodyAngVelocitiesGlobal() linkInertias = dartModel.getBodyInertiasGlobal() CM = dartModel.skeleton.com() dCM = dartModel.skeleton.com_velocity() CM_plane = copy.copy(CM) CM_plane[1]=0. dCM_plane = copy.copy(dCM) dCM_plane[1]=0. P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM, linkInertias) dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses, linkVelocities, dCM, linkAngVelocities, linkInertias) #calculate contact state #if g_initFlag == 1 and contact == 1 and refFootR[1] < doubleTosingleOffset and footCenterR[1] < 0.08: if g_initFlag == 1: #contact state # 0: flying 1: right only 2: left only 3: double #if contact == 2 and refFootR[1] < doubleTosingleOffset: if contact == 2 and contactR==1: contact = 3 maxContactChangeCount+=30 contactChangeCount += maxContactChangeCount contactChangeType = 'StoD' #elif contact == 3 and refFootL[1] < doubleTosingleOffset: elif contact == 1 and contactL==1: contact = 3 maxContactChangeCount+=30 contactChangeCount += maxContactChangeCount contactChangeType = 'StoD' #elif contact == 3 and refFootR[1] > doubleTosingleOffset: elif contact == 3 and contactR == 0: contact = 2 contactChangeCount += maxContactChangeCount contactChangeType = 'DtoS' #elif contact == 3 and refFootL[1] > doubleTosingleOffset: elif contact == 3 and contactL == 0: contact = 1 contactChangeCount += maxContactChangeCount contactChangeType = 'DtoS' else: contact = 0 #if refFootR[1] < doubleTosingleOffset: if contactR == 1: contact +=1 #if refFootL[1] < doubleTosingleOffset: if contactL == 1: contact +=2 #initialization if g_initFlag == 0: softConstPoint = footCenterR.copy() footCenter = footCenterL + (footCenterR - footCenterL)/2.0 footCenter[1] = 0. preFootCenter = footCenter.copy() #footToBodyFootRotL = np.dot(np.transpose(footOriL), footBodyOriL) #footToBodyFootRotR = np.dot(np.transpose(footOriR), footBodyOriR) # if refFootR[1] < doubleTosingleOffset: # contact +=1 # if refFootL[1] < doubleTosingleOffset: # contact +=2 if refFootR[1] < footHeight: contact +=1 if refFootL[1] < footHeight: contact +=2 g_initFlag = 1 #calculate jacobian body_num = dartModel.getBodyNum() Jsys = np.zeros((6*body_num, totalDOF)) dJsys = np.zeros((6*body_num, totalDOF)) for i in range(dartModel.getBodyNum()): # body_i_jacobian = dartModel.getBody(i).world_jacobian()[range(-3, 3), :] # body_i_jacobian_deriv = dartModel.getBody(i).world_jacobian_classic_deriv()[range(-3, 3), :] # Jsys[6*i:6*i+6, :] = body_i_jacobian # dJsys[6*i:6*i+6, :] = body_i_jacobian_deriv Jsys[6*i:6*i+6, :] = dartModel.getBody(i).world_jacobian()[range(-3, 3), :] dJsys[6*i:6*i+6, :] = dartModel.getBody(i).world_jacobian_classic_deriv()[range(-3, 3), :] # dJsys = (Jsys - Jpre[0])/frame_step_size # Jpre[0] = Jsys.copy() JsupL = dartModel.getBody(supL).world_jacobian()[range(-3, 3), :] dJsupL = dartModel.getBody(supL).world_jacobian_classic_deriv()[range(-3, 3), :] # dJsupL = np.zeros_like(JsupL) # dJsupL = (JsupL - Jpre[1])/frame_step_size # Jpre[1] = JsupL.copy() JsupR = dartModel.getBody(supR).world_jacobian()[range(-3, 3), :] dJsupR = dartModel.getBody(supR).world_jacobian_classic_deriv()[range(-3, 3), :] # dJsupR = np.zeros_like(JsupR) # dJsupR = (JsupR - Jpre[2])/frame_step_size # Jpre[2] = JsupR.copy() #calculate footCenter footCenter = .5 * (footCenterL + footCenterR) + footOffset #if refFootR[1] >doubleTosingleOffset: #if refFootR[1] > doubleTosingleOffset or footCenterR[1] > 0.08: #if contact == 1 or footCenterR[1] > 0.08: #if contact == 2 or footCenterR[1] > doubleTosingleOffset/2: if contact == 2: footCenter = footCenterL.copy() + footOffset #elif contact == 1 or footCenterL[1] > doubleTosingleOffset/2: if contact == 1: footCenter = footCenterR.copy() + footOffset footCenter[1] = 0. if contactChangeCount > 0 and contactChangeType == 'StoD': #change footcenter gradually footCenter = preFootCenter + (maxContactChangeCount - contactChangeCount)*(footCenter-preFootCenter)/maxContactChangeCount preFootCenter = footCenter.copy() # linear momentum #TODO: # We should consider dCM_ref, shouldn't we? # add getBodyPositionGlobal and getBodyPositionsGlobal in csVpModel! # todo that, set joint velocities to vpModel CM_ref_plane = footCenter dL_des_plane = Kl*totalMass*(CM_ref_plane - CM_plane) - Dl*totalMass*dCM_plane dL_des_plane[1] = 0. # CM_ref = footCenter.copy() # CM_ref[1] = dartMotionModel.getCOM()[1] # CM_ref += np.array((0., com_offset_y, 0.)) # dL_des_plane = Kl*totalMass*(CM_ref - CM) - Dl*totalMass*dCM # angular momentum CP_ref = footCenter bodyIDs, contactPositions, contactPositionLocals, contactForces = [], [], [], [] if DART_CONTACT_ON: bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.get_dart_contact_info() else: bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds) #bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds) CP = yrp.getCP(contactPositions, contactForces) if CP_old[0] is None or CP is None: dCP = None else: dCP = (CP - CP_old[0])/frame_step_size CP_old[0] = CP CP_des[0] = None # if CP_des[0] is None: # CP_des[0] = footCenter if CP is not None and dCP is not None: ddCP_des = Kh*(CP_ref - CP) - Dh*(dCP) CP_des[0] = CP + dCP * frame_step_size + .5 * ddCP_des*(frame_step_size**2) # dCP_des[0] += ddCP_des * frame_step_size # CP_des[0] += dCP_des[0] * frame_step_size + .5 * ddCP_des*(frame_step_size ** 2) dH_des = np.cross(CP_des[0] - CM, dL_des_plane + totalMass*mm.s2v(wcfg.gravity)) if contactChangeCount > 0:# and contactChangeType == 'DtoS': #dH_des *= (maxContactChangeCount - contactChangeCount)/(maxContactChangeCount*10) dH_des *= (maxContactChangeCount - contactChangeCount)/(maxContactChangeCount) #dH_des *= (contactChangeCount)/(maxContactChangeCount)*.9+.1 else: dH_des = None # H = np.dot(P, np.dot(Jsys, dth_flat)) # dH_des = -Kh* H[3:] # soft point constraint #softConstPoint = refFootR.copy() ##softConstPoint[0] += 0.2 #Ksc = 50 #Dsc = 2*(Ksc**.5) #Bsc = 1. #P_des = softConstPoint #P_cur = controlModel.getBodyPositionGlobal(constBody) #dP_des = [0, 0, 0] #dP_cur = controlModel.getBodyVelocityGlobal(constBody) #ddP_des1 = Ksc*(P_des - P_cur) + Dsc*(dP_des - dP_cur) #r = P_des - P_cur #I = np.vstack(([1,0,0],[0,1,0],[0,0,1])) #Z = np.hstack((I, mm.getCrossMatrixForm(-r))) #yjc.computeJacobian2(Jconst, DOFs, jointPositions, jointAxeses, [softConstPoint], constJointMasks) #dJconst = (Jconst - Jconst)/(1/30.) #JconstPre = Jconst.copy() ##yjc.computeJacobianDerivative2(dJconst, DOFs, jointPositions, jointAxeses, linkAngVelocities, [softConstPoint], constJointMasks, False) #JL, JA = np.vsplit(Jconst, 2) #Q1 = np.dot(Z, Jconst) #q1 = np.dot(JA, dth_flat) #q2 = np.dot(mm.getCrossMatrixForm(q1), np.dot(mm.getCrossMatrixForm(q1), r)) #q_bias1 = np.dot(np.dot(Z, dJconst), dth_flat) + q2 #set up equality constraint a_oriL = mm.logSO3(mm.getSO3FromVectors(np.dot(footBodyOriL, np.array([0,1,0])), np.array([0,1,0]))) a_oriR = mm.logSO3(mm.getSO3FromVectors(np.dot(footBodyOriR, np.array([0,1,0])), np.array([0,1,0]))) footErrorL = refFootL.copy() footErrorL[1] = dartModel.getBody(supL).shapenodes[0].shape.size()[1]/2. footErrorL += -footCenterL + contMotionOffset footErrorR = refFootR.copy() footErrorR[1] = dartModel.getBody(supR).shapenodes[0].shape.size()[1]/2. footErrorR += -footCenterR + contMotionOffset a_supL = np.append(kt_sup*footErrorL + dt_sup*(refFootVelL - footBodyVelL), kt_sup*a_oriL+dt_sup*(refFootAngVelL-footBodyAngVelL)) a_supR = np.append(kt_sup*footErrorR + dt_sup*(refFootVelR - footBodyVelR), kt_sup*a_oriR+dt_sup*(refFootAngVelR-footBodyAngVelR)) if contactChangeCount > 0 and contactChangeType == 'DtoS': a_supL = np.append(kt_sup*(refFootL - footCenterL + contMotionOffset) + dt_sup*(refFootVelL - footBodyVelL), 4*kt_sup*a_oriL+2*dt_sup*(refFootAngVelL-footBodyAngVelL)) a_supR = np.append(kt_sup*(refFootR - footCenterR + contMotionOffset) + dt_sup*(refFootVelR - footBodyVelR), 4*kt_sup*a_oriR+2*dt_sup*(refFootAngVelR-footBodyAngVelR)) elif contactChangeCount > 0 and contactChangeType == 'StoD': linkt = (13.*contactChangeCount)/(maxContactChangeCount)+1. lindt = 2*(linkt**.5) angkt = (13.*contactChangeCount)/(maxContactChangeCount)+1. angdt = 2*(angkt**.5) a_supL = np.append(linkt*kt_sup*(refFootL - footCenterL + contMotionOffset) + lindt*dt_sup*(refFootVelL - footBodyVelL), angkt*kt_sup*a_oriL+angdt*dt_sup*(refFootAngVelL-footBodyAngVelL)) a_supR = np.append(linkt*kt_sup*(refFootR - footCenterR + contMotionOffset) + lindt*dt_sup*(refFootVelR - footBodyVelR), angkt*kt_sup*a_oriR+angdt*dt_sup*(refFootAngVelR-footBodyAngVelR)) # momentum matrix RS = np.dot(P, Jsys) R, S = np.vsplit(RS, 2) rs = np.dot((np.dot(dP, Jsys) + np.dot(P, dJsys)), dth_flat) r_bias, s_bias = np.hsplit(rs, 2) ####################################################### # optimization ####################################################### #if contact == 2 and footCenterR[1] > doubleTosingleOffset/2: if contact == 2: config['weightMap']['RightUpLeg'] = .8 config['weightMap']['RightLeg'] = .8 config['weightMap']['RightFoot'] = .8 else: config['weightMap']['RightUpLeg'] = .1 config['weightMap']['RightLeg'] = .25 config['weightMap']['RightFoot'] = .2 #if contact == 1 and footCenterL[1] > doubleTosingleOffset/2: if contact == 1: config['weightMap']['LeftUpLeg'] = .8 config['weightMap']['LeftLeg'] = .8 config['weightMap']['LeftFoot'] = .8 else: config['weightMap']['LeftUpLeg'] = .1 config['weightMap']['LeftLeg'] = .25 config['weightMap']['LeftFoot'] = .2 w = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap']) #if contact == 2: #mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1, q_bias1) mot.addTrackingTerms(problem, totalDOF, Bt, w, ddth_des_flat) mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias) if dH_des is not None: mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias) #mot.setConstraint(problem, totalDOF, Jsup, dJsup, dth_flat, a_sup) #mot.addConstraint(problem, totalDOF, Jsup, dJsup, dth_flat, a_sup) #if contact & 1 and contactChangeCount == 0: if contact & 1: #if refFootR[1] < doubleTosingleOffset: mot.addConstraint(problem, totalDOF, JsupR, dJsupR, dth_flat, a_supR) if contact & 2: #if refFootL[1] < doubleTosingleOffset: mot.addConstraint(problem, totalDOF, JsupL, dJsupL, dth_flat, a_supL) if contactChangeCount >0: contactChangeCount -= 1 if contactChangeCount == 0: maxContactChangeCount = 30 contactChangeType = 0 r = problem.solve() problem.clear() # ype.nested(r['x'], ddth_sol) ddth_sol = np.asarray(r['x']) # ddth_sol[:6] = np.zeros(6) rootPos[0] = dartModel.getBodyPositionGlobal(selectedBody) localPos = [[0, 0, 0]] for i in range(stepsPerFrame): # apply penalty force if not DART_CONTACT_ON: bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds) dartModel.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces) #bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds) # dartModel.skeleton.set_accelerations(ddth_sol) # dartModel.skeleton.set_accelerations(ddth_des_flat) # dartModel.skeleton.set_forces(np.zeros(totalDOF)) # ddth_des_flat[:6] = np.zeros(6) th_r_flat = dartMotionModel.get_q() ddth_des_flat = pdcontroller.compute_flat(th_r_flat) dartModel.skeleton.set_forces(ddth_des_flat) if forceShowTime > viewer.objectInfoWnd.labelForceDur.value(): forceShowTime = 0 viewer_ResetForceState() forceforce = np.array([viewer.objectInfoWnd.labelForceX.value(), viewer.objectInfoWnd.labelForceY.value(), viewer.objectInfoWnd.labelForceZ.value()]) extraForce[0] = getParamVal('Fm') * mm.normalize2(forceforce) if viewer_GetForceState(): forceShowTime += wcfg.timeStep dartModel.applyPenaltyForce(selectedBodyId, localPos, extraForce) dartModel.step() if DART_CONTACT_ON: bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.get_dart_contact_info() else: bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds) if viewer.reset: viewer.reset = False dartModel.reset() # print(dartModel.getCOM()) # rendering rightFootVectorX[0] = np.dot(footOriL, np.array([.1, 0, 0])) rightFootVectorY[0] = np.dot(footOriL, np.array([0, .1, 0])) rightFootVectorZ[0] = np.dot(footOriL, np.array([0, 0,.1])) rightFootPos[0] = footCenterL rightVectorX[0] = np.dot(footBodyOriL, np.array([.1,0,0])) rightVectorY[0] = np.dot(footBodyOriL, np.array([0,.1,0])) rightVectorZ[0] = np.dot(footBodyOriL, np.array([0,0,.1])) rightPos[0] = footCenterL + np.array([.1,0,0]) rd_footCenter[0] = footCenter rd_footCenterL[0] = footCenterL rd_footCenterR[0] = footCenterR rd_CM[0] = CM rd_CM_plane[0] = CM.copy() rd_CM_plane[0][1] = 0. if CP is not None and dCP is not None: rd_CP[0] = CP rd_CP_des[0] = CP_des[0] rd_dL_des_plane[0] = [dL_des_plane[0]/100, dL_des_plane[1]/100, dL_des_plane[2]/100] rd_dH_des[0] = dH_des rd_grf_des[0] = dL_des_plane - totalMass*mm.s2v(wcfg.gravity) rd_root_des[0] = rootPos[0] del rd_CF[:] del rd_CF_pos[:] for i in range(len(contactPositions)): rd_CF.append( contactForces[i]/100) rd_CF_pos.append(contactPositions[i].copy()) if viewer_GetForceState(): rd_exfen_des[0] = [extraForce[0][0]/100, extraForce[0][1]/100, extraForce[0][2]/100] rd_exf_des[0] = [0,0,0] else: rd_exf_des[0] = [extraForce[0][0]/100, extraForce[0][1]/100, extraForce[0][2]/100] rd_exfen_des[0] = [0,0,0] extraForcePos[0] = dartModel.getBodyPositionGlobal(selectedBody) viewer.setSimulateCallback(simulateCallback) viewer.startTimer(1/30.) viewer.show() Fl.run()
def main(): MOTION_ONLY = False pydart.init() env_name = 'walk' ppo = PPO(env_name, 1, visualize_only=True) if not MOTION_ONLY: ppo.LoadModel('model/' + env_name + '.pt') ppo.env.Resets(False) ppo.env.ref_skel.set_positions( ppo.env.ref_motion.get_q(ppo.env.phase_frame)) # viewer settings rd_contact_positions = [None] rd_contact_forces = [None] dart_world = ppo.env.world viewer = hsv.hpSimpleViewer(rect=(0, 0, 1200, 800), viewForceWnd=False) viewer.doc.addRenderer( 'MotionModel', yr.DartRenderer(ppo.env.ref_world, (150, 150, 255), yr.POLYGON_FILL)) if not MOTION_ONLY: viewer.doc.addRenderer( 'controlModel', yr.DartRenderer(dart_world, (255, 240, 255), yr.POLYGON_FILL)) viewer.doc.addRenderer( 'contact', yr.VectorsRenderer(rd_contact_forces, rd_contact_positions, (255, 0, 0))) def postCallback(frame): ppo.env.ref_skel.set_positions(ppo.env.ref_motion.get_q(frame)) def simulateCallback(frame): state = ppo.env.GetState(0) action_dist, _ = ppo.model(torch.tensor(state.reshape(1, -1)).float()) action = action_dist.loc.detach().numpy() res = ppo.env.Steps(action) # res = ppo.env.Steps(np.zeros_like(action)) # print(frame, ppo.env.ref_skel.current_frame, ppo.env.world.time()*ppo.env.ref_motion.fps) # print(frame, res[0][0]) # if res[0][0] > 0.46: # ppo.env.continue_from_now_by_phase(0.2) if res[2]: print(frame, 'Done') ppo.env.reset() # contact rendering contacts = ppo.env.world.collision_result.contacts del rd_contact_forces[:] del rd_contact_positions[:] for contact in contacts: rd_contact_forces.append(contact.f / 1000.) rd_contact_positions.append(contact.p) if MOTION_ONLY: viewer.setPostFrameCallback_Always(postCallback) viewer.setMaxFrame(len(ppo.env.ref_motion) - 1) else: viewer.setSimulateCallback(simulateCallback) viewer.setMaxFrame(3000) viewer.startTimer(1. / 30.) viewer.show() Fl.run()
import os from gym import error, spaces from gym.utils import seeding import numpy as np from os import path import gym import six from gym.envs.dart.static_window import * try: import pydart2 as pydart from pydart2.gui.trackball import Trackball pydart.init() except ImportError as e: raise error.DependencyNotInstalled( "{}. (HINT: you need to install pydart2.)".format(e)) class DartEnv(gym.Env): """Superclass for all Dart environments. """ 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')
def main(): MOTION_ONLY = False pydart.init() env_name = 'walk' ppo = PPO(env_name, 0, visualize_only=True) if not MOTION_ONLY: ppo.LoadModel('model/param.pt') # ppo.LoadModel('walk_model_10240127/'+'181'+'.pt') ppo.generate_rnn_motion() ppo.envs_send_rnn_motion() ppo.env.Resets(False) # ppo.replace_motion_num = ppo.rnn_len # ppo.env.ref_skel.set_positions(ppo.env.ref_motion.get_q(ppo.env.phase_frame)) # viewer settings rd_contact_positions = [None] rd_contact_forces = [None] rd_target_position = [None] rd_com = [None] dart_world = ppo.env.world viewer = hsv.hpSimpleViewer(rect=[0, 0, 1280 + 300, 720 + 1 + 55], viewForceWnd=False) viewer.doc.addRenderer( 'MotionModel', yr.DartRenderer(ppo.env.ref_world, (150, 150, 255), yr.POLYGON_FILL)) viewer.doc.addRenderer('target', yr.PointsRenderer(rd_target_position, (0, 255, 0))) viewer.doc.addRenderer('motion', yr.JointMotionRenderer(ppo.env.ref_motion)) if not MOTION_ONLY: viewer.doc.addRenderer( 'controlModel', yr.DartRenderer(dart_world, (255, 240, 255), yr.POLYGON_FILL)) viewer.doc.addRenderer( 'contact', yr.VectorsRenderer(rd_contact_forces, rd_contact_positions, (255, 0, 0))) viewer.doc.addRenderer('CM_plane', yr.PointsRenderer(rd_com, (0, 0, 255))) last_frame = [0] q = ppo.env.ref_skel.q def simulateCallback(frame): ppo.env.ref_motion.frame = frame - last_frame[0] del rd_target_position[:] del rd_com[:] rd_target_position.append( ppo.env.goals_in_world_frame[ppo.env.phase_frame]) com = ppo.env.skel.com() com[1] = 0. rd_com.append(com) state = ppo.env.GetState(0) action_dist, _ = ppo.model(torch.tensor(state.reshape(1, -1)).float()) action = action_dist.loc.detach().numpy() # res = ppo.env.Steps(np.zeros_like(action)) res = ppo.env.Steps(action) # res = [False, False, False] # print(res[1]) # res = ppo.env.Steps(np.zeros_like(action)) # print(frame, ppo.env.ref_skel.current_frame, ppo.env.world.time()*ppo.env.ref_motion.fps) # print(frame, res[0][0]) # if res[0][0] > 0.46: # ppo.env.continue_from_now_by_phase(0.2) # print(ppo.env.goal) if res[2]: print(frame, 'Done') last_frame[0] = frame ppo.generate_rnn_motion() ppo.envs_send_rnn_motion() ppo.env.reset() # contact rendering contacts = ppo.env.world.collision_result.contacts del rd_contact_forces[:] del rd_contact_positions[:] for contact in contacts: rd_contact_forces.append(contact.f / 1000.) rd_contact_positions.append(contact.p) # viewer.setPreFrameCallback_Always(preCallback) viewer.setSimulateCallback(simulateCallback) viewer.setMaxFrame(3000) viewer.startTimer(1. / 30.) viewer.show() Fl.run()