def main(): # np.set_printoptions(precision=4, linewidth=200) np.set_printoptions(precision=5, threshold=np.inf, suppress=True, linewidth=3000) motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped() # motion, mcfg, wcfg, stepsPerFrame, config = mit.create_jump_biped() vpWorld = cvw.VpWorld(wcfg) motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg) controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg) controlModel_shadow_for_ik = cvm.VpControlModel(vpWorld, motion[0], mcfg) vpWorld.initialize() controlModel.initializeHybridDynamics() # controlToMotionOffset = (1.5, -0.02, 0) controlToMotionOffset = (1.5, 0, 0) controlModel.translateByOffset(controlToMotionOffset) controlModel_shadow_for_ik.computeJacobian(0, np.array([0., 0., 0.])) pydart.init() dartModel = cdm.DartModel(wcfg, motion[0], mcfg, DART_CONTACT_ON) dartModel.set_q(controlModel.get_q()) totalDOF = controlModel.getTotalDOF() DOFs = controlModel.getDOFs() foot_dofs = [] left_foot_dofs = [] right_foot_dofs = [] foot_seg_dofs = [] left_foot_seg_dofs = [] right_foot_seg_dofs = [] # for joint_idx in range(motion[0].skeleton.getJointNum()): for joint_idx in range(controlModel.getJointNum()): joint_name = controlModel.index2name(joint_idx) # joint_name = motion[0].skeleton.getJointName(joint_idx) if 'Foot' in joint_name: foot_dofs_temp = controlModel.getJointDOFIndexes(joint_idx) foot_dofs.extend(foot_dofs_temp) if 'Left' in joint_name: left_foot_dofs.extend(foot_dofs_temp) elif 'Right' in joint_name: right_foot_dofs.extend(foot_dofs_temp) if 'foot' in joint_name: foot_dofs_temp = controlModel.getJointDOFIndexes(joint_idx) foot_seg_dofs.extend(foot_dofs_temp) if 'Left' in joint_name: left_foot_seg_dofs.extend(foot_dofs_temp) elif 'Right' in joint_name: right_foot_seg_dofs.extend(foot_dofs_temp) # 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'] 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') # jacobian # JsupL = yjc.makeEmptyJacobian(DOFs, 1) # dJsupL = JsupL.copy() # JsupPreL = JsupL.copy() # # JsupR = yjc.makeEmptyJacobian(DOFs, 1) # dJsupR = JsupR.copy() # JsupPreR = JsupR.copy() Jconst = yjc.makeEmptyJacobian(DOFs, 1) dJconst = Jconst.copy() JconstPre = Jconst.copy() Jsys = yjc.makeEmptyJacobian(DOFs, controlModel.getBodyNum()) dJsys = Jsys.copy() JsysPre = Jsys.copy() constJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, constBody)] allLinkJointMasks = yjc.getAllLinkJointMasks(motion[0].skeleton) # momentum matrix linkMasses = controlModel.getBodyMasses() totalMass = controlModel.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 CP_old = [mm.v3(0., 0., 0.)] # penalty method bodyIDsToCheck = list(range(vpWorld.getBodyNum())) # 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(rect=[0, 0, 1024, 768], 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.VpModelRenderer(motionModel, (150, 150, 255), yr.POLYGON_FILL)) viewer.doc.addRenderer( 'dartModel', yr.DartModelRenderer(dartModel, (150, 150, 255), yr.POLYGON_LINE)) # viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_LINE)) control_model_renderer = yr.VpModelRenderer(controlModel, (255, 240, 255), yr.POLYGON_FILL) viewer.doc.addRenderer('controlModel', control_model_renderer) 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, 255, 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))) # foot_viewer = FootWindow(viewer.x() + viewer.w() + 20, viewer.y(), 300, 400, 'foot contact modifier', controlModel) foot_viewer = None # type: FootWindow # 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 = 100. initKh = 100. initBl = .1 initBh = .13 initSupKt = 17 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., 300., 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) def getParamVal(paramname): return viewer.objectInfoWnd.getVal(paramname) def getParamVals(paramnames): return (getParamVal(name) for name in paramnames) extendedFootName = [ 'Foot_foot_0_0', 'Foot_foot_0_1', 'Foot_foot_0_0_0', 'Foot_foot_0_1_0', 'Foot_foot_1_0' ] 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 } footIdDic = lIDdic.copy() footIdDic.update(rIDdic) lIDlist = [ motion[0].skeleton.getJointIndex('Left' + name) for name in extendedFootName ] rIDlist = [ motion[0].skeleton.getJointIndex('Right' + name) for name in extendedFootName ] footIdlist = [] footIdlist.extend(lIDlist) footIdlist.extend(rIDlist) foot_left_idx = motion[0].skeleton.getJointIndex('LeftFoot') foot_right_idx = motion[0].skeleton.getJointIndex('RightFoot') foot_left_idx_temp = motion[0].skeleton.getJointIndex('LeftFoot_foot_1_0') foot_right_idx_temp = motion[0].skeleton.getJointIndex( 'RightFoot_foot_1_0') def get_jacobianbase_and_masks(skeleton, DOFs, joint_idx): J = yjc.makeEmptyJacobian(DOFs, 1) joint_masks = [yjc.getLinkJointMask(skeleton, joint_idx)] return J, joint_masks ################################### # simulate ################################### def simulateCallback(frame): motionModel.update(motion[frame]) # dartModel.update(motion[frame]) dartModel.set_q(controlModel.get_q()) global g_initFlag global forceShowTime global JsysPre global JsupPreL global JsupPreR global JconstPre global preFootCenter global maxContactChangeCount global contactChangeCount global contact global contactChangeType # Kt, Kl, Kh, Bl, Bh, kt_sup = viewer.GetParam() Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals( ['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt']) Dt = 2 * (Kt**.5) Dl = 2 * (Kl**.5) Dh = 2 * (Kh**.5) dt_sup = 2 * (kt_sup**.5) doubleTosingleOffset = 0.15 singleTodoubleOffset = 0.30 # doubleTosingleOffset = 0.09 doubleTosingleVelOffset = 0.0 # tracking # print(len(motion.get_q(frame))) # print(motion.get_q(frame)) # print(motion.get_dq(frame)) # print(len(controlModel.get_q())) # print(controlModel.get_q()) # print(controlModel.get_dq()) # print(np.asarray(motion.get_dq(frame)) - np.asarray(controlModel.get_dq())) # print(np.asarray(dartModel.get_q())[:6]) # print(controlModel.get_q()[:6]) th_r = motion.getDOFPositions(frame) th = controlModel.getDOFPositions() dth_r = motion.getDOFVelocities(frame) dth = controlModel.getDOFVelocities() ddth_r = motion.getDOFAccelerations(frame) ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt) ype.flatten(ddth_des, ddth_des_flat) # ddth_des_flat = Kt * (motion.get_q(frame) - np.array(controlModel.get_q())) - Dt * np.array(controlModel.get_dq()) ype.flatten(dth, dth_flat) # dth_flat = np.array(controlModel.get_dq()) ################################################# # jacobian ################################################# contact_ids = list() # contact_ids = [supL, supR] # contact_ids = footIdlist if foot_viewer.check_om_l.value(): contact_ids.append( motion[0].skeleton.getJointIndex('LeftFoot_foot_0_0')) if foot_viewer.check_op_l.value(): contact_ids.append( motion[0].skeleton.getJointIndex('LeftFoot_foot_0_0_0')) if foot_viewer.check_im_l.value(): contact_ids.append( motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1')) if foot_viewer.check_ip_l.value(): contact_ids.append( motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1_0')) if foot_viewer.check_h_l.value(): contact_ids.append( motion[0].skeleton.getJointIndex('LeftFoot_foot_1_0')) if foot_viewer.check_om_r.value(): contact_ids.append( motion[0].skeleton.getJointIndex('RightFoot_foot_0_0')) if foot_viewer.check_op_r.value(): contact_ids.append( motion[0].skeleton.getJointIndex('RightFoot_foot_0_0_0')) if foot_viewer.check_im_r.value(): contact_ids.append( motion[0].skeleton.getJointIndex('RightFoot_foot_0_1')) if foot_viewer.check_ip_r.value(): contact_ids.append( motion[0].skeleton.getJointIndex('RightFoot_foot_0_1_0')) if foot_viewer.check_h_r.value(): contact_ids.append( motion[0].skeleton.getJointIndex('RightFoot_foot_1_0')) contact_joint_ori = list( map(controlModel.getJointOrientationGlobal, contact_ids)) contact_joint_pos = list( map(controlModel.getJointPositionGlobal, contact_ids)) contact_body_ori = list( map(controlModel.getBodyOrientationGlobal, contact_ids)) contact_body_pos = list( map(controlModel.getBodyPositionGlobal, contact_ids)) contact_body_vel = list( map(controlModel.getBodyVelocityGlobal, contact_ids)) contact_body_angvel = list( map(controlModel.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(motionModel.getBodyOrientationGlobal, contact_ids)) ref_body_pos = list(map(motionModel.getBodyPositionGlobal, contact_ids)) # 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)) ] J_contacts = [ yjc.makeEmptyJacobian(DOFs, 1) for i in range(len(contact_ids)) ] dJ_contacts = [ yjc.makeEmptyJacobian(DOFs, 1) for i in range(len(contact_ids)) ] joint_masks = [ yjc.getLinkJointMask(motion[0].skeleton, joint_idx) for joint_idx in contact_ids ] # caution!! body orientation and joint orientation of foot are totally different!! footOriL = controlModel.getJointOrientationGlobal(supL) footOriR = controlModel.getJointOrientationGlobal(supR) # desire footCenter[1] = 0.041135 # desire footCenter[1] = 0.0197 footCenterL = controlModel.getBodyPositionGlobal(supL) footCenterR = controlModel.getBodyPositionGlobal(supR) footBodyOriL = controlModel.getBodyOrientationGlobal(supL) footBodyOriR = controlModel.getBodyOrientationGlobal(supR) footBodyVelL = controlModel.getBodyVelocityGlobal(supL) footBodyVelR = controlModel.getBodyVelocityGlobal(supR) footBodyAngVelL = controlModel.getBodyAngVelocityGlobal(supL) footBodyAngVelR = controlModel.getBodyAngVelocityGlobal(supR) refFootL = motionModel.getBodyPositionGlobal(supL) refFootR = motionModel.getBodyPositionGlobal(supR) refFootVelL = motionModel.getBodyVelocityGlobal(supL) refFootVelR = motionModel.getBodyVelocityGlobal(supR) refFootAngVelL = motionModel.getBodyAngVelocityGlobal(supL) refFootAngVelR = motionModel.getBodyAngVelocityGlobal(supR) refFootJointVelR = motion.getJointVelocityGlobal(supR, frame) refFootJointAngVelR = motion.getJointAngVelocityGlobal(supR, frame) refFootJointR = motion.getJointPositionGlobal(supR, frame) refFootVelR = refFootJointVelR + np.cross(refFootJointAngVelR, (refFootR - refFootJointR)) refFootJointVelL = motion.getJointVelocityGlobal(supL, frame) refFootJointAngVelL = motion.getJointAngVelocityGlobal(supL, frame) refFootJointL = motion.getJointPositionGlobal(supL, frame) refFootVelL = refFootJointVelL + np.cross(refFootJointAngVelL, (refFootL - refFootJointL)) is_contact = [1] * len(contact_ids) contactR = 1 contactL = 1 if refFootVelR[1] < 0 and refFootVelR[1] / 30. + refFootR[ 1] > singleTodoubleOffset: contactR = 0 if refFootVelL[1] < 0 and refFootVelL[1] / 30. + refFootL[ 1] > singleTodoubleOffset: contactL = 0 if refFootVelR[1] > 0 and refFootVelR[1] / 30. + refFootR[ 1] > doubleTosingleOffset: contactR = 0 if refFootVelL[1] > 0 and refFootVelL[1] / 30. + refFootL[ 1] > doubleTosingleOffset: contactL = 0 # contactR = 1 contMotionOffset = th[0][0] - th_r[0][0] linkPositions = controlModel.getBodyPositionsGlobal() linkVelocities = controlModel.getBodyVelocitiesGlobal() linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal() linkInertias = controlModel.getBodyInertiasGlobal() jointPositions = controlModel.getJointPositionsGlobal() jointAxeses = controlModel.getDOFAxeses() CM = yrp.getCM(linkPositions, linkMasses, totalMass) dCM = yrp.getCM(linkVelocities, linkMasses, totalMass) 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: # JsysPre = Jsys.copy() JconstPre = Jconst.copy() softConstPoint = footCenterR.copy() # yjc.computeJacobian2(JsysPre, DOFs, jointPositions, jointAxeses, linkPositions, allLinkJointMasks) # yjc.computeJacobian2(JconstPre, DOFs, jointPositions, jointAxeses, [softConstPoint], constJointMasks) 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 g_initFlag = 1 # calculate jacobian Jsys = yjc.makeEmptyJacobian(DOFs, controlModel.getBodyNum()) yjc.computeJacobian2(Jsys, DOFs, jointPositions, jointAxeses, linkPositions, allLinkJointMasks) dJsys = (Jsys - JsysPre) / (1 / 30.) JsysPre = Jsys.copy() # # yjc.computeJacobianDerivative2(dJsys, DOFs, jointPositions, jointAxeses, linkAngVelocities, linkPositions, allLinkJointMasks) # print(np.dot(Jsys, dth_flat)) vp_legacy = np.dot(Jsys, dth_flat) # print(Jsys) # ''' # calculate jacobian using dart body_num = dartModel.getBodyNum() Jsys_dart = np.zeros((6 * body_num, totalDOF)) dJsys_dart = 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_dart[6 * i:6 * i + 6, :] = dartModel.getBody( i).world_jacobian()[range(-3, 3), :] dJsys_dart[6 * i:6 * i + 6, :] = dartModel.getBody( i).world_jacobian_classic_deriv()[range(-3, 3), :] # print(np.dot(Jsys, controlModel.get_dq())) dart_result = np.dot(Jsys_dart, controlModel.get_dq()) # print(Jsys) # ''' Jsys_hp = np.zeros_like(Jsys_dart) for i in range(len(linkPositions)): Jsys_hp[6 * i:6 * i + 6, :] = controlModel.computeJacobian( i, linkPositions[i]) # Jsys = Jsys_hp # print('vpJ : ', vp_legacy) # print('hpJ : ', np.dot(Jsys_hp, controlModel.get_dq())) # print('dart: ', dart_result) # print('vp : ', np.asarray([[controlModel.getBodyVelocityGlobal(i), controlModel.getBodyAngVelocityGlobal(i)] for i in range(controlModel.getBodyNum())]).flatten()) # print(np.linalg.norm(vp_legacy - dart_result)) for i in range(len(J_contacts)): J_contacts[i] = Jsys[6 * contact_ids[i]:6 * contact_ids[i] + 6, :] dJ_contacts[i] = dJsys[6 * contact_ids[i]:6 * contact_ids[i] + 6, :] # yjc.computeJacobian2(J_contacts[i], DOFs, jointPositions, jointAxeses, [contact_body_pos[i]], [joint_masks[i]]) # yjc.computeJacobianDerivative2( # dJ_contacts[i], DOFs, jointPositions, jointAxeses, linkAngVelocities, [contact_body_pos[i]], [joint_masks[i]]) # calculate footCenter footCenter = sum(contact_body_pos) / len(contact_body_pos) if len(contact_body_pos) > 0 \ else .5 * (controlModel.getBodyPositionGlobal(supL) + controlModel.getBodyPositionGlobal(supR)) # if len(contact_body_pos) > 2: # hull = ConvexHull(contact_body_pos) footCenter_ref = sum(ref_body_pos) / len(ref_body_pos) if len(ref_body_pos) > 0 \ else .5 * (motionModel.getBodyPositionGlobal(supL) + motionModel.getBodyPositionGlobal(supR)) footCenter_ref = footCenter_ref + contMotionOffset # if len(ref_body_pos) > 2: # hull = ConvexHull(ref_body_pos) footCenter_ref[1] = 0. # footCenter = footCenterL + (footCenterR - footCenterL)/2.0 # 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() # elif contact == 1 or footCenterL[1] > doubleTosingleOffset/2: if contact == 1: footCenter = footCenterR.copy() 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! # to do that, set joint velocities to vpModel CM_ref_plane = footCenter # CM_ref_plane = footCenter_ref dL_des_plane = Kl * totalMass * (CM_ref_plane - CM_plane) - Dl * totalMass * dCM_plane # dL_des_plane[1] = 0. # angular momentum CP_ref = footCenter # CP_ref = footCenter_ref bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.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]) / (1 / 30.) CP_old[0] = CP if CP is not None and dCP is not None: ddCP_des = Kh * (CP_ref - CP) - Dh * dCP CP_des = CP + dCP * (1 / 30.) + .5 * ddCP_des * ((1 / 30.)**2) dH_des = np.cross( (CP_des - CM), (dL_des_plane + totalMass * mm.s2v(wcfg.gravity))) if contactChangeCount > 0: # and contactChangeType == 'DtoS': dH_des *= (maxContactChangeCount - contactChangeCount) / maxContactChangeCount else: dH_des = None # set up equality constraint # TODO: # logSO3 is just q'', not acceleration. # To make a_oris acceleration, q'' -> a will be needed # body_ddqs = list(map(mm.logSO3, [mm.getSO3FromVectors(np.dot(body_ori, mm.unitY()), mm.unitY()) for body_ori in contact_body_ori])) body_ddqs = list( map(mm.logSO3, [ np.dot( contact_body_ori[i].T, np.dot( ref_body_ori[i], mm.getSO3FromVectors( np.dot(ref_body_ori[i], mm.unitY()), mm.unitY()))) for i in range(len(contact_body_ori)) ])) body_qs = list(map(mm.logSO3, contact_body_ori)) body_angs = [ np.dot(contact_body_ori[i], contact_body_angvel[i]) for i in range(len(contact_body_ori)) ] body_dqs = [ mm.vel2qd(body_angs[i], body_qs[i]) for i in range(len(body_angs)) ] a_oris = [ np.dot(contact_body_ori[i], mm.qdd2accel(body_ddqs[i], body_dqs[i], body_qs[i])) for i in range(len(contact_body_ori)) ] # body_ddq = body_ddqs[0] # body_ori = contact_body_ori[0] # body_ang = np.dot(body_ori.T, contact_body_angvel[0]) # # body_q = mm.logSO3(body_ori) # body_dq = mm.vel2qd(body_ang, body_q) # a_ori = np.dot(body_ori, mm.qdd2accel(body_ddq, body_dq, body_q)) # a_oris = list(map(mm.logSO3, [mm.getSO3FromVectors(np.dot(body_ori, mm.unitY()), mm.unitY()) for body_ori in contact_body_ori])) a_sups = [ np.append( kt_sup * (ref_body_pos[i] - contact_body_pos[i] + contMotionOffset) + dt_sup * (ref_body_vel[i] - contact_body_vel[i]), kt_sup * a_oris[i] + dt_sup * (ref_body_angvel[i] - contact_body_angvel[i])) for i in range(len(a_oris)) ] # 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) if dH_des is not None: mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias) 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 True: for c_idx in range(len(contact_ids)): mot.addConstraint(problem, totalDOF, J_contacts[c_idx], dJ_contacts[c_idx], dth_flat, a_sups[c_idx]) if contactChangeCount > 0: contactChangeCount = contactChangeCount - 1 if contactChangeCount == 0: maxContactChangeCount = 30 contactChangeType = 0 r = problem.solve() problem.clear() ddth_sol_flat = np.asarray(r['x']) # ddth_sol_flat[foot_seg_dofs] = np.array(ddth_des_flat)[foot_seg_dofs] ype.nested(ddth_sol_flat, ddth_sol) rootPos[0] = controlModel.getBodyPositionGlobal(selectedBody) localPos = [[0, 0, 0]] for i in range(stepsPerFrame): # apply penalty force bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds) # bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds) vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces) controlModel.setDOFAccelerations(ddth_sol) # controlModel.set_ddq(ddth_sol_flat) # controlModel.set_ddq(ddth_des_flat) controlModel.solveHybridDynamics() 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 vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce) vpWorld.step() dartModel.set_q(controlModel.get_q()) if foot_viewer is not None: foot_viewer.foot_pressure_gl_window.refresh_foot_contact_info( frame, vpWorld, bodyIDsToCheck, mus, Ks, Ds) foot_viewer.foot_pressure_gl_window.goToFrame(frame) # rendering for foot_seg_id in footIdlist: control_model_renderer.body_colors[foot_seg_id] = (255, 240, 255) for contact_id in contact_ids: control_model_renderer.body_colors[contact_id] = (255, 0, 0) 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 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] / 400) 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] = controlModel.getBodyPositionGlobal(selectedBody) # render contact_ids viewer.setSimulateCallback(simulateCallback) viewer.startTimer(1 / 30.) # viewer.play() viewer.show() foot_viewer = FootWindow(viewer.x() + viewer.w() + 20, viewer.y(), 300, 500, 'foot contact modifier', controlModel) foot_viewer.show() Fl.run()
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 main(): # np.set_printoptions(precision=4, linewidth=200) np.set_printoptions(precision=5, threshold=np.inf, suppress=True, linewidth=3000) motionFile = 'wd2_tiptoe.bvh' motionFile = 'wd2_tiptoe_zygote.bvh' # motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped(motionFile, SEGMENT_FOOT_RAD=0.008) motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped( motionFile, SEGMENT_FOOT_MAG=0.01, SEGMENT_FOOT_RAD=0.008) # motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped() # motion, mcfg, wcfg, stepsPerFrame, config = mit.create_jump_biped() vpWorld = cvw.VpWorld(wcfg) vpWorld.SetGlobalDamping(0.999) motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg) controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg) # controlModel_shadow_for_ik = cvm.VpControlModel(vpWorld, motion[0], mcfg) vpWorld.initialize() controlModel.initializeHybridDynamics() # controlToMotionOffset = (1.5, -0.02, 0) controlToMotionOffset = (0., 0., 0) controlModel.translateByOffset(controlToMotionOffset) # controlModel_shadow_for_ik.set_q(controlModel.get_q()) # controlModel_shadow_for_ik.computeJacobian(0, np.array([0., 0., 0.])) wcfg_ik = copy.deepcopy(wcfg) vpWorld_ik = cvw.VpWorld(wcfg_ik) controlModel_ik = cvm.VpControlModel(vpWorld_ik, motion[0], mcfg) vpWorld_ik.initialize() controlModel_ik.set_q(np.zeros_like(controlModel.get_q())) controlModel_q = np.zeros_like(controlModel.get_q()) controlModel_q[4] = controlModel_q[4] + 1.2 - 0.24 controlModel.set_q(controlModel_q) totalDOF = controlModel.getTotalDOF() DOFs = controlModel.getDOFs() foot_dofs = [] left_foot_dofs = [] right_foot_dofs = [] foot_seg_dofs = [] left_foot_seg_dofs = [] right_foot_seg_dofs = [] # for joint_idx in range(motion[0].skeleton.getJointNum()): for joint_idx in range(controlModel.getJointNum()): joint_name = controlModel.index2name(joint_idx) # joint_name = motion[0].skeleton.getJointName(joint_idx) if 'Foot' in joint_name: foot_dofs_temp = controlModel.getJointDOFIndexes(joint_idx) foot_dofs.extend(foot_dofs_temp) if 'Left' in joint_name: left_foot_dofs.extend(foot_dofs_temp) elif 'Right' in joint_name: right_foot_dofs.extend(foot_dofs_temp) if 'foot' in joint_name: foot_dofs_temp = controlModel.getJointDOFIndexes(joint_idx) foot_seg_dofs.extend(foot_dofs_temp) if 'Left' in joint_name: left_foot_seg_dofs.extend(foot_dofs_temp) elif 'Right' in joint_name: right_foot_seg_dofs.extend(foot_dofs_temp) # 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'] selectedBody = motion[0].skeleton.getJointIndex(config['end']) constBody = motion[0].skeleton.getJointIndex('RightFoot') supL = motion[0].skeleton.getJointIndex('LeftFoot') supR = motion[0].skeleton.getJointIndex('RightFoot') # momentum matrix linkMasses = controlModel.getBodyMasses() totalMass = controlModel.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 CP_old = [mm.v3(0., 0., 0.)] # penalty method bodyIDsToCheck = list(range(vpWorld.getBodyNum())) # 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_footCenter_ref = [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_foot_ori = [None] rd_foot_pos = [None] rd_root_ori = [None] rd_root_pos = [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] 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 # viewer = ysv.SimpleViewer() # viewer = hsv.hpSimpleViewer(rect=[0, 0, 1024, 768], viewForceWnd=False) viewer = hsv.hpSimpleViewer(rect=[0, 0, 1920 + 300, 1 + 1080 + 55], 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.VpModelRenderer(motionModel, (150, 150, 255), yr.POLYGON_FILL)) viewer.doc.setRendererVisible('motionModel', False) viewer.doc.addRenderer( 'ikModel', yr.VpModelRenderer(controlModel_ik, (50, 50, 50), yr.POLYGON_LINE)) # viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_LINE)) control_model_renderer = yr.VpModelRenderer(controlModel, (255, 240, 255), yr.POLYGON_FILL) viewer.doc.addRenderer('controlModel', control_model_renderer) viewer.doc.setRendererVisible('controlModel', False) 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) viewer.doc.setRendererVisible('skeleton', False) viewer.doc.addRenderer('rd_footCenter', yr.PointsRenderer(rd_footCenter)) viewer.doc.setRendererVisible('rd_footCenter', False) viewer.doc.addRenderer('rd_footCenter_ref', yr.PointsRenderer(rd_footCenter_ref)) viewer.doc.setRendererVisible('rd_footCenter_ref', False) viewer.doc.addRenderer('rd_CM_plane', yr.PointsRenderer(rd_CM_plane, (255, 255, 0))) viewer.doc.setRendererVisible('rd_CM_plane', False) viewer.doc.addRenderer('rd_CP', yr.PointsRenderer(rd_CP, (0, 255, 0))) viewer.doc.setRendererVisible('rd_CP', False) viewer.doc.addRenderer('rd_CP_des', yr.PointsRenderer(rd_CP_des, (255, 0, 255))) viewer.doc.setRendererVisible('rd_CP_des', False) viewer.doc.addRenderer( 'rd_dL_des_plane', yr.VectorsRenderer(rd_dL_des_plane, rd_CM, (255, 255, 0))) viewer.doc.setRendererVisible('rd_dL_des_plane', False) viewer.doc.addRenderer('rd_dH_des', yr.VectorsRenderer(rd_dH_des, rd_CM, (0, 255, 0))) viewer.doc.setRendererVisible('rd_dH_des', False) # 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, 255, 0))) viewer.doc.setRendererVisible('rd_CF', False) viewer.doc.addRenderer( 'rd_foot_ori', yr.OrientationsRenderer(rd_foot_ori, rd_foot_pos, (255, 255, 0))) viewer.doc.setRendererVisible('rd_foot_ori', False) viewer.doc.addRenderer( 'rd_root_ori', yr.OrientationsRenderer(rd_root_ori, rd_root_pos, (255, 255, 0))) viewer.doc.setRendererVisible('rd_root_ori', False) viewer.doc.addRenderer( 'extraForceEnable', yr.JointArrowRenderer(rd_exf_des, extraForcePos, (255, 0, 0), lineWidth=0.004, polygonStyle=yr.POLYGON_FILL)) # 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))) # foot_viewer = FootWindow(viewer.x() + viewer.w() + 20, viewer.y(), 300, 400, 'foot contact modifier', controlModel) foot_viewer = None # type: FootWindow initKt = 25. # initKt = 60. initKl = 100. initKh = 100. initBl = .1 initBh = .13 # initSupKt = 17 initSupKt = 22 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., 300., 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.001, initComY) viewer.objectInfoWnd.add1DSlider("com Z offset", -1., 1., 0.01, initComZ) viewer.objectInfoWnd.add1DSlider("tiptoe angle", -0.5, .5, 0.001, 0.) viewer.objectInfoWnd.add1DSlider("left tilt angle", -0.5, .5, 0.001, 0.) viewer.objectInfoWnd.add1DSlider("right tilt angle", -0.5, .5, 0.001, 0.) 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) def getParamVal(paramname): return viewer.objectInfoWnd.getVal(paramname) def getParamVals(paramnames): return (getParamVal(name) for name in paramnames) def setParamVal(paramname, val): viewer.objectInfoWnd.setVal(paramname, val) idDic = dict() for i in range(motion[0].skeleton.getJointNum()): idDic[motion[0].skeleton.getJointName(i)] = i # extendedFootName = ['Foot_foot_0_0', 'Foot_foot_0_1', 'Foot_foot_0_0_0', 'Foot_foot_0_1_0', 'Foot_foot_1_0'] extendedFootName = [ 'Foot_foot_0_0', 'Foot_foot_0_0_0', 'Foot_foot_0_1_0', 'Foot_foot_1_0' ] 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 } footIdDic = lIDdic.copy() footIdDic.update(rIDdic) lIDlist = [ motion[0].skeleton.getJointIndex('Left' + name) for name in extendedFootName ] rIDlist = [ motion[0].skeleton.getJointIndex('Right' + name) for name in extendedFootName ] footIdlist = [] footIdlist.extend(lIDlist) footIdlist.extend(rIDlist) foot_left_idx = motion[0].skeleton.getJointIndex('LeftFoot') foot_right_idx = motion[0].skeleton.getJointIndex('RightFoot') foot_left_idx_temp = motion[0].skeleton.getJointIndex('LeftFoot_foot_1_0') foot_right_idx_temp = motion[0].skeleton.getJointIndex( 'RightFoot_foot_1_0') # ik_solver = hik.numIkSolver(dartIkModel) # ik_solver.clear() # bodyIDsToCheck = rIDlist.copy() joint_names = [ motion[0].skeleton.getJointName(i) for i in range(motion[0].skeleton.getJointNum()) ] def fix_dofs(_DOFs, nested_dof_values, _mcfg, _joint_names): fixed_nested_dof_values = list() fixed_nested_dof_values.append(nested_dof_values[0]) for i in range(1, len(_DOFs)): dof = _DOFs[i] if dof == 1: node = _mcfg.getNode(_joint_names[i]) axis = mm.unitZ() if node.jointAxes[0] == 'X': axis = mm.unitX() elif node.jointAxes[0] == 'Y': axis = mm.unitY() fixed_nested_dof_values.append( np.array([np.dot(nested_dof_values[i], axis)])) else: fixed_nested_dof_values.append(nested_dof_values[i]) return fixed_nested_dof_values start_frame = 130 up_vec_in_each_link = dict() for foot_id in footIdlist: up_vec_in_each_link[ foot_id] = controlModel_ik.getBodyOrientationGlobal(foot_id)[1, :] controlModel_ik.set_q(controlModel.get_q()) controlModel.update(motion[0]) ################################### # simulate ################################### def preFrameCallback_Always(frame): if False: viewer.motionViewWnd.glWindow.camera.rotateX = math.pi / 180. * -25. if frame > start_frame: viewer.motionViewWnd.glWindow.camera.rotateY = mm.deg2Rad( (frame - start_frame) * 3 + 45.) else: viewer.motionViewWnd.glWindow.camera.rotateY = mm.deg2Rad(45.) viewer.motionViewWnd.glWindow.camera.distance = .4 viewer.motionViewWnd.glWindow.camera.center = \ .5*(controlModel.getBodyPositionGlobal(idDic['RightFoot']) + controlModel.getBodyPositionGlobal(idDic['LeftFoot'])) + np.array([0., -0.05, 0.]) # viewer.motionViewWnd.glWindow.projectionOrtho = True # viewer.motionViewWnd.glWindow.projectionChanged = True # viewer.motionViewWnd.glWindow.camera.rotateY = mm.deg2Rad(182.) def simulateCallback(frame): # print(frame) # print(motion[frame].getJointOrientationLocal(footIdDic['RightFoot_foot_0_1_0'])) if False: if frame == 200: if motionFile == 'wd2_tiptoe.bvh': setParamVal('tiptoe angle', 0.3) if motionFile == 'wd2_tiptoe_zygote.bvh': setParamVal('tiptoe angle', 0.3) # elif 210 < frame < 240: # if motionFile == 'wd2_tiptoe_zygote.bvh': # setParamVal('com Y offset', 0.01/30. * (frame-110)) elif frame == 400: setParamVal('com Y offset', 0.) setParamVal('tiptoe angle', 0.) elif frame == 430: foot_viewer.check_all_seg() # setParamVal('SupKt', 30.) # elif frame == 400: # setParamVal('SupKt', 17.) # hfi.footAdjust(motion[frame], idDic, SEGMENT_FOOT_MAG=.03, SEGMENT_FOOT_RAD=.015, baseHeight=0.02) if abs(getParamVal('tiptoe angle')) > 0.001: tiptoe_angle = getParamVal('tiptoe angle') motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_0_0_0'], mm.exp(mm.unitX(), -math.pi * tiptoe_angle)) motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_0_1_0'], mm.exp(mm.unitX(), -math.pi * tiptoe_angle)) motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_0_0_0'], mm.exp(mm.unitX(), -math.pi * tiptoe_angle)) motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_0_1_0'], mm.exp(mm.unitX(), -math.pi * tiptoe_angle)) # motion[frame].mulJointOrientationLocal(idDic['LeftFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle * 0.95)) # motion[frame].mulJointOrientationLocal(idDic['RightFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle * 0.95)) motion[frame].mulJointOrientationLocal( idDic['LeftFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle)) motion[frame].mulJointOrientationLocal( idDic['RightFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle)) if getParamVal('left tilt angle') > 0.001: left_tilt_angle = getParamVal('left tilt angle') motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_0_0'], mm.exp(mm.unitZ(), math.pi * left_tilt_angle)) if motion[0].skeleton.getJointIndex( 'LeftFoot_foot_0_1') is not None: motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_0_1'], mm.exp(mm.unitZ(), -math.pi * left_tilt_angle)) # else: # motion[frame].mulJointOrientationLocal(idDic['LeftFoot_foot_0_1_0'], mm.exp(mm.unitZ(), -math.pi * left_tilt_angle)) motion[frame].mulJointOrientationLocal( idDic['LeftFoot'], mm.exp(mm.unitZ(), math.pi * left_tilt_angle / 5.)) elif getParamVal('left tilt angle') < -0.001: left_tilt_angle = getParamVal('left tilt angle') motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_0_0'], mm.exp(mm.unitZ(), -math.pi * left_tilt_angle)) if motion[0].skeleton.getJointIndex( 'LeftFoot_foot_0_1') is not None: motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_0_1'], mm.exp(mm.unitZ(), math.pi * left_tilt_angle)) # else: # motion[frame].mulJointOrientationLocal(idDic['LeftFoot_foot_0_1_0'], mm.exp(mm.unitZ(), math.pi * left_tilt_angle)) motion[frame].mulJointOrientationLocal( idDic['LeftFoot'], mm.exp(mm.unitZ(), math.pi * left_tilt_angle)) if getParamVal('right tilt angle') > 0.001: right_tilt_angle = getParamVal('right tilt angle') if motion[0].skeleton.getJointIndex( 'RightFoot_foot_0_1') is not None: motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_0_1'], mm.exp(mm.unitZ(), math.pi * right_tilt_angle)) else: motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_0_1_0'], mm.exp(mm.unitZ(), math.pi * right_tilt_angle)) motion[frame].mulJointOrientationLocal( idDic['RightFoot'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle)) elif getParamVal('right tilt angle') < -0.001: right_tilt_angle = getParamVal('right tilt angle') motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_0_0'], mm.exp(mm.unitZ(), math.pi * right_tilt_angle)) if motion[0].skeleton.getJointIndex( 'RightFoot_foot_0_1') is not None: motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_0_1'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle)) # else: # motion[frame].mulJointOrientationLocal(idDic['RightFoot_foot_0_1_0'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle)) motion[frame].mulJointOrientationLocal( idDic['RightFoot'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle)) controlModel.update(motion[frame]) controlModel.translateByOffset( np.array([ getParamVal('com X offset'), getParamVal('com Y offset'), getParamVal('com Z offset') ])) motionModel.update(motion[frame]) motionModel.translateByOffset( np.array([ getParamVal('com X offset'), getParamVal('com Y offset'), getParamVal('com Z offset') ])) controlModel_ik.set_q(controlModel.get_q()) global g_initFlag global forceShowTime global JsysPre global JsupPreL global JsupPreR global JconstPre global preFootCenter global maxContactChangeCount global contactChangeCount global contact global contactChangeType Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals( ['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt']) Dt = 2 * (Kt**.5) Dl = 2 * (Kl**.5) Dh = 2 * (Kh**.5) dt_sup = 2 * (kt_sup**.5) # tracking th_r = motion.getDOFPositions(frame) th = controlModel.getDOFPositions() dth_r = motion.getDOFVelocities(frame) dth = controlModel.getDOFVelocities() ddth_r = motion.getDOFAccelerations(frame) ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt) # ype.flatten(fix_dofs(DOFs, ddth_des, mcfg, joint_names), ddth_des_flat) # ype.flatten(fix_dofs(DOFs, dth, mcfg, joint_names), dth_flat) ype.flatten(ddth_des, ddth_des_flat) ype.flatten(dth, dth_flat) ################################################# # jacobian ################################################# contact_des_ids = list() # desired contact segments if foot_viewer.check_om_l.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('LeftFoot_foot_0_0')) if foot_viewer.check_op_l.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('LeftFoot_foot_0_0_0')) if foot_viewer.check_im_l is not None and foot_viewer.check_im_l.value( ): contact_des_ids.append( motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1')) if foot_viewer.check_ip_l.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1_0')) if foot_viewer.check_h_l.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('LeftFoot_foot_1_0')) if foot_viewer.check_om_r.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('RightFoot_foot_0_0')) if foot_viewer.check_op_r.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('RightFoot_foot_0_0_0')) if foot_viewer.check_im_r is not None and foot_viewer.check_im_r.value( ): contact_des_ids.append( motion[0].skeleton.getJointIndex('RightFoot_foot_0_1')) if foot_viewer.check_ip_r.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('RightFoot_foot_0_1_0')) if foot_viewer.check_h_r.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('RightFoot_foot_1_0')) contact_ids = list() # temp idx for balancing contact_ids.extend(contact_des_ids) contact_joint_ori = list( map(controlModel.getJointOrientationGlobal, contact_ids)) contact_joint_pos = list( map(controlModel.getJointPositionGlobal, contact_ids)) contact_body_ori = list( map(controlModel.getBodyOrientationGlobal, contact_ids)) contact_body_pos = list( map(controlModel.getBodyPositionGlobal, contact_ids)) contact_body_vel = list( map(controlModel.getBodyVelocityGlobal, contact_ids)) contact_body_angvel = list( map(controlModel.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(motionModel.getBodyOrientationGlobal, contact_ids)) ref_body_pos = list(map(motionModel.getBodyPositionGlobal, contact_ids)) # 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 contMotionOffset = th[0][0] - th_r[0][0] linkPositions = controlModel.getBodyPositionsGlobal() linkVelocities = controlModel.getBodyVelocitiesGlobal() linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal() linkInertias = controlModel.getBodyInertiasGlobal() CM = yrp.getCM(linkPositions, linkMasses, totalMass) dCM = yrp.getCM(linkVelocities, linkMasses, totalMass) 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) if foot_viewer is not None: foot_viewer.foot_pressure_gl_window.refresh_foot_contact_info( frame, vpWorld, bodyIDsToCheck, mus, Ks, Ds) foot_viewer.foot_pressure_gl_window.goToFrame(frame) # rendering for foot_seg_id in footIdlist: control_model_renderer.body_colors[foot_seg_id] = (255, 240, 255) for contact_id in contact_ids: control_model_renderer.body_colors[contact_id] = (255, 0, 0) forceforce = np.array([ viewer.objectInfoWnd.labelForceX.value(), viewer.objectInfoWnd.labelForceY.value(), viewer.objectInfoWnd.labelForceZ.value() ]) extraForce[0] = getParamVal('Fm') * mm.normalize2(forceforce) 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] = controlModel.getBodyPositionGlobal( selectedBody) - 0.1 * np.array([ viewer.objectInfoWnd.labelForceX.value(), 0., viewer.objectInfoWnd.labelForceZ.value() ]) del rd_exf_des[:] del extraForcePos[:] extraForcePos.append( controlModel_ik.getJointPositionGlobal(idDic['LeftFoot'])) rd_exf_des.append( controlModel_ik.getJointPositionGlobal(idDic['LeftFoot_foot_0_0']) - controlModel_ik.getJointPositionGlobal(idDic['LeftFoot'])) extraForcePos.append( controlModel_ik.getJointPositionGlobal(idDic['LeftFoot'])) rd_exf_des.append( controlModel_ik.getJointPositionGlobal( idDic['LeftFoot_foot_0_1_0']) - controlModel_ik.getJointPositionGlobal(idDic['LeftFoot'])) extraForcePos.append( controlModel_ik.getJointPositionGlobal(idDic['LeftFoot'])) rd_exf_des.append( controlModel_ik.getJointPositionGlobal(idDic['LeftFoot_foot_1_0']) - controlModel_ik.getJointPositionGlobal(idDic['LeftFoot'])) extraForcePos.append( controlModel_ik.getJointPositionGlobal(idDic['LeftFoot_foot_0_0'])) rd_exf_des.append( controlModel_ik.getJointPositionGlobal( idDic['LeftFoot_foot_0_0_0']) - controlModel_ik.getJointPositionGlobal(idDic['LeftFoot_foot_0_0'])) # render contact_ids # render skeleton if SKELETON_ON: Ts = dict() Ts['pelvis'] = controlModel.getJointTransform(idDic['Hips']) Ts['thigh_R'] = controlModel.getJointTransform(idDic['RightUpLeg']) Ts['shin_R'] = controlModel.getJointTransform(idDic['RightLeg']) Ts['foot_R'] = controlModel.getJointTransform(idDic['RightFoot']) Ts['foot_heel_R'] = controlModel.getJointTransform( idDic['RightFoot']) Ts['heel_R'] = np.eye(4) Ts['outside_metatarsal_R'] = controlModel.getJointTransform( idDic['RightFoot_foot_0_0']) Ts['outside_phalanges_R'] = controlModel.getJointTransform( idDic['RightFoot_foot_0_0_0']) # Ts['inside_metatarsal_R'] = controlModel.getJointTransform(idDic['RightFoot_foot_0_1']) Ts['inside_metatarsal_R'] = np.eye(4) Ts['inside_phalanges_R'] = controlModel.getJointTransform( idDic['RightFoot_foot_0_1_0']) Ts['spine_ribs'] = controlModel.getJointTransform(idDic['Spine']) Ts['head'] = controlModel.getJointTransform(idDic['Spine1']) Ts['upper_limb_R'] = controlModel.getJointTransform( idDic['RightArm']) Ts['lower_limb_R'] = controlModel.getJointTransform( idDic['RightForeArm']) Ts['thigh_L'] = controlModel.getJointTransform(idDic['LeftUpLeg']) Ts['shin_L'] = controlModel.getJointTransform(idDic['LeftLeg']) Ts['foot_L'] = controlModel.getJointTransform(idDic['LeftFoot']) Ts['foot_heel_L'] = controlModel.getJointTransform( idDic['LeftFoot']) Ts['heel_L'] = np.eye(4) Ts['outside_metatarsal_L'] = controlModel.getJointTransform( idDic['LeftFoot_foot_0_0']) Ts['outside_phalanges_L'] = controlModel.getJointTransform( idDic['LeftFoot_foot_0_0_0']) # Ts['inside_metatarsal_L'] = controlModel.getJointTransform(idDic['LeftFoot_foot_0_1']) Ts['inside_metatarsal_L'] = np.eye(4) Ts['inside_phalanges_L'] = controlModel.getJointTransform( idDic['LeftFoot_foot_0_1_0']) Ts['upper_limb_L'] = controlModel.getJointTransform( idDic['LeftArm']) Ts['lower_limb_L'] = controlModel.getJointTransform( idDic['LeftForeArm']) skeleton_renderer.appendFrameState(Ts) viewer.setSimulateCallback(simulateCallback) viewer.setPreFrameCallback_Always(preFrameCallback_Always) viewer.startTimer(1 / 30.) # viewer.play() viewer.show() foot_viewer = FootWindow(viewer.x() + viewer.w() + 20, viewer.y(), 300, 500, 'foot contact modifier', controlModel) foot_viewer.show() foot_viewer.check_op_l.value(True) foot_viewer.check_ip_l.value(True) foot_viewer.check_op_r.value(True) foot_viewer.check_ip_r.value(True) foot_viewer.check_not_all_seg() viewer.motionViewWnd.goToFrame(0) Fl.run()
def main(): # np.set_printoptions(precision=4, linewidth=200) np.set_printoptions(precision=5, threshold=np.inf, suppress=True, linewidth=3000) motionFile = 'wd2_tiptoe.bvh' motionFile = 'wd2_tiptoe_zygote.bvh' # motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped(motionFile, SEGMENT_FOOT_RAD=0.008) motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped( motionFile, SEGMENT_FOOT_MAG=0.01, SEGMENT_FOOT_RAD=0.008) # motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped() # motion, mcfg, wcfg, stepsPerFrame, config = mit.create_jump_biped() motion.translateByOffset(np.array([-1.5378, 0., +0.29121])) vpWorld = cvw.VpWorld(wcfg) vpWorld.SetGlobalDamping(0.999) motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg) controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg) # controlModel_shadow_for_ik = cvm.VpControlModel(vpWorld, motion[0], mcfg) vpWorld.initialize() controlModel.initializeHybridDynamics() # controlToMotionOffset = (1.5, -0.02, 0) controlToMotionOffset = (1.5, 0., 0) controlModel.translateByOffset(controlToMotionOffset) # controlModel_shadow_for_ik.set_q(controlModel.get_q()) # controlModel_shadow_for_ik.computeJacobian(0, np.array([0., 0., 0.])) wcfg_ik = copy.deepcopy(wcfg) vpWorld_ik = cvw.VpWorld(wcfg_ik) controlModel_ik = cvm.VpControlModel(vpWorld_ik, motion[0], mcfg) vpWorld_ik.initialize() controlModel_ik.set_q(np.zeros_like(controlModel.get_q())) totalDOF = controlModel.getTotalDOF() DOFs = controlModel.getDOFs() print(totalDOF) print(controlModel.getTotalMass()) foot_dofs = [] left_foot_dofs = [] right_foot_dofs = [] foot_seg_dofs = [] left_foot_seg_dofs = [] right_foot_seg_dofs = [] # for joint_idx in range(motion[0].skeleton.getJointNum()): for joint_idx in range(controlModel.getJointNum()): joint_name = controlModel.index2name(joint_idx) # joint_name = motion[0].skeleton.getJointName(joint_idx) if 'Foot' in joint_name: foot_dofs_temp = controlModel.getJointDOFIndexes(joint_idx) foot_dofs.extend(foot_dofs_temp) if 'Left' in joint_name: left_foot_dofs.extend(foot_dofs_temp) elif 'Right' in joint_name: right_foot_dofs.extend(foot_dofs_temp) if 'foot' in joint_name: foot_dofs_temp = controlModel.getJointDOFIndexes(joint_idx) foot_seg_dofs.extend(foot_dofs_temp) if 'Left' in joint_name: left_foot_seg_dofs.extend(foot_dofs_temp) elif 'Right' in joint_name: right_foot_seg_dofs.extend(foot_dofs_temp) # 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'] # selectedBody = motion[0].skeleton.getJointIndex(config['end']) selectedBody = motion[0].skeleton.getJointIndex('Spine') constBody = motion[0].skeleton.getJointIndex('RightFoot') supL = motion[0].skeleton.getJointIndex('LeftFoot') supR = motion[0].skeleton.getJointIndex('RightFoot') # momentum matrix linkMasses = controlModel.getBodyMasses() totalMass = controlModel.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 CP_old = [mm.v3(0., 0., 0.)] # penalty method bodyIDsToCheck = list(range(vpWorld.getBodyNum())) # 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_footCenter_ref = [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_foot_ori = [None] rd_foot_pos = [None] rd_root_ori = [None] rd_root_pos = [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] 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 # viewer = ysv.SimpleViewer() # viewer = hsv.hpSimpleViewer(rect=[0, 0, 1024, 768], viewForceWnd=False) viewer = hsv.hpSimpleViewer(rect=[0, 0, 1920 + 300, 1 + 1080 + 55], 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('world', yr.VpWorldRenderer(vpWorld, (150, 150, 150))) viewer.doc.addRenderer( 'motionModel', yr.VpModelRenderer(motionModel, (150, 150, 255), yr.POLYGON_FILL)) viewer.doc.setRendererVisible('motionModel', False) viewer.doc.addRenderer( 'ikModel', yr.VpModelRenderer(controlModel_ik, (150, 150, 255), yr.POLYGON_LINE)) viewer.doc.setRendererVisible('ikModel', False) # viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_LINE)) control_model_renderer = yr.VpModelRenderer(controlModel, (255, 240, 255), yr.POLYGON_FILL) viewer.doc.addRenderer('controlModel', control_model_renderer) 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) viewer.doc.addRenderer('rd_footCenter', yr.PointsRenderer(rd_footCenter)) viewer.doc.setRendererVisible('rd_footCenter', False) viewer.doc.addRenderer('rd_footCenter_ref', yr.PointsRenderer(rd_footCenter_ref)) viewer.doc.setRendererVisible('rd_footCenter_ref', False) viewer.doc.addRenderer('rd_CM_plane', yr.PointsRenderer(rd_CM_plane, (255, 255, 0))) viewer.doc.setRendererVisible('rd_CM_plane', False) viewer.doc.addRenderer('rd_CP', yr.PointsRenderer(rd_CP, (0, 255, 0))) viewer.doc.setRendererVisible('rd_CP', False) viewer.doc.addRenderer('rd_CP_des', yr.PointsRenderer(rd_CP_des, (255, 0, 255))) viewer.doc.setRendererVisible('rd_CP_des', False) viewer.doc.addRenderer( 'rd_dL_des_plane', yr.VectorsRenderer(rd_dL_des_plane, rd_CM, (255, 255, 0))) viewer.doc.setRendererVisible('rd_dL_des_plane', False) viewer.doc.addRenderer('rd_dH_des', yr.VectorsRenderer(rd_dH_des, rd_CM, (0, 255, 0))) viewer.doc.setRendererVisible('rd_dH_des', False) # 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, 255, 0))) viewer.doc.setRendererVisible('rd_CF', False) viewer.doc.addRenderer( 'rd_foot_ori', yr.OrientationsRenderer(rd_foot_ori, rd_foot_pos, (255, 255, 0))) viewer.doc.setRendererVisible('rd_foot_ori', False) viewer.doc.addRenderer( 'rd_root_ori', yr.OrientationsRenderer(rd_root_ori, rd_root_pos, (255, 255, 0))) viewer.doc.setRendererVisible('rd_root_ori', False) viewer.doc.addRenderer( 'extraForce', yr.VectorsRenderer(rd_exf_des, extraForcePos, (0, 255, 0))) viewer.doc.setRendererVisible('extraForce', False) # viewer.doc.addRenderer('extraForceEnable', yr.VectorsRenderer(rd_exfen_des, extraForcePos, (255,0,0))) viewer.doc.addRenderer( 'extraForceEnable', yr.WideArrowRenderer(rd_exfen_des, extraForcePos, (255, 0, 0), lineWidth=.05, fromPoint=False)) # 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))) # foot_viewer = FootWindow(viewer.x() + viewer.w() + 20, viewer.y(), 300, 400, 'foot contact modifier', controlModel) foot_viewer = None # type: FootWindow initKt = 25. # initKt = 60. initKl = 100. initKh = 100. initBl = .1 initBh = .13 # initSupKt = 17 initSupKt = 22 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., 300., 0.1, initSupKt) viewer.objectInfoWnd.add1DSlider("Fm", 0., 1000., 1., 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.objectInfoWnd.add1DSlider("tiptoe angle", -0.5, .5, 0.001, 0.) viewer.objectInfoWnd.add1DSlider("left tilt angle", -0.5, .5, 0.001, 0.) viewer.objectInfoWnd.add1DSlider("right tilt angle", -0.5, .5, 0.001, 0.) viewer.objectInfoWnd.add1DSlider("heel angle", -0.5, .5, 0.001, 0.) viewer.objectInfoWnd.add1DSlider("pha angle", -0.5, .5, 0.001, 0.) 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.4) viewer.objectInfoWnd.end() # self.sliderFm = Fl_Hor_Nice_Slider(10, 42+offset*6, 250, 10) def getParamVal(paramname): return viewer.objectInfoWnd.getVal(paramname) def getParamVals(paramnames): return (getParamVal(name) for name in paramnames) def setParamVal(paramname, val): viewer.objectInfoWnd.setVal(paramname, val) idDic = dict() for i in range(motion[0].skeleton.getJointNum()): idDic[motion[0].skeleton.getJointName(i)] = i # extendedFootName = ['Foot_foot_0_0', 'Foot_foot_0_1', 'Foot_foot_0_0_0', 'Foot_foot_0_1_0', 'Foot_foot_1_0'] extendedFootName = [ 'Foot_foot_0_0', 'Foot_foot_0_0_0', 'Foot_foot_0_1_0', 'Foot_foot_1_0' ] 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 } footIdDic = lIDdic.copy() footIdDic.update(rIDdic) lIDlist = [ motion[0].skeleton.getJointIndex('Left' + name) for name in extendedFootName ] rIDlist = [ motion[0].skeleton.getJointIndex('Right' + name) for name in extendedFootName ] footIdlist = [] footIdlist.extend(lIDlist) footIdlist.extend(rIDlist) foot_left_idx = motion[0].skeleton.getJointIndex('LeftFoot') foot_right_idx = motion[0].skeleton.getJointIndex('RightFoot') foot_left_idx_temp = motion[0].skeleton.getJointIndex('LeftFoot_foot_1_0') foot_right_idx_temp = motion[0].skeleton.getJointIndex( 'RightFoot_foot_1_0') # ik_solver = hik.numIkSolver(dartIkModel) # ik_solver.clear() # bodyIDsToCheck = rIDlist.copy() joint_names = [ motion[0].skeleton.getJointName(i) for i in range(motion[0].skeleton.getJointNum()) ] def fix_dofs(_DOFs, nested_dof_values, _mcfg, _joint_names): fixed_nested_dof_values = list() fixed_nested_dof_values.append(nested_dof_values[0]) for i in range(1, len(_DOFs)): dof = _DOFs[i] if dof == 1: node = _mcfg.getNode(_joint_names[i]) axis = mm.unitZ() if node.jointAxes[0] == 'X': axis = mm.unitX() elif node.jointAxes[0] == 'Y': axis = mm.unitY() fixed_nested_dof_values.append( np.array([np.dot(nested_dof_values[i], axis)])) else: fixed_nested_dof_values.append(nested_dof_values[i]) return fixed_nested_dof_values start_frame = 200 up_vec_in_each_link = dict() for foot_id in footIdlist: up_vec_in_each_link[ foot_id] = controlModel_ik.getBodyOrientationGlobal(foot_id)[1, :] controlModel_ik.set_q(controlModel.get_q()) ################################### # simulate ################################### def preFrameCallback_Always(frame): # print(mm.rad2Deg(math.pi/6.*math.sin((frame-30)*math.pi/180.))) if frame <= start_frame: vpWorld.set_plane(0, mm.unitY(), np.zeros(3)) if frame > start_frame: if math.sin((frame - start_frame) / 360. * math.pi) > 0.: if frame < start_frame + 50: setParamVal( 'com Z offset', 0.02 * math.sin(2. * (frame - start_frame) / 360. * math.pi)) else: setParamVal('com Z offset', 0.0) if math.sin((frame - start_frame) / 360. * math.pi) > 0.: foot_viewer.check_not_all_seg() foot_viewer.check_tiptoe_all() setParamVal( 'tiptoe angle', mm.deg2Rad(10.) * math.sin( (frame - start_frame) / 360. * math.pi)) # foot_viewer.check_h_l.value(False) # foot_viewer.check_h_r.value(False) else: foot_viewer.check_all_seg() # foot_viewer.check_tiptoe_all() # foot_viewer.check_h_l.value(True) # foot_viewer.check_h_r.value(True) vpWorld.set_plane( 0, np.dot( mm.exp( -mm.unitX(), mm.deg2Rad(10.) * math.sin( (frame - start_frame) / 360. * math.pi)), mm.unitY()), np.zeros(3)) plane_list = vpWorld.get_plane_list() plane_normal = plane_list[0][0] plane_origin = plane_list[0][1] viewer.motionViewWnd.glWindow.pOnPlaneshadow = plane_origin + plane_normal * 0.001 viewer.motionViewWnd.glWindow.normalshadow = plane_normal def simulateCallback(frame): # print(frame) # print(motion[frame].getJointOrientationLocal(footIdDic['RightFoot_foot_0_1_0'])) # hfi.footAdjust(motion[frame], idDic, SEGMENT_FOOT_MAG=.03, SEGMENT_FOOT_RAD=.015, baseHeight=0.02) if frame == 0: setParamVal('com Y offset', 0.02) if abs(getParamVal('tiptoe angle')) > 0.001: tiptoe_angle = getParamVal('tiptoe angle') motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_0_0_0'], mm.exp(mm.unitX(), -math.pi * tiptoe_angle)) motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_0_1_0'], mm.exp(mm.unitX(), -math.pi * tiptoe_angle)) motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_0_0_0'], mm.exp(mm.unitX(), -math.pi * tiptoe_angle)) motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_0_1_0'], mm.exp(mm.unitX(), -math.pi * tiptoe_angle)) # motion[frame].mulJointOrientationLocal(idDic['LeftFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle * 0.95)) # motion[frame].mulJointOrientationLocal(idDic['RightFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle * 0.95)) # motion[frame].mulJointOrientationLocal(idDic['LeftFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle)) # motion[frame].mulJointOrientationLocal(idDic['RightFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle)) if getParamVal('left tilt angle') > 0.001: left_tilt_angle = getParamVal('left tilt angle') if motion[0].skeleton.getJointIndex( 'LeftFoot_foot_0_1') is not None: motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_0_1'], mm.exp(mm.unitZ(), -math.pi * left_tilt_angle)) else: motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_0_1_0'], mm.exp(mm.unitZ(), -math.pi * left_tilt_angle)) motion[frame].mulJointOrientationLocal( idDic['LeftFoot'], mm.exp(mm.unitZ(), math.pi * left_tilt_angle)) elif getParamVal('left tilt angle') < -0.001: left_tilt_angle = getParamVal('left tilt angle') motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_0_0'], mm.exp(mm.unitZ(), -math.pi * left_tilt_angle)) if motion[0].skeleton.getJointIndex( 'LeftFoot_foot_0_1') is not None: motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_0_1'], mm.exp(mm.unitZ(), math.pi * left_tilt_angle)) else: motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_0_1_0'], mm.exp(mm.unitZ(), math.pi * left_tilt_angle)) motion[frame].mulJointOrientationLocal( idDic['LeftFoot'], mm.exp(mm.unitZ(), math.pi * left_tilt_angle)) if getParamVal('right tilt angle') > 0.001: right_tilt_angle = getParamVal('right tilt angle') if motion[0].skeleton.getJointIndex( 'RightFoot_foot_0_1') is not None: motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_0_1'], mm.exp(mm.unitZ(), math.pi * right_tilt_angle)) else: motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_0_1_0'], mm.exp(mm.unitZ(), math.pi * right_tilt_angle)) motion[frame].mulJointOrientationLocal( idDic['RightFoot'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle)) elif getParamVal('right tilt angle') < -0.001: right_tilt_angle = getParamVal('right tilt angle') motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_0_0'], mm.exp(mm.unitZ(), math.pi * right_tilt_angle)) if motion[0].skeleton.getJointIndex( 'RightFoot_foot_0_1') is not None: motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_0_1'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle)) # else: # motion[frame].mulJointOrientationLocal(idDic['RightFoot_foot_0_1_0'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle)) motion[frame].mulJointOrientationLocal( idDic['RightFoot'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle)) if abs(getParamVal('heel angle')) > 0.001: heel_angle = getParamVal('heel angle') motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_1_0'], mm.exp(mm.unitX(), math.pi * heel_angle)) motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_1_0'], mm.exp(mm.unitX(), math.pi * heel_angle)) if abs(getParamVal('pha angle')) > 0.001: pha_angle = getParamVal('pha angle') motion[frame].mulJointOrientationLocal( idDic['RightFoot_foot_0_1_0'], mm.exp(mm.unitX(), math.pi * pha_angle)) motion[frame].mulJointOrientationLocal( idDic['LeftFoot_foot_0_1_0'], mm.exp(mm.unitX(), math.pi * pha_angle)) # motion[frame].mulJointOrientationLocal(idDic['RightFoot_foot_0_1_0'], mm.exp(mm.unitZ(), -math.pi * pha_angle)) # motion[frame].mulJointOrientationLocal(idDic['LeftFoot_foot_0_1_0'], mm.exp(mm.unitZ(), math.pi * pha_angle)) motionModel.update(motion[frame]) motionModel.translateByOffset( np.array([ getParamVal('com X offset'), getParamVal('com Y offset'), getParamVal('com Z offset') ])) controlModel_ik.set_q(controlModel.get_q()) global g_initFlag global forceShowTime global JsysPre global JsupPreL global JsupPreR global JconstPre global preFootCenter global maxContactChangeCount global contactChangeCount global contact global contactChangeType Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals( ['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt']) Dt = 2 * (Kt**.5) Dl = 2 * (Kl**.5) Dh = 2 * (Kh**.5) dt_sup = 2 * (kt_sup**.5) # tracking th_r = motion.getDOFPositions(frame) th = controlModel.getDOFPositions() dth_r = motion.getDOFVelocities(frame) dth = controlModel.getDOFVelocities() ddth_r = motion.getDOFAccelerations(frame) ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt) # ype.flatten(fix_dofs(DOFs, ddth_des, mcfg, joint_names), ddth_des_flat) # ype.flatten(fix_dofs(DOFs, dth, mcfg, joint_names), dth_flat) ype.flatten(ddth_des, ddth_des_flat) ype.flatten(dth, dth_flat) ################################################# # jacobian ################################################# contact_des_ids = list() # desired contact segments if foot_viewer.check_om_l.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('LeftFoot_foot_0_0')) if foot_viewer.check_op_l.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('LeftFoot_foot_0_0_0')) if foot_viewer.check_im_l is not None and foot_viewer.check_im_l.value( ): contact_des_ids.append( motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1')) if foot_viewer.check_ip_l.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1_0')) if foot_viewer.check_h_l.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('LeftFoot_foot_1_0')) if foot_viewer.check_om_r.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('RightFoot_foot_0_0')) if foot_viewer.check_op_r.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('RightFoot_foot_0_0_0')) if foot_viewer.check_im_r is not None and foot_viewer.check_im_r.value( ): contact_des_ids.append( motion[0].skeleton.getJointIndex('RightFoot_foot_0_1')) if foot_viewer.check_ip_r.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('RightFoot_foot_0_1_0')) if foot_viewer.check_h_r.value(): contact_des_ids.append( motion[0].skeleton.getJointIndex('RightFoot_foot_1_0')) contact_ids = list() # temp idx for balancing contact_ids.extend(contact_des_ids) contact_joint_ori = list( map(controlModel.getJointOrientationGlobal, contact_ids)) contact_joint_pos = list( map(controlModel.getJointPositionGlobal, contact_ids)) contact_body_ori = list( map(controlModel.getBodyOrientationGlobal, contact_ids)) contact_body_pos = list( map(controlModel.getBodyPositionGlobal, contact_ids)) contact_body_vel = list( map(controlModel.getBodyVelocityGlobal, contact_ids)) contact_body_angvel = list( map(controlModel.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(motionModel.getBodyOrientationGlobal, contact_ids)) ref_body_pos = list(map(motionModel.getBodyPositionGlobal, contact_ids)) # 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 contMotionOffset = th[0][0] - th_r[0][0] linkPositions = controlModel.getBodyPositionsGlobal() linkVelocities = controlModel.getBodyVelocitiesGlobal() linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal() linkInertias = controlModel.getBodyInertiasGlobal() CM = yrp.getCM(linkPositions, linkMasses, totalMass) dCM = yrp.getCM(linkVelocities, linkMasses, totalMass) 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 Jsys, dJsys = controlModel.computeCom_J_dJdq() 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(dJsys[6 * contact_id:6 * contact_id + 6]) # calculate footCenter footCenter = sum(contact_body_pos) / len(contact_body_pos) if len(contact_body_pos) > 0 \ else .5 * (controlModel.getBodyPositionGlobal(supL) + controlModel.getBodyPositionGlobal(supR)) footCenter[1] = 0. # if len(contact_body_pos) > 2: # hull = ConvexHull(contact_body_pos) footCenter_ref = sum(ref_body_pos) / len(ref_body_pos) if len(ref_body_pos) > 0 \ else .5 * (motionModel.getBodyPositionGlobal(supL) + motionModel.getBodyPositionGlobal(supR)) footCenter_ref = footCenter_ref + contMotionOffset # if len(ref_body_pos) > 2: # hull = ConvexHull(ref_body_pos) footCenter_ref[1] = 0. # footCenter[0] = footCenter[0] + getParamVal('com X offset') # footCenter[1] = footCenter[0] + getParamVal('com Y offset') # footCenter[2] = footCenter[2] + getParamVal('com Z offset') # initialization if g_initFlag == 0: preFootCenter[0] = footCenter.copy() g_initFlag = 1 # if contactChangeCount == 0 and np.linalg.norm(footCenter - preFootCenter[0]) > 0.01: # contactChangeCount += 30 if contactChangeCount > 0: # change footcenter gradually footCenter = preFootCenter[0] + ( maxContactChangeCount - contactChangeCount) * ( footCenter - preFootCenter[0]) / maxContactChangeCount else: preFootCenter[0] = footCenter.copy() # linear momentum # TODO: # We should consider dCM_ref, shouldn't we? # add getBodyPositionGlobal and getBodyPositionsGlobal in csVpModel! # to do that, set joint velocities to vpModel CM_ref_plane = footCenter # CM_ref_plane = footCenter_ref CM_ref = footCenter + np.array([ getParamVal('com X offset'), motionModel.getCOM()[1] + getParamVal('com Y offset'), getParamVal('com Z offset') ]) dL_des_plane = Kl * totalMass * (CM_ref - CM) - Dl * totalMass * dCM # dL_des_plane = Kl * totalMass * (CM_ref_plane - CM_plane) - Dl * totalMass * dCM_plane # dL_des_plane[1] = 0. # print('dCM_plane : ', np.linalg.norm(dCM_plane)) # angular momentum CP_ref = footCenter # CP_ref = footCenter_ref bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce( 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]) / (1 / 30.) CP_old[0] = CP if CP is not None and dCP is not None: ddCP_des = Kh * (CP_ref - CP) - Dh * dCP dCP_des = dCP + ddCP_des * (1 / 30.) CP_des = CP + dCP_des * (1 / 30.) # CP_des = footCenter CP_des = CP + dCP * (1 / 30.) + .5 * ddCP_des * ((1 / 30.)**2) dH_des = np.cross( (CP_des - CM), (dL_des_plane + totalMass * mm.s2v(wcfg.gravity))) if contactChangeCount > 0: # and contactChangeType == 'DtoS': dH_des *= (maxContactChangeCount - contactChangeCount) / maxContactChangeCount else: dH_des = None # convex hull contact_pos_2d = np.asarray([ np.array([contactPosition[0], contactPosition[2]]) for contactPosition in contactPositions ]) p = np.array([CM_plane[0], CM_plane[2]]) # hull = None # type: Delaunay # if contact_pos_2d.shape[0] > 0: # hull = Delaunay(contact_pos_2d) # print(hull.find_simplex(p) >= 0) # set up equality constraint # 1. calculate desired up vector of each contacting body. plane_list = vpWorld.get_plane_list() # 1.2. set des_up_vec to normal direction des_up_vec = [ np.asarray(plane_list[0][0]) for i in range(len(contact_ids)) ] # TODO: # logSO3 is just q'', not acceleration. # To make a_oris acceleration, q'' -> a will be needed # body_ddqs = list(map(mm.logSO3, [mm.getSO3FromVectors(np.dot(body_ori, mm.unitY()), mm.unitY()) for body_ori in contact_body_ori])) # body_ddqs = list(map(mm.logSO3, [np.dot(contact_body_ori[i].T, np.dot(ref_body_ori[i], mm.getSO3FromVectors(np.dot(ref_body_ori[i], mm.unitY()), mm.unitY()))) for i in range(len(contact_body_ori))])) # body_ddqs = list(map(mm.logSO3, [np.dot(contact_body_ori[i].T, np.dot(ref_body_ori[i], mm.getSO3FromVectors(np.dot(ref_body_ori[i], up_vec_in_each_link[contact_ids[i]]), mm.unitY()))) for i in range(len(contact_body_ori))])) # a_oris = list(map(mm.logSO3, [np.dot(contact_body_ori[i].T, np.dot(ref_body_ori[i], mm.getSO3FromVectors(np.dot(ref_body_ori[i], up_vec_in_each_link[contact_ids[i]]), mm.unitY()))) for i in range(len(contact_body_ori))])) # a_oris = list(map(mm.logSO3, [np.dot(np.dot(ref_body_ori[i], mm.getSO3FromVectors(np.dot(ref_body_ori[i], up_vec_in_each_link[contact_ids[i]]), mm.unitY())), contact_body_ori[i].T) for i in range(len(contact_body_ori))])) a_oris = list( map(mm.logSO3, [ np.dot( np.dot( ref_body_ori[i], mm.getSO3FromVectors( np.dot(ref_body_ori[i], up_vec_in_each_link[contact_ids[i]]), np.asarray(des_up_vec[i]))), contact_body_ori[i].T) for i in range(len(contact_body_ori)) ])) body_qs = list(map(mm.logSO3, contact_body_ori)) body_angs = [ np.dot(contact_body_ori[i], contact_body_angvel[i]) for i in range(len(contact_body_ori)) ] body_dqs = [ mm.vel2qd(body_angs[i], body_qs[i]) for i in range(len(body_angs)) ] # a_oris = [np.dot(contact_body_ori[i], mm.qdd2accel(body_ddqs[i], body_dqs[i], body_qs[i])) for i in range(len(contact_body_ori))] # body_ddq = body_ddqs[0] # body_ori = contact_body_ori[0] # body_ang = np.dot(body_ori.T, contact_body_angvel[0]) # # body_q = mm.logSO3(body_ori) # body_dq = mm.vel2qd(body_ang, body_q) # a_ori = np.dot(body_ori, mm.qdd2accel(body_ddq, body_dq, body_q)) # KT_SUP = np.diag([kt_sup/10., kt_sup, kt_sup/10.]) KT_SUP = np.diag([kt_sup / 5., kt_sup, kt_sup / 5.]) # a_oris = list(map(mm.logSO3, [mm.getSO3FromVectors(np.dot(body_ori, mm.unitY()), mm.unitY()) for body_ori in contact_body_ori])) # a_oris = list(map(mm.logSO3, [mm.getSO3FromVectors(np.dot(contact_body_ori[i], up_vec_in_each_link[contact_ids[i]]), mm.unitY()) for i in range(len(contact_body_ori))])) # a_sups = [np.append(kt_sup*(ref_body_pos[i] - contact_body_pos[i] + contMotionOffset) + dt_sup*(ref_body_vel[i] - contact_body_vel[i]), # kt_sup*a_oris[i]+dt_sup*(ref_body_angvel[i]-contact_body_angvel[i])) for i in range(len(a_oris))] # a_sups = [np.append(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))] 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)) ] # a_sups = [np.append(np.dot(KT_SUP, (des_pos[i]-contact_body_pos[i])) - dt_sup * contact_body_vel[i], # 10.*kt_sup*a_oris[i] - dt_sup * contact_body_angvel[i]) for i in range(len(a_oris))] # for i in range(len(a_sups)): # a_sups[i][1] = -kt_sup * des_pos[i][1] - dt_sup * contact_body_vel[i][1] # 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, dJsys) r_bias, s_bias = np.hsplit(rs, 2) ####################################################### # optimization ####################################################### # if contact == 2 and footCenterR[1] > doubleTosingleOffset/2: if contact_left and not contact_right: 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_right and not contact_left: 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']) mot.addTrackingTerms(problem, totalDOF, Bt, w, ddth_des_flat) if dH_des is not None: mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias) 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], dth_flat, a_sups[c_idx]) if contactChangeCount > 0: contactChangeCount = contactChangeCount - 1 if contactChangeCount == 0: maxContactChangeCount = 30 contactChangeType = 0 r = problem.solve() problem.clear() ddth_sol_flat = np.asarray(r['x']) # ddth_sol_flat[foot_seg_dofs] = np.array(ddth_des_flat)[foot_seg_dofs] ype.nested(ddth_sol_flat, ddth_sol) rootPos[0] = controlModel.getBodyPositionGlobal(selectedBody) localPos = [[0, 0, 0]] for i in range(stepsPerFrame): # apply penalty force bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds) # bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds) vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces) controlModel.setDOFAccelerations(ddth_sol) # controlModel.setDOFAccelerations(ddth_des) # controlModel.set_ddq(ddth_sol_flat) # controlModel.set_ddq(ddth_des_flat) controlModel.solveHybridDynamics() 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 vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce) vpWorld.step() controlModel_ik.set_q(controlModel.get_q()) # rendering bodyIDs, geomIDs, positionLocalsForGeom = vpWorld.getContactInfoForcePlate( bodyIDsToCheck) for foot_seg_id in range(controlModel.getBodyNum()): control_model_renderer.body_colors[foot_seg_id] = (255, 240, 255) control_model_renderer.geom_colors[foot_seg_id] = [ (255, 240, 255) ] * controlModel.getBodyGeomNum(foot_seg_id) for i in range(len(geomIDs)): control_model_renderer.geom_colors[bodyIDs[i]][geomIDs[i]] = (255, 0, 0) # for foot_seg_id in footIdlist: # control_model_renderer.body_colors[foot_seg_id] = (255, 240, 255) # # for contact_id in contact_ids: # control_model_renderer.body_colors[contact_id] = (255, 0, 0) rd_footCenter[0] = footCenter rd_footCenter_ref[0] = footCenter_ref 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 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) del rd_foot_ori[:] del rd_foot_pos[:] # for seg_foot_id in footIdlist: # rd_foot_ori.append(controlModel.getJointOrientationGlobal(seg_foot_id)) # rd_foot_pos.append(controlModel.getJointPositionGlobal(seg_foot_id)) rd_foot_ori.append(controlModel.getJointOrientationGlobal(supL)) rd_foot_ori.append(controlModel.getJointOrientationGlobal(supR)) rd_foot_pos.append(controlModel.getJointPositionGlobal(supL)) rd_foot_pos.append(controlModel.getJointPositionGlobal(supR)) rd_root_des[0] = rootPos[0] rd_root_ori[0] = controlModel.getBodyOrientationGlobal(0) rd_root_pos[0] = controlModel.getBodyPositionGlobal(0) del rd_CF[:] del rd_CF_pos[:] for i in range(len(contactPositions)): rd_CF.append(contactForces[i] / 400) 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] = controlModel.getBodyPositionGlobal(selectedBody) extraForcePos[0] = controlModel.getBodyPositionGlobal( selectedBody) - 0.1 * np.array([ viewer.objectInfoWnd.labelForceX.value(), 0., viewer.objectInfoWnd.labelForceZ.value() ]) # render contact_ids # render skeleton if SKELETON_ON: Ts = dict() Ts['pelvis'] = controlModel.getJointTransform(idDic['Hips']) Ts['thigh_R'] = controlModel.getJointTransform(idDic['RightUpLeg']) Ts['shin_R'] = controlModel.getJointTransform(idDic['RightLeg']) Ts['foot_R'] = controlModel.getJointTransform(idDic['RightFoot']) Ts['foot_heel_R'] = controlModel.getJointTransform( idDic['RightFoot']) Ts['heel_R'] = np.eye(4) Ts['outside_metatarsal_R'] = controlModel.getJointTransform( idDic['RightFoot_foot_0_0']) Ts['outside_phalanges_R'] = controlModel.getJointTransform( idDic['RightFoot_foot_0_0_0']) # Ts['inside_metatarsal_R'] = controlModel.getJointTransform(idDic['RightFoot_foot_0_1']) Ts['inside_metatarsal_R'] = np.eye(4) Ts['inside_phalanges_R'] = controlModel.getJointTransform( idDic['RightFoot_foot_0_1_0']) Ts['spine_ribs'] = controlModel.getJointTransform(idDic['Spine']) Ts['head'] = controlModel.getJointTransform(idDic['Spine1']) Ts['upper_limb_R'] = controlModel.getJointTransform( idDic['RightArm']) Ts['lower_limb_R'] = controlModel.getJointTransform( idDic['RightForeArm']) Ts['thigh_L'] = controlModel.getJointTransform(idDic['LeftUpLeg']) Ts['shin_L'] = controlModel.getJointTransform(idDic['LeftLeg']) Ts['foot_L'] = controlModel.getJointTransform(idDic['LeftFoot']) Ts['foot_heel_L'] = controlModel.getJointTransform( idDic['LeftFoot']) Ts['heel_L'] = np.eye(4) Ts['outside_metatarsal_L'] = controlModel.getJointTransform( idDic['LeftFoot_foot_0_0']) Ts['outside_phalanges_L'] = controlModel.getJointTransform( idDic['LeftFoot_foot_0_0_0']) # Ts['inside_metatarsal_L'] = controlModel.getJointTransform(idDic['LeftFoot_foot_0_1']) Ts['inside_metatarsal_L'] = np.eye(4) Ts['inside_phalanges_L'] = controlModel.getJointTransform( idDic['LeftFoot_foot_0_1_0']) Ts['upper_limb_L'] = controlModel.getJointTransform( idDic['LeftArm']) Ts['lower_limb_L'] = controlModel.getJointTransform( idDic['LeftForeArm']) skeleton_renderer.appendFrameState(Ts) def postFrameCallback(frame): if foot_viewer is not None: foot_viewer.foot_pressure_gl_window.refresh_foot_contact_info( frame, vpWorld, bodyIDsToCheck, mus, Ks, Ds) foot_viewer.foot_pressure_gl_window.goToFrame(frame) viewer.setPreFrameCallback_Always(preFrameCallback_Always) viewer.setSimulateCallback(simulateCallback) viewer.setPostFrameCallback_Always(postFrameCallback) viewer.startTimer(1 / 30.) # viewer.play() viewer.show() foot_viewer = FootWindow(viewer.x() + viewer.w() + 20, viewer.y(), 300, 500, 'foot contact modifier', controlModel) foot_viewer.show() foot_viewer.check_all_seg() viewer.motionViewWnd.goToFrame(0) Fl.run()
def create_biped(): # motion #motionName = 'wd2_n_kick.bvh' if MOTION == STAND: #motionName = 'wd2_stand.bvh' motionName = 'wd2_stand2.bvh' elif MOTION == STAND2: motionName = 'ww13_41_V001.bvh' elif MOTION == FORWARD_JUMP: motionName = 'woddy2_jump0.bvh' elif MOTION == TAEKWONDO: motionName = './MotionFile/wd2_098_V001.bvh' elif MOTION == TAEKWONDO2: motionName = './MotionFile/wd2_098_V001.bvh' elif MOTION == KICK: motionName = 'wd2_n_kick.bvh' elif MOTION == WALK: motionName = 'wd2_WalkForwardNormal00.bvh' elif MOTION == TIPTOE: motionName = './MotionFile/cmu/15_07_15_07.bvh' #motionName = 'ww13_41_V001.bvh' scale = 0.01 if MOTION == WALK: scale = 1.0 elif MOTION == TIPTOE: scale = 0.01 motion = yf.readBvhFile(motionName, scale) yme.removeJoint(motion, HEAD, False) yme.removeJoint(motion, RIGHT_SHOULDER, False) yme.removeJoint(motion, LEFT_SHOULDER, False) yme.removeJoint(motion, RIGHT_TOES, False) yme.removeJoint(motion, RIGHT_TOES_END, False) yme.removeJoint(motion, LEFT_TOES, False) yme.removeJoint(motion, LEFT_TOES_END, False) yme.removeJoint(motion, RIGHT_HAND_END, False) yme.removeJoint(motion, LEFT_HAND_END, False) yme.offsetJointLocal(motion, RIGHT_ARM, (.03, -.05, 0), False) yme.offsetJointLocal(motion, LEFT_ARM, (-.03, -.05, 0), False) yme.rotateJointLocal(motion, HIP, mm.exp(mm.v3(1, 0, 0), .01), False) yme.rotateJointLocal(motion, HIP, mm.exp(mm.v3(0, 0, 1), -.01), False) #addFootSegment yme.addJoint(motion, LEFT_FOOT, LEFT_TALUS_1, (-0.045, -0.06, -0.05)) yme.addJoint(motion, LEFT_FOOT, LEFT_TALUS_2, (0.0, -0.06, -0.05)) yme.addJoint(motion, LEFT_FOOT, LEFT_TALUS_3, (0.045, -0.06, -0.05)) #-0.0037 yme.addJoint(motion, LEFT_TALUS_1, LEFT_METATARSAL_1, (0.0, 0.0, 0.1)) yme.addJoint(motion, LEFT_TALUS_3, LEFT_METATARSAL_3, (0.0, 0.0, 0.1)) #-0.0037 yme.addJoint(motion, LEFT_TALUS_2, LEFT_METATARSAL_2, (0.0, 0.0, 0.1)) yme.addJoint(motion, LEFT_METATARSAL_1, LEFT_PHALANGE_1, (0.0, 0.0, 0.07)) yme.addJoint(motion, LEFT_METATARSAL_3, LEFT_PHALANGE_3, (0.0, 0.0, 0.07)) yme.addJoint(motion, LEFT_METATARSAL_2, LEFT_PHALANGE_2, (0.0, 0.0, 0.07)) yme.addJoint(motion, LEFT_PHALANGE_1, 'LEFT_PHALANGE_Effector1', (0.0, 0.0, 0.06)) yme.addJoint(motion, LEFT_PHALANGE_3, 'LEFT_PHALANGE_Effector3', (0.0, 0.0, 0.06)) yme.addJoint(motion, LEFT_PHALANGE_2, 'LEFT_PHALANGE_Effector2', (0.0, 0.0, 0.06)) #yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_1, (-0.045, -0.06, -0.04)) #yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_2, (0.0, -0.06, -0.04)) #yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_3, (0.045, -0.06, -0.04)) yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_1, (-0.045, -0.06, -0.05)) yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_2, (0.0, -0.06, -0.05)) yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_3, (0.045, -0.06, -0.05)) yme.addJoint(motion, LEFT_CALCANEUS_1, 'LEFT_CALCANEUS_Effector1', (0., 0.0, -0.07)) yme.addJoint(motion, LEFT_CALCANEUS_2, 'LEFT_CALCANEUS_Effector2', (0., 0.0, -0.07)) yme.addJoint(motion, LEFT_CALCANEUS_3, 'LEFT_CALCANEUS_Effector3', (0., 0.0, -0.07)) yme.addJoint(motion, RIGHT_FOOT, RIGHT_TALUS_1, (0.045, -0.06, -0.05)) yme.addJoint(motion, RIGHT_FOOT, RIGHT_TALUS_2, (0.0, -0.06, -0.05)) yme.addJoint(motion, RIGHT_FOOT, RIGHT_TALUS_3, (-0.045, -0.06, -0.05)) yme.addJoint(motion, RIGHT_TALUS_1, RIGHT_METATARSAL_1, (0.0, 0.0, 0.1)) yme.addJoint(motion, RIGHT_TALUS_2, RIGHT_METATARSAL_2, (0.0, 0.0, 0.1)) yme.addJoint(motion, RIGHT_TALUS_3, RIGHT_METATARSAL_3, (0.0, 0.0, 0.1)) yme.addJoint(motion, RIGHT_METATARSAL_1, RIGHT_PHALANGE_1, (0.0, 0.0, 0.07)) yme.addJoint(motion, RIGHT_METATARSAL_2, RIGHT_PHALANGE_2, (0.0, 0.0, 0.07)) yme.addJoint(motion, RIGHT_METATARSAL_3, RIGHT_PHALANGE_3, (0.0, 0.0, 0.07)) yme.addJoint(motion, RIGHT_PHALANGE_1, 'RIGHT_PHALANGE_Effector1', (0.0, 0.0, 0.06)) yme.addJoint(motion, RIGHT_PHALANGE_2, 'RIGHT_PHALANGE_Effector2', (0.0, 0.0, 0.06)) yme.addJoint(motion, RIGHT_PHALANGE_3, 'RIGHT_PHALANGE_Effector3', (0.0, 0.0, 0.06)) #yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_1, (0.045, -0.06, -0.04)) #yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_2, (0.0, -0.06, -0.04)) #yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_3, (-0.045, -0.06, -0.04)) yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_1, (0.045, -0.06, -0.05)) yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_2, (0.0, -0.06, -0.05)) yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_3, (-0.045, -0.06, -0.05)) yme.addJoint(motion, RIGHT_CALCANEUS_1, 'RIGHT_CALCANEUS_Effector1', (0.0, 0.0, -0.07)) yme.addJoint(motion, RIGHT_CALCANEUS_2, 'RIGHT_CALCANEUS_Effector2', (0.0, 0.0, -0.07)) yme.addJoint(motion, RIGHT_CALCANEUS_3, 'RIGHT_CALCANEUS_Effector3', (0.0, 0.0, -0.07)) yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False) yme.rotateJointLocal(motion, RIGHT_TALUS_1, mm.exp(mm.v3(1.0, 0.0, 0.0), .5), False) yme.rotateJointLocal(motion, RIGHT_TALUS_2, mm.exp(mm.v3(1.0, 0.0, 0.0), .5), False) yme.rotateJointLocal(motion, RIGHT_TALUS_3, mm.exp(mm.v3(1.0, 0.0, 0.0), .5), False) yme.rotateJointLocal(motion, RIGHT_CALCANEUS_1, mm.exp(mm.v3(0.0, 0.0, 1.0), 3.14), False) yme.rotateJointLocal(motion, RIGHT_CALCANEUS_2, mm.exp(mm.v3(0.0, 0.0, 1.0), 3.14), False) yme.rotateJointLocal(motion, RIGHT_CALCANEUS_3, mm.exp(mm.v3(0.0, 0.0, 1.0), 3.14), False) yme.rotateJointLocal(motion, RIGHT_CALCANEUS_1, mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False) yme.rotateJointLocal(motion, RIGHT_CALCANEUS_2, mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False) yme.rotateJointLocal(motion, RIGHT_CALCANEUS_3, mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False) yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False) yme.rotateJointLocal(motion, LEFT_TALUS_1, mm.exp(mm.v3(1.0, 0.0, 0.0), .5), False) yme.rotateJointLocal(motion, LEFT_TALUS_3, mm.exp(mm.v3(1.0, 0.0, 0.0), .5), False) yme.rotateJointLocal(motion, LEFT_TALUS_2, mm.exp(mm.v3(1.0, 0.0, 0.0), .5), False) yme.rotateJointLocal(motion, LEFT_CALCANEUS_1, mm.exp(mm.v3(0.0, 0.0, 1.0), 3.14), False) yme.rotateJointLocal(motion, LEFT_CALCANEUS_2, mm.exp(mm.v3(0.0, 0.0, 1.0), 3.14), False) yme.rotateJointLocal(motion, LEFT_CALCANEUS_3, mm.exp(mm.v3(0.0, 0.0, 1.0), 3.14), False) yme.rotateJointLocal(motion, LEFT_CALCANEUS_1, mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False) yme.rotateJointLocal(motion, LEFT_CALCANEUS_2, mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False) yme.rotateJointLocal(motion, LEFT_CALCANEUS_3, mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False) #yme.rotateJointLocal(motion, LEFT_CALCANEUS_1, mm.exp(mm.v3(0.0,-1.0,0.0), 1.57), False) yme.updateGlobalT(motion) ################ if MOTION == FORWARD_JUMP: motion = motion[515:555] elif MOTION == TAEKWONDO: ## Taekwondo base-step motion = motion[0:31] #motion = motion[564:600] elif MOTION == TAEKWONDO2: ## Taekwondo base-step #motion = motion[0:31+40] ## Taekwondo turning-kick motion = motion[108:-1] #motion = motion[108:109] elif MOTION == KICK: #motion = motion[141:-1] #motion = motion[100:-1] #motion = motion[58:-1] motion = motion[82:-1] #motion = motion[0:-1] elif MOTION == STAND2: motion = motion[1:-1] elif MOTION == TIPTOE: #motion = motion[183:440] #motion = motion[350:410] motion = motion[350:550] motion[0:0] = [motion[0]] * 40 motion.extend([motion[-1]] * 5000) # world, model mcfg = ypc.ModelConfig() mcfg.defaultDensity = 1000. mcfg.defaultBoneRatio = .9 for name in massMap: node = mcfg.addNode(name) node.mass = massMap[name] node = mcfg.getNode(HIP) node.length = .2 node.width = .25 node = mcfg.getNode(SPINE1) node.length = .2 node.offset = (0, 0, 0.1) node = mcfg.getNode(SPINE) node.width = .22 #node.length = .2 #### node = mcfg.getNode('RightFoot') node.length = .1 node.width = .1 node = mcfg.getNode('LeftFoot') node.length = .1 node.width = .1 #return mcfg # #mass0 = .4 #width0 = 0.028 #length0 = 0.1 # ##Metatarsal1 #length1 = .12 #width1 = 0.03 #mass1 = 0.4 # #length2 = .1 #width2 = 0.026 #mass2 = 0.4 # ##Metatarsal3 #length3 = .08 #width3 = 0.024 #mass3 = 0.4 # ##Calcaneus1 #length4 = .1 #width4 = 0.032 #mass4 = 0.4 # ##Phalange1 #length5 = .08 #width5 = 0.01 #mass5 = 0.4 ##Phalange3 #length7 = length5 #width7 = width5 #mass7 = mass5 # ##Talus ##length8 = .13 ##width8 = width0*3 ##mass8 = mass0*2. # #length8 = .1 #width8 = width0*3 #mass8 = mass0*1.5 width0 = 0.028 length0 = 0.1 mass0 = .4 #Metatarsal1 length1 = .1 width1 = 0.03 mass1 = 0.4 length2 = length1 width2 = width1 mass2 = 0.4 #Metatarsal3 length3 = length1 width3 = width1 mass3 = 0.4 #Calcaneus1 length4 = length1 width4 = width1 mass4 = 0.4 #Phalange1 length5 = length1 width5 = width1 mass5 = 0.4 #Phalange3 length7 = length1 width7 = width1 mass7 = 0.4 #Talus #length8 = .13 #width8 = width0*3 #mass8 = mass0*2. length8 = .1 width8 = width0 * 3 mass8 = mass0 * 1.5 node = mcfg.getNode(RIGHT_FOOT) node.length = length8 node.width = width8 node.mass = mass8 node = mcfg.getNode(RIGHT_TALUS_1) node.length = length1 node.width = width1 node.mass = mass1 node = mcfg.getNode(RIGHT_TALUS_3) node.length = length1 node.width = width1 node.mass = mass1 node = mcfg.getNode(RIGHT_TALUS_2) node.length = length1 node.width = width1 node.mass = mass1 node = mcfg.getNode(RIGHT_METATARSAL_1) node.length = length2 node.width = width2 node.mass = mass2 node = mcfg.getNode(RIGHT_METATARSAL_3) node.length = length2 node.width = width2 node.mass = mass2 node = mcfg.getNode(RIGHT_METATARSAL_2) node.length = length2 node.width = width2 node.mass = mass2 node = mcfg.getNode(RIGHT_PHALANGE_1) node.length = length3 node.width = width3 node.mass = mass3 node = mcfg.getNode(RIGHT_PHALANGE_2) node.length = length3 node.width = width3 node.mass = mass3 node = mcfg.getNode(RIGHT_PHALANGE_3) node.length = length3 node.width = width3 node.mass = mass3 node = mcfg.getNode(RIGHT_CALCANEUS_1) node.length = length4 node.width = width4 node.mass = mass4 node = mcfg.getNode(RIGHT_CALCANEUS_2) node.length = length4 node.width = width4 node.mass = mass4 node = mcfg.getNode(RIGHT_CALCANEUS_3) node.length = length4 node.width = width4 node.mass = mass4 node = mcfg.getNode(LEFT_FOOT) node.length = length8 node.width = width8 node.mass = mass8 node = mcfg.getNode(LEFT_TALUS_1) node.length = length1 node.width = width1 node.mass = mass1 node = mcfg.getNode(LEFT_TALUS_3) node.length = length1 node.width = width1 node.mass = mass1 node = mcfg.getNode(LEFT_TALUS_2) node.length = length1 node.width = width1 node.mass = mass1 node = mcfg.getNode(LEFT_METATARSAL_1) node.length = length2 node.width = width2 node.mass = mass2 node = mcfg.getNode(LEFT_METATARSAL_3) node.length = length2 node.width = width2 node.mass = mass2 node = mcfg.getNode(LEFT_METATARSAL_2) node.length = length2 node.width = width2 node.mass = mass2 node = mcfg.getNode(LEFT_PHALANGE_1) node.length = length3 node.width = width3 node.mass = mass3 node = mcfg.getNode(LEFT_PHALANGE_2) node.length = length3 node.width = width3 node.mass = mass3 node = mcfg.getNode(LEFT_PHALANGE_3) node.length = length3 node.width = width3 node.mass = mass3 node = mcfg.getNode(LEFT_CALCANEUS_1) node.length = length4 node.width = width4 node.mass = mass4 node = mcfg.getNode(LEFT_CALCANEUS_2) node.length = length4 node.width = width4 node.mass = mass4 node = mcfg.getNode(LEFT_CALCANEUS_3) node.length = length4 node.width = width4 node.mass = mass4 #node.offset = (0.0, -0.025, 0.0) node = mcfg.getNode('LeftFoot') node = mcfg.getNode(RIGHT_TALUS_1) node.geom = 'MyFoot3' node = mcfg.getNode(RIGHT_TALUS_3) node.geom = 'MyFoot3' node = mcfg.getNode(RIGHT_TALUS_2) node.geom = 'MyFoot3' node = mcfg.getNode(LEFT_TALUS_1) node.geom = 'MyFoot3' node = mcfg.getNode(LEFT_TALUS_3) node.geom = 'MyFoot3' node = mcfg.getNode(LEFT_TALUS_2) node.geom = 'MyFoot3' node = mcfg.getNode(RIGHT_METATARSAL_1) node.geom = 'MyFoot3' node = mcfg.getNode(RIGHT_METATARSAL_2) node.geom = 'MyFoot3' node = mcfg.getNode(RIGHT_METATARSAL_3) node.geom = 'MyFoot3' node = mcfg.getNode(LEFT_METATARSAL_1) node.geom = 'MyFoot3' node = mcfg.getNode(LEFT_METATARSAL_3) node.geom = 'MyFoot3' node = mcfg.getNode(LEFT_METATARSAL_2) node.geom = 'MyFoot3' node = mcfg.getNode(RIGHT_PHALANGE_1) node.geom = 'MyFoot3' node = mcfg.getNode(RIGHT_PHALANGE_2) node.geom = 'MyFoot3' node = mcfg.getNode(RIGHT_PHALANGE_3) node.geom = 'MyFoot3' node = mcfg.getNode(LEFT_PHALANGE_1) node.geom = 'MyFoot3' node = mcfg.getNode(LEFT_PHALANGE_2) node.geom = 'MyFoot3' node = mcfg.getNode(LEFT_PHALANGE_3) node.geom = 'MyFoot3' node = mcfg.getNode(RIGHT_CALCANEUS_1) node.geom = 'MyFoot3' node = mcfg.getNode(RIGHT_CALCANEUS_2) node.geom = 'MyFoot3' node = mcfg.getNode(RIGHT_CALCANEUS_3) node.geom = 'MyFoot3' node = mcfg.getNode(LEFT_CALCANEUS_1) node.geom = 'MyFoot3' node = mcfg.getNode(LEFT_CALCANEUS_2) node.geom = 'MyFoot3' node = mcfg.getNode(LEFT_CALCANEUS_3) node.geom = 'MyFoot3' wcfg = ypc.WorldConfig() wcfg.planeHeight = 0. wcfg.useDefaultContactModel = False stepsPerFrame = 60 wcfg.timeStep = (1 / 30.) / (stepsPerFrame) #stepsPerFrame = 10 #wcfg.timeStep = (1/120.)/(stepsPerFrame) #wcfg.timeStep = (1/1800.) # parameter config = {} config['Kt'] = 200 config['Dt'] = 2 * (config['Kt']**.5) # tracking gain config['Kl'] = .10 config['Dl'] = 2 * (config['Kl']**.5) # linear balance gain config['Kh'] = 0.1 config['Dh'] = 2 * (config['Kh']**.5) # angular balance gain config['Ks'] = 20000 config['Ds'] = 2 * (config['Ks']**.5) # penalty force spring gain config['Bt'] = 1. config['Bl'] = 1. #0.5 config['Bh'] = 1. config['weightMap2'] = { RIGHT_ARM: .2, RIGHT_FORE_ARM: .2, LEFT_ARM: .2, LEFT_FORE_ARM: .2, SPINE: .5, SPINE1: .3, RIGHT_FOOT: .7, LEFT_FOOT: .7, HIP: .5, RIGHT_UP_LEG: .7, RIGHT_LEG: .7, LEFT_UP_LEG: .7, LEFT_LEG: .7, LEFT_TALUS_1: .7, RIGHT_TALUS_1: .7, LEFT_TALUS_2: .7, RIGHT_TALUS_2: .7, LEFT_TALUS_3: .7, RIGHT_TALUS_3: .7, LEFT_METATARSAL_1: .7, RIGHT_METATARSAL_1: .7, LEFT_METATARSAL_2: .7, RIGHT_METATARSAL_2: .7, LEFT_METATARSAL_3: .7, RIGHT_METATARSAL_3: .7, RIGHT_CALCANEUS_1: .7, LEFT_CALCANEUS_1: .7, RIGHT_CALCANEUS_2: .7, LEFT_CALCANEUS_2: .7, RIGHT_CALCANEUS_3: .7, LEFT_CALCANEUS_3: .7, LEFT_PHALANGE_1: .4, LEFT_PHALANGE_2: .4, LEFT_PHALANGE_3: .4, RIGHT_PHALANGE_1: .4, RIGHT_PHALANGE_2: .4, RIGHT_PHALANGE_3: .4 } config['weightMap'] = { RIGHT_ARM: .2, RIGHT_FORE_ARM: .2, LEFT_ARM: .2, LEFT_FORE_ARM: .2, SPINE: .3, SPINE1: .2, RIGHT_FOOT: .3, LEFT_FOOT: .3, HIP: .3, RIGHT_UP_LEG: .1, RIGHT_LEG: .2, LEFT_UP_LEG: .1, LEFT_LEG: .2, LEFT_TALUS_1: .1, RIGHT_TALUS_1: .1, LEFT_TALUS_2: .1, RIGHT_TALUS_2: .1, LEFT_TALUS_3: .1, RIGHT_TALUS_3: .1, LEFT_METATARSAL_1: .1, RIGHT_METATARSAL_1: .1, LEFT_METATARSAL_2: .1, RIGHT_METATARSAL_2: .1, LEFT_METATARSAL_3: .1, RIGHT_METATARSAL_3: .1, RIGHT_CALCANEUS_1: .2, LEFT_CALCANEUS_1: .2, RIGHT_CALCANEUS_2: .2, LEFT_CALCANEUS_2: .2, RIGHT_CALCANEUS_3: .2, LEFT_CALCANEUS_3: .2, LEFT_PHALANGE_1: .1, LEFT_PHALANGE_2: .1, LEFT_PHALANGE_3: .1, RIGHT_PHALANGE_1: .1, RIGHT_PHALANGE_2: .1, RIGHT_PHALANGE_3: .1 } ''' config['weightMap']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:.3, SPINE1:.2, RIGHT_FOOT:.3, LEFT_FOOT:.3, HIP:.3, RIGHT_UP_LEG:.1, RIGHT_LEG:.2, LEFT_UP_LEG:.1, LEFT_LEG:.2, LEFT_TALUS_1:.01, RIGHT_TALUS_1:.01, LEFT_TALUS_2:.01, RIGHT_TALUS_2:.01, LEFT_TALUS_3:.01, RIGHT_TALUS_3:.01, LEFT_METATARSAL_1:.01, RIGHT_METATARSAL_1:.01, LEFT_METATARSAL_2:.01, RIGHT_METATARSAL_2:.01, LEFT_METATARSAL_3:.01, RIGHT_METATARSAL_3:.01, RIGHT_CALCANEUS_1:.01, LEFT_CALCANEUS_1:.01, RIGHT_CALCANEUS_2:.01, LEFT_CALCANEUS_2:.01, RIGHT_CALCANEUS_3:.01, LEFT_CALCANEUS_3:.01, LEFT_PHALANGE_1:.01, LEFT_PHALANGE_2:.01, LEFT_PHALANGE_3:.01, RIGHT_PHALANGE_1:.01, RIGHT_PHALANGE_2:.01, RIGHT_PHALANGE_3:.01} ''' #config['weightMap']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:.3, SPINE1:.2, RIGHT_FOOT:.3, LEFT_FOOT:.3, HIP:.3, # RIGHT_UP_LEG:.1, RIGHT_LEG:.2, LEFT_UP_LEG:.1, LEFT_LEG:.2, # LEFT_TALUS_1:.1, RIGHT_TALUS_1:.1, LEFT_TALUS_3:.1, RIGHT_TALUS_3:.1, # LEFT_METATARSAL_1:.1, RIGHT_METATARSAL_1:.1, LEFT_METATARSAL_3:.1, RIGHT_METATARSAL_3:.1, # RIGHT_CALCANEUS_1:.2, LEFT_CALCANEUS_1:.2, RIGHT_CALCANEUS_3:.2, LEFT_CALCANEUS_3:.2, # LEFT_PHALANGE_1:.1, LEFT_PHALANGE_3:.1, RIGHT_PHALANGE_1:.1, RIGHT_PHALANGE_3:.1} # #config['weightMap2']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:.5, SPINE1:.3, RIGHT_FOOT:.7, LEFT_FOOT:.7, HIP:.5, # RIGHT_UP_LEG:.7, RIGHT_LEG:.7, LEFT_UP_LEG:.7, LEFT_LEG:.7, # LEFT_TALUS_1:.7, RIGHT_TALUS_1:.7, LEFT_TALUS_3:.7, RIGHT_TALUS_3:.7, # LEFT_METATARSAL_1:.7, RIGHT_METATARSAL_1:.7, LEFT_METATARSAL_3:.7, RIGHT_METATARSAL_3:.7, # RIGHT_CALCANEUS_1:.7, LEFT_CALCANEUS_1:.7, RIGHT_CALCANEUS_3:.7, LEFT_CALCANEUS_3:.7, # LEFT_PHALANGE_1:.4, LEFT_PHALANGE_3:.4, RIGHT_PHALANGE_1:.4, RIGHT_PHALANGE_3:.4} config['supLink'] = LEFT_FOOT config['supLink2'] = RIGHT_FOOT #config['end'] = HIP config['end'] = SPINE1 config['const'] = HIP config['root'] = HIP config['FootPartNum'] = FOOT_PART_NUM config['FootLPart'] = [ LEFT_FOOT, LEFT_CALCANEUS_1, LEFT_CALCANEUS_2, LEFT_CALCANEUS_3, LEFT_TALUS_1, LEFT_TALUS_2, LEFT_TALUS_3, LEFT_METATARSAL_1, LEFT_METATARSAL_2, LEFT_METATARSAL_3, LEFT_PHALANGE_1, LEFT_PHALANGE_2, LEFT_PHALANGE_3 ] config['FootRPart'] = [ RIGHT_FOOT, RIGHT_CALCANEUS_1, RIGHT_CALCANEUS_2, RIGHT_CALCANEUS_3, RIGHT_TALUS_1, RIGHT_TALUS_2, RIGHT_TALUS_3, RIGHT_METATARSAL_1, RIGHT_METATARSAL_2, RIGHT_METATARSAL_3, RIGHT_PHALANGE_1, RIGHT_PHALANGE_2, RIGHT_PHALANGE_3 ] #config['FootLPart'] = [LEFT_FOOT, LEFT_CALCANEUS_1, LEFT_METATARSAL_1, LEFT_METATARSAL_3, LEFT_PHALANGE_1, LEFT_PHALANGE_3] #config['FootRPart'] = [RIGHT_FOOT, RIGHT_CALCANEUS_1, RIGHT_METATARSAL_1, RIGHT_METATARSAL_3, RIGHT_PHALANGE_1, RIGHT_PHALANGE_3] return motion, mcfg, wcfg, stepsPerFrame, config
def main(): np.set_printoptions(precision=4, linewidth=200) motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped( SEGMENT_FOOT, SEGMENT_FOOT_MAG) #motion, mcfg, wcfg, stepsPerFrame, config = mit.create_jump_biped() # motion_offset = np.array((0, 0.15, 0)) # motion.translateByOffset(motion_offset, True) 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.15, 0) controlToMotionOffset = (1.5, 0.03, 0) dartModel.translateByOffset(controlToMotionOffset) totalDOF = dartModel.getTotalDOF() DOFs = dartModel.getDOFs() foot_dofs = [] left_foot_dofs = [] right_foot_dofs = [] foot_seg_dofs = [] left_foot_seg_dofs = [] right_foot_seg_dofs = [] # for joint_idx in range(motion[0].skeleton.getJointNum()): for joint_idx in range(dartModel.getJointNum()): joint_name = dartModel.getJoint(joint_idx).name # joint_name = motion[0].skeleton.getJointName(joint_idx) if 'Foot' in joint_name: foot_dofs_temp = dartModel.getJointDOFIndexes(joint_idx) foot_dofs.extend(foot_dofs_temp) if 'Left' in joint_name: left_foot_dofs.extend(foot_dofs_temp) elif 'Right' in joint_name: right_foot_dofs.extend(foot_dofs_temp) if 'foot' in joint_name: foot_dofs_temp = dartModel.getJointDOFIndexes(joint_idx) foot_seg_dofs.extend(foot_dofs_temp) if 'Left' in joint_name: left_foot_dofs.extend(foot_dofs_temp) elif 'Right' in joint_name: right_foot_dofs.extend(foot_dofs_temp) # for dart_foot_joint in (dartModel.getJoint(i) for i in foot_seg_dofs): # dart_foot_joint.set_actuator_type(pydart.Joint.FORCE) # 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()) #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('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 = 100. initKh = 100. initBl = .1 initBh = .13 initSupKt = 17. initFm = 50.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.labelKt.value(initKt) # viewer.objectInfoWnd.labelKl.value(initKl) # viewer.objectInfoWnd.labelKh.value(initKh) # viewer.objectInfoWnd.labelBl.value(initBl) # viewer.objectInfoWnd.labelBh.value(initBh) # viewer.objectInfoWnd.labelSupKt.value(initSupKt) # viewer.objectInfoWnd.labelFm.value(initFm) # # viewer.objectInfoWnd.sliderKt.value(initKt*50) # viewer.objectInfoWnd.sliderKl.value(initKl*10) # viewer.objectInfoWnd.sliderKh.value(initKh*10) # viewer.objectInfoWnd.sliderBl.value(initBl*100) # viewer.objectInfoWnd.sliderBh.value(initBh*100) # viewer.objectInfoWnd.sliderSupKt.value(initSupKt*10) # viewer.objectInfoWnd.sliderFm.value(initFm) viewer.force_on = False def viewer_SetForceState(obj): 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 * 7, 40, 20, 'X') viewer.objectInfoWnd.labelForceX.value(0) viewer.objectInfoWnd.labelForceY = Fl_Value_Input(80, 30 + offset * 7, 40, 20, 'Y') viewer.objectInfoWnd.labelForceY.value(0) viewer.objectInfoWnd.labelForceZ = Fl_Value_Input(140, 30 + offset * 7, 40, 20, 'Z') viewer.objectInfoWnd.labelForceZ.value(1) viewer.objectInfoWnd.labelForceDur = Fl_Value_Input( 220, 30 + offset * 7, 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, 16., 8.) def getParamVal(paramname): return viewer.objectInfoWnd.getVal(paramname) def getParamVals(paramnames): return (getParamVal(name) for name in paramnames) extendedFootName = [ 'Foot_foot_0_0', 'Foot_foot_0_1', 'Foot_foot_0_0_0', 'Foot_foot_0_1_0', 'Foot_foot_1_0', 'Foot_foot_1_1', 'Foot_foot_1_2' ] 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 } footIdDic = lIDdic.copy() footIdDic.update(rIDdic) foot_left_idx = motion[0].skeleton.getJointIndex('LeftFoot') foot_right_idx = motion[0].skeleton.getJointIndex('RightFoot') ################################### #simulate ################################### def simulateCallback(frame): global g_initFlag global forceShowTime global preFootCenter global maxContactChangeCount global contactChangeCount global contact global contactChangeType Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals( ['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt']) Dt = 2. * (Kt**.5) Dl = 2. * (Kl**.5) Dh = 2. * (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) doubleTosingleOffset = 0.15 singleTodoubleOffset = 0.30 #doubleTosingleOffset = 0.09 doubleTosingleVelOffset = 0.0 # tracking # th_r = motion.getDOFPositions(frame) # th = dartModel.getDOFPositions() # 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() # ype.flatten(ddth_des, ddth_des_flat) # ype.flatten(dth, dth_flat) ################################################# # jacobian ################################################# #caution!! body orientation and joint orientation of foot are totally different!! footOriL = dartModel.getJointOrientationGlobal(supL) footOriR = dartModel.getJointOrientationGlobal(supR) #desire footCenter[1] = 0.041135 #desire footCenter[1] = 0.0197 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) refFootJointVelR = motion.getJointVelocityGlobal(supR, frame) refFootJointAngVelR = motion.getJointAngVelocityGlobal(supR, frame) refFootJointR = motion.getJointPositionGlobal(supR, frame) refFootVelR = refFootJointVelR + np.cross(refFootJointAngVelR, (refFootR - refFootJointR)) refFootJointVelL = motion.getJointVelocityGlobal(supL, frame) refFootJointAngVelL = motion.getJointAngVelocityGlobal(supL, frame) refFootJointL = motion.getJointPositionGlobal(supL, frame) refFootVelL = refFootJointVelL + np.cross(refFootJointAngVelL, (refFootL - refFootJointL)) 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 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 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), :] dartMotionModel.update(motion[frame]) # ddth_des_flat = pdcontroller.compute(dartMotionModel.get_q()) ddth_des_flat = pdcontroller.compute(motion.getDOFPositions(frame)) #calculate footCenter footCenter = .5 * (footCenterL + footCenterR) #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() #elif contact == 1 or footCenterL[1] > doubleTosingleOffset/2: if contact == 1: footCenter = footCenterR.copy() footCenter[1] = 0. if contactChangeCount > 0 and contactChangeType == 'StoD': #change footcenter gradually footCenter = preFootCenter + ( maxContactChangeCount - contactChangeCount) * ( footCenter - preFootCenter) / maxContactChangeCount preFootCenter = footCenter.copy() # foot adjustment foot_angle_weight = 1. foot_dCM_weight = 5. foot_center_diff = CM_plane + dCM_plane * frame_step_size * foot_dCM_weight - footCenter foot_center_diff_norm = np.linalg.norm(foot_center_diff) foot_left_height = dartModel.getJointPositionGlobal(foot_left_idx)[1] foot_right_height = dartModel.getJointPositionGlobal(foot_left_idx)[1] foot_left_angle = foot_angle_weight * math.atan2( foot_center_diff_norm, foot_left_height) foot_right_angle = foot_angle_weight * math.atan2( foot_center_diff_norm, foot_right_height) foot_axis = np.cross(np.array((0., 1., 0.)), foot_center_diff) foot_left_R = mm.exp(foot_axis, foot_left_angle) foot_right_R = mm.exp(foot_axis, foot_right_angle) # motion[frame].mulJointOrientationGlobal(foot_left_idx, foot_left_R) # motion[frame].mulJointOrientationGlobal(foot_right_idx, foot_right_R) # hfi.footAdjust(motion[frame], footIdDic, SEGMENT_FOOT_MAG, SEGMENT_FOOT_RAD, 0.) # linear momentum #TODO: # We should consider dCM_ref, shouldn't we? # add getBodyPositionGlobal and getBodyPositionsGlobal in csVpModel! # to do 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. # 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 = 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]))) left_foot_up_vec, right_foot_up_vec = hfi.get_foot_up_vector( motion[frame], footIdDic, None) a_oriL = mm.logSO3( mm.getSO3FromVectors(left_foot_up_vec, np.array([0, 1, 0]))) a_oriR = mm.logSO3( mm.getSO3FromVectors(right_foot_up_vec, np.array([0, 1, 0]))) #if contact == 3 and contactChangeCount < maxContactChangeCount/4 and contactChangeCount >=1: #kt_sup = 30 #viewer.objectInfoWnd.labelSupKt.value(kt_sup) #viewer.objectInfoWnd.sliderSupKt.value(initSupKt*10) # a_supL = np.append(kt_sup*(refFootL - footCenterL + contMotionOffset) + dt_sup*(refFootVelL - footBodyVelL), kt_sup*a_oriL+dt_sup*(refFootAngVelL-footBodyAngVelL)) # a_supR = np.append(kt_sup*(refFootR - footCenterR + contMotionOffset) + dt_sup*(refFootVelR - footBodyVelR), kt_sup*a_oriR+dt_sup*(refFootAngVelR-footBodyAngVelR)) a_supL = np.append( kt_sup * (refFootL - footCenterL + contMotionOffset) + dt_sup * (refFootVelL - footBodyVelL), kt_sup * a_oriL + dt_sup * (refFootAngVelL - footBodyAngVelL)) a_supR = np.append( kt_sup * (refFootR - footCenterR + contMotionOffset) + dt_sup * (refFootVelR - footBodyVelR), kt_sup * a_oriR + dt_sup * (refFootAngVelR - footBodyAngVelR)) # a_supL[3:] = 0. # a_supR[3:] = 0. if contactChangeCount > 0 and contactChangeType == 'DtoS': #refFootR += (footCenter-CM_plane)/2. #refFootR[1] = 0 #pre contact value are needed #if contact == 2: ##refFootR[0] += 0.2 ##refFootR[2] -= 0.05 #offsetDropR = (footCenter-CM_plane)/2. #refFootR += offsetDropR #refFootR[1] = 0. ##refFootR[2] = footCenterR[2] - contMotionOffset[2] ##refFootR[0] = footCenterR[0] - contMotionOffset[0] #refFootL[0] += 0.05 #refFootL[2] -= 0.05 #elif contact == 1: #offsetDropL = (footCenter-CM_plane)/2. #refFootL += offsetDropL #refFootL[1] = 0. #a_supL = np.append(kt_sup*(refFootL - footCenterL + contMotionOffset) + dt_sup*(refFootVelL - footBodyVelL), kt_sup*a_oriL+dt_sup*(refFootAngVelL-footBodyAngVelL)) #a_supR = np.append(kt_sup*(refFootR - footCenterR + contMotionOffset) + dt_sup*(refFootVelR - footBodyVelR), kt_sup*a_oriR+dt_sup*(refFootAngVelR-footBodyAngVelR)) #a_supL = np.append(kt_sup*(refFootL - footCenterL + contMotionOffset) + dt_sup*(refFootVelL - footBodyVelL), 16*kt_sup*a_oriL+4*dt_sup*(refFootAngVelL-footBodyAngVelL)) #a_supR = np.append(kt_sup*(refFootR - footCenterR + contMotionOffset) + dt_sup*(refFootVelR - footBodyVelR), 16*kt_sup*a_oriR+4*dt_sup*(refFootAngVelR-footBodyAngVelR)) 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': #refFootR[0] +=0.05 #refFootR[2] +=0.05 linkt = (13. * contactChangeCount) / maxContactChangeCount + 1. lindt = 2 * (linkt**.5) angkt = (13. * contactChangeCount) / maxContactChangeCount + 1. angdt = 2 * (angkt**.5) #a_supL = np.append(4*kt_sup*(refFootL - footCenterL + contMotionOffset) + 2*dt_sup*(refFootVelL - footBodyVelL), 16*kt_sup*a_oriL+4*dt_sup*(refFootAngVelL-footBodyAngVelL)) #a_supR = np.append(4*kt_sup*(refFootR - footCenterR + contMotionOffset) + 2*dt_sup*(refFootVelR - footBodyVelR), 16*kt_sup*a_oriR+4*dt_sup*(refFootAngVelR-footBodyAngVelR)) 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)) #a_supL = np.append(16*kt_sup*(refFootL - footCenterL + contMotionOffset) + 4*dt_sup*(refFootVelL - footBodyVelL), 16*kt_sup*a_oriL+4*dt_sup*(refFootAngVelL-footBodyAngVelL)) #a_supR = np.append(16*kt_sup*(refFootR - footCenterR + contMotionOffset) + 4*dt_sup*(refFootVelR - footBodyVelR), 16*kt_sup*a_oriR+4*dt_sup*(refFootAngVelR-footBodyAngVelR)) #a_supL = np.append(4*kt_sup*(refFootL - footCenterL + contMotionOffset) + 2*dt_sup*(refFootVelL - footBodyVelL), 32*kt_sup*a_oriL+5.6*dt_sup*(refFootAngVelL-footBodyAngVelL)) #a_supR = np.append(4*kt_sup*(refFootR - footCenterR + contMotionOffset) + 2*dt_sup*(refFootVelR - footBodyVelR), 32*kt_sup*a_oriR+5.6*dt_sup*(refFootAngVelR-footBodyAngVelR)) #a_supL[1] = kt_sup*(refFootL[1] - footCenterL[1] + contMotionOffset[1]) + dt_sup*(refFootVelL[1] - footBodyVelL[1]) #a_supR[1] = kt_sup*(refFootR[1] - footCenterR[1] + contMotionOffset[1]) + dt_sup*(refFootVelR[1] - footBodyVelR[1]) ##if contact == 2: #if refFootR[1] <doubleTosingleOffset : #Jsup = np.vstack((JsupL, JsupR)) #dJsup = np.vstack((dJsupL, dJsupR)) #a_sup = np.append(a_supL, a_supR) #else: #Jsup = JsupL.copy() #dJsup = dJsupL.copy() #a_sup = a_supL.copy() # 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) if dH_des is not None: mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias) 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']) # remove foot seg effect ddth_sol[foot_dofs] = ddth_des_flat[foot_dofs] # ddth_sol[:] = ddth_des_flat[:] 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) 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) # 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 create_biped(): # motion # motionName = 'wd2_n_kick.bvh' if MOTION == STAND: # motionName = 'wd2_stand.bvh' motionName = 'wd2_stand2.bvh' # motionName = 'woddy2_jump0_short.bvh' elif MOTION == STAND2: motionName = 'ww13_41_V001.bvh' elif MOTION == FORWARD_JUMP: motionName = 'woddy2_jump0.bvh' elif MOTION == TAEKWONDO : motionName = './MotionFile/wd2_098_V001.bvh' elif MOTION == TAEKWONDO2 : motionName = './MotionFile/wd2_098_V001.bvh' elif MOTION == KICK : motionName = 'wd2_n_kick.bvh' elif MOTION == WALK : motionName = 'wd2_WalkForwardNormal00.bvh' elif MOTION == TIPTOE: motionName = './MotionFile/cmu/15_07_15_07.bvh' # motionName = 'ww13_41_V001.bvh' scale = 0.01 if MOTION == WALK : scale = 1.0 elif MOTION == TIPTOE: scale = 0.01 motion = yf.readBvhFile(motionName, scale) yme.removeJoint(motion, HEAD, False) yme.removeJoint(motion, RIGHT_SHOULDER, False) yme.removeJoint(motion, LEFT_SHOULDER, False) yme.removeJoint(motion, RIGHT_TOES, False) yme.removeJoint(motion, RIGHT_TOES_END, False) yme.removeJoint(motion, LEFT_TOES, False) yme.removeJoint(motion, LEFT_TOES_END, False) yme.removeJoint(motion, RIGHT_HAND_END, False) yme.removeJoint(motion, LEFT_HAND_END, False) yme.offsetJointLocal(motion, RIGHT_ARM, (.03, -.05, 0.), False) yme.offsetJointLocal(motion, LEFT_ARM, (-.03, -.05, 0.), False) yme.rotateJointLocal(motion, HIP, mm.exp(mm.v3(1., 0., 0.), .01), False) # yme.rotateJointLocal(motion, HIP, mm.exp(mm.v3(0,0,1), -.01), False) # addFootSegment # Talus length1 = .15 width1 = .03 mass1 = .4 # Phalange length3 = length1*0.75 width3 = width1 mass3 = 0.4 length3_2 = length1*0.6 width3_2 = width1 mass3_2 = 0.4 #Calcaneus length4 = 0.05 width4 = width1 mass4 = 0.4 width1 = 0.03 yme.addJoint(motion, LEFT_FOOT, LEFT_TALUS_1, (-0.025, -0.06, -0.05)) yme.addJoint(motion, LEFT_FOOT, LEFT_TALUS_2, ( 0.025, -0.06, -0.05)) yme.addJoint(motion, RIGHT_FOOT, RIGHT_TALUS_1, ( 0.025, -0.06, -0.05)) yme.addJoint(motion, RIGHT_FOOT, RIGHT_TALUS_2, (-0.025, -0.06, -0.05)) yme.addJoint(motion, LEFT_TALUS_1, LEFT_PHALANGE_1, (0.0, 0.0, length1-width1-width3 )) yme.addJoint(motion, LEFT_TALUS_2, LEFT_PHALANGE_2, (0.0, 0.0, length1-width1-width3_2)) yme.addJoint(motion, RIGHT_TALUS_1, RIGHT_PHALANGE_1, (0.0, 0.0, length1-width1-width3 )) yme.addJoint(motion, RIGHT_TALUS_2, RIGHT_PHALANGE_2, (0.0, 0.0, length1-width1-width3_2)) yme.addJoint(motion, LEFT_PHALANGE_1, 'LEFT_PHALANGE_Effector1', (0.0, 0.0, length3 -width3 )) yme.addJoint(motion, LEFT_PHALANGE_2, 'LEFT_PHALANGE_Effector2', (0.0, 0.0, length3_2-width3_2)) yme.addJoint(motion, RIGHT_PHALANGE_1, 'RIGHT_PHALANGE_Effector1', (0.0, 0.0, length3 -width3 )) yme.addJoint(motion, RIGHT_PHALANGE_2, 'RIGHT_PHALANGE_Effector2', (0.0, 0.0, length3_2-width3_2)) yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_1, (-0.025, -0.06, -0.05)) yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_2, ( 0.025, -0.06, -0.05)) yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_1, ( 0.025, -0.06, -0.05)) yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_2, (-0.025, -0.06, -0.05)) yme.addJoint(motion, LEFT_CALCANEUS_1, 'LEFT_CALCANEUS_Effector1', (0.0, 0.0, -length4)) yme.addJoint(motion, LEFT_CALCANEUS_2, 'LEFT_CALCANEUS_Effector2', (0.0, 0.0, -length4)) yme.addJoint(motion, RIGHT_CALCANEUS_1, 'RIGHT_CALCANEUS_Effector1', (0.0, 0.0, -length4)) yme.addJoint(motion, RIGHT_CALCANEUS_2, 'RIGHT_CALCANEUS_Effector2', (0.0, 0.0, -length4)) yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1.0,0.0,0.0), -.5), False) yme.rotateJointLocal(motion, RIGHT_TALUS_1, mm.exp(mm.v3(1.0,0.0,0.0), .5), False) yme.rotateJointLocal(motion, RIGHT_TALUS_2, mm.exp(mm.v3(1.0,0.0,0.0), .5), False) yme.rotateJointLocal(motion, RIGHT_CALCANEUS_1, mm.exp(mm.v3(0.0,0.0,1.0), 3.14), False) yme.rotateJointLocal(motion, RIGHT_CALCANEUS_2, mm.exp(mm.v3(0.0,0.0,1.0), 3.14), False) yme.rotateJointLocal(motion, RIGHT_CALCANEUS_1, mm.exp(mm.v3(1.0,0.0,0.0), -.5), False) yme.rotateJointLocal(motion, RIGHT_CALCANEUS_2, mm.exp(mm.v3(1.0,0.0,0.0), -.5), False) #yme.rotateJointLocal(motion, LEFT_PHALANGE_1, mm.exp(mm.v3(0., 1., 0.), 1.57), False) yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(1.0,0.0,0.0), -.5), False) yme.rotateJointLocal(motion, LEFT_TALUS_1, mm.exp(mm.v3(1.0,0.0,0.0), .5), False) yme.rotateJointLocal(motion, LEFT_TALUS_2, mm.exp(mm.v3(1.0,0.0,0.0), .5), False) yme.rotateJointLocal(motion, LEFT_CALCANEUS_1, mm.exp(mm.v3(0.0,0.0,1.0), 3.14), False) yme.rotateJointLocal(motion, LEFT_CALCANEUS_2, mm.exp(mm.v3(0.0,0.0,1.0), 3.14), False) yme.rotateJointLocal(motion, LEFT_CALCANEUS_1, mm.exp(mm.v3(1.0,0.0,0.0), -.5), False) yme.rotateJointLocal(motion, LEFT_CALCANEUS_2, mm.exp(mm.v3(1.0,0.0,0.0), -.5), False) #yme.rotateJointLocal(motion, LEFT_CALCANEUS_1, mm.exp(mm.v3(0.0,-1.0,0.0), 1.57), False) yme.updateGlobalT(motion) ################ if MOTION == FORWARD_JUMP: motion = motion[515:555] elif MOTION == TAEKWONDO: ## Taekwondo base-step motion = motion[0:31] #motion = motion[564:600] elif MOTION == TAEKWONDO2: ## Taekwondo base-step #motion = motion[0:31+40] ## Taekwondo turning-kick motion = motion[108:-1] #motion = motion[108:109] elif MOTION == KICK: #motion = motion[141:-1] #motion = motion[100:-1] #motion = motion[58:-1] motion = motion[82:-1] #motion = motion[0:-1] elif MOTION == STAND2: motion = motion[1:-1] elif MOTION == TIPTOE: #motion = motion[183:440] #motion = motion[350:410] motion = motion[350:550] motion[0:0] = [motion[0]]*40 motion.extend([motion[-1]]*2000) # world, model mcfg = ypc.ModelConfig() mcfg.defaultDensity = 1000. mcfg.defaultBoneRatio = 1. for name in massMap: node = mcfg.addNode(name) node.mass = massMap[name] node = mcfg.getNode(HIP) node.length = .2 node.width = .25 node = mcfg.getNode(SPINE1) node.length = .2 node.offset = (0,0,0.1) node = mcfg.getNode(SPINE) node.width = .22 # node.length = .2 #### node = mcfg.getNode('RightFoot') node.length = .1 node.width = .1 node = mcfg.getNode('LeftFoot') node.length = .1 node.width = .1 #Talus #length1 = .12 #width1 = 0.03 #mass1 = 0.4 #Phalange #length3 = length1 #width3 = width1 #mass3 = 0.4 #length3_2 = length1*0.8 #width3_2 = width1 #mass3_2 = 0.4 #Calcaneus1 #length4 = 0.1 #width4 = width1 #mass4 = 0.4 #Foot length8 = .1 width8 = 0.028*3 mass8 = .4*1.5 node = mcfg.getNode(RIGHT_FOOT) node.length = length8 node.width = width8 node.mass = mass8 node = mcfg.getNode(RIGHT_TALUS_1) node.length = length1 - width1 node.width = width1 node.mass = mass1 node = mcfg.getNode(RIGHT_TALUS_2) node.length = length1 - width1 node.width = width1 node.mass = mass1 node = mcfg.getNode(RIGHT_PHALANGE_1) node.length = length3 node.width = width3 node.mass = mass3 node = mcfg.getNode(RIGHT_PHALANGE_2) node.length = length3_2 node.width = width3_2 node.mass = mass3_2 node = mcfg.getNode(RIGHT_CALCANEUS_1) node.length = length4 - width4 node.width = width4 node.mass = mass4 node = mcfg.getNode(RIGHT_CALCANEUS_2) node.length = length4 - width4 node.width = width4 node.mass = mass4 node = mcfg.getNode(LEFT_FOOT) node.length = length8 node.width = width8 node.mass = mass8 node = mcfg.getNode(LEFT_TALUS_1) node.length = length1 - width1 node.width = width1 node.mass = mass1 node = mcfg.getNode(LEFT_TALUS_2) node.length = length1 - width1 node.width = width1 node.mass = mass1 node = mcfg.getNode(LEFT_PHALANGE_1) node.length = length3 node.width = width3 node.mass = mass3 node = mcfg.getNode(LEFT_PHALANGE_2) node.length = length3_2 node.width = width3_2 node.mass = mass3_2 node = mcfg.getNode(LEFT_CALCANEUS_1) node.length = length4# - width4 node.width = width4 node.mass = mass4 node = mcfg.getNode(LEFT_CALCANEUS_2) node.length = length4# - width4 node.width = width4 node.mass = mass4 #node.offset = (0.0, -0.025, 0.0) node = mcfg.getNode('LeftFoot') node = mcfg.getNode(RIGHT_TALUS_1) node.geom = 'MyFoot3' node = mcfg.getNode(RIGHT_TALUS_2) node.geom = 'MyFoot3' node = mcfg.getNode(LEFT_TALUS_1) node.geom = 'MyFoot3' node = mcfg.getNode(LEFT_TALUS_2) node.geom = 'MyFoot3' node = mcfg.getNode(RIGHT_METATARSAL_1) node.geom = 'MyFoot3' node = mcfg.getNode(RIGHT_METATARSAL_2) node.geom = 'MyFoot3' node = mcfg.getNode(LEFT_METATARSAL_1) node.geom = 'MyFoot3' node = mcfg.getNode(LEFT_METATARSAL_2) node.geom = 'MyFoot3' node = mcfg.getNode(RIGHT_PHALANGE_1) node.geom = 'MyFoot4' node = mcfg.getNode(RIGHT_PHALANGE_2) node.geom = 'MyFoot4' node = mcfg.getNode(LEFT_PHALANGE_1) node.geom = 'MyFoot4' node = mcfg.getNode(LEFT_PHALANGE_2) node.geom = 'MyFoot4' node = mcfg.getNode(RIGHT_CALCANEUS_1) node.geom = 'MyFoot4' node = mcfg.getNode(RIGHT_CALCANEUS_2) node.geom = 'MyFoot4' node = mcfg.getNode(LEFT_CALCANEUS_1) node.geom = 'MyFoot4' node = mcfg.getNode(LEFT_CALCANEUS_2) node.geom = 'MyFoot4' wcfg = ypc.WorldConfig() wcfg.planeHeight = 0. wcfg.useDefaultContactModel = False stepsPerFrame = 120 wcfg.timeStep = (1/30.)/(stepsPerFrame) #stepsPerFrame = 10 #wcfg.timeStep = (1/120.)/(stepsPerFrame) #wcfg.timeStep = (1/1800.) # parameter config = {} config['Kt'] = 200; config['Dt'] = 2*(config['Kt']**.5) # tracking gain config['Kl'] = .10; config['Dl'] = 2*(config['Kl']**.5) # linear balance gain config['Kh'] = 0.1; config['Dh'] = 2*(config['Kh']**.5) # angular balance gain config['Ks'] = 20000; config['Ds'] = 2*(config['Ks']**.5) # penalty force spring gain config['Bt'] = 1. config['Bl'] = 1.#0.5 config['Bh'] = 1. config['weightMap'] = {RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:.3, SPINE1:.2, RIGHT_FOOT:.3, LEFT_FOOT:.3, HIP:.3, RIGHT_UP_LEG:.1, RIGHT_LEG:.2, LEFT_UP_LEG:.1, LEFT_LEG:.2, LEFT_TALUS_1:.1, RIGHT_TALUS_1:.1, LEFT_TALUS_2:.1, RIGHT_TALUS_2:.1, RIGHT_CALCANEUS_1:.2, LEFT_CALCANEUS_1:.2, RIGHT_CALCANEUS_2:.2, LEFT_CALCANEUS_2:.2, LEFT_PHALANGE_1:.1, LEFT_PHALANGE_2:.1, RIGHT_PHALANGE_1:.1, RIGHT_PHALANGE_2:.1} config['weightMap2'] = {RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:.5, SPINE1:.3, RIGHT_FOOT:.7, LEFT_FOOT:.7, HIP:.5, RIGHT_UP_LEG:.7, RIGHT_LEG:.7, LEFT_UP_LEG:.7, LEFT_LEG:.7, LEFT_TALUS_1:.7, RIGHT_TALUS_1:.7, LEFT_TALUS_2:.7, RIGHT_TALUS_2:.7, RIGHT_CALCANEUS_1:.7, LEFT_CALCANEUS_1:.7, RIGHT_CALCANEUS_2:.7, LEFT_CALCANEUS_2:.7, LEFT_PHALANGE_1:.4, LEFT_PHALANGE_2:.4, RIGHT_PHALANGE_1:.4, RIGHT_PHALANGE_2:.4} ''' (1, 'RightUpLeg') (2, 'RightLeg') (3, 'RightFoot') (4, 'Spine') (5, 'Spine1') (6, 'LeftArm') (7, 'LeftForeArm') (8, 'RightArm') (9, 'RightForeArm') (10, 'LeftUpLeg') (11, 'LeftLeg') (12, 'LeftFoot') (13, 'LeftTalus_1') (14, 'LeftTalus_2') (15, 'RightTalus_1') (16, 'RightTalus_2') (17, 'LeftPhalange_1') (18, 'LeftPhalange_2') (19, 'RightPhalange_1') (20, 'RightPhalange_2') (21, 'LeftCalcaneus_1') (22, 'LeftCalcaneus_2') (23, 'RightCalcaneus_1') (24, 'RightCalcaneus_2') ''' config['trackWMap']={10, 5, .1, 20, 10, 5, 2, 10, 5, .1, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.10, 0.01} config['supLink'] = LEFT_FOOT config['supLink2'] = RIGHT_FOOT # config['end'] = HIP config['end'] = SPINE1 config['trunk'] = SPINE config['const'] = HIP config['root'] = HIP config['FootPartNum'] = FOOT_PART_NUM config['FootLPart'] = [LEFT_FOOT, LEFT_CALCANEUS_1, LEFT_CALCANEUS_2, LEFT_TALUS_1, LEFT_TALUS_2, LEFT_PHALANGE_1, LEFT_PHALANGE_2] config['FootRPart'] = [RIGHT_FOOT, RIGHT_CALCANEUS_1, RIGHT_CALCANEUS_2, RIGHT_TALUS_1, RIGHT_TALUS_2, RIGHT_PHALANGE_1, RIGHT_PHALANGE_2] return motion, mcfg, wcfg, stepsPerFrame, config
RIGHT_PHALANGE_2, RIGHT_PHALANGE_3 ] #config['FootLPart'] = [LEFT_FOOT, LEFT_CALCANEUS_1, LEFT_METATARSAL_1, LEFT_METATARSAL_3, LEFT_PHALANGE_1, LEFT_PHALANGE_3] #config['FootRPart'] = [RIGHT_FOOT, RIGHT_CALCANEUS_1, RIGHT_METATARSAL_1, RIGHT_METATARSAL_3, RIGHT_PHALANGE_1, RIGHT_PHALANGE_3] return motion, mcfg, wcfg, stepsPerFrame, config #=============================================================================== # biped config #=============================================================================== # motion, mesh config g_motionDirConfigMap = {} g_motionDirConfigMap['../Data/woody2/Motion/Physics2/'] = \ {'footRot': mm.exp(mm.v3(1,0,0), .05), 'yOffset': .0, 'scale':1.,\ 'rootRot': mm.I_SO3()} g_motionDirConfigMap['../Data/woody2/Motion/Balancing/'] = \ {'footRot': mm.exp(mm.v3(1,-.5,0), -.6), 'yOffset': .0, 'scale':1.,\ 'rootRot': mm.exp(mm.v3(1,0,0), .01)} g_motionDirConfigMap['../Data/woody2/Motion/VideoMotion/'] = \ {'footRot': mm.exp(mm.v3(1,0,0), -.05), 'yOffset': .01, 'scale':2.53999905501,\ 'rootRot': mm.exp(mm.v3(1,0,0), .0)} g_motionDirConfigMap['../Data/woody2/Motion/Samsung/'] = \ {'footRot': mm.exp(mm.v3(1,0,0), -.03), 'yOffset': .0, 'scale':2.53999905501,\ 'rootRot': mm.exp(mm.v3(1,0,0), .03)} #=============================================================================== # # reloadable config #===============================================================================
def main(): np.set_printoptions(precision=4, linewidth=200) # motion, mcfg, wcfg, stepsPerFrame, config = mit.create_vchain_5() motion, mcfg, wcfg, stepsPerFrame, config = mit.create_biped_zygote() # motion, mcfg, wcfg, stepsPerFrame, config = mit.create_jump_biped() vpWorld = cvw.VpWorld(wcfg) sphere_radius = 0.5 vpWorld.add_sphere_bump(sphere_radius, (1.6361, -sphere_radius + 0.08, -0.3209)) vpWorld.add_sphere_bump(sphere_radius, (1.4543, -sphere_radius + 0.08, -0.3301)) # vpWorld.add_sphere_bump(sphere_radius, (1.6361, -sphere_radius + 0.08, -0.2909)) # vpWorld.add_sphere_bump(sphere_radius, (1.4543, -sphere_radius + 0.08, -0.2901)) motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg) controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg) vpWorld.initialize() controlModel.initializeHybridDynamics() # controlToMotionOffset = (1.5, -0.02, 0) controlToMotionOffset = (1.5, 0, 0) controlModel.translateByOffset(controlToMotionOffset) totalDOF = controlModel.getTotalDOF() DOFs = controlModel.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']) selectedBody = motion[0].skeleton.getJointIndex('Spine') constBody = motion[0].skeleton.getJointIndex('RightFoot') # jacobian JsupL = yjc.makeEmptyJacobian(DOFs, 1) dJsupL = JsupL.copy() JsupPreL = JsupL.copy() JsupR = yjc.makeEmptyJacobian(DOFs, 1) dJsupR = JsupR.copy() JsupPreR = JsupR.copy() Jconst = yjc.makeEmptyJacobian(DOFs, 1) dJconst = Jconst.copy() JconstPre = Jconst.copy() Jsys = yjc.makeEmptyJacobian(DOFs, controlModel.getBodyNum()) dJsys = Jsys.copy() JsysPre = Jsys.copy() supLJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, supL)] supRJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, supR)] constJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, constBody)] allLinkJointMasks = yjc.getAllLinkJointMasks(motion[0].skeleton) # momentum matrix linkMasses = controlModel.getBodyMasses() totalMass = controlModel.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.)] # penalty method bodyIDsToCheck = list(range(vpWorld.getBodyNum())) 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(rect=(0, 0, 960+300, 1080+56), 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('world', yr.VpWorldRenderer(vpWorld, (150, 150, 150))) viewer.doc.addRenderer('motionModel', yr.VpModelRenderer(motionModel, (150,150,255), yr.POLYGON_FILL)) viewer.doc.setRendererVisible('motionModel', False) # viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_LINE)) viewer.doc.addRenderer('controlModel', yr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_FILL)) viewer.doc.addRenderer('rd_footCenter', yr.PointsRenderer(rd_footCenter)) viewer.doc.setRendererVisible('rd_footCenter', False) viewer.doc.addRenderer('rd_CM_plane', yr.PointsRenderer(rd_CM_plane, (255,255,0))) viewer.doc.setRendererVisible('rd_CM_plane', False) viewer.doc.addRenderer('rd_CP', yr.PointsRenderer(rd_CP, (0,255,0))) viewer.doc.setRendererVisible('rd_CP', False) viewer.doc.addRenderer('rd_CP_des', yr.PointsRenderer(rd_CP_des, (255,0,255))) viewer.doc.setRendererVisible('rd_CP_des', False) viewer.doc.addRenderer('rd_dL_des_plane', yr.VectorsRenderer(rd_dL_des_plane, rd_CM, (255,255,0))) viewer.doc.setRendererVisible('rd_dL_des_plane', False) viewer.doc.addRenderer('rd_dH_des', yr.VectorsRenderer(rd_dH_des, rd_CM, (0,255,0))) viewer.doc.setRendererVisible('rd_dH_des', False) # 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,255,0))) # viewer.doc.setRendererVisible('rd_CF', False) viewer.doc.addRenderer('extraForce', yr.VectorsRenderer(rd_exf_des, extraForcePos, (0,255,0))) viewer.doc.setRendererVisible('extraForce', False) # viewer.doc.addRenderer('extraForceEnable', yr.VectorsRenderer(rd_exfen_des, extraForcePos, (255,0,0))) viewer.doc.addRenderer('extraForceEnable', yr.WideArrowRenderer(rd_exfen_des, extraForcePos, (255,0,0), lineWidth=.05, fromPoint=False)) # 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 = 100. initKh = 100. initBl = .1 initBh = .13 initSupKt = 17 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., 1., 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.4) viewer.objectInfoWnd.end() # self.sliderFm = Fl_Hor_Nice_Slider(10, 42+offset*6, 250, 10) def getParamVal(paramname): return viewer.objectInfoWnd.getVal(paramname) def getParamVals(paramnames): return (getParamVal(name) for name in paramnames) ################################### # simulate ################################### def simulateCallback(frame): motionModel.update(motion[frame]) global g_initFlag global forceShowTime global JsysPre global JsupPreL global JsupPreR global JsupPre global JconstPre global preFootCenter global maxContactChangeCount global contactChangeCount global contact global contactChangeType # Kt, Kl, Kh, Bl, Bh, kt_sup = viewer.GetParam() Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals(['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt']) Dt = 2*(Kt**.5) Dl = 2*(Kl**.5) Dh = 2*(Kh**.5) dt_sup = 2*(kt_sup**.5) doubleTosingleOffset = 0.15 singleTodoubleOffset = 0.30 # doubleTosingleOffset = 0.09 doubleTosingleVelOffset = 0.0 # tracking th_r = motion.getDOFPositions(frame) th = controlModel.getDOFPositions() dth_r = motion.getDOFVelocities(frame) dth = controlModel.getDOFVelocities() ddth_r = motion.getDOFAccelerations(frame) ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt) ype.flatten(ddth_des, ddth_des_flat) ype.flatten(dth, dth_flat) ################################################# # jacobian ################################################# # caution!! body orientation and joint orientation of foot are totally different!! footOriL = controlModel.getJointOrientationGlobal(supL) footOriR = controlModel.getJointOrientationGlobal(supR) # desire footCenter[1] = 0.041135 # desire footCenter[1] = 0.0197 footCenterL = controlModel.getBodyPositionGlobal(supL) footCenterR = controlModel.getBodyPositionGlobal(supR) footBodyOriL = controlModel.getBodyOrientationGlobal(supL) footBodyOriR = controlModel.getBodyOrientationGlobal(supR) footBodyVelL = controlModel.getBodyVelocityGlobal(supL) footBodyVelR = controlModel.getBodyVelocityGlobal(supR) footBodyAngVelL = controlModel.getBodyAngVelocityGlobal(supL) footBodyAngVelR = controlModel.getBodyAngVelocityGlobal(supR) refFootL = motionModel.getBodyPositionGlobal(supL) refFootR = motionModel.getBodyPositionGlobal(supR) refFootVelL = motionModel.getBodyVelocityGlobal(supL) refFootVelR = motionModel.getBodyVelocityGlobal(supR) refFootAngVelL = motionModel.getBodyAngVelocityGlobal(supL) refFootAngVelR = motionModel.getBodyAngVelocityGlobal(supR) refFootOriL = motionModel.getBodyOrientationGlobal(supL) refFootOriR = motionModel.getBodyOrientationGlobal(supR) refFootJointVelR = motion.getJointVelocityGlobal(supR, frame) refFootJointAngVelR = motion.getJointAngVelocityGlobal(supR, frame) refFootJointR = motion.getJointPositionGlobal(supR, frame) refFootVelR = refFootJointVelR + np.cross(refFootJointAngVelR, (refFootR-refFootJointR)) refFootJointVelL = motion.getJointVelocityGlobal(supL, frame) refFootJointAngVelL = motion.getJointAngVelocityGlobal(supL, frame) refFootJointL = motion.getJointPositionGlobal(supL, frame) refFootVelL = refFootJointVelL + np.cross(refFootJointAngVelL, (refFootL-refFootJointL)) contactR = 1 contactL = 1 if refFootVelR[1] < 0 and refFootVelR[1]/30. + refFootR[1] > singleTodoubleOffset: contactR = 0 if refFootVelL[1] < 0 and refFootVelL[1]/30. + refFootL[1] > singleTodoubleOffset: contactL = 0 if refFootVelR[1] > 0 and refFootVelR[1]/30. + refFootR[1] > doubleTosingleOffset: contactR = 0 if refFootVelL[1] > 0 and refFootVelL[1]/30. + refFootL[1] > doubleTosingleOffset: contactL = 0 # if 32 < frame < 147: # contactR = 0 contMotionOffset = th[0][0] - th_r[0][0] linkPositions = controlModel.getBodyPositionsGlobal() linkVelocities = controlModel.getBodyVelocitiesGlobal() linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal() linkInertias = controlModel.getBodyInertiasGlobal() jointPositions = controlModel.getJointPositionsGlobal() jointAxeses = controlModel.getDOFAxeses() CM = yrp.getCM(linkPositions, linkMasses, totalMass) dCM = yrp.getCM(linkVelocities, linkMasses, totalMass) 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 Jsys, dJsys = controlModel.computeCom_J_dJdq() JsupL = Jsys[6*supL:6*supL+6, :] dJsupL = dJsys[6*supL:6*supL+6] JsupR = Jsys[6*supR:6*supR+6, :] dJsupR = dJsys[6*supR:6*supR+6] # 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: JsysPre = Jsys.copy() JsupPreL = JsupL.copy() JsupPreR = JsupR.copy() JconstPre = Jconst.copy() softConstPoint = footCenterR.copy() # yjc.computeJacobian2(JsysPre, DOFs, jointPositions, jointAxeses, linkPositions, allLinkJointMasks) # yjc.computeJacobian2(JsupPreL, DOFs, jointPositions, jointAxeses, [footCenterL], supLJointMasks) # yjc.computeJacobian2(JsupPreR, DOFs, jointPositions, jointAxeses, [footCenterR], supRJointMasks) # yjc.computeJacobian2(JconstPre, DOFs, jointPositions, jointAxeses, [softConstPoint], constJointMasks) 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 g_initFlag = 1 # calculate footCenter footCenter = footCenterL + (footCenterR - footCenterL)/2.0 # 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() # elif contact == 1 or footCenterL[1] > doubleTosingleOffset/2: if contact == 1: footCenter = footCenterR.copy() 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 # CM_ref_plane[1] += motionModel.getCOM()[1] dL_des_plane = Kl*totalMass*(CM_ref_plane - CM_plane) - Dl*totalMass*dCM_plane # dL_des_plane[1] = 0. # angular momentum CP_ref = footCenter bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.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])/(1/30.) CP_old[0] = CP if CP is not None and dCP is not None: ddCP_des = Kh*(CP_ref - CP) - Dh*(dCP) CP_des = CP + dCP*(1/30.) + .5*ddCP_des*((1/30.)**2) dH_des = np.cross((CP_des - 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 footBodyOriL L_ddq = mm.logSO3(np.dot(footBodyOriL.T, np.dot(refFootOriL, mm.getSO3FromVectors(np.dot(refFootOriL, mm.unitY()), mm.unitY())))) R_ddq = mm.logSO3(np.dot(footBodyOriR.T, np.dot(refFootOriR, mm.getSO3FromVectors(np.dot(refFootOriR, mm.unitY()), mm.unitY())))) L_q = mm.logSO3(footBodyOriL) R_q = mm.logSO3(footBodyOriR) L_ang = np.dot(footBodyOriL, footBodyAngVelL) R_ang = np.dot(footBodyOriR, footBodyAngVelR) L_dq = mm.vel2qd(L_ang, L_q) R_dq = mm.vel2qd(R_ang, R_q) a_oriL = np.dot(footBodyOriL, mm.qdd2accel(L_dq, L_dq, L_q)) a_oriR = np.dot(footBodyOriR, mm.qdd2accel(R_dq, R_dq, R_q)) # body_ddqs = list(map(mm.logSO3, [np.dot(contact_body_ori[i].T, np.dot(ref_body_ori[i], mm.getSO3FromVectors(np.dot(ref_body_ori[i], up_vec_in_each_link[contact_ids[i]]), mm.unitY()))) for i in range(len(contact_body_ori))])) # body_qs = list(map(mm.logSO3, contact_body_ori)) # body_angs = [np.dot(contact_body_ori[i], contact_body_angvel[i]) for i in range(len(contact_body_ori))] # body_dqs = [mm.vel2qd(body_angs[i], body_qs[i]) for i in range(len(body_angs))] # a_oris = [np.dot(contact_body_ori[i], mm.qdd2accel(body_ddqs[i], body_dqs[i], body_qs[i])) for i in range(len(contact_body_ori))] # 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]))) #if contact == 3 and contactChangeCount < maxContactChangeCount/4 and contactChangeCount >=1: #kt_sup = 30 #viewer.objectInfoWnd.labelSupKt.value(kt_sup) #viewer.objectInfoWnd.sliderSupKt.value(initSupKt*10) # a_supL = np.append(kt_sup*(refFootL - footCenterL + contMotionOffset) + dt_sup*(refFootVelL - footBodyVelL), kt_sup*a_oriL+dt_sup*(refFootAngVelL-footBodyAngVelL)) # a_supR = np.append(kt_sup*(refFootR - footCenterR + contMotionOffset) + dt_sup*(refFootVelR - footBodyVelR), kt_sup*a_oriR+dt_sup*(refFootAngVelR-footBodyAngVelR)) a_supL = np.append(kt_sup*(refFootL - footCenterL + contMotionOffset) - dt_sup*footBodyVelL, kt_sup*a_oriL-dt_sup*footBodyAngVelL) a_supL[1] = -kt_sup*footCenterL[1] -dt_sup*footBodyVelL[1] a_supR = np.append(kt_sup*(refFootR - footCenterR + contMotionOffset) - dt_sup*footBodyVelR, kt_sup*a_oriR-dt_sup*footBodyAngVelR) a_supR[1] = -kt_sup*footCenterR[1] -dt_sup*footBodyVelR[1] ##if contact == 2: #if refFootR[1] <doubleTosingleOffset : #Jsup = np.vstack((JsupL, JsupR)) #dJsup = np.vstack((dJsupL, dJsupR)) #a_sup = np.append(a_supL, a_supR) #else: #Jsup = JsupL.copy() #dJsup = dJsupL.copy() #a_sup = a_supL.copy() # 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, dJsys) 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) if dH_des is not None: mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias) mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias) #if contact & 1 and contactChangeCount == 0: if contact & 1: #if refFootR[1] < doubleTosingleOffset: mot.addConstraint2(problem, totalDOF, JsupR, dJsupR, dth_flat, a_supR) if contact & 2: #if refFootL[1] < doubleTosingleOffset: mot.addConstraint2(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) rootPos[0] = controlModel.getBodyPositionGlobal(selectedBody) localPos = [[0, 0, 0]] for i in range(stepsPerFrame): # apply penalty force bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds) # print(contactForces) #bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds) vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces) controlModel.setDOFAccelerations(ddth_sol) controlModel.solveHybridDynamics() 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) # extraForce[0] = viewer.objectInfoWnd.labelFm.value() * mm.normalize2(forceforce) if viewer_GetForceState(): forceShowTime += wcfg.timeStep vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce) vpWorld.step() # 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 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]/400) 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] = controlModel.getBodyPositionGlobal(selectedBody) - 0.1 * np.array([viewer.objectInfoWnd.labelForceX.value(), 0., viewer.objectInfoWnd.labelForceZ.value()]) viewer.setSimulateCallback(simulateCallback) viewer.startTimer(1/30.) viewer.show() Fl.run()
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() #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 = 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.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))] ################################### #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, 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) 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(wcfg.gravity)) else: dH_des = None H = np.dot(P, np.dot(Jsys, dth_flat)) dH_des = -Kh * H[3:] # 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 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']) print(w) #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]] inv_h = 1. / dartModel.world.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 += 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) # 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(): np.set_printoptions(precision=4, linewidth=200) motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped() # motion, mcfg, wcfg, stepsPerFrame, config = mit.create_jump_biped() vpWorld = cvw.VpWorld(wcfg) motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg) controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg) vpWorld.initialize() controlModel.initializeHybridDynamics() #controlToMotionOffset = (1.5, -0.02, 0) controlToMotionOffset = (1.5, 0, 0) controlModel.translateByOffset(controlToMotionOffset) totalDOF = controlModel.getTotalDOF() DOFs = controlModel.getDOFs() foot_dofs = [] left_foot_dofs = [] right_foot_dofs = [] foot_seg_dofs = [] left_foot_seg_dofs = [] right_foot_seg_dofs = [] # for joint_idx in range(motion[0].skeleton.getJointNum()): for joint_idx in range(controlModel.getJointNum()): joint_name = controlModel.index2name(joint_idx) print(joint_idx, joint_name) # joint_name = motion[0].skeleton.getJointName(joint_idx) if 'Foot' in joint_name: foot_dofs_temp = controlModel.getJointDOFIndexes(joint_idx) foot_dofs.extend(foot_dofs_temp) if 'Left' in joint_name: left_foot_dofs.extend(foot_dofs_temp) elif 'Right' in joint_name: right_foot_dofs.extend(foot_dofs_temp) if 'foot' in joint_name: foot_dofs_temp = controlModel.getJointDOFIndexes(joint_idx) foot_seg_dofs.extend(foot_dofs_temp) if 'Left' in joint_name: left_foot_dofs.extend(foot_dofs_temp) elif 'Right' in joint_name: right_foot_dofs.extend(foot_dofs_temp) # 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'] 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') # jacobian JsupL = yjc.makeEmptyJacobian(DOFs, 1) dJsupL = JsupL.copy() JsupPreL = JsupL.copy() JsupR = yjc.makeEmptyJacobian(DOFs, 1) dJsupR = JsupR.copy() JsupPreR = JsupR.copy() Jconst = yjc.makeEmptyJacobian(DOFs, 1) dJconst = Jconst.copy() JconstPre = Jconst.copy() Jsys = yjc.makeEmptyJacobian(DOFs, controlModel.getBodyNum()) dJsys = Jsys.copy() JsysPre = Jsys.copy() supLJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, supL)] supRJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, supR)] constJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, constBody)] allLinkJointMasks = yjc.getAllLinkJointMasks(motion[0].skeleton) # momentum matrix linkMasses = controlModel.getBodyMasses() totalMass = controlModel.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.)] # penalty method bodyIDsToCheck = range(vpWorld.getBodyNum()) # 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', yr.VpModelRenderer(motionModel, (150,150,255), yr.POLYGON_FILL)) # viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_LINE)) control_model_renderer = yr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_FILL) viewer.doc.addRenderer('controlModel', control_model_renderer) control_model_renderer.body_colors[supL] = (255, 0, 0) control_model_renderer.body_colors[supR] = (255, 0, 0) 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,255,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 = 100. initKh = 100. initBl = .1 initBh = .13 initSupKt = 17 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) def getParamVal(paramname): return viewer.objectInfoWnd.getVal(paramname) def getParamVals(paramnames): return (getParamVal(name) for name in paramnames) extendedFootName = ['Foot_foot_0_0', 'Foot_foot_0_1', 'Foot_foot_0_0_0', 'Foot_foot_0_1_0', 'Foot_foot_1_0'] 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} footIdDic = lIDdic.copy() footIdDic.update(rIDdic) lIDlist = [motion[0].skeleton.getJointIndex('Left'+name) for name in extendedFootName] rIDlist = [motion[0].skeleton.getJointIndex('Right'+name) for name in extendedFootName] footIdlist = [] footIdlist.extend(lIDlist) footIdlist.extend(rIDlist) foot_left_idx = motion[0].skeleton.getJointIndex('LeftFoot') foot_right_idx = motion[0].skeleton.getJointIndex('RightFoot') foot_left_idx_temp = motion[0].skeleton.getJointIndex('LeftFoot_foot_1_0') foot_right_idx_temp = motion[0].skeleton.getJointIndex('RightFoot_foot_1_0') def get_jacobianbase_and_masks(skeleton, DOFs, joint_idx): J = yjc.makeEmptyJacobian(DOFs, 1) joint_masks = [yjc.getLinkJointMask(skeleton, joint_idx)] return J, joint_masks ################################### # simulate ################################### def simulateCallback(frame): motionModel.update(motion[frame]) global g_initFlag global forceShowTime global JsysPre global JsupPreL global JsupPreR global JsupPre global JconstPre global preFootCenter global maxContactChangeCount global contactChangeCount global contact global contactChangeType # Kt, Kl, Kh, Bl, Bh, kt_sup = viewer.GetParam() Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals(['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt']) Dt = 2*(Kt**.5) Dl = 2*(Kl**.5) Dh = 2*(Kh**.5) dt_sup = 2*(kt_sup**.5) doubleTosingleOffset = 0.15 singleTodoubleOffset = 0.30 #doubleTosingleOffset = 0.09 doubleTosingleVelOffset = 0.0 # tracking th_r = motion.getDOFPositions(frame) th = controlModel.getDOFPositions() dth_r = motion.getDOFVelocities(frame) dth = controlModel.getDOFVelocities() ddth_r = motion.getDOFAccelerations(frame) ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt) ype.flatten(ddth_des, ddth_des_flat) ype.flatten(dth, dth_flat) ################################################# # jacobian ################################################# # caution!! body orientation and joint orientation of foot are totally different!! footOriL = controlModel.getJointOrientationGlobal(supL) footOriR = controlModel.getJointOrientationGlobal(supR) # desire footCenter[1] = 0.041135 # desire footCenter[1] = 0.0197 footCenterL = controlModel.getBodyPositionGlobal(supL) footCenterR = controlModel.getBodyPositionGlobal(supR) footBodyOriL = controlModel.getBodyOrientationGlobal(supL) footBodyOriR = controlModel.getBodyOrientationGlobal(supR) footBodyVelL = controlModel.getBodyVelocityGlobal(supL) footBodyVelR = controlModel.getBodyVelocityGlobal(supR) footBodyAngVelL = controlModel.getBodyAngVelocityGlobal(supL) footBodyAngVelR = controlModel.getBodyAngVelocityGlobal(supR) refFootL = motionModel.getBodyPositionGlobal(supL) refFootR = motionModel.getBodyPositionGlobal(supR) refFootVelL = motionModel.getBodyVelocityGlobal(supL) refFootVelR = motionModel.getBodyVelocityGlobal(supR) refFootAngVelL = motionModel.getBodyAngVelocityGlobal(supL) refFootAngVelR = motionModel.getBodyAngVelocityGlobal(supR) refFootJointVelR = motion.getJointVelocityGlobal(supR, frame) refFootJointAngVelR = motion.getJointAngVelocityGlobal(supR, frame) refFootJointR = motion.getJointPositionGlobal(supR, frame) refFootVelR = refFootJointVelR + np.cross(refFootJointAngVelR, (refFootR-refFootJointR)) refFootJointVelL = motion.getJointVelocityGlobal(supL, frame) refFootJointAngVelL = motion.getJointAngVelocityGlobal(supL, frame) refFootJointL = motion.getJointPositionGlobal(supL, frame) refFootVelL = refFootJointVelL + np.cross(refFootJointAngVelL, (refFootL-refFootJointL)) contactR = 1 contactL = 1 if refFootVelR[1] < 0 and refFootVelR[1]/30. + refFootR[1] > singleTodoubleOffset: contactR = 0 if refFootVelL[1] < 0 and refFootVelL[1]/30. + refFootL[1] > singleTodoubleOffset: contactL = 0 if refFootVelR[1] > 0 and refFootVelR[1]/30. + refFootR[1] > doubleTosingleOffset: contactR = 0 if refFootVelL[1] > 0 and refFootVelL[1]/30. + refFootL[1] > doubleTosingleOffset: contactL = 0 # contactR = 0 contMotionOffset = th[0][0] - th_r[0][0] linkPositions = controlModel.getBodyPositionsGlobal() linkVelocities = controlModel.getBodyVelocitiesGlobal() linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal() linkInertias = controlModel.getBodyInertiasGlobal() jointPositions = controlModel.getJointPositionsGlobal() jointAxeses = controlModel.getDOFAxeses() CM = yrp.getCM(linkPositions, linkMasses, totalMass) dCM = yrp.getCM(linkVelocities, linkMasses, totalMass) 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: JsysPre = Jsys.copy() # JsupPreL = JsupL.copy() # JsupPreR = JsupR.copy() JconstPre = Jconst.copy() softConstPoint = footCenterR.copy() yjc.computeJacobian2(JsysPre, DOFs, jointPositions, jointAxeses, linkPositions, allLinkJointMasks) # yjc.computeJacobian2(JsupPreL, DOFs, jointPositions, jointAxeses, [footCenterL], supLJointMasks) # yjc.computeJacobian2(JsupPreR, DOFs, jointPositions, jointAxeses, [footCenterR], supRJointMasks) yjc.computeJacobian2(JconstPre, DOFs, jointPositions, jointAxeses, [softConstPoint], constJointMasks) 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 g_initFlag = 1 # calculate jacobian yjc.computeJacobian2(Jsys, DOFs, jointPositions, jointAxeses, linkPositions, allLinkJointMasks) # dJsys = (Jsys - JsysPre)/(1/30.) # JsysPre = Jsys.copy() yjc.computeJacobianDerivative2(dJsys, DOFs, jointPositions, jointAxeses, linkAngVelocities, linkPositions, allLinkJointMasks) JsupL, supLJointMasks = get_jacobianbase_and_masks(motion[0].skeleton, DOFs, supL) yjc.computeJacobian2(JsupL, DOFs, jointPositions, jointAxeses, [footCenterL], supLJointMasks) # dJsupL = (JsupL - JsupPreL)/(1/30.) # JsupPreL = JsupL.copy() yjc.computeJacobianDerivative2(dJsupL, DOFs, jointPositions, jointAxeses, linkAngVelocities, [footCenterL], supLJointMasks) JsupR, supRJointMasks = get_jacobianbase_and_masks(motion[0].skeleton, DOFs, supR) yjc.computeJacobian2(JsupR, DOFs, jointPositions, jointAxeses, [footCenterR], supRJointMasks) # dJsupR = (JsupR - JsupPreR)/(1/30.) # JsupPreR = JsupR.copy() yjc.computeJacobianDerivative2(dJsupR, DOFs, jointPositions, jointAxeses, linkAngVelocities, [footCenterR], supRJointMasks) # calculate footCenter footCenter = footCenterL + (footCenterR - footCenterL)/2.0 #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() #elif contact == 1 or footCenterL[1] > doubleTosingleOffset/2: if contact == 1: footCenter = footCenterR.copy() 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! # to do 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. # angular momentum CP_ref = footCenter bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.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])/(1/30.) CP_old[0] = CP if CP is not None and dCP is not None: ddCP_des = Kh*(CP_ref - CP) - Dh*(dCP) CP_des = CP + dCP*(1/30.) + .5*ddCP_des*((1/30.)**2) dH_des = np.cross((CP_des - CM), (dL_des_plane + totalMass*mm.s2v(wcfg.gravity))) if contactChangeCount > 0: # and contactChangeType == 'DtoS': dH_des *= (maxContactChangeCount - contactChangeCount)/(maxContactChangeCount) else: dH_des = None # set up equality constraint # left_foot_up_vec, right_foot_up_vec = hfi.get_foot_up_vector(motion[frame], footIdDic, None) left_foot_up_vec, right_foot_up_vec = hfi.get_foot_up_vector(controlModel, footIdDic, None) # print(left_foot_up_vec, right_foot_up_vec) # 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]))) a_oriL = mm.logSO3(mm.getSO3FromVectors(left_foot_up_vec, np.array([0,1,0]))) a_oriR = mm.logSO3(mm.getSO3FromVectors(right_foot_up_vec, np.array([0,1,0]))) a_supL = np.append(kt_sup*(refFootL - footCenterL + contMotionOffset) + dt_sup*(refFootVelL - footBodyVelL), kt_sup*a_oriL+dt_sup*(refFootAngVelL-footBodyAngVelL)) a_supR = np.append(kt_sup*(refFootR - footCenterR + contMotionOffset) + 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) if dH_des is not None: mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias) 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() ddth_sol_flat = np.asarray(r['x']) # ddth_sol_flat[foot_seg_dofs] = np.array(ddth_des_flat)[foot_seg_dofs] print(foot_seg_dofs) ype.nested(ddth_sol_flat, ddth_sol) rootPos[0] = controlModel.getBodyPositionGlobal(selectedBody) localPos = [[0, 0, 0]] for i in range(stepsPerFrame): # apply penalty force bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds) # bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds) vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces) controlModel.setDOFAccelerations(ddth_sol) controlModel.solveHybridDynamics() 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 vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce) vpWorld.step() # 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 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]/400) 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] = controlModel.getBodyPositionGlobal(selectedBody) viewer.setSimulateCallback(simulateCallback) viewer.startTimer(1/30.) viewer.show() Fl.run()
def create_jump_biped(): # motion #motionName = 'wd2_n_kick.bvh' #motionName = 'wd2_jump.bvh' #motionName = 'wd2_stand.bvh' motionName = 'woddy2_jump_ori.bvh' motion = yf.readBvhFile(motionName, .01) #yme.removeJoint(motion, 'Head', False) yme.removeJoint(motion, 'HEad', False) yme.removeJoint(motion, 'RightShoulder', False) yme.removeJoint(motion, 'LeftShoulder1', False) yme.removeJoint(motion, 'RightToes_Effector', False) yme.removeJoint(motion, 'LeftToes_Effector', False) yme.removeJoint(motion, 'RightHand_Effector', False) yme.removeJoint(motion, 'LeftHand_Effector', False) yme.offsetJointLocal(motion, 'RightArm', (.03, -.05, 0), False) yme.offsetJointLocal(motion, 'LeftArm', (-.03, -.05, 0), False) yme.rotateJointLocal(motion, 'Hips', mm.exp(mm.v3(1, 0, 0), .01), False) #yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1,-0.0,.3), -.5), False) #yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1,0.0,-.3), -.5), False) yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1, -0.5, 0), -.6), False) yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1, 0.5, 0), -.6), False) yme.updateGlobalT(motion) motion.translateByOffset((0, -0.05, 0)) #motion = motion[130:] #motion = motion[120:] motion = motion[67:162] #motion = motion[-249:-248] motion[0:0] = [motion[0]] * 200 motion.extend([motion[-1]] * 500) # world, model mcfg = ypc.ModelConfig() mcfg.defaultDensity = 1000. mcfg.defaultBoneRatio = .9 for name in massMap: node = mcfg.addNode(name) node.mass = massMap[name] node = mcfg.getNode('Hips') node.length = .2 node.width = .25 node = mcfg.getNode('Spine1') node.length = .2 node.offset = (0, 0, 0.1) node = mcfg.getNode('Spine') node.width = .25 node = mcfg.getNode('RightFoot') node.length = .25 node.width = .15 node.mass = 2. #node.width = .2 #node.mass = 1. node = mcfg.getNode('LeftFoot') node.length = .25 node.width = .15 node.mass = 2. wcfg = ypc.WorldConfig() wcfg.planeHeight = 0. wcfg.useDefaultContactModel = False stepsPerFrame = 60 #stepsPerFrame = 30 wcfg.timeStep = (1 / 30.) / (stepsPerFrame) #wcfg.timeStep = (1/1000.) # parameter config = {} ''' config['Kt'] = 200; config['Dt'] = 2*(config['Kt']**.5) # tracking gain config['Kl'] = 2.5; config['Dl'] = 2*(config['Kl']**.5) # linear balance gain config['Kh'] = 1; config['Dh'] = 2*(config['Kh']**.5) # angular balance gain config['Ks'] = 20000; config['Ds'] = 2*(config['Ks']**.5) # penalty force spring gain config['Bt'] = 1. config['Bl'] = 2.5 config['Bh'] = 1. ''' config['Kt'] = 200 config['Dt'] = 2 * (config['Kt']**.5) # tracking gain config['Kl'] = .10 config['Dl'] = 2 * (config['Kl']**.5) # linear balance gain config['Kh'] = 0.1 config['Dh'] = 2 * (config['Kh']**.5) # angular balance gain config['Ks'] = 20000 config['Ds'] = 2 * (config['Ks']**.5) # penalty force spring gain config['Bt'] = 1. config['Bl'] = 1. #0.5 config['Bh'] = 1. config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ 'Spine':1., 'Spine1':1., 'RightFoot':.5, 'LeftFoot':.5, 'Hips':1.,\ 'RightUpLeg':.5, 'RightLeg':.5, 'LeftUpLeg':.5, 'LeftLeg':.5} #config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ #'Spine':1., 'Spine1':1., 'RightFoot':1.0, 'LeftFoot':1.0, 'Hips':1.5,\ #'RightUpLeg':2., 'RightLeg':2., 'LeftUpLeg':2., 'LeftLeg':2.} #config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ #'Spine':.6, 'Spine1':.6, 'RightFoot':.2, 'LeftFoot':.2, 'Hips':1.,\ #'RightUpLeg':.1, 'RightLeg':.3, 'LeftUpLeg':.1, 'LeftLeg':.3} #success!! ''' config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ 'Spine':.5, 'Spine1':.5, 'RightFoot':1., 'LeftFoot':1., 'Hips':0.5,\ 'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1., 'LeftLeg':1.} ''' #config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ #'Spine':1.5, 'LeftFoot':1., 'Hips':1.5,\ #'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1.5, 'LeftLeg':1.5} config['supLink'] = 'LeftFoot' config['supLink1'] = 'LeftFoot' config['supLink2'] = 'RightFoot' #config['end'] = 'Hips' config['end'] = 'Spine1' return motion, mcfg, wcfg, stepsPerFrame, config
def create_any_motion(motion): #motion yme.removeJoint(motion, 'Head', False) #yme.removeJoint(motion, 'HEad', False) yme.removeJoint(motion, 'RightShoulder', False) yme.removeJoint(motion, 'LeftShoulder1', False) yme.removeJoint(motion, 'RightToes_Effector', False) yme.removeJoint(motion, 'LeftToes_Effector', False) yme.removeJoint(motion, 'RightHand_Effector', False) yme.removeJoint(motion, 'LeftHand_Effector', False) yme.offsetJointLocal(motion, 'RightArm', (.03, -.05, 0), False) yme.offsetJointLocal(motion, 'LeftArm', (-.03, -.05, 0), False) yme.rotateJointLocal(motion, 'Hips', mm.exp(mm.v3(1, 0, 0), .01), False) #yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1,-0.0,.3), -.5), False) #yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1,0.0,-.3), -.5), False) yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1, -0.5, 0), -.45), False) yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1, 0.5, 0), -.45), False) yme.updateGlobalT(motion) # world, model mcfg = ypc.ModelConfig() mcfg.defaultDensity = 1000. mcfg.defaultBoneRatio = .9 for name in massMap: node = mcfg.addNode(name) node.mass = massMap[name] node = mcfg.getNode('Hips') node.length = .2 node.width = .25 node = mcfg.getNode('Spine1') node.length = .2 node.offset = (0, 0, 0.1) node = mcfg.getNode('Spine') node.width = .22 node = mcfg.getNode('RightFoot') node.length = .25 #node.length = .2 #node.width = .12 #node.width = .2 node.width = .15 node.mass = 2. #node.mass = 1. node = mcfg.getNode('LeftFoot') node.length = .25 #node.length = .2 #node.width = .12 node.width = .15 #node.width = .2 node.mass = 2. wcfg = ypc.WorldConfig() wcfg.planeHeight = 0. wcfg.useDefaultContactModel = False stepsPerFrame = 60 wcfg.timeStep = (1 / 30.) / (stepsPerFrame) # parameter config = {} config['Kt'] = 200 config['Dt'] = 2 * (config['Kt']**.5) # tracking gain config['Kl'] = .10 config['Dl'] = 2 * (config['Kl']**.5) # linear balance gain config['Kh'] = 0.1 config['Dh'] = 2 * (config['Kh']**.5) # angular balance gain config['Ks'] = 20000 config['Ds'] = 2 * (config['Ks']**.5) # penalty force spring gain config['Bt'] = 1. config['Bl'] = 1. #0.5 config['Bh'] = 1. config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ 'Spine':.6, 'Spine1':.6, 'RightFoot':.2, 'LeftFoot':.2, 'Hips':0.5,\ 'RightUpLeg':.1, 'RightLeg':.3, 'LeftUpLeg':.1, 'LeftLeg':.3} return mcfg, wcfg, stepsPerFrame, config
def create_biped(): # motion #motionName = 'wd2_n_kick.bvh' #motionName = 'wd2_jump_ori.bvh' if 1: #motionName = 'wd2_stand.bvh' motionName = 'woddy2_jump0.bvh' motion = yf.readBvhFile(motionName, .01) motion.translateByOffset((0., -0.023, 0.)) yme.removeJoint(motion, 'HEad', False) yme.removeJoint(motion, 'RightShoulder', False) yme.removeJoint(motion, 'LeftShoulder1', False) yme.removeJoint(motion, 'RightToes_Effector', False) yme.removeJoint(motion, 'LeftToes_Effector', False) yme.removeJoint(motion, 'RightHand_Effector', False) yme.removeJoint(motion, 'LeftHand_Effector', False) yme.offsetJointLocal(motion, 'RightArm', (.03,-.05,0), False) yme.offsetJointLocal(motion, 'LeftArm', (-.03,-.05,0), False) yme.rotateJointLocal(motion, 'Hips', mm.exp(mm.v3(1,0,0), .01), False) yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(2.5,-0.0,.3), -.5), False) yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(2.5,0.0,-.3), -.5), False) #yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1,-0.0,.2), -.5), False) #yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1,0.0,-.2), -.5), False) yme.rotateJointLocal(motion, 'LeftUpLeg', mm.exp(mm.v3(0.0,.0,1.), .08), False) yme.rotateJointLocal(motion, 'LeftLeg', mm.exp(mm.v3(0.0,1.0,0.), -.2), False) #yme.rotateJointLocal(motion, 'RightLeg', mm.exp(mm.v3(1.0,0.0,0.), -.1), False) yme.updateGlobalT(motion) # yf.writeBvhFile('wd2_jump0.bvh', motion) else : motionName = 'ww13_41.bvh' motion = yf.readBvhFile(motionName, 0.056444) yme.removeJoint(motion, 'LHipJoint', False) yme.removeJoint(motion, 'RHipJoint', False) yme.removeJoint(motion, 'Neck', False) yme.removeJoint(motion, 'Neck1', False) yme.removeJoint(motion, 'Head', False) yme.removeJoint(motion, 'RightShoulder', False) yme.removeJoint(motion, 'LeftShoulder', False) yme.removeJoint(motion, 'RightToeBase_Effector', False) yme.removeJoint(motion, 'LeftToeBase_Effector', False) yme.removeJoint(motion, 'LeftHand', False) yme.removeJoint(motion, 'LeftFingerBase', False) yme.removeJoint(motion, 'LeftHandIndex1_Effector', False) yme.removeJoint(motion, 'LThumb', False) yme.removeJoint(motion, 'RightHand', False) yme.removeJoint(motion, 'RightFingerBase', False) yme.removeJoint(motion, 'RightHandIndex1_Effector', False) yme.removeJoint(motion, 'RThumb', False) yme.removeJoint(motion, 'LowerBack', False) yme.offsetJointLocal(motion, 'RightArm', (-.03,-.05,0), False) yme.offsetJointLocal(motion, 'LeftArm', (.03,-.05,0), False) #yme.rotateJointLocal(motion, 'Hips', mm.exp(mm.v3(1,0,0), .01), False) yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(-1.5,0,1), .4), False) yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1.5,0,1), -.4), False) yme.updateGlobalT(motion) #motion = motion[50:-1] motion = motion[240:-1] #motion = motion[40:-58] #motion = motion[56:-248] #motion = motion[-249:-248] #motion = motion[62:65] #motion = motion[216:217] #motion = motion[515:555] ################ motion = motion[515:555] # motion = motion[164:280] #motion = motion[96:97] motion[0:0] = [motion[0]]*100 motion.extend([motion[-1]]*5000) # world, model mcfg = ypc.ModelConfig() mcfg.defaultDensity = 1000. mcfg.defaultBoneRatio = .9 for name in massMap: node = mcfg.addNode(name) node.mass = massMap[name] node = mcfg.getNode('Hips') node.length = .2 node.width = .25 node = mcfg.getNode('Spine1') node.length = .2 node.offset = (0,0,0.1) node = mcfg.getNode('Spine') node.width = .22 #node.length = .2 #### node = mcfg.getNode('RightFoot') node.length = .25 node.width = .2 node.mass = 4. node = mcfg.getNode('LeftFoot') node.length = .25 node.width = .2 node.mass = 4. wcfg = ypc.WorldConfig() wcfg.planeHeight = 0. wcfg.useDefaultContactModel = False stepsPerFrame = 30 wcfg.timeStep = (1/30.)/(stepsPerFrame) #stepsPerFrame = 10 #wcfg.timeStep = (1/120.)/(stepsPerFrame) #wcfg.timeStep = (1/1800.) # parameter config = {} ''' config['Kt'] = 200; config['Dt'] = 2*(config['Kt']**.5) # tracking gain config['Kl'] = 2.5; config['Dl'] = 2*(config['Kl']**.5) # linear balance gain config['Kh'] = 1; config['Dh'] = 2*(config['Kh']**.5) # angular balance gain config['Ks'] = 20000; config['Ds'] = 2*(config['Ks']**.5) # penalty force spring gain config['Bt'] = 1. config['Bl'] = 2.5 config['Bh'] = 1. ''' config['Kt'] = 200; config['Dt'] = 2*(config['Kt']**.5) # tracking gain config['Kl'] = .10; config['Dl'] = 2*(config['Kl']**.5) # linear balance gain config['Kh'] = 0.1; config['Dh'] = 2*(config['Kh']**.5) # angular balance gain config['Ks'] = 20000; config['Ds'] = 2*(config['Ks']**.5) # penalty force spring gain config['Bt'] = 1. config['Bl'] = 1.#0.5 config['Bh'] = 1. config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ 'Spine':.3, 'Spine1':.3, 'RightFoot':.3, 'LeftFoot':.3, 'Hips':.5,\ 'RightUpLeg':.1, 'RightLeg':.3, 'LeftUpLeg':.1, 'LeftLeg':.3} config['IKweightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ 'Spine':.3, 'Spine1':.3, 'RightFoot':.3, 'LeftFoot':.3, 'Hips':.5,\ 'RightUpLeg':.1, 'RightLeg':.3, 'LeftUpLeg':.1, 'LeftLeg':.3} ''' config['IKweightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ 'Spine':0.5, 'Spine1':0.5, 'RightFoot':1.2, 'LeftFoot':1.2, 'Hips':1.2,\ 'RightUpLeg':.9, 'RightLeg':.9, 'LeftUpLeg':.9, 'LeftLeg':.9} ''' config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ 'Spine':1.5, 'LeftFoot':1., 'Hips':1.5,\ 'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1.5, 'LeftLeg':1.5} config['supLink'] = 'LeftFoot' config['supLink2'] = 'RightFoot' #config['end'] = 'Hips' config['end'] = 'Spine1' return motion, mcfg, wcfg, stepsPerFrame, config
def create_biped(): # motion #motionName = 'wd2_n_kick.bvh' if MOTION == STAND: motionName = 'wd2_stand.bvh' elif MOTION == STAND2: motionName = 'ww13_41_V001.bvh' elif MOTION == FORWARD_JUMP: motionName = 'woddy2_jump0.bvh' elif MOTION == TAEKWONDO: motionName = './MotionFile/wd2_098_V001.bvh' elif MOTION == TAEKWONDO2: motionName = './MotionFile/wd2_098_V001.bvh' elif MOTION == KICK: motionName = 'wd2_n_kick.bvh' elif MOTION == WALK: motionName = 'wd2_WalkForwardNormal00.bvh' elif MOTION == TIPTOE: motionName = './MotionFile/cmu/15_07_15_07.bvh' #motionName = 'ww13_41_V001.bvh' scale = 0.01 if MOTION == WALK: scale = 1.0 elif MOTION == TIPTOE: scale = 0.01 motion = yf.readBvhFile(motionName, scale) if MOTION != WALK: yme.removeJoint(motion, HEAD, False) yme.removeJoint(motion, RIGHT_SHOULDER, False) yme.removeJoint(motion, LEFT_SHOULDER, False) if FOOT_PART_NUM == 1 and MOTION != WALK: yme.removeJoint(motion, RIGHT_TOES_END, False) yme.removeJoint(motion, LEFT_TOES_END, False) elif (FOOT_PART_NUM == 5 or FOOT_PART_NUM == 6) and MOTION != WALK: yme.removeJoint(motion, RIGHT_TOES, False) yme.removeJoint(motion, RIGHT_TOES_END, False) yme.removeJoint(motion, LEFT_TOES, False) yme.removeJoint(motion, LEFT_TOES_END, False) if MOTION != WALK: yme.removeJoint(motion, RIGHT_HAND_END, False) yme.removeJoint(motion, LEFT_HAND_END, False) yme.offsetJointLocal(motion, RIGHT_ARM, (.03, -.05, 0), False) yme.offsetJointLocal(motion, LEFT_ARM, (-.03, -.05, 0), False) yme.rotateJointLocal(motion, HIP, mm.exp(mm.v3(1, 0, 0), .01), False) yme.rotateJointLocal(motion, HIP, mm.exp(mm.v3(0, 0, 1), -.01), False) if FOOT_PART_NUM == 1: yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(2.5, -0.0, .3), -.5), False) yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(2.5, 0.0, -.3), -.5), False) if MOTION == WALK: yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(1., -0.0, .0), .4), False) yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1., 0.0, -.0), .4), False) #yme.rotateJointLocal(motion, SPINE1, mm.exp(mm.v3(1.,0.0,0.0), -.8), False) if MOTION == FORWARD_JUMP: yme.rotateJointLocal(motion, LEFT_UP_LEG, mm.exp(mm.v3(0.0, .0, 1.), .08), False) yme.rotateJointLocal(motion, LEFT_LEG, mm.exp(mm.v3(0.0, 1.0, 0.), -.2), False) if FOOT_PART_NUM == 6: yme.addJoint(motion, LEFT_FOOT, LEFT_METATARSAL_1, (-0.045, -0.06, 0.03)) yme.addJoint(motion, LEFT_FOOT, LEFT_METATARSAL_3, (0.045, -0.06, 0.03)) #-0.0037 yme.addJoint(motion, LEFT_METATARSAL_1, LEFT_PHALANGE_1, (0.0, 0.0, 0.0715)) yme.addJoint(motion, LEFT_METATARSAL_3, LEFT_PHALANGE_3, (0.0, 0.0, 0.0715)) yme.addJoint(motion, LEFT_PHALANGE_1, 'LEFT_PHALANGE_Dummy1', (0.0, 0.0, 0.1)) yme.addJoint(motion, LEFT_PHALANGE_3, 'LEFT_PHALANGE_Dummy2', (0.0, 0.0, 0.1)) yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_1, (0.0, -0.06, -0.02)) yme.addJoint(motion, LEFT_CALCANEUS_1, 'LEFT_CALCANEUS_Dummy1', (0.0, 0.0, -0.1)) yme.addJoint(motion, RIGHT_FOOT, RIGHT_METATARSAL_1, (0.045, -0.06, 0.03)) yme.addJoint(motion, RIGHT_FOOT, RIGHT_METATARSAL_3, (-0.045, -0.06 - 0.0062, 0.03 + 0.0035)) yme.addJoint(motion, RIGHT_METATARSAL_1, RIGHT_PHALANGE_1, (0.0, 0.0, 0.0715)) yme.addJoint(motion, RIGHT_METATARSAL_3, RIGHT_PHALANGE_3, (0.0, 0.0, 0.0715)) yme.addJoint(motion, RIGHT_PHALANGE_1, 'RIGHT_PHALANGE_Dummy1', (0.0, 0.0, 0.1)) yme.addJoint(motion, RIGHT_PHALANGE_3, 'RIGHT_PHALANGE_Dummy2', (0.0, 0.0, 0.1)) yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_1, (0.0, -0.06, -0.02)) yme.addJoint(motion, RIGHT_CALCANEUS_1, 'RIGHT_CALCANEUS_Dummy1', (0.0, 0.0, -0.1)) yme.rotateJointLocal(motion, RIGHT_CALCANEUS_1, mm.exp(mm.v3(.0, 0.0, 1.0), 3.14), False) yme.rotateJointLocal(motion, LEFT_CALCANEUS_1, mm.exp(mm.v3(.0, 0.0, 1.0), 3.14), False) ''' yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1.0,0.0,0.0), 3.14*0.3), False) yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(1.0,0.0, 0.0), 3.14*0.3), False) yme.rotateJointLocal(motion, RIGHT_PHALANGE_1, mm.exp(mm.v3(1.0,0.0,0.0), -3.14*0.3), False) yme.rotateJointLocal(motion, RIGHT_PHALANGE_3, mm.exp(mm.v3(1.0,0.0, 0.0), -3.14*0.3), False) yme.rotateJointLocal(motion, LEFT_PHALANGE_1, mm.exp(mm.v3(1.0,0.0,0.0), -3.14*0.3), False) yme.rotateJointLocal(motion, LEFT_PHALANGE_3, mm.exp(mm.v3(1.0,0.0, 0.0), -3.14*0.3), False) ''' yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(1.0, 0.0, 0.0), 0.2012), False) yme.rotateJointLocal(motion, LEFT_METATARSAL_1, mm.exp(mm.v3(1.0, 0.0, 0.0), -0.2172), False) yme.rotateJointLocal(motion, LEFT_METATARSAL_3, mm.exp(mm.v3(1.0, 0.0, 0.0), -0.2172), False) yme.rotateJointLocal(motion, LEFT_CALCANEUS_1, mm.exp(mm.v3(1.0, 0.0, 0.0), 0.2172), False) yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1.0, 0.0, 0.0), 0.2283), False) yme.rotateJointLocal(motion, RIGHT_METATARSAL_1, mm.exp(mm.v3(1.0, 0.0, 0.0), -0.2171), False) yme.rotateJointLocal(motion, RIGHT_METATARSAL_3, mm.exp(mm.v3(1.0, 0.0, 0.0), -0.2171), False) yme.rotateJointLocal(motion, RIGHT_CALCANEUS_1, mm.exp(mm.v3(1.0, 0.0, 0.0), 0.2171), False) yme.rotateJointLocal(motion, RIGHT_METATARSAL_1, mm.exp(mm.v3(0.0, 0.0, 1.0), 0.0747), False) yme.rotateJointLocal(motion, RIGHT_METATARSAL_3, mm.exp(mm.v3(0.0, 0.0, 1.0), 0.0747), False) ''' yme.rotateJointLocal(motion, RIGHT_METATARSAL_1, mm.exp(mm.v3(1.0,0.0,0.0), -0.1), False) yme.rotateJointLocal(motion, RIGHT_METATARSAL_3, mm.exp(mm.v3(1.0,0.0,0.0), -0.1), False) yme.rotateJointLocal(motion, RIGHT_METATARSAL_1, mm.exp(mm.v3(0.0,0.0,1.0), 0.06), False) yme.rotateJointLocal(motion, RIGHT_METATARSAL_3, mm.exp(mm.v3(0.0,0.0,1.0), 0.06), False) yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(1.0,0.0,0.0), -0.05), False) ''' #yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(0.0,0.0,1.0), 0.0656), False) #yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1.0,0.0,0.0), 0.0871), False) #yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(0.0,0.0,1.0), 0.0087), False) #yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1.0,0.0,0.0), 0.0933), False) #yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(0.0,0.0,1.0), 0.0746), False) #yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1.0,0.0,0.0), -6.3773e-03), False) #yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(1.0,0.0, 0.0), math.pi*0.25), False) #yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1.0,0.0, 0.0), math.pi*0.25), False) #yme.rotateJointLocal(motion, LEFT_PHALANGE_1, mm.exp(mm.v3(1.0,0.0,0.0), -math.pi*0.25), False) #yme.rotateJointLocal(motion, RIGHT_PHALANGE_1, mm.exp(mm.v3(1.0,0.0,0.0), -math.pi*0.25), False) #yme.rotateJointLocal(motion, LEFT_PHALANGE_3, mm.exp(mm.v3(1.0,0.0,0.0), -math.pi*0.25), False) #yme.rotateJointLocal(motion, RIGHT_PHALANGE_3, mm.exp(mm.v3(1.0,0.0,0.0), -math.pi*0.25), False) #yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(1.0,0.0, 0.0), -math.pi*0.16), False) #yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1.0,0.0, 0.0), -math.pi*0.16), False) #yme.rotateJointLocal(motion, LEFT_CALCANEUS_1, mm.exp(mm.v3(1.0,0.0,0.0), -math.pi*0.16), False) #yme.rotateJointLocal(motion, RIGHT_CALCANEUS_1, mm.exp(mm.v3(1.0,0.0,0.0), -math.pi*0.16), False) #yme.rotateJointLocal(motion, LEFT_METATARSAL_3, mm.exp(mm.v3(1.0,0.0,0.0), -math.pi*0.5), False) yme.updateGlobalT(motion) ################ if MOTION == FORWARD_JUMP: motion = motion[515:555] elif MOTION == TAEKWONDO: ## Taekwondo base-step motion = motion[0:31] #motion = motion[564:600] elif MOTION == TAEKWONDO2: ## Taekwondo base-step #motion = motion[0:31+40] ## Taekwondo turning-kick motion = motion[108:-1] #motion = motion[108:109] elif MOTION == KICK: #motion = motion[141:-1] #motion = motion[100:-1] #motion = motion[58:-1] motion = motion[82:-1] #motion = motion[0:-1] elif MOTION == STAND2: motion = motion[1:-1] elif MOTION == TIPTOE: #motion = motion[183:440] #motion = motion[350:410] motion = motion[350:550] motion[0:0] = [motion[0]] * 40 motion.extend([motion[-1]] * 5000) # world, model mcfg = ypc.ModelConfig() mcfg.defaultDensity = 1000. mcfg.defaultBoneRatio = .9 for name in massMap: node = mcfg.addNode(name) node.mass = massMap[name] node = mcfg.getNode(HIP) node.length = .2 node.width = .25 node = mcfg.getNode(SPINE1) node.length = .2 node.offset = (0, 0, 0.1) node = mcfg.getNode(SPINE) node.width = .22 #node.length = .2 #### if FOOT_PART_NUM == 1: length1 = .35 width1 = .2 mass1 = 4.3 elif FOOT_PART_NUM == 6: mass0 = .4 width0 = 0.028 length0 = 0.1 #Metatarsal1 length1 = .15 width1 = width0 * 3 mass1 = mass0 * 1.8 #Metatarsal3 length3 = .11 width3 = width0 * 3 mass3 = mass0 * 1.86 #Calcaneus1 length4 = .08 width4 = 0.15 * 1 mass4 = mass0 * 1.2 * 2. #Phalange1 length5 = .08 width5 = width1 mass5 = mass0 * 1.355 #Phalange3 length7 = length5 width7 = width5 mass7 = mass5 #Talus length8 = .13 width8 = width0 * 3 mass8 = mass0 * 2. if FOOT_PART_NUM == 6: node = mcfg.getNode(RIGHT_FOOT) node.length = length8 node.width = width8 node.mass = mass8 node = mcfg.getNode(RIGHT_METATARSAL_1) node.length = length1 node.width = width1 node.mass = mass1 node = mcfg.getNode(RIGHT_METATARSAL_3) node.length = length3 node.width = width3 node.mass = mass3 node = mcfg.getNode(RIGHT_PHALANGE_1) node.length = length5 node.width = width5 node.mass = mass5 node.offset = (0.0, 0.0, 0.03) node = mcfg.getNode(RIGHT_PHALANGE_3) node.length = length7 node.width = width7 node.mass = mass7 node.offset = (0.0, 0.0, 0.01) node = mcfg.getNode(RIGHT_CALCANEUS_1) node.length = length4 node.width = width4 node.mass = mass4 node = mcfg.getNode(LEFT_FOOT) node.length = length8 node.width = width8 node.mass = mass8 node = mcfg.getNode(LEFT_METATARSAL_1) node.length = length1 node.width = width1 node.mass = mass1 node = mcfg.getNode(LEFT_METATARSAL_3) node.length = length3 node.width = width3 node.mass = mass3 node = mcfg.getNode(LEFT_PHALANGE_1) node.length = length5 node.width = width5 node.mass = mass5 node.offset = (0.0, 0.0, 0.03) node = mcfg.getNode(LEFT_PHALANGE_3) node.length = length7 node.width = width7 node.mass = mass7 node.offset = (0.0, 0.0, 0.01) node = mcfg.getNode(LEFT_CALCANEUS_1) node.length = length4 node.width = width4 node.mass = mass4 if FOOT_PART_NUM < 5: node = mcfg.getNode(RIGHT_FOOT) node.length = length1 node.width = width1 node.mass = mass1 node = mcfg.getNode(LEFT_FOOT) node.length = length1 node.width = width1 node.mass = mass1 wcfg = ypc.WorldConfig() wcfg.planeHeight = 0. wcfg.useDefaultContactModel = False stepsPerFrame = 30 wcfg.timeStep = (1 / 30.) / (stepsPerFrame) #stepsPerFrame = 10 #wcfg.timeStep = (1/120.)/(stepsPerFrame) #wcfg.timeStep = (1/1800.) # parameter config = {} config['Kt'] = 200 config['Dt'] = 2 * (config['Kt']**.5) # tracking gain config['Kl'] = .10 config['Dl'] = 2 * (config['Kl']**.5) # linear balance gain config['Kh'] = 0.1 config['Dh'] = 2 * (config['Kh']**.5) # angular balance gain config['Ks'] = 20000 config['Ds'] = 2 * (config['Ks']**.5) # penalty force spring gain config['Bt'] = 1. config['Bl'] = 1. #0.5 config['Bh'] = 1. if FOOT_PART_NUM == 1: config['weightMap'] = { RIGHT_ARM: .2, RIGHT_FORE_ARM: .2, LEFT_ARM: .2, LEFT_FORE_ARM: .2, SPINE: .3, SPINE1: .3, RIGHT_FOOT: .3, LEFT_FOOT: .3, HIP: .5, RIGHT_UP_LEG: .1, RIGHT_LEG: .3, LEFT_UP_LEG: .1, LEFT_LEG: .3 } config['weightMap2'] = { RIGHT_ARM: .2, RIGHT_FORE_ARM: .2, LEFT_ARM: .2, LEFT_FORE_ARM: .2, SPINE: 1., SPINE1: .3, RIGHT_FOOT: 1., LEFT_FOOT: 1., HIP: 1., RIGHT_UP_LEG: 1., RIGHT_LEG: 1., LEFT_UP_LEG: 1., LEFT_LEG: 1. } elif FOOT_PART_NUM == 6: config['weightMap2'] = { RIGHT_ARM: .2, RIGHT_FORE_ARM: .2, LEFT_ARM: .2, LEFT_FORE_ARM: .2, SPINE: .5, SPINE1: .3, RIGHT_FOOT: .7, LEFT_FOOT: .7, HIP: .5, RIGHT_UP_LEG: .7, RIGHT_LEG: .7, LEFT_UP_LEG: .7, LEFT_LEG: .7, LEFT_METATARSAL_1: .7, RIGHT_METATARSAL_1: .7, LEFT_METATARSAL_3: .7, RIGHT_METATARSAL_3: .7, RIGHT_CALCANEUS_1: .7, LEFT_CALCANEUS_1: .7, LEFT_PHALANGE_1: .4, LEFT_PHALANGE_3: .4, RIGHT_PHALANGE_1: .4, RIGHT_PHALANGE_3: .4 } config['weightMap'] = { RIGHT_ARM: .2, RIGHT_FORE_ARM: .2, LEFT_ARM: .2, LEFT_FORE_ARM: .2, SPINE: .3, SPINE1: .2, RIGHT_FOOT: .3, LEFT_FOOT: .3, HIP: .3, RIGHT_UP_LEG: .1, RIGHT_LEG: .2, LEFT_UP_LEG: .1, LEFT_LEG: .2, LEFT_METATARSAL_1: .1, RIGHT_METATARSAL_1: .1, LEFT_METATARSAL_3: .1, RIGHT_METATARSAL_3: .1, RIGHT_CALCANEUS_1: .2, LEFT_CALCANEUS_1: .2, LEFT_PHALANGE_1: .1, LEFT_PHALANGE_3: .1, RIGHT_PHALANGE_1: .1, RIGHT_PHALANGE_3: .1 } ''' config['weightMap2']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:.3, SPINE1:.3, RIGHT_FOOT:.3, LEFT_FOOT:.3, HIP:.5, RIGHT_UP_LEG:.1, RIGHT_LEG:.3, LEFT_UP_LEG:.1, LEFT_LEG:.3, LEFT_METATARSAL_3:.2, RIGHT_METATARSAL_3:.2, RIGHT_CALCANEUS:1.2, LEFT_CALCANEUS:1.2, LEFT_PHALANGE_1:.2, LEFT_PHALANGE_3:.1, RIGHT_PHALANGE_1:.2, RIGHT_PHALANGE_3:.2} config['weightMap2']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:.8, SPINE1:.3, RIGHT_FOOT:1., LEFT_FOOT:1., HIP:1., RIGHT_UP_LEG:1., RIGHT_LEG:1., LEFT_UP_LEG:1., LEFT_LEG:1., LEFT_METATARSAL_3:1., RIGHT_METATARSAL_3:1., RIGHT_CALCANEUS:.3, LEFT_CALCANEUS:.3, LEFT_PHALANGE_1:.3, LEFT_PHALANGE_3:.3, RIGHT_PHALANGE_1:.3, RIGHT_PHALANGE_3:.3} ''' config['supLink'] = LEFT_FOOT config['supLink2'] = RIGHT_FOOT #config['end'] = HIP config['end'] = SPINE1 config['const'] = HIP config['root'] = HIP config['FootPartNum'] = FOOT_PART_NUM if FOOT_PART_NUM == 6: config['FootLPart'] = [ LEFT_FOOT, LEFT_METATARSAL_1, LEFT_METATARSAL_3, LEFT_PHALANGE_1, LEFT_PHALANGE_3, LEFT_CALCANEUS_1 ] config['FootRPart'] = [ RIGHT_FOOT, RIGHT_METATARSAL_1, RIGHT_METATARSAL_3, RIGHT_PHALANGE_1, RIGHT_PHALANGE_3, RIGHT_CALCANEUS_1 ] else: config['FootLPart'] = [ LEFT_FOOT, LEFT_TOES, LEFT_TARSUS, LEFT_TOES_SIDE_R, LEFT_FOOT_SIDE_L, LEFT_FOOT_SIDE_R ] config['FootRPart'] = [ RIGHT_FOOT, RIGHT_TOES, RIGHT_TARSUS, RIGHT_TOES_SIDE_R, RIGHT_FOOT_SIDE_L, RIGHT_FOOT_SIDE_R ] return motion, mcfg, wcfg, stepsPerFrame, config