def sample_target(self): angle = 2. * pi * random() radius = 5. * random() self.goal_in_world_frame = self.skel.body(0).to_world() + radius * ( cos(angle) * mm.unitX() - sin(angle) * mm.unitZ()) self.goal_in_world_frame[1] = 0.
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
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 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)) 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 # 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)) ])) 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, kt_sup, kt_sup]) # 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)) ] # for i in range(len(a_sups)): # a_sups[i][1] = -kt_sup * contact_body_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()) 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) 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 create_biped(motionName='wd2_n_kick.bvh', SEGMENT_FOOT=True, SEGMENT_FOOT_MAG=.03, SEGMENT_FOOT_RAD=None): """ :param motionName: motion file name :param SEGMENT_FOOT: whether segment foot is :param SEGMENT_FOOT_MAG: :return: """ # :rtype: ym.JointMotion, ypc.ModelConfig, ypc.WorldConfig, int, dict[str, float|dict[str, float]], float if SEGMENT_FOOT_RAD is None: SEGMENT_FOOT_RAD = SEGMENT_FOOT_MAG * .5 SEGMENT_FOOT_SEPARATE = False SEGMENT_FOOT_OUTSIDE_JOINT_FIRST = True SEGMENT_FOOT_ARC = True SEGMENT_BETWEEN_SPACE = 1.2 SEGMENT_METATARSAL_LEN = 2.5 SEGMENT_THIRD_PHA_LEN = 1.8 SEGMENT_FOURTH_PHA_RATIO = 5. / 6. SEGMENT_HEEL_LEN = 1.2 # motion # motionName = 'wd2_n_kick.bvh' # motionName = 'wd2_tiptoe.bvh' # motionName = 'wd2_n_kick_zygote.bvh' # motionName = 'wd2_jump.bvh' # motionName = 'wd2_stand.bvh' bvh = yf.readBvhFileAsBvh(motionName) bvh.set_scale(.01) if SEGMENT_FOOT: # partBvhFilePath = '../PyCommon/modules/samples/simpleJump_long_test2.bvh' current_path = os.path.dirname(os.path.abspath(__file__)) partBvhFilePath = current_path + '/../../PyCommon/modules/samples/' if SEGMENT_FOOT_SEPARATE: partBvhFilePath = partBvhFilePath + 'simpleJump_long_test5.bvh' elif SEGMENT_FOOT_OUTSIDE_JOINT_FIRST: # partBvhFilePath = partBvhFilePath + 'simpleJump_long_test3.bvh' # partBvhFilePath = partBvhFilePath + 'foot_model_01.bvh' partBvhFilePath = partBvhFilePath + 'foot_model_01.bvh' else: partBvhFilePath = partBvhFilePath + 'simpleJump_long_test4.bvh' partBvh = yf.readBvhFileAsBvh(partBvhFilePath) partSkeleton = partBvh.toJointSkeleton(1., False) SEGMENT_BETWEEN_SPACE = partSkeleton.getOffset( partSkeleton.getElementIndex('foot_0_1'))[0] SEGMENT_METATARSAL_LEN = partSkeleton.getOffset( partSkeleton.getElementIndex('foot_0_1_0'))[2] SEGMENT_THIRD_PHA_LEN = partSkeleton.getOffset( partSkeleton.getElementIndex('foot_0_0_0_Effector'))[2] SEGMENT_HEEL_LEN = abs( partSkeleton.getOffset( partSkeleton.getElementIndex('foot_1_0_Effector'))[2]) bvh.replaceJointFromBvh('RightFoot', partBvh, SEGMENT_FOOT_MAG) partBvh = yf.readBvhFileAsBvh(partBvhFilePath) partBvh.mirror('YZ') bvh.replaceJointFromBvh('LeftFoot', partBvh, SEGMENT_FOOT_MAG) motion = bvh.toJointMotion(1., False) # type: ym.JointMotion # motion.translateByOffset((0., 0.15, 0.)) # motion.translateByOffset((0., -0.12, 0.)) # motion.rotateByOffset(mm.rotZ(math.pi*1./18.)) # motion = yf.readBvhFile(motionName, .01) # 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.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1,-0.0,.3), -.1), False) # yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1,0.0,-.3), -.1), False) # yme.removeJoint(motion, 'RightFoot_foot_1_1') # yme.removeJoint(motion, 'RightFoot_foot_1_2') # yme.removeJoint(motion, 'LeftFoot_foot_1_1') # yme.removeJoint(motion, 'LeftFoot_foot_1_2') if motionName == 'wd2_n_kick.bvh' or motionName == 'wd2_n_kick_zygote.bvh': yme.rotateJointLocal(motion, 'Hips', mm.exp(mm.v3(1, 0, 0), .01), False) yme.updateGlobalT(motion) motion.translateByOffset((0, 0.04, 0)) for i in range(2000): motion.data.insert(0, copy.deepcopy(motion[0])) motion.extend([motion[-1]] * 300) elif motionName == 'wd2_tiptoe.bvh' or motionName == 'wd2_tiptoe_zygote.bvh': yme.rotateJointLocal(motion, 'Hips', mm.exp(mm.v3(1, 0, 0), .01), False) yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1., 0., 0.), -.1), False) yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1., 0., 0.), -.1), False) yme.updateGlobalT(motion) motion.translateByOffset((0, 0.06, 0)) # if motionName == 'wd2_tiptoe.bvh': # motion.translateByOffset((0, 0.06, 0)) # else: # motion.translateByOffset((0, -0.03, 0)) del motion[:270] for i in range(2000): motion.data.insert(0, copy.deepcopy(motion[0])) # world, model mcfg = ypc.ModelConfig() mcfg.defaultDensity = 1000. mcfg.defaultBoneRatio = .9 for name in massMap: node = mcfg.addNode(name) node.mass = massMap[name] wcfg = ypc.WorldConfig() wcfg.planeHeight = 0. wcfg.useDefaultContactModel = False stepsPerFrame = 60 # stepsPerFrame = 30 frame_rate = 30 wcfg.timeStep = 1. / (frame_rate * stepsPerFrame) # wcfg.timeStep = (1/30.)/(stepsPerFrame) # wcfg.timeStep = (1/1000.) # width : x axis on body frame # height: y axis on body frame # length: z axis on body frame node = mcfg.getNode('Hips') node.length = 4. / 27. node.width = .25 # node.height = .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 = .15 node.width = .2 node.mass = 2. node = mcfg.getNode('LeftFoot') node.length = .25 #node.length = .2 #node.width = .15 node.width = .2 node.mass = 2. def capsulize(node_name): node_capsule = mcfg.getNode(node_name) node_capsule.geom = 'MyFoot4' node_capsule.width = 0.01 node_capsule.density = 200. # node.addGeom('MyFoot4', [np.array([0.]*3), mm.exp([0., math.pi/4., 0.])], ypc.CapsuleMaterial(1000., .02, .2)) # node.addGeom('MyFoot4', [np.array([0.]*3), mm.exp([0., math.pi/4., 0.])], ypc.CapsuleMaterial(1000., .02, .1)) # node.addGeom('MyFoot4', [np.array([0.]*3), mm.exp([0., 0., 0.])], ypc.CapsuleMaterial(1000., .01, -1)) # node.addGeom('MyFoot4', None, ypc.CapsuleMaterial(1000., .02, .1)) # capsulize('RightFoot') # capsulize('LeftFoot') if SEGMENT_FOOT: node = mcfg.getNode('RightFoot') node.density = 200. node.geom = 'MyFoot5' node.width = 0.01 node.jointType = 'B' node = mcfg.getNode('LeftFoot') node.density = 200. node.geom = 'MyFoot5' node.width = 0.01 node.jointType = 'B' # bird foot # capsulize('RightFoot_foot_0_0') # capsulize('RightFoot_foot_0_1') # capsulize('RightFoot_foot_1_0') # capsulize('RightFoot_foot_1_1') # capsulize('RightFoot_foot_2_0') # capsulize('RightFoot_foot_2_1') # capsulize('LeftFoot_foot_0_0') # capsulize('LeftFoot_foot_0_1') # capsulize('LeftFoot_foot_1_0') # capsulize('LeftFoot_foot_1_1') # capsulize('LeftFoot_foot_2_0') # capsulize('LeftFoot_foot_2_1') # human foot if SEGMENT_FOOT: footJointType = 'B' capsulDensity = 400. if SEGMENT_FOOT_SEPARATE: # RightFoot_foot_0_0 : outside metatarsals capsulize('RightFoot_foot_0_0') node = mcfg.getNode('RightFoot_foot_0_0') # node.addGeom('MyFoot3', [SEGMENT_FOOT_MAG*np.array([0., 0., 2.5*0.25]), mm.exp([0., 0., 0.])], node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([0., 0., 0.]), mm.exp([0., 0., 0.]) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([0. - 1.2, 0., 0.]), mm.exp([0., 0., 0.]) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD)) node.jointType = footJointType # RightFoot_foot_0_0_0 : outside phalanges capsulize('RightFoot_foot_0_0_0') node = mcfg.getNode('RightFoot_foot_0_0_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [ SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # RightFoot_foot_0_1 : inside metatarsals capsulize('RightFoot_foot_0_1') node = mcfg.getNode('RightFoot_foot_0_1') node.addGeom( 'MyFoot3', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # RightFoot_foot_0_1_0 : inside phalanges capsulize('RightFoot_foot_0_1_0') node = mcfg.getNode('RightFoot_foot_0_1_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # RightFoot_foot_1_0 : center heel capsulize('RightFoot_foot_1_0') node = mcfg.getNode('RightFoot_foot_1_0') node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([-.6, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([+.6, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD)) node.jointType = footJointType capsulize('LeftFoot_foot_0_0') node = mcfg.getNode('LeftFoot_foot_0_0') node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([0., 0., 0.]), mm.exp([0., 0., 0.]) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([0. + 1.2, 0., 0.]), mm.exp([0., 0., 0.]) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD)) node.jointType = footJointType capsulize('LeftFoot_foot_0_0_0') node = mcfg.getNode('LeftFoot_foot_0_0_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType capsulize('LeftFoot_foot_0_1') node = mcfg.getNode('LeftFoot_foot_0_1') node.addGeom( 'MyFoot3', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType capsulize('LeftFoot_foot_0_1_0') node = mcfg.getNode('LeftFoot_foot_0_1_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [ SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType capsulize('LeftFoot_foot_1_0') node = mcfg.getNode('LeftFoot_foot_1_0') node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([-.6, 0., .0]), mm.exp([0.] * 3)], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([+.6, 0., .0]), mm.exp([0.] * 3)], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD)) node.jointType = footJointType elif SEGMENT_FOOT_OUTSIDE_JOINT_FIRST and not SEGMENT_FOOT_ARC: # RightFoot_foot_0_0 : outside metatarsals capsulize('RightFoot_foot_0_0') node = mcfg.getNode('RightFoot_foot_0_0') node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([-0.3, 0., 2.5 * 0.25]), mm.exp([0., -math.atan2(1.2, 2.5), 0.]) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([-0.3 - 1.2, 0., 2.5 * 0.25]), mm.exp([0., -math.atan2(1.2, 2.5), 0.]) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD)) # node.jointType = footJointType node.jointType = 'B' # RightFoot_foot_0_0_0 : outside phalanges capsulize('RightFoot_foot_0_0_0') node = mcfg.getNode('RightFoot_foot_0_0_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [ SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # node.jointType = 'R' # node.jointAxes = ['X'] # RightFoot_foot_0_1 : inside metatarsals capsulize('RightFoot_foot_0_1') node = mcfg.getNode('RightFoot_foot_0_1') node.addGeom( 'MyFoot3', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # node.jointType = 'R' # node.jointAxes = ['Z'] # RightFoot_foot_0_1_0 : inside phalanges capsulize('RightFoot_foot_0_1_0') node = mcfg.getNode('RightFoot_foot_0_1_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # node.jointType = 'R' # node.jointAxes = ['X'] # RightFoot_foot_1_0 : center heel capsulize('RightFoot_foot_1_0') node = mcfg.getNode('RightFoot_foot_1_0') node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([-.6, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([+.6, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD)) # node.jointType = footJointType node.jointType = 'B' # left foot # outside metatarsals capsulize('LeftFoot_foot_0_0') node = mcfg.getNode('LeftFoot_foot_0_0') node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([0.3, 0., 2.5 * 0.25]), mm.exp([0., math.atan2(1.2, 2.5), 0.]) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([0.3 + 1.2, 0., 2.5 * 0.25]), mm.exp([0., math.atan2(1.2, 2.5), 0.]) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD)) # node.jointType = footJointType node.jointType = 'B' capsulize('LeftFoot_foot_0_0_0') node = mcfg.getNode('LeftFoot_foot_0_0_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # node.jointType = 'R' # node.jointAxes = ['X'] capsulize('LeftFoot_foot_0_1') node = mcfg.getNode('LeftFoot_foot_0_1') node.addGeom( 'MyFoot3', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # node.jointType = 'R' # node.jointAxes = ['Z'] capsulize('LeftFoot_foot_0_1_0') node = mcfg.getNode('LeftFoot_foot_0_1_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [ SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # node.jointType = 'R' # node.jointAxes = ['X'] capsulize('LeftFoot_foot_1_0') node = mcfg.getNode('LeftFoot_foot_1_0') node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([-.6, 0., .0]), mm.exp([0.] * 3)], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([+.6, 0., .0]), mm.exp([0.] * 3)], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD)) # node.jointType = footJointType node.jointType = 'B' elif SEGMENT_FOOT_OUTSIDE_JOINT_FIRST and SEGMENT_FOOT_ARC: FIRST_METATARSAL_ANGLE = mm.deg2Rad(30.) SECOND_METATARSAL_ANGLE = mm.deg2Rad(20.) THIRD_METATARSAL_ANGLE = mm.deg2Rad(15.) # RightFoot_foot_0_0 : outside metatarsals capsulize('RightFoot_foot_0_0') node = mcfg.getNode('RightFoot_foot_0_0') node.bone_dir_child = 'RightFoot_foot_0_0_0' # third node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([ 0., .5 * SEGMENT_METATARSAL_LEN * math.tan(THIRD_METATARSAL_ANGLE), 0. ]), mm.exp(THIRD_METATARSAL_ANGLE * mm.unitX()) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * SEGMENT_METATARSAL_LEN / math.cos(THIRD_METATARSAL_ANGLE) + 2. * SEGMENT_FOOT_RAD)) # fourth node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([-SEGMENT_BETWEEN_SPACE, 0., 0.]), mm.exp([0., 0., 0.]) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) # node.jointType = footJointType node.jointType = 'B' capsulize('LeftFoot_foot_0_0') node = mcfg.getNode('LeftFoot_foot_0_0') node.bone_dir_child = 'LeftFoot_foot_0_0_0' node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([ 0., .5 * SEGMENT_METATARSAL_LEN * math.tan(THIRD_METATARSAL_ANGLE), 0. ]), mm.exp(THIRD_METATARSAL_ANGLE * mm.unitX()) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * SEGMENT_METATARSAL_LEN / math.cos(THIRD_METATARSAL_ANGLE) + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([SEGMENT_BETWEEN_SPACE, 0., 0.]), mm.exp([0., 0., 0.]) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) # node.jointType = footJointType node.jointType = 'B' # RightFoot_foot_0_0_0 : outside phalanges SEGMENT_FOURTH_PHA_OFFSET = .5 * SEGMENT_THIRD_PHA_LEN * ( 1. - SEGMENT_FOURTH_PHA_RATIO) capsulize('RightFoot_foot_0_0_0') node = mcfg.getNode('RightFoot_foot_0_0_0') # third node.addGeom( 'MyFoot4', [SEGMENT_FOOT_MAG * np.array([0., 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) # fourth node.addGeom( 'MyFoot4', [ SEGMENT_FOOT_MAG * np.array([ -SEGMENT_BETWEEN_SPACE, 0., -SEGMENT_FOURTH_PHA_OFFSET ]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * SEGMENT_THIRD_PHA_LEN * SEGMENT_FOURTH_PHA_RATIO + 2. * SEGMENT_FOOT_RAD)) node.jointType = footJointType # node.jointType = 'R' # node.jointAxes = ['X'] capsulize('LeftFoot_foot_0_0_0') node = mcfg.getNode('LeftFoot_foot_0_0_0') node.addGeom( 'MyFoot4', [SEGMENT_FOOT_MAG * np.array([0., 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [ SEGMENT_FOOT_MAG * np.array([ SEGMENT_BETWEEN_SPACE, 0., -SEGMENT_FOURTH_PHA_OFFSET ]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * SEGMENT_THIRD_PHA_LEN * SEGMENT_FOURTH_PHA_RATIO + 2. * SEGMENT_FOOT_RAD)) node.jointType = footJointType # node.jointType = 'R' # node.jointAxes = ['X'] # RightFoot_foot_0_1 : inside metatarsals capsulize('RightFoot_foot_0_1') node = mcfg.getNode('RightFoot_foot_0_1') # second node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([ 0., .5 * SEGMENT_METATARSAL_LEN * math.tan(SECOND_METATARSAL_ANGLE), 0. ]), mm.exp(SECOND_METATARSAL_ANGLE * mm.unitX()) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * SEGMENT_METATARSAL_LEN / math.cos(SECOND_METATARSAL_ANGLE) + 2. * SEGMENT_FOOT_RAD)) # first node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([ SEGMENT_BETWEEN_SPACE, .5 * SEGMENT_METATARSAL_LEN * math.tan(FIRST_METATARSAL_ANGLE), 0. ]), mm.exp(FIRST_METATARSAL_ANGLE * mm.unitX()) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * SEGMENT_METATARSAL_LEN / math.cos(FIRST_METATARSAL_ANGLE) + 2. * SEGMENT_FOOT_RAD)) node.jointType = footJointType # node.jointType = 'R' # node.jointAxes = ['Z'] capsulize('LeftFoot_foot_0_1') node = mcfg.getNode('LeftFoot_foot_0_1') node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([ 0., .5 * SEGMENT_METATARSAL_LEN * math.tan(SECOND_METATARSAL_ANGLE), 0. ]), mm.exp(SECOND_METATARSAL_ANGLE * mm.unitX()) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * SEGMENT_METATARSAL_LEN / math.cos(SECOND_METATARSAL_ANGLE) + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([ -SEGMENT_BETWEEN_SPACE, .5 * SEGMENT_METATARSAL_LEN * math.tan(FIRST_METATARSAL_ANGLE), 0. ]), mm.exp(FIRST_METATARSAL_ANGLE * mm.unitX()) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * SEGMENT_METATARSAL_LEN / math.cos(FIRST_METATARSAL_ANGLE) + 2. * SEGMENT_FOOT_RAD)) node.jointType = footJointType # node.jointType = 'R' # node.jointAxes = ['Z'] # RightFoot_foot_0_1_0 : inside phalanges capsulize('RightFoot_foot_0_1_0') node = mcfg.getNode('RightFoot_foot_0_1_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [ SEGMENT_FOOT_MAG * np.array([SEGMENT_BETWEEN_SPACE, 0., 0.]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # node.jointType = 'R' # node.jointAxes = ['X'] capsulize('LeftFoot_foot_0_1_0') node = mcfg.getNode('LeftFoot_foot_0_1_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [ SEGMENT_FOOT_MAG * np.array([-SEGMENT_BETWEEN_SPACE, 0., 0.]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # node.jointType = 'R' # node.jointAxes = ['X'] # RightFoot_foot_1_0 : center heel capsulize('RightFoot_foot_1_0') node = mcfg.getNode('RightFoot_foot_1_0') node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([-SEGMENT_BETWEEN_SPACE / 2., 0., 0.]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * SEGMENT_HEEL_LEN + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([+SEGMENT_BETWEEN_SPACE / 2., 0., 0.]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * SEGMENT_HEEL_LEN + 2. * SEGMENT_FOOT_RAD)) # node.jointType = footJointType node.jointType = 'B' capsulize('LeftFoot_foot_1_0') node = mcfg.getNode('LeftFoot_foot_1_0') node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([-SEGMENT_BETWEEN_SPACE / 2., 0., .0]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * SEGMENT_HEEL_LEN + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([+SEGMENT_BETWEEN_SPACE / 2., 0., .0]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * SEGMENT_HEEL_LEN + 2. * SEGMENT_FOOT_RAD)) # node.jointType = footJointType node.jointType = 'B' else: # SEGMENT_FOOT_INSIDE_FIRST # TODO: # adjust transformation of geometries # RightFoot_foot_0_1 : inside metatarsals capsulize('RightFoot_foot_0_1') node = mcfg.getNode('RightFoot_foot_0_1') node.addGeom( 'MyFoot3', [np.array([0.] * 3), mm.exp([0., math.atan2(1.2, 2.5), 0.])], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]), mm.exp([0., math.atan2(1.2, 2.5), 0.]) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # RightFoot_foot_0_1_0 : inside phalanges capsulize('RightFoot_foot_0_1_0') node = mcfg.getNode('RightFoot_foot_0_1_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # RightFoot_foot_0_0 : outside metatarsals capsulize('RightFoot_foot_0_0') node = mcfg.getNode('RightFoot_foot_0_0') node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([-0.3, 0., 2.5 * 0.25]), mm.exp([0., -math.atan2(1.2, 2.5), 0.]) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([-0.3 - 1.2, 0., 2.5 * 0.25]), mm.exp([0., -math.atan2(1.2, 2.5), 0.]) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD)) node.jointType = footJointType # RightFoot_foot_0_0_0 : outside phalanges capsulize('RightFoot_foot_0_0_0') node = mcfg.getNode('RightFoot_foot_0_0_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [ SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType # RightFoot_foot_1_0 : center heel capsulize('RightFoot_foot_1_0') node = mcfg.getNode('RightFoot_foot_1_0') node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([-.6, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([+.6, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD)) node.jointType = footJointType capsulize('LeftFoot_foot_0_1') node = mcfg.getNode('LeftFoot_foot_0_1') node.addGeom( 'MyFoot3', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType capsulize('LeftFoot_foot_0_1_0') node = mcfg.getNode('LeftFoot_foot_0_1_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [ SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]), mm.exp([0.] * 3) ], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType capsulize('LeftFoot_foot_0_0') node = mcfg.getNode('LeftFoot_foot_0_0') node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([0.3, 0., 2.5 * 0.25]), mm.exp([0., math.atan2(1.2, 2.5), 0.]) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [ SEGMENT_FOOT_MAG * np.array([0.3 + 1.2, 0., 2.5 * 0.25]), mm.exp([0., math.atan2(1.2, 2.5), 0.]) ], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD)) node.jointType = footJointType capsulize('LeftFoot_foot_0_0_0') node = mcfg.getNode('LeftFoot_foot_0_0_0') node.addGeom( 'MyFoot4', [np.array([0.] * 3), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.addGeom( 'MyFoot4', [SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]), mm.exp([0.] * 3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1)) node.jointType = footJointType capsulize('LeftFoot_foot_1_0') node = mcfg.getNode('LeftFoot_foot_1_0') node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([-.6, 0., .0]), mm.exp([0.] * 3)], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD)) node.addGeom( 'MyFoot3', [SEGMENT_FOOT_MAG * np.array([+.6, 0., .0]), mm.exp([0.] * 3)], ypc.CapsuleMaterial( capsulDensity, SEGMENT_FOOT_RAD, SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD)) node.jointType = footJointType # 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'] = 15000 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.5,\ #'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1., 'LeftLeg':1.} #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': 0.5, 'RightUpLeg': .1, 'RightLeg': .3, 'LeftUpLeg': .1, 'LeftLeg': .3 } if SEGMENT_FOOT: segfoot_weight = 10. # segfoot_weight = .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, 'RightFoot_foot_0_0': segfoot_weight, 'RightFoot_foot_0_1': segfoot_weight, 'RightFoot_foot_1_0': segfoot_weight, 'RightFoot_foot_1_1': segfoot_weight, 'RightFoot_foot_1_2': segfoot_weight, 'RightFoot_foot_0_0_0': segfoot_weight, 'RightFoot_foot_0_1_0': segfoot_weight, 'LeftFoot_foot_0_0': segfoot_weight, 'LeftFoot_foot_0_1': segfoot_weight, 'LeftFoot_foot_1_0': segfoot_weight, 'LeftFoot_foot_1_1': segfoot_weight, 'LeftFoot_foot_1_2': segfoot_weight, 'LeftFoot_foot_0_0_0': segfoot_weight, 'LeftFoot_foot_0_1_0': segfoot_weight } #config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ #'Spine':.6, 'Spine1':.6, 'RightFoot':.2, 'LeftFoot':1., 'Hips':0.5,\ #'RightUpLeg':.1, 'RightLeg':.3, 'LeftUpLeg':.5, 'LeftLeg':1.5} #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, frame_rate
def simulateCallback(frame): global COLOR_ON # 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') 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_foot_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_foot_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_foot_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_foot_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)) motionModel.update(motion[frame]) motionModel.translateByOffset( np.array([ getParamVal('com X offset'), getParamVal('com Y offset'), getParamVal('com Z offset') ])) controlModel.update(motion[frame]) # controlModel_ik.set_q(controlModel.get_q()) # controlModel_ik.set_q(controlModel.get_q()) controlModel_ik.update(motion[frame]) controlModel_ik.translateByOffset( np.array([ -getParamVal('com X offset') * 2., getParamVal('com Y offset'), getParamVal('com Z offset') ])) 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: motion_model_renderer.body_colors[foot_seg_id] = (255, 240, 255) for contact_id in contact_ids: motion_model_renderer.body_colors[contact_id] = (255, 0, 0) pallete2 = list() pallete2.append((244, 198, 61)) pallete2.append((4, 105, 113)) pallete2.append((234, 219, 196)) pallete2.append((216, 1, 6)) pallete2.append((230, 230, 230)) pallete = pallete2 color = dict() color['RightFoot'] = pallete[0] color['RightFoot_foot_1_0'] = pallete[4] color['RightFoot_foot_0_0'] = pallete[1] color['RightFoot_foot_0_0_0'] = pallete[2] color['RightFoot_foot_0_1_0'] = pallete[3] color['LeftFoot'] = pallete[0] color['LeftFoot_foot_1_0'] = pallete[4] color['LeftFoot_foot_0_0'] = pallete[1] color['LeftFoot_foot_0_0_0'] = pallete[2] color['LeftFoot_foot_0_1_0'] = pallete[3] if COLOR_ON: for color_key in color.keys(): motion_model_renderer.body_colors[ idDic[color_key]] = color[color_key] rd_CM[0] = CM rd_CM_plane[0] = CM.copy() rd_CM_plane[0][1] = 0. 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[:] # render contact_ids # render skeleton if SKELETON_ON: Ts = dict() Ts['pelvis'] = controlModel_ik.getJointTransform(idDic['Hips']) Ts['thigh_R'] = controlModel_ik.getJointTransform( idDic['RightUpLeg']) Ts['shin_R'] = controlModel_ik.getJointTransform(idDic['RightLeg']) Ts['foot_R'] = controlModel_ik.getJointTransform( idDic['RightFoot']) Ts['foot_heel_R'] = controlModel_ik.getJointTransform( idDic['RightFoot']) Ts['heel_R'] = np.eye(4) Ts['outside_metatarsal_R'] = controlModel_ik.getJointTransform( idDic['RightFoot_foot_0_0']) Ts['outside_phalanges_R'] = controlModel_ik.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_ik.getJointTransform( idDic['RightFoot_foot_0_1_0']) Ts['spine_ribs'] = controlModel_ik.getJointTransform( idDic['Spine']) Ts['head'] = controlModel_ik.getJointTransform(idDic['Spine1']) Ts['upper_limb_R'] = controlModel_ik.getJointTransform( idDic['RightArm']) Ts['lower_limb_R'] = controlModel_ik.getJointTransform( idDic['RightForeArm']) Ts['thigh_L'] = controlModel_ik.getJointTransform( idDic['LeftUpLeg']) Ts['shin_L'] = controlModel_ik.getJointTransform(idDic['LeftLeg']) Ts['foot_L'] = controlModel_ik.getJointTransform(idDic['LeftFoot']) Ts['foot_heel_L'] = controlModel_ik.getJointTransform( idDic['LeftFoot']) Ts['heel_L'] = np.eye(4) Ts['outside_metatarsal_L'] = controlModel_ik.getJointTransform( idDic['LeftFoot_foot_0_0']) Ts['outside_phalanges_L'] = controlModel_ik.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_ik.getJointTransform( idDic['LeftFoot_foot_0_1_0']) Ts['upper_limb_L'] = controlModel_ik.getJointTransform( idDic['LeftArm']) Ts['lower_limb_L'] = controlModel_ik.getJointTransform( idDic['LeftForeArm']) color = dict() color['foot_R'] = pallete[0] color['heel_R'] = pallete[4] color['outside_metatarsal_R'] = pallete[1] color['outside_phalanges_R'] = pallete[2] color['inside_phalanges_R'] = pallete[3] color['foot_L'] = pallete[0] color['heel_L'] = pallete[4] color['outside_metatarsal_L'] = pallete[1] color['outside_phalanges_L'] = pallete[2] color['inside_phalanges_L'] = pallete[3] if COLOR_ON: skeleton_renderer.appendFrameState(Ts, color) else: skeleton_renderer.appendFrameState(Ts)
def simulateCallback(frame): # print(frame) # print(motion[frame].getJointOrientationLocal(footIdDic['RightFoot_foot_0_1_0'])) if False and viewer_GetForceState(): # print('force on, frame: ', frame) motion[frame].mulJointOrientationLocal( footIdDic['LeftFoot_foot_0_0_0'], mm.exp(mm.unitX(), -math.pi * mm.SCALAR_1_6)) motion[frame].mulJointOrientationLocal( footIdDic['LeftFoot_foot_0_1_0'], mm.exp(mm.unitX(), -math.pi * mm.SCALAR_1_6)) motion[frame].mulJointOrientationLocal( footIdDic['RightFoot_foot_0_0_0'], mm.exp(mm.unitX(), -math.pi * mm.SCALAR_1_6)) motion[frame].mulJointOrientationLocal( footIdDic['RightFoot_foot_0_1_0'], mm.exp(mm.unitX(), -math.pi * mm.SCALAR_1_6)) # print(motion[frame].getJointOrientationLocal(footIdDic['RightFoot_foot_0_1_0'])) motionModel.update(motion[frame]) 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 = 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) # 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_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.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.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)) ] 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() 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, dJsys = controlModel.computeCom_J_dJdq() 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] # 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. footCenter[0] = footCenter[0] + getParamVal('com X offset') 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. # 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) # 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 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 # 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) 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) # 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]) 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()) 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_footCenter_ref[0] = footCenter_ref 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] 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) # render contact_ids # render skeleton if SKELETON_ON: Ts = dict() Ts['pelvis'] = controlModel.getJointTransform(0) Ts['thigh_R'] = controlModel.getJointTransform(1) Ts['shin_R'] = controlModel.getJointTransform(2) Ts['foot_R'] = controlModel.getJointTransform(3) Ts['spine_ribs'] = controlModel.getJointTransform(9) Ts['head'] = controlModel.getJointTransform(10) Ts['upper_limb_R'] = controlModel.getJointTransform(13) Ts['lower_limb_R'] = controlModel.getJointTransform(14) Ts['thigh_L'] = controlModel.getJointTransform(15) Ts['shin_L'] = controlModel.getJointTransform(16) Ts['foot_L'] = controlModel.getJointTransform(17) Ts['upper_limb_L'] = controlModel.getJointTransform(11) Ts['lower_limb_L'] = controlModel.getJointTransform(12) skeleton_renderer.appendFrameState(Ts)
def footAdjust(posture_ori, touch_body_indices, SEGMENT_FOOT_MAG, SEGMENT_FOOT_RAD, baseHeight=0.): """ :param posture_ori: :type posture_ori: ym.JointPosture :param touch_body_indices: :type touch_body_indices: list[str] :param SEGMENT_FOOT_MAG: :type SEGMENT_FOOT_MAG: float :param SEGMENT_FOOT_RAD: :type SEGMENT_FOOT_RAD: float :param baseHeight: :type baseHeight: float :return: """ if len(touch_body_indices) == 0: return # idDic: name to idx idDic = dict() for i in range(posture_ori.skeleton.getJointNum()): idDic[posture_ori.skeleton.getJointName(i)] = i # nameDic: index to name nameDic = dict() for i in range(posture_ori.skeleton.getJointNum()): nameDic[i] = posture_ori.skeleton.getJointName(i) # specified foot_name = "LeftFoot" # side_touch_body_indices: left or right foot touch body indices side_touch_body_indices = [touch_body_idx for touch_body_idx in touch_body_indices if foot_name in nameDic[touch_body_idx]] # foot_joint_pos: ankle joint position foot_joint_pos = posture_ori.getJointPositionGlobal(idDic[foot_name]) seg_joint_ori = [posture_ori.getJointOrientationGlobal(touch_body_idx) for touch_body_idx in side_touch_body_indices] seg_joint_pos = [] for touch_body_idx in side_touch_body_indices: if nameDic[touch_body_idx] == foot_name+'_foot_0_0': seg_joint_pos.append(posture_ori.getJointPositionGlobal(touch_body_idx) + SEGMENT_FOOT_MAG*np.dot(seg_joint_ori, -1.936*mm.unitY()).flatten()) else: seg_joint_pos.append(posture_ori.getJointPositionGlobal(touch_body_idx)) if len(side_touch_body_indices) == 0: pass elif len(side_touch_body_indices) == 1: seg_idx = side_touch_body_indices[0] # seg joint y pos to 0 ankle_to_joint_vec = seg_joint_pos[0] - foot_joint_pos joint_vec_rot_axis, temp_angle = mm.getRotAxisAngleFromVectors(ankle_to_joint_vec, -mm.unitY()) joint_y_to_zero_angle = temp_angle - math.acos((foot_joint_pos[1] - SEGMENT_FOOT_RAD - baseHeight)/np.linalg.norm(ankle_to_joint_vec)) posture_ori.mulJointOrientationGlobal(idDic[foot_name], mm.exp(joint_vec_rot_axis, joint_y_to_zero_angle)) ############################################################### # TODO: ############################################################### # rotate seg to parallel ground seg_ori = posture_ori.getJointOrientationGlobal(seg_idx) posture_ori.mulJointOrientationGlobal(seg_idx, seg_ori.T) elif len(side_touch_body_indices) == 2: seg_idx = [side_touch_body_indices[0], side_touch_body_indices[1]] ankle_to_joint_vecs = [seg_joint_pos[i] - foot_joint_pos for i in range(len(seg_idx))] ankle_to_joint_vec = .5*(ankle_to_joint_vecs[0] + ankle_to_joint_vecs[1]) joint_vec_rot_axis, temp_angle = mm.getRotAxisAngleFromVectors(ankle_to_joint_vec, -mm.unitY()) joint_y_to_zero_angle = temp_angle - math.acos((foot_joint_pos[1] - SEGMENT_FOOT_RAD - baseHeight)/np.linalg.norm(ankle_to_joint_vec)) posture_ori.mulJointOrientationGlobal(idDic[foot_name], mm.exp(joint_vec_rot_axis, joint_y_to_zero_angle)) seg_contact_pos = [] for touch_body_idx in side_touch_body_indices: ith_seg_contact_pos = [] if nameDic[touch_body_idx] == foot_name+'_foot_0_0_0': ith_seg_contact_pos.append(seg_joint_pos) ith_seg_contact_pos.append(seg_joint_pos + np.dot(seg_joint_ori, mm.unitZ())) ith_seg_contact_pos.append(seg_joint_pos + np.dot(seg_joint_ori, mm.unitX())) if nameDic[touch_body_idx] == foot_name+'_foot_0_1_0': ith_seg_contact_pos.append(seg_joint_pos + np.dot(seg_joint_ori, np.array([0., 0., 0.]))) pass if nameDic[touch_body_idx] == foot_name+'_foot_0_0': pass if nameDic[touch_body_idx] == foot_name+'_foot_1_0': pass # unspecified pass
def preFrameCallback_Always(frame): if frame < 120+start_frame: viewer.doc.setRendererVisible('controlModel', True) viewer.doc.setRendererVisible('skeleton', True) viewer.motionViewWnd.glWindow.camera.rotateX = math.pi /180. * -90. viewer.motionViewWnd.glWindow.camera.rotateY = math.pi viewer.motionViewWnd.glWindow.camera.distance = .3 viewer.motionViewWnd.glWindow.camera.center = controlModel.getBodyPositionGlobal(idDic['RightFoot']) - 0.10*mm.unitX() viewer.motionViewWnd.glWindow.projectionOrtho = True viewer.motionViewWnd.glWindow.projectionChanged = True else: if 0 <= frame % 50 < 22: viewer.doc.setRendererVisible('controlModel', True) viewer.doc.setRendererVisible('skeleton', False) elif 22 <= frame % 50 < 25: viewer.doc.setRendererVisible('controlModel', False) viewer.doc.setRendererVisible('skeleton', False) elif 25 <= frame % 50 < 47: viewer.doc.setRendererVisible('controlModel', False) viewer.doc.setRendererVisible('skeleton', True) elif 47 <= frame % 50 < 50: viewer.doc.setRendererVisible('controlModel', False) viewer.doc.setRendererVisible('skeleton', False)
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() init_tiptoe = 0.4 if True: for i in range(len(motion)): motion[i].mulJointOrientationLocal( 19, mm.exp(mm.unitX(), -math.pi * init_tiptoe)) motion[i].mulJointOrientationLocal( 21, mm.exp(mm.unitX(), -math.pi * init_tiptoe)) motion[i].mulJointOrientationLocal( 5, mm.exp(mm.unitX(), -math.pi * init_tiptoe)) motion[i].mulJointOrientationLocal( 7, mm.exp(mm.unitX(), -math.pi * init_tiptoe)) motion[i].mulJointOrientationLocal( 17, mm.exp(mm.unitX(), math.pi * init_tiptoe * 0.95)) motion[i].mulJointOrientationLocal( 3, mm.exp(mm.unitX(), math.pi * init_tiptoe * 0.95)) 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) controlToMotionOffset = (1.5, 0.1, 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(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'] 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_R'] = init Ts['upper_limb_R'] = init Ts['lower_limb_R'] = init Ts['thigh_L'] = init Ts['shin_L'] = init Ts['foot_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.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( 'ikModel', yr.VpModelRenderer(controlModel_ik, (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) skeleton_renderer = None if SKELETON_ON: skeleton_renderer = yr.BasicSkeletonRenderer( makeEmptyBasicSkeletonTransformDict(np.eye(4)), offset_Y=-0.08) viewer.doc.addRenderer('skeleton', skeleton_renderer) viewer.doc.addRenderer('rd_footCenter', yr.PointsRenderer(rd_footCenter)) viewer.doc.addRenderer('rd_footCenter_ref', yr.PointsRenderer(rd_footCenter_ref)) 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( 'rd_foot_ori', yr.OrientationsRenderer(rd_foot_ori, rd_foot_pos, (255, 255, 0))) viewer.doc.addRenderer( 'rd_root_ori', yr.OrientationsRenderer(rd_root_ori, rd_root_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 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.objectInfoWnd.add1DSlider("tiptoe angle", -.5, .5, 0.001, 0.) viewer.objectInfoWnd.add1DSlider("left tilt angle", .0, .5, 0.001, 0.) viewer.objectInfoWnd.add1DSlider("right tilt angle", .0, .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) 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') # 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 ################################### # simulate ################################### def simulateCallback(frame): print(frame) if True: if frame == 100: setParamVal('tiptoe angle', -0.4) elif frame == 130: foot_viewer.check_all_seg() setParamVal('SupKt', 30.) elif frame == 200: setParamVal('SupKt', 17.) elif frame == 350: setParamVal('com X offset', -0.04) elif frame == 400: setParamVal('com X offset', -0.08) elif False and frame == 500: setParamVal('com X offset', 0.) setParamVal('tiptoe angle', 0.) foot_viewer.check_tiptoe_all() # print(motion[frame].getJointOrientationLocal(footIdDic['RightFoot_foot_0_1_0'])) if abs(getParamVal('tiptoe angle')) > 0.001: tiptoe_angle = getParamVal('tiptoe angle') motion[frame].mulJointOrientationLocal( footIdDic['LeftFoot_foot_0_0_0'], mm.exp(mm.unitX(), -math.pi * tiptoe_angle)) motion[frame].mulJointOrientationLocal( footIdDic['LeftFoot_foot_0_1_0'], mm.exp(mm.unitX(), -math.pi * tiptoe_angle)) motion[frame].mulJointOrientationLocal( footIdDic['RightFoot_foot_0_0_0'], mm.exp(mm.unitX(), -math.pi * tiptoe_angle)) motion[frame].mulJointOrientationLocal( footIdDic['RightFoot_foot_0_1_0'], mm.exp(mm.unitX(), -math.pi * tiptoe_angle)) motion[frame].mulJointOrientationLocal( supL, mm.exp(mm.unitX(), math.pi * tiptoe_angle * 0.95)) motion[frame].mulJointOrientationLocal( supR, mm.exp(mm.unitX(), math.pi * tiptoe_angle * 0.95)) if getParamVal('left tilt angle') > 0.001: left_tilt_angle = getParamVal('left tilt angle') motion[frame].mulJointOrientationLocal( footIdDic['LeftFoot_foot_0_1'], mm.exp(mm.unitZ(), -math.pi * left_tilt_angle)) motion[frame].mulJointOrientationLocal( footIdDic['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( footIdDic['LeftFoot_foot_0_0'], mm.exp(mm.unitZ(), -math.pi * left_tilt_angle)) motion[frame].mulJointOrientationLocal( footIdDic['LeftFoot_foot_0_1'], mm.exp(mm.unitZ(), math.pi * left_tilt_angle)) motion[frame].mulJointOrientationLocal( footIdDic['LeftFoot'], mm.exp(mm.unitZ(), math.pi * left_tilt_angle)) if getParamVal('right tilt angle') > 0.001: right_tilt_angle = getParamVal('right tilt angle') motion[frame].mulJointOrientationLocal( footIdDic['RightFoot_foot_0_1'], mm.exp(mm.unitZ(), math.pi * right_tilt_angle)) motion[frame].mulJointOrientationLocal( footIdDic['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( footIdDic['RightFoot_foot_0_0'], mm.exp(mm.unitZ(), math.pi * right_tilt_angle)) motion[frame].mulJointOrientationLocal( footIdDic['RightFoot_foot_0_1'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle)) motion[frame].mulJointOrientationLocal( footIdDic['RightFoot'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle)) motionModel.update(motion[frame]) 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) ################################################# # 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.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.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[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 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 # 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))] 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)) ] # 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()) 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) 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) # render contact_ids # render skeleton if SKELETON_ON: Ts = dict() Ts['pelvis'] = controlModel.getJointTransform(0) Ts['thigh_R'] = controlModel.getJointTransform(1) Ts['shin_R'] = controlModel.getJointTransform(2) Ts['foot_R'] = controlModel.getJointTransform(3) Ts['spine_ribs'] = controlModel.getJointTransform(9) Ts['head'] = controlModel.getJointTransform(10) Ts['upper_limb_R'] = controlModel.getJointTransform(13) Ts['lower_limb_R'] = controlModel.getJointTransform(14) Ts['thigh_L'] = controlModel.getJointTransform(15) Ts['shin_L'] = controlModel.getJointTransform(16) Ts['foot_L'] = controlModel.getJointTransform(17) Ts['upper_limb_L'] = controlModel.getJointTransform(11) Ts['lower_limb_L'] = controlModel.getJointTransform(12) skeleton_renderer.appendFrameState(Ts) 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() 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) Fl.run()