def simulateCallback(frame): th_r = motion_ori.getDOFPositions(frame) th = controlModel.getDOFPositions() dth_r = motion_ori.getDOFVelocities(frame) dth = controlModel.getDOFVelocities() ddth_r = motion_ori.getDOFAccelerations(frame) ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt) for i in range(stepsPerFrame): bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds) vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces) controlModel.setDOFAccelerations(ddth_des) controlModel.solveHybridDynamics() vpWorld.step() motionModel.update(motion_ori[frame])
def simulateCallback(frame): 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) for i in range(stepsPerFrame): # get penalty forces # bodyIDs, positions, positionLocals, forces = vpWorld.calcPenaltyForce(bodyIDsToCheck, mus, 1000, 10) bodyIDs, positions, positionLocals, forces = vpWorld.calcPenaltyForce_Boxes(rd_box.boxSizes, rd_box.Ts, bodyIDsToCheck, mus, 1000, 10) # apply penalty forces vpWorld.applyPenaltyForce(bodyIDs, positionLocals, forces) controlModel.setDOFAccelerations(ddth_des) controlModel.solveHybridDynamics() vpWorld.step() contactPositions[:] = positions contactForces[:] = forces
def simulateCallback(frame): # seginfo segIndex = seg_index[0] curState = seginfo[segIndex]['state'] curInterval = yma.offsetInterval(acc_offset[0], seginfo[segIndex]['interval']) stanceLegs = seginfo[segIndex]['stanceHips'] swingLegs = seginfo[segIndex]['swingHips'] stanceFoots = seginfo[segIndex]['stanceFoots'] swingFoots = seginfo[segIndex]['swingFoots'] swingKnees = seginfo[segIndex]['swingKnees'] groundHeight = seginfo[segIndex]['ground_height'] maxStfPushFrame = seginfo[segIndex]['max_stf_push_frame'] prev_frame = frame-1 if frame>0 else 0 # prev_frame = frame # information # dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(frame), bodyMasses, upperMass, uppers) # CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(frame), bodyMasses, upperMass, uppers) ## dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(frame), bodyMasses, totalMass) ## CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(frame), bodyMasses, totalMass) # stf_tar = motion_seg.getJointPositionGlobal(stanceFoots[0], frame) # CMr_tar = CM_tar - stf_tar dCM_tar = motion_seg.getJointVelocityGlobal(0, prev_frame) CM_tar = motion_seg.getJointPositionGlobal(0, prev_frame) # dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(prev_frame), bodyMasses, upperMass, uppers) # CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(prev_frame), bodyMasses, upperMass, uppers) # dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(prev_frame), bodyMasses, totalMass) # CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(prev_frame), bodyMasses, totalMass) stf_tar = motion_seg.getJointPositionGlobal(stanceFoots[0], prev_frame) CMr_tar = CM_tar - stf_tar dCM = avg_dCM[0] CM = controlModel.getJointPositionGlobal(0) # CM = yrp.getCM(controlModel.getJointPositionsGlobal(), bodyMasses, upperMass, uppers) # CM = yrp.getCM(controlModel.getJointPositionsGlobal(), bodyMasses, totalMass) CMreal = yrp.getCM(controlModel.getJointPositionsGlobal(), bodyMasses, totalMass) stf = controlModel.getJointPositionGlobal(stanceFoots[0]) CMr = CM - stf diff_dCM = mm.projectionOnPlane(dCM-dCM_tar, (1,0,0), (0,0,1)) diff_dCM_axis = np.cross((0,1,0), diff_dCM) rd_vec1[0] = diff_dCM; rd_vecori1[0] = CM_tar diff_CMr = mm.projectionOnPlane(CMr-CMr_tar, (1,0,0), (0,0,1)) # rd_vec1[0] = diff_CMr; rd_vecori1[0] = stf_tar diff_CMr_axis = np.cross((0,1,0), diff_CMr) direction = mm.normalize2(mm.projectionOnPlane(dCM_tar, (1,0,0), (0,0,1))) # direction = mm.normalize2(mm.projectionOnPlane(dCM, (1,0,0), (0,0,1))) directionAxis = np.cross((0,1,0), direction) diff_dCM_sag, diff_dCM_cor = mm.projectionOnVector2(diff_dCM, direction) # rd_vec1[0] = diff_dCM_sag; rd_vecori1[0] = CM_tar diff_dCM_sag_axis = np.cross((0,1,0), diff_dCM_sag) diff_dCM_cor_axis = np.cross((0,1,0), diff_dCM_cor) diff_CMr_sag, diff_CMr_cor = mm.projectionOnVector2(diff_CMr, direction) diff_CMr_sag_axis = np.cross((0,1,0), diff_CMr_sag) diff_CMr_cor_axis = np.cross((0,1,0), diff_CMr_cor) t = (frame-curInterval[0])/float(curInterval[1]-curInterval[0]) t_raw = t if t>1.: t=1. p_root = motion_stitch[frame].getJointPositionGlobal(0) R_root = motion_stitch[frame].getJointOrientationGlobal(0) motion_seg_orig.goToFrame(frame) motion_seg.goToFrame(frame) motion_stitch.goToFrame(frame) motion_debug1.append(motion_stitch[frame].copy()) motion_debug1.goToFrame(frame) motion_debug2.append(motion_stitch[frame].copy()) motion_debug2.goToFrame(frame) motion_debug3.append(motion_stitch[frame].copy()) motion_debug3.goToFrame(frame) # paper implementation M_tc.append(motion_stitch[prev_frame]) M_tc.goToFrame(frame) P_hat.append(M_tc[frame].copy()) P_hat.goToFrame(frame) p_temp = ym.JointPosture(skeleton) p_temp.rootPos = controlModel.getJointPositionGlobal(0) p_temp.setJointOrientationsLocal(controlModel.getJointOrientationsLocal()) P.append(p_temp) P.goToFrame(frame) # stance foot stabilize motion_stf_stabilize.append(motion_stitch[frame].copy()) motion_stf_stabilize.goToFrame(frame) if STANCE_FOOT_STABILIZE: for stanceFoot in stanceFoots: R_target_foot = motion_seg[frame].getJointOrientationGlobal(stanceFoot) R_current_foot = motion_stf_stabilize[frame].getJointOrientationGlobal(stanceFoot) motion_stf_stabilize[frame].setJointOrientationGlobal(stanceFoot, cm.slerp(R_current_foot, R_target_foot , stf_stabilize_func(t))) # R_target_foot = motion_seg[frame].getJointOrientationLocal(stanceFoot) # R_current_foot = motion_stf_stabilize[frame].getJointOrientationLocal(stanceFoot) # motion_stf_stabilize[frame].setJointOrientationLocal(stanceFoot, cm.slerp(R_current_foot, R_target_foot , stf_stabilize_func(t))) # match stance leg motion_match_stl.append(motion_stf_stabilize[frame].copy()) motion_match_stl.goToFrame(frame) if MATCH_STANCE_LEG: if curState!=yba.GaitState.STOP: for i in range(len(stanceLegs)): stanceLeg = stanceLegs[i] stanceFoot = stanceFoots[i] # # motion stance leg -> character stance leg as time goes R_motion = motion_match_stl[frame].getJointOrientationGlobal(stanceLeg) R_character = controlModel.getJointOrientationGlobal(stanceLeg) motion_match_stl[frame].setJointOrientationGlobal(stanceLeg, cm.slerp(R_motion, R_character, match_stl_func(t))) # t_y = match_stl_func_y(t) # t_xz = match_stl_func(t) # # R_motion = motion_match_stl[frame].getJointOrientationGlobal(stanceLeg) # R_character = controlModel.getJointOrientationGlobal(stanceLeg) # R = np.dot(R_character, R_motion.T) # R_y, R_xz = mm.projectRotation((0,1,0), R) # motion_match_stl[frame].mulJointOrientationGlobal(stanceLeg, mm.scaleSO3(R_xz, t_xz)) # motion_match_stl[frame].mulJointOrientationGlobal(stanceLeg, mm.scaleSO3(R_y, t_y)) # swing foot placement motion_swf_placement.append(motion_match_stl[frame].copy()) motion_swf_placement.goToFrame(frame) if SWING_FOOT_PLACEMENT: t_swing_foot_placement = swf_placement_func(t); if extended[0]: R_swp_sag = prev_R_swp[0][0] R_swp_cor = prev_R_swp[0][1] else: R_swp_sag = mm.I_SO3(); R_swp_cor = mm.I_SO3() R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_dCM_sag_axis * K_swp_vel_sag * -t_swing_foot_placement)) R_swp_cor = np.dot(R_swp_cor, mm.exp(diff_dCM_cor_axis * K_swp_vel_cor * -t_swing_foot_placement)) if np.dot(direction, diff_CMr_sag) < 0: R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_CMr_sag_axis * K_swp_pos_sag * -t_swing_foot_placement)) R_swp_cor = np.dot(R_swp_cor, mm.exp(diff_CMr_cor_axis * K_swp_pos_cor * -t_swing_foot_placement)) for i in range(len(swingLegs)): swingLeg = swingLegs[i] swingFoot = swingFoots[i] # save swing foot global orientation # R_swf = motion_swf_placement[frame].getJointOrientationGlobal(swingFoot) # rotate swing leg motion_swf_placement[frame].mulJointOrientationGlobal(swingLeg, R_swp_sag) motion_swf_placement[frame].mulJointOrientationGlobal(swingLeg, R_swp_cor) # restore swing foot global orientation # motion_swf_placement[frame].setJointOrientationGlobal(swingFoot, R_swf) prev_R_swp[0] = (R_swp_sag, R_swp_cor) # swing foot height motion_swf_height.append(motion_swf_placement[frame].copy()) motion_swf_height.goToFrame(frame) if SWING_FOOT_HEIGHT: for swingFoot in swingFoots: stanceFoot = stanceFoots[0] # save foot global orientation R_foot = motion_swf_height[frame].getJointOrientationGlobal(swingFoot) R_stance_foot = motion_swf_height[frame].getJointOrientationGlobal(stanceFoot) height_tar = motion_swf_height[frame].getJointPositionGlobal(swingFoot)[1] - motion_swf_height[frame].getJointPositionGlobal(stanceFoot)[1] # motion_debug1[frame] = motion_swf_height[frame].copy() # rotate motion_swf_height[frame].rotateByTarget(controlModel.getJointOrientationGlobal(0)) # motion_debug2[frame] = motion_swf_height[frame].copy() # motion_debug2[frame].translateByTarget(controlModel.getJointPositionGlobal(0)) height_cur = motion_swf_height[frame].getJointPositionGlobal(swingFoot)[1] - motion_swf_height[frame].getJointPositionGlobal(stanceFoot)[1] offset_height = (height_tar - height_cur) * swf_height_func(t) * c_swf_stability offset_sine = c_swf_mid_offset * swf_height_sine_func(t) offset = 0. offset += offset_height offset += offset_sine if offset > 0.: newPosition = motion_swf_height[frame].getJointPositionGlobal(swingFoot) newPosition[1] += offset aik.ik_analytic(motion_swf_height[frame], swingFoot, newPosition) else: newPosition = motion_swf_height[frame].getJointPositionGlobal(stanceFoot) newPosition[1] -= offset aik.ik_analytic(motion_swf_height[frame], stanceFoot, newPosition) # return # motion_debug3[frame] = motion_swf_height[frame].copy() # motion_debug3[frame].translateByTarget(controlModel.getJointPositionGlobal(0)) motion_swf_height[frame].rotateByTarget(R_root) # restore foot global orientation motion_swf_height[frame].setJointOrientationGlobal(swingFoot, R_foot) motion_swf_height[frame].setJointOrientationGlobal(stanceFoot, R_stance_foot) if plot!=None: plot.addDataPoint('debug1', frame, height_tar) plot.addDataPoint('debug2', frame, height_cur) # plot.addDataPoint('diff', frame, diff) # swing foot orientation motion_swf_orientation.append(motion_swf_height[frame].copy()) motion_swf_orientation.goToFrame(frame) if SWING_FOOT_ORIENTATION: # swf_orientation_func = yfg.concatenate([yfg.zero, yfg.hermite2nd, yfg.one], [.25, .75]) for swingFoot in swingFoots: R_target_foot = motion_seg[curInterval[1]].getJointOrientationGlobal(swingFoot) R_current_foot = motion_swf_orientation[frame].getJointOrientationGlobal(swingFoot) motion_swf_orientation[frame].setJointOrientationGlobal(swingFoot, cm.slerp(R_current_foot, R_target_foot, swf_orientation_func(t))) # swf_stabilize_func = yfg.concatenate([yfg.hermite2nd, yfg.one], [c_taking_duration]) # push orientation # for swingFoot in swingFoots: # R_target_foot = motion_seg[frame].getJointOrientationGlobal(swingFoot) # R_current_foot = motion_swf_orientation[frame].getJointOrientationGlobal(swingFoot) # motion_swf_orientation[frame].setJointOrientationGlobal(swingFoot, cm.slerp(R_current_foot, R_target_foot , swf_stabilize_func(t))) # stance foot push motion_stf_push.append(motion_swf_orientation[frame].copy()) motion_stf_push.goToFrame(frame) if STANCE_FOOT_PUSH: for swingFoot in swingFoots: # max_t = (maxStfPushFrame)/float(curInterval[1]-curInterval[0]) # stf_push_func = yfg.concatenate([yfg.sine, yfg.zero], [max_t*2]) stf_push_func = yfg.concatenate([yfg.sine, yfg.zero], [c_taking_duration*2]) R_swp_sag = mm.I_SO3() # R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_dCM_sag_axis * K_stp_vel * -stf_push_func(t))) # if step_length_cur[0] < step_length_tar[0]: # ratio = step_length_cur[0] / step_length_tar[0] # R_max = maxmaxStfPushFrame # R_zero = R_swp_sag = np.dot(R_swp_sag, mm.exp((step_length_tar[0] - step_length_cur[0])*step_axis[0] * K_stp_pos * -stf_push_func(t))) motion_stf_push[frame].mulJointOrientationGlobal(swingFoot, R_swp_sag) # stance foot balancing motion_stf_balancing.append(motion_stf_push[frame].copy()) motion_stf_balancing.goToFrame(frame) if STANCE_FOOT_BALANCING: R_stb = mm.exp(diff_dCM_axis * K_stb_vel * stf_balancing_func(t)) for stanceFoot in stanceFoots: if frame < 5: continue motion_stf_balancing[frame].mulJointOrientationGlobal(stanceFoot, R_stb) # control trajectory motion_control.append(motion_stf_balancing[frame].copy()) motion_control.goToFrame(frame) #======================================================================= # tracking with inverse dynamics #======================================================================= th_r = motion_control.getDOFPositions(frame) th = controlModel.getDOFPositions() dth_r = motion_control.getDOFVelocities(frame) dth = controlModel.getDOFVelocities() ddth_r = motion_control.getDOFAccelerations(frame) ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt) #======================================================================= # simulation #======================================================================= CP = mm.v3(0.,0.,0.) F = mm.v3(0.,0.,0.) avg_dCM[0] = mm.v3(0.,0.,0.) # external force rendering info del rd_forces[:]; del rd_force_points[:] for fi in forceInfos: if fi.startFrame <= frame and frame < fi.startFrame + fi.duration*(1/frameTime): rd_forces.append(fi.force) rd_force_points.append(controlModel.getBodyPositionGlobal(fi.targetBody)) for i in range(stepsPerFrame): bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds) vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces) # apply external force for fi in forceInfos: if fi.startFrame <= frame and frame < fi.startFrame + fi.duration*(1/frameTime): controlModel.applyBodyForceGlobal(fi.targetBody, fi.force) controlModel.setDOFAccelerations(ddth_des) controlModel.solveHybridDynamics() if TORQUE_PLOT: rhip_torques[frame] += mm.length(controlModel.getJointTorqueLocal(rUpLeg)) rknee_torques[frame] += mm.length(controlModel.getJointTorqueLocal(rKnee)) rankle_torques[frame] += mm.length(controlModel.getJointTorqueLocal(rFoot)) rd_torques[:] = [controlModel.getJointTorqueLocal(i)/100. for i in range(skeleton.getJointNum())] rd_joint_positions[:] = controlModel.getJointPositionsGlobal() vpWorld.step() # yvu.align2D(controlModel) if len(contactForces) > 0: CP += yrp.getCP(contactPositions, contactForces) F += sum(contactForces) avg_dCM[0] += controlModel.getJointVelocityGlobal(0) # avg_dCM[0] += yrp.getCM(controlModel.getJointVelocitiesGlobal(), bodyMasses, upperMass, uppers) # avg_dCM[0] += yrp.getCM(controlModel.getJointVelocitiesGlobal(), bodyMasses, totalMass) # if len(stanceFoots)>0: # avg_stf_v[0] += controlModel.getJointVelocityGlobal(stanceFoots[0]) # avg_stf_av[0] += controlModel.getJointAngVelocityGlobal(stanceFoots[0]) CP /= stepsPerFrame F /= stepsPerFrame avg_dCM[0] /= stepsPerFrame # if len(stanceFoots)>0: # avg_stf_v[0] /= stepsPerFrame # avg_stf_av[0] /= stepsPerFrame # rd_vec1[0] = avg_stf_av[0]; rd_vec1[0][0] = 0.; rd_vec1[0][2] = 0. # rd_vecori1[0]= controlModel.getJointPositionGlobal(stanceFoots[0]) #======================================================================= # segment editing #======================================================================= lastFrame = False if SEGMENT_EDITING: if curState==yba.GaitState.STOP: if frame == len(motion_seg)-1: lastFrame = True elif (curState==yba.GaitState.LSWING or curState==yba.GaitState.RSWING) and t>c_min_contact_time: swingID = lID if curState==yba.GaitState.LSWING else rID contact = False if swingID in bodyIDs: minContactVel = 1000. for i in range(len(bodyIDs)): if bodyIDs[i]==swingID: vel = controlModel.getBodyVelocityGlobal(swingID, contactPositionLocals[i]) vel[1] = 0 contactVel = mm.length(vel) if contactVel < minContactVel: minContactVel = contactVel if minContactVel < c_min_contact_vel: contact = True extended[0] = False if contact: # print frame, 'foot touch' lastFrame = True acc_offset[0] += frame - curInterval[1] elif frame == len(motion_seg)-1: print frame, 'extend frame', frame+1 preserveJoints = [] # preserveJoints = [lFoot, rFoot] # preserveJoints = [lFoot, rFoot, lKnee, rKnee] # preserveJoints = [lFoot, rFoot, lKnee, rKnee, lUpLeg, rUpLeg] stanceKnees = [rKnee] if curState==yba.GaitState.LSWING else [lKnee] preserveJoints = [stanceFoots[0], stanceKnees[0], stanceLegs[0]] diff = 3 motion_seg_orig.extend([motion_seg_orig[-1]]) motion_seg.extend(ymt.extendByIntegration_root(motion_seg, 1, diff)) motion_stitch.extend(ymt.extendByIntegration_constant(motion_stitch, 1, preserveJoints, diff)) # # extend for swing foot ground speed matching & swing foot height lower ## extendedPostures = ymt.extendByIntegration(motion_stitch, 1, preserveJoints, diff) ## extendedPostures = [motion_stitch[-1]] ## # extendFrameNum = frame - curInterval[1] + 1 # k = 1.-extendFrameNum/5. # if k<0.: k=0. # extendedPostures = ymt.extendByIntegrationAttenuation(motion_stitch, 1, preserveJoints, diff, k) # ## if len(swingFoots)>0 and np.inner(dCM_tar, dCM)>0.: ## print frame, 'speed matching' ## R_swf = motion_stitch[-1].getJointOrientationGlobal(swingFoots[0]) ## ## p_swf = motion_stitch[-1].getJointPositionGlobal(swingFoots[0]) ## v_swf = motion_stitch.getJointVelocityGlobal(swingFoots[0], frame-diff, frame) ## a_swf = motion_stitch.getJointAccelerationGlobal(swingFoots[0], frame-diff, frame) ## p_swf += v_swf * (frameTime) + a_swf * (frameTime)*(frameTime) ## aik.ik_analytic(extendedPostures[0], swingFoots[0], p_swf) ## ## extendedPostures[0].setJointOrientationGlobal(swingFoots[0], R_swf) # # motion_stitch.extend(extendedPostures) extended[0] = True else: if frame == len(motion_seg)-1: lastFrame = True if lastFrame: if segIndex < len(segments)-1: print '%d (%d): end of %dth seg (%s, %s)'%(frame, frame-curInterval[1],segIndex, yba.GaitState.text[curState], curInterval) if plot!=None: plot.addDataPoint('diff', frame, (frame-curInterval[1])*.01) if len(stanceFoots)>0 and len(swingFoots)>0: # step_cur = controlModel.getJointPositionGlobal(swingFoots[0]) - controlModel.getJointPositionGlobal(stanceFoots[0]) # step_tar = motion_seg[curInterval[1]].getJointPositionGlobal(swingFoots[0]) - motion_seg[curInterval[1]].getJointPositionGlobal(stanceFoots[0]) step_cur = controlModel.getJointPositionGlobal(0) - controlModel.getJointPositionGlobal(stanceFoots[0]) step_tar = motion_seg[curInterval[1]].getJointPositionGlobal(0) - motion_seg[curInterval[1]].getJointPositionGlobal(stanceFoots[0]) step_cur = mm.projectionOnPlane(step_cur, (1,0,0), (0,0,1)) step_tar = mm.projectionOnPlane(step_tar, (1,0,0), (0,0,1)) step_cur_sag, step_cur_cor = mm.projectionOnVector2(step_cur, direction) step_tar_sag, step_tar_cor = mm.projectionOnVector2(step_tar, direction) step_length_tar[0] = mm.length(step_tar_sag) if np.inner(step_tar_sag, step_cur_sag) > 0: step_length_cur[0] = mm.length(step_cur_sag) else: step_length_cur[0] = -mm.length(step_cur_sag) step_axis[0] = directionAxis # rd_vec1[0] = step_tar_sag # rd_vecori1[0] = motion_seg[curInterval[1]].getJointPositionGlobal(stanceFoots[0]) # rd_vec2[0] = step_cur_sag # rd_vecori2[0] = controlModel.getJointPositionGlobal(stanceFoots[0]) seg_index[0] += 1 curSeg = segments[seg_index[0]] stl_y_limit_num[0] = 0 stl_xz_limit_num[0] = 0 del motion_seg_orig[frame+1:] motion_seg_orig.extend(ymb.getAttachedNextMotion(curSeg, motion_seg_orig[-1], False, False)) del motion_seg[frame+1:] del motion_stitch[frame+1:] transitionLength = len(curSeg)-1 # motion_seg.extend(ymb.getAttachedNextMotion(curSeg, motion_seg[-1], False, False)) # motion_stitch.extend(ymb.getStitchedNextMotion(curSeg, motion_control[-1], transitionLength, stitch_func, True, False)) d = motion_seg[-1] - curSeg[0] d.rootPos[1] = 0. motion_seg.extend(ymb.getAttachedNextMotion(curSeg, d, True, False)) if NO_FOOT_SLIDING: if segIndex == len(segments)-2: Rl = motion_control[-1].getJointOrientationLocal(lUpLeg) Rr = motion_control[-1].getJointOrientationLocal(rUpLeg) Rlk = motion_control[-1].getJointOrientationLocal(lKnee) Rrk = motion_control[-1].getJointOrientationLocal(rKnee) Rlf = motion_control[-1].getJointOrientationLocal(lFoot) Rrf = motion_control[-1].getJointOrientationLocal(rFoot) for p in curSeg: p.setJointOrientationLocal(lUpLeg, Rl, False) p.setJointOrientationLocal(rUpLeg, Rr, False) p.setJointOrientationLocal(lKnee, Rlk, False) p.setJointOrientationLocal(rKnee, Rrk, False) p.setJointOrientationLocal(lFoot, Rlf, False) p.setJointOrientationLocal(rFoot, Rrf, False) p.updateGlobalT() d = motion_control[-1] - curSeg[0] d.rootPos[1] = 0. motion_stitch.extend(ymb.getStitchedNextMotion(curSeg, d, transitionLength, stitch_func, True, False)) # motion_seg.extend(ymb.getAttachedNextMotion(curSeg, motion_seg[-1], False, True)) # motion_stitch.extend(ymb.getStitchedNextMotion(curSeg, motion_control[-1], transitionLength, stitch_func, True, True)) else: motion_seg_orig.append(motion_seg_orig[-1]) motion_seg.append(motion_seg[-1]) motion_stitch.append(motion_control[-1]) # rendering motionModel.update(motion_ori[frame]) # motionModel.update(motion_seg[frame]) if SAVE_SIMULATION: p_temp = ym.JointPosture(skeleton) p_temp.initLocalRs() p_temp.rootPos = controlModel.getJointPositionGlobal(0) p_temp.setJointOrientationsLocal(controlModel.getJointOrientationsLocal()) motion_simulation.append(p_temp) if frame == viewer.getMaxFrame(): saveFilePath = SAVE_DIR+'simulated_'+filename yf.writeBvhFile(saveFilePath, motion_simulation) print saveFilePath, 'saved'
def simulateCallback(frame): print "main:frame : ", frame curTime = time.time() if frame % 30 == 1: pt[0] = time.time() global g_initFlag global forceShowFrame global forceApplyFrame global JsysPre global JsupPreL global JsupPreR global JsupPre global softConstPoint global stage global contactRendererName global desCOMOffset motionModel.update(motion[0]) Kt, Kk, Kl, Kh, Ksc, Bt, Bl, Bh, B_CM, B_CMSd, B_Toe = viewer.GetParam( ) Dt = 2 * (Kt**.5) Dk = 2 * (Kk**.5) Dl = 2 * (Kl**.5) Dh = 2 * (Kh**.5) Dsc = 2 * (Ksc**.5) # tracking th_r_ori = motion.getDOFPositions(frame) th_r = copy.copy(th_r_ori) ############################ #Reference motion modulation dCM_k = 10. linkVelocities = controlModel.getBodyVelocitiesGlobal() dCM = yrp.getCM(linkVelocities, linkMasses, totalMass) dCM_plane = copy.copy(dCM) dCM_plane[1] = 0. global leftHipTimer if viewer.objectInfoWnd.onLeftHip: leftHipTimer = 60 viewer.objectInfoWnd.onLeftHip = False if leftHipTimer > 0: viewer.objectInfoWnd.comOffsetX.value( 0.08 * np.sin(2 * 3.14 * leftHipTimer / 60.)) #viewer.objectInfoWnd.comOffsetZ.value(0.04*np.cos(2*3.14*leftHipTimer/90.)) #B_Hipd = viewer.objectInfoWnd.labelLeftHip.value() #newR1 = mm.exp(mm.v3(0.0,1.0,0.0), 3.14*0.5*B_Hipd/100.) #idx = motion[0].skeleton.getJointIndex('LeftUpLeg') #th_r[idx] = np.dot(th_r[idx], newR1) #idx = motion[0].skeleton.getJointIndex('RightUpLeg') #th_r[idx] = np.dot(th_r[idx], newR1) leftHipTimer -= 1 timeReport[0] += time.time() - curTime curTime = time.time() 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) ddth_c = controlModel.getDOFAccelerations() ype.flatten(ddth_des, ddth_des_flat) ype.flatten(dth, dth_flat) ype.flatten(ddth_c, ddth_c_flat) # jacobian refFootL = motionModel.getBodyPositionGlobal(supL) refFootR = motionModel.getBodyPositionGlobal(supR) positionFootL = [None] * footPartNum positionFootR = [None] * footPartNum for i in range(footPartNum): positionFootL[i] = controlModel.getBodyPositionGlobal( indexFootL[i]) positionFootR[i] = controlModel.getBodyPositionGlobal( indexFootR[i]) linkPositions = controlModel.getBodyPositionsGlobal() linkVelocities = controlModel.getBodyVelocitiesGlobal() linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal() linkInertias = controlModel.getBodyInertiasGlobal() jointPositions = controlModel.getJointPositionsGlobal() jointAxeses = controlModel.getDOFAxeses() CM = yrp.getCM(linkPositions, linkMasses, totalMass) dCM = yrp.getCM(linkVelocities, linkMasses, totalMass) CM_plane = copy.copy(CM) CM_plane[1] = 0. dCM_plane = copy.copy(dCM) dCM_plane[1] = 0. linkPositions_ref = motionModel.getBodyPositionsGlobal() linkVelocities_ref = motionModel.getBodyVelocitiesGlobal() linkAngVelocities_ref = motionModel.getBodyAngVelocitiesGlobal() linkInertias_ref = motionModel.getBodyInertiasGlobal() CM_ref = yrp.getCM(linkPositions_ref, linkMasses, totalMass) CM_plane_ref = copy.copy(CM_ref) CM_plane_ref[1] = 0. P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM, linkInertias) dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses, linkVelocities, dCM, linkAngVelocities, linkInertias) timeReport[1] += time.time() - curTime curTime = time.time() yjc.computeJacobian2(Jsys, DOFs, jointPositions, jointAxeses, linkPositions, allLinkJointMasks) timeReport[2] += time.time() - curTime curTime = time.time() # yjc.computeJacobianDerivative2(dJsys, DOFs, jointPositions, jointAxeses, linkAngVelocities, linkPositions, allLinkJointMasks) if frame > 0: dJsys = (Jsys - JsysPre) * 30. else: dJsys = (Jsys - Jsys) JsysPre = Jsys.copy() timeReport[3] += time.time() - curTime curTime = time.time() lcpBodyIDs, lcpContactPositions, lcpContactPositionLocals, lcpContactForces = hls.calcLCPForces( motion, vpWorld, controlModel, bodyIDsToCheck, 1., 4, None) # bodyIDs : IDs for Virtual Physics, not VpModel !!! bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds) CP = yrp.getCP(contactPositions, contactForces) if (CP is not None): CP[1] = 0. for i in range(controlModel.getBodyNum()): controlModel.SetBodyColor(bodyIDsToCheck[i], 0, 0, 0, 255) contactFlagFootL = [0] * footPartNum contactFlagFootR = [0] * footPartNum for i in range(len(bodyIDs)): controlModel.SetBodyColor(bodyIDs[i], 255, 105, 105, 200) index = controlModel.id2index(bodyIDs[i]) for j in range(len(indexFootL)): if index == indexFootL[j]: contactFlagFootL[j] = 1 for j in range(len(indexFootR)): if index == indexFootR[j]: contactFlagFootR[j] = 1 for j in range(0, footPartNum): jFootR[j] = Jsys[6 * indexFootR[j]:6 * indexFootR[j] + 6] #.copy() jFootL[j] = Jsys[6 * indexFootL[j]:6 * indexFootL[j] + 6] #.copy() dJFootR[j] = dJsys[6 * indexFootR[j]:6 * indexFootR[j] + 6] #.copy() dJFootL[j] = dJsys[6 * indexFootL[j]:6 * indexFootL[j] + 6] #.copy() if footPartNum == 1: desFCL = (controlModel.getBodyPositionGlobal(supL)) desFCR = (controlModel.getBodyPositionGlobal(supR)) else: r = .5 + desCOMOffset desFCL = (controlModel.getBodyPositionGlobal(indexFootL[0]) * r + controlModel.getBodyPositionGlobal(indexFootL[1]) * (1.0 - r) ) #controlModel.getBodyPositionGlobal(indexFootL[1]) desFCR = (controlModel.getBodyPositionGlobal(indexFootR[0]) * r + controlModel.getBodyPositionGlobal(indexFootR[1]) * (1.0 - r) ) #controlModel.getBodyPositionGlobal(indexFootR[1]) desFC = desFCL + (desFCR - desFCL) / 2.0 desFC[1] = 0 rd_footCenter_des[0] = desFC.copy() curRelCMVec = CM_plane - desFC vecRatio = mm.length(curRelCMVec) * 0. #print(frame, vecRatio) footCenter = desFC - curRelCMVec * (vecRatio) #/10.0 footCenter = ( getBodyGlobalPos(controlModel, motion, 'LeftCalcaneus_1') + getBodyGlobalPos(controlModel, motion, 'LeftPhalange_1') + getBodyGlobalPos(controlModel, motion, 'RightCalcaneus_1') + getBodyGlobalPos(controlModel, motion, 'RightPhalange_1')) / 4. #footCenter = (getBodyGlobalPos(controlModel, motion, 'LeftCalcaneus_1') + getBodyGlobalPos(controlModel, motion, 'LeftTalus_1') + getBodyGlobalPos(controlModel, motion, 'RightCalcaneus_1') + getBodyGlobalPos(controlModel, motion, 'RightTalus_1'))/4. footCenter_ref = refFootL + (refFootR - refFootL) / 2.0 #footCenter_ref[1] = 0. footCenter[1] = 0. footCenterOffset = np.array([ viewer.objectInfoWnd.comOffsetX.value(), 0, viewer.objectInfoWnd.comOffsetZ.value() ]) #footCenter += footCenterOffset vecRatio = mm.length(curRelCMVec) * 0. softConstPointOffset = -curRelCMVec * (vecRatio) #/10.0 #print(frame, vecRatio, softConstPointOffset) desForeSupLAcc = [0, 0, 0] desForeSupRAcc = [0, 0, 0] totalNormalForce = [0, 0, 0] for i in range(len(contactForces)): totalNormalForce[0] += contactForces[i][0] totalNormalForce[1] += contactForces[i][1] totalNormalForce[2] += contactForces[i][2] #print((totalMass*mm.s2v(wcfg.gravity))[1]) footCenterOffset = np.array([ viewer.objectInfoWnd.comOffsetX.value(), viewer.objectInfoWnd.comOffsetY.value(), viewer.objectInfoWnd.comOffsetZ.value() ]) ###################### # optimization terms ###################### # linear momentum CM_ref_plane = footCenter + footCenterOffset dL_des_plane = Kl * totalMass * (CM_ref_plane - CM_plane) - Dl * totalMass * dCM_plane dL_des_plane[1] = Kl * totalMass * (CM_ref[1] + footCenterOffset[1] - CM[1]) - Dl * totalMass * dCM[1] #dL_des_plane[1] = 0. #print 'dL_des_plane', dL_des_plane # angular momentum CP_ref = footCenter + footCenterOffset CP_ref[1] = 0. timeStep = 30. if (CP_old[0] is None) or (CP is None): dCP = None else: dCP = (CP - CP_old[0]) * timeStep CP_old[0] = CP if (CP is not None) and (dCP is not None): ddCP_des = Kh * (CP_ref - CP) - Dh * (dCP) CP_des = CP + dCP * (1 / timeStep) + .5 * ddCP_des * ( (1 / timeStep)**2) #print 'dCP: ', dCP #print 'ddCP_des: ', ddCP_des #print 'CP_des: ', CP_des #dH_des = np.cross((CP_des - CM), (dL_des_plane + totalMass*mm.s2v(wcfg.gravity))) dH_des = np.cross( (CP_des - CM_plane), (dL_des_plane + totalMass * mm.s2v(wcfg.gravity))) else: dH_des = None # momentum matrix RS = np.dot(P, Jsys) R, S = np.vsplit(RS, 2) rs = np.dot((np.dot(dP, Jsys) + np.dot(P, dJsys)), dth_flat) r_bias, s_bias = np.hsplit(rs, 2) flagContact = True if (dH_des is None) or np.any(np.isnan(dH_des)) == True: flagContact = False #viewer.doc.showRenderer('rd_grf_des', False) #viewer.motionViewWnd.update(1, viewer.doc) #else: #viewer.doc.showRenderer('rd_grf_des', True) #viewer.motionViewWnd.update(1, viewer.doc) ''' 0 : initial 1 : contact 2 : fly 3 : landing ''' #MOTION = FORWARD_JUMP if mit.MOTION == mit.FORWARD_JUMP: frame_index = [136, 100] #frame_index = [100000, 100000] elif mit.MOTION == mit.TAEKWONDO: frame_index = [130, 100] #frame_index = [100000, 100000] elif mit.MOTION == mit.TAEKWONDO2: frame_index = [130 + 40, 100] elif mit.MOTION == mit.WALK: frame_index = [10000, 60] elif mit.MOTION == mit.TIPTOE: frame_index = [1000000, 1000000] #frame_index = [10000, 165] else: frame_index = [1000000, 1000000] #MOTION = TAEKWONDO #frame_index = [135, 100] if frame > frame_index[0]: if stage != POWERFUL_BALANCING: print("#", frame, "-POWERFUL_BALANCING") stage = POWERFUL_BALANCING Kk = Kk * 2 Dk = 2 * (Kk**.5) elif frame > frame_index[1]: if stage != MOTION_TRACKING: print("#", frame, "-MOTION_TRACKING") stage = MOTION_TRACKING trackingW = w #if checkAll(contactFlagFootR, 0) != 1 : if 0: #stage == MOTION_TRACKING: trackingW = w2 #stage = POWERFUL_BALANCING Bt = Bt * 2 # optimization mot.addTrackingTerms(problem, totalDOF, Bt, trackingW, ddth_des_flat) #mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1, q_bias1) if flagContact == True: if stage != MOTION_TRACKING + 10: mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias) #mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias) # using || dH ||^2 instead mot.addAnotherTerms(problem, totalDOF, Bh, S, -(s_bias + Kh * np.dot(S, dth_flat))) a_sup_2 = None Jsup_2 = None dJsup_2 = None ############################## # Hard constraint Kk2 = Kk * 4.0 Dk2 = 2 * (Kk2**.5) ankleW = 0 ankleOffset = ankleW * curRelCMVec[2] metatarW = 0 metatarOffset = metatarW * curRelCMVec[2] ############################## ############################## # Additional constraint if stage != MOTION_TRACKING and frame > 5: # ankle strategy idx = 0 #LEFT/RIGHT_TOES if mit.FOOT_PART_NUM == 1: yOffset = 0.03 else: yOffset = 0.069 #yOffset = 0.06 # ankleOffset = (footCenter - CM_plane)*4. ankleOffset = footCenterOffset * 10. ankleOffset[1] = 0. #ankleOffset[2] = 0. ankleOffset[2] = ankleOffset[2] * 20. ankleOffsetL = ankleOffset.copy() ankleOffsetR = ankleOffset.copy() #ankleOffset= np.array((0,0,0)) if footCenterOffset[0] > 0.0: ankleOffsetL[0] = 0. else: ankleOffsetR[0] = 0. # print 'ankleOffset=', ankleOffset desLinearAccL, desPosL = getDesFootLinearAcc( motionModel, controlModel, indexFootL[idx], ModelOffset, CM_ref, CM, Kk, Dk, yOffset) #0.076) #0.14) desLinearAccR, desPosR = getDesFootLinearAcc( motionModel, controlModel, indexFootR[idx], ModelOffset, CM_ref, CM, Kk, Dk, yOffset) ax = [0, 0, -1] aaa = getBodyGlobalOri(controlModel, motion, 'RightFoot') #print np.dot(aaa, ax) if mit.FOOT_PART_NUM == 1: ax = [0, 1, 0] desAngularAccL = getDesFootAngularAcc( motionModel, controlModel, indexFootL[idx], Kk, Dk, ax, mm.normalize([0, 1, 0] + ankleOffsetL)) desAngularAccR = getDesFootAngularAcc( motionModel, controlModel, indexFootR[idx], Kk, Dk, ax, mm.normalize([0, 1, 0] + ankleOffsetR)) a_sup_2 = np.hstack((np.hstack((desLinearAccL, desAngularAccL)), np.hstack((desLinearAccR, desAngularAccR)))) Jsup_2 = np.vstack((jFootL[idx], jFootR[idx])) dJsup_2 = np.vstack((dJFootL[idx], dJFootR[idx])) #mot.addConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat, a_sup_2) #mot.addConstraint(problem, totalDOF, Jsup_2[:1], dJsup_2[:1], dth_flat, a_sup_2[:1]) #mot.addConstraint(problem, totalDOF, Jsup_2[2:], dJsup_2[2:], dth_flat, a_sup_2[2:]) #mot.addConstraint(problem, totalDOF, Jsup_2[3:], dJsup_2[3:], dth_flat, a_sup_2[3:]) mot.addAnotherTerms(problem, totalDOF, viewer.objectInfoWnd.Bc.value(), Jsup_2[3:], a_sup_2[3:] - np.dot(dJsup_2[3:], dth_flat)) #mot.addAnotherTerms(problem, totalDOF, viewer.objectInfoWnd.Bc.value(), Jsup_2, a_sup_2 - np.dot(dJsup_2, dth_flat)) #mot.addAnotherTerms(problem, totalDOF, 1.*viewer.objectInfoWnd.Bc.value(), Jsup_2[0:1], a_sup_2[0:1] - np.dot(dJsup_2[0:1] , dth_flat)) #mot.addAnotherTerms(problem, totalDOF, 1.*viewer.objectInfoWnd.Bc.value(), Jsup_2[2:], a_sup_2[2:] - np.dot(dJsup_2[2:] , dth_flat)) desCOMOffset = 0.0 rd_DesPosL[0] = desPosL.copy() rd_DesPosR[0] = desPosR.copy() if stage == STATIC_BALANCING and frame > 10: # and False: del rd_desPoints[:] # foot strategy #Kk2 = Kk * 2.5 #Kk2 = Kk * .2 #Dk2 = 2*(Kk2**.5) desForePosL = [0, 0, 0] desForePosR = [0, 0, 0] desRearPosL = [0, 0, 0] desRearPosR = [0, 0, 0] footPartPos = [] footPartPos.append( controlModel.getBodyPositionGlobal( motion[0].skeleton.getJointIndex('LeftCalcaneus_1'))) footPartPos.append( controlModel.getBodyPositionGlobal( motion[0].skeleton.getJointIndex('LeftPhalange_1'))) footPartPos.append( controlModel.getBodyPositionGlobal( motion[0].skeleton.getJointIndex('RightCalcaneus_1'))) footPartPos.append( controlModel.getBodyPositionGlobal( motion[0].skeleton.getJointIndex('RightPhalange_1'))) for i in range(1, footPartNum): contactFlagFootL[i] = 1 contactFlagFootR[i] = 1 SupPts = np.vstack( (np.array((footPartPos[0][0], footPartPos[1][0], footPartPos[2][0], footPartPos[3][0])), np.array( (footPartPos[0][2], footPartPos[1][2], footPartPos[2][2], footPartPos[3][2])), np.array((1., 1., 1., 1.)))) coordWidthLen = 2. coordLengthLen = 1.5 SupUV = np.vstack( (np.array((-coordWidthLen, -coordWidthLen, coordWidthLen, coordWidthLen)), np.array((-coordLengthLen, coordLengthLen, -coordLengthLen, coordLengthLen)), np.array((1., 1., 1., 1.)))) SupMap = np.dot(np.dot(SupUV, SupUV.T), np.linalg.inv(np.dot(SupPts, SupUV.T))) #print SupMap desFootCenter = footCenter + footCenterOffset footCenterPts = np.array((desFootCenter[0], desFootCenter[2], 1)) #print np.dot(SupMap, footCenterPts) #print np.dot(getBodyGlobalOri(controlModel, motion, 'LeftMetatarsal_1'), np.array((0,1,0))) CM_plane_2D = np.array((CM[0], CM[2], 1)) # CM_plane_UV = np.dot(SupMap, CM_plane_2D) CM_plane_UV = np.dot(SupMap, footCenterPts) # print CM_plane_UV # for i in range(1, footPartNum): if CM_plane_UV[1] > .5: # com is in front for i in range(1, 5): contactFlagFootL[i] = 0 contactFlagFootR[i] = 0 elif CM_plane_UV[1] < -.5: # com is back for i in range(3, footPartNum): contactFlagFootL[i] = 0 contactFlagFootR[i] = 0 else: # com is in middle position for i in range(3, 5): contactFlagFootL[i] = 0 contactFlagFootR[i] = 0 contactFlagFoot = contactFlagFootL if CM_plane_UV[0] < 0.: contactFlagFoot = contactFlagFootR # CM_plane_UV[0] = -CM_plane_UV[0] if abs(CM_plane_UV[0]) > 1.: for j in range(0, 3): contactFlagFoot[2 * j + 2] = 0 # print 'footL : ',contactFlagFootL # print 'footR : ',contactFlagFootR for i in range(1, footPartNum): axis = [0, 0, 1] if i == 1 or i == 2: axis = [0, 0, -1] desAng = [0, 0, 1] if i == 1 or i == 2: desAng = [0, 0, -1] desY = 0.029 if contactFlagFootL[i] == 1: desLinearAccL, desForePosL = getDesFootLinearAcc( motionModel, controlModel, indexFootL[i], ModelOffset, CM_ref, CM, Kk2, Dk2, desY) desAngularAccL = getDesFootAngularAcc( motionModel, controlModel, indexFootL[i], Kk2, Dk2, axis, desAng) a_sup_2 = np.hstack((desLinearAccL, desAngularAccL)) Jsup_2 = jFootL[i].copy() dJsup_2 = dJFootL[i].copy() mot.addConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat, a_sup_2) #mot.addAnotherTerms(problem, totalDOF, viewer.objectInfoWnd.Bc.value(), Jsup_2, a_sup_2 - np.dot(dJsup_2, dth_flat)) #mot.addAnotherTerms(problem, totalDOF, viewer.objectInfoWnd.Bc.value(), Jsup_2[3:], a_sup_2[3:] - np.dot(dJsup_2[3:] , dth_flat)) rd_desPoints.append(desForePosL.copy()) if contactFlagFootR[i] == 1: desLinearAccR, desForePosR = getDesFootLinearAcc( motionModel, controlModel, indexFootR[i], ModelOffset, CM_ref, CM, Kk2, Dk2, desY) desAngularAccR = getDesFootAngularAcc( motionModel, controlModel, indexFootR[i], Kk2, Dk2, axis, desAng) a_sup_2 = np.hstack((desLinearAccR, desAngularAccR)) Jsup_2 = jFootR[i].copy() dJsup_2 = dJFootR[i].copy() mot.addConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat, a_sup_2) #mot.addAnotherTerms(problem, totalDOF, viewer.objectInfoWnd.Bc.value(), Jsup_2, a_sup_2 - np.dot(dJsup_2, dth_flat)) #mot.addAnotherTerms(problem, totalDOF, viewer.objectInfoWnd.Bc.value(), Jsup_2[3:], a_sup_2[3:] - np.dot(dJsup_2[3:], dth_flat)) rd_desPoints.append(desForePosR.copy()) rd_DesForePosL[0] = desForePosL rd_DesForePosR[0] = desForePosR rd_DesRearPosL[0] = desRearPosL rd_DesRearPosR[0] = desRearPosR ############################## #if Jsup_2 is not None: # mot.addConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat, a_sup_2) timeReport[4] += time.time() - curTime curTime = time.time() r = problem.solve() #print frame #Ashape = np.shape(problem.A) #if len(Ashape) >0 : # for i in range(0, Ashape[0]): # print problem.A[i] #print problem.A[] #print problem.b #print r problem.clear() #print r['x'] ype.nested(r['x'], ddth_sol) #print ddth_sol rootPos[0] = controlModel.getBodyPositionGlobal(selectedBody) localPos = [[0, 0, 0]] ########################################### ##Jacobian Transpose control # COM Position control #fCom = Wcp*(pHatComDes - pHatCom) + Wcv*(vComDes - vCom) + Wcm*(footCenter_plane - CM_plane) w1 = 10 #10.1 w2 = 1 #1#2*(w1**.5) if frame > 100: w1 = 10.1 #10.1 w2 = 1 footToCMVec = CM - footCenter desCMPos = [footCenter[0], mm.length(footToCMVec), footCenter[2]] #print("desCMPos", desCMPos) #print("CM", CM) fCom = w1 * (desCMPos - CM) + w2 * (-dCM) #print("fCom", fCom) #fCom[0] = 0. #fCom[1] = 0 #fCom[2] = 0 rd_virtualForce[0] = fCom.copy() #hipPos = controlModel.getBodyPositionGlobal(rootB) headPos = controlModel.getBodyPositionGlobal(selectedBody) hipPos = controlModel.getBodyPositionGlobal(rootB) yjc.computeJacobian2(Jcom, DOFs, jointPositions, jointAxeses, [headPos], comUpperJointMasks) #yjc.computeJacobianDerivative2(dJcom, DOFs, jointPositions, jointAxeses, linkAngVelocities, [CM], comUpperJointMasks, False) JcomT = Jcom.T TauJT = np.dot(JcomT, fCom) # Angular Momentum Hc = ymt.getAngularMomentum(CM, linkInertias, linkAngVelocities, linkPositions, linkMasses, linkVelocities) Href = ymt.getAngularMomentum(CM_ref, linkInertias_ref, linkAngVelocities_ref, linkPositions_ref, linkMasses, linkVelocities_ref) Wam = .05 Tam = Wam * (Href - Hc) #print("Tam", Tam) yjc.computeAngJacobian2(JcomAng, DOFs, jointPositions, jointAxeses, [headPos], comUpperJointMasks) TauAM = np.dot(JcomAng.T, Tam) timeReport[5] += time.time() - curTime curTime = time.time() for i in range(stepsPerFrame): # apply penalty force bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds) #print frame, bodyIDs, contactPositions, contactPositionLocals, contactForces vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces) extraForce[0] = viewer.GetForce() if (extraForce[0][0] != 0 or extraForce[0][1] != 0 or extraForce[0][2] != 0): forceApplyFrame += 1 #vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce) controlModel.applyBodyForceGlobal(selectedBody, extraForce[0]) applyedExtraForce[0] = extraForce[0] if forceApplyFrame * wcfg.timeStep > 0.1: viewer.ResetForce() forceApplyFrame = 0 #print ddth_sol controlModel.setDOFAccelerations(ddth_sol) controlModel.solveHybridDynamics() vpWorld.step() #if frame%30==0: print 'elapsed time for 30 frames:', time.time()-pt[0] # rendering rd_footCenter[0] = footCenter rd_CM[0] = CM.copy() rd_CM_plane[0] = CM_plane.copy() rd_footCenter_ref[0] = footCenter_ref rd_CM_plane_ref[0] = CM_ref.copy() rd_CM_ref[0] = CM_ref.copy() rd_CM_ref_vec[0] = (CM_ref - footCenter_ref) * 3. rd_CM_vec[0] = (CM - CM_plane) rd_CM_des[0] = CM_ref_plane.copy() rd_CM_des[0][1] = .01 #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 rd_dH_des[0] = dH_des rd_grf_des[ 0] = totalNormalForce # - totalMass*mm.s2v(wcfg.gravity)#dL_des_plane - totalMass*mm.s2v(wcfg.gravity) rd_exf_des[0] = applyedExtraForce[0] rd_root_des[0] = rootPos[0] rd_CMP[0] = softConstPoint rd_soft_const_vec[0] = controlModel.getBodyPositionGlobal( constBody) - softConstPoint del rd_contactForces[:] del rd_contactPositions[:] if CP is not None: for i in range(len(lcpBodyIDs)): rd_contactForces.append(lcpContactForces[i].copy() / 200.) rd_contactPositions.append(lcpContactPositions[i].copy()) timeReport[6] += time.time() - curTime
def simulateCallback(frame): global g_initFlag global forceShowFrame global forceApplyFrame global JsysPre global JsupPreL global JsupPreR global JsupPre global softConstPoint global stage motionModel.update(motion[frame]) Kt, Kk, Kl, Kh, Ksc, Bt, Bl, Bh, Bsc = viewer.GetParam() Dt = 2 * (Kt**.5) Dk = 2 * (Kk**.5) Dl = 2 * (Kl**.5) Dh = 2 * (Kh**.5) Dsc = 2 * (Ksc**.5) if Bsc == 0.0: viewer.doc.showRenderer('softConstraint', False) viewer.motionViewWnd.update(1, viewer.doc) else: viewer.doc.showRenderer('softConstraint', True) renderer1 = viewer.doc.getRenderer('softConstraint') renderer1.rc.setLineWidth(0.1 + Bsc * 3) viewer.motionViewWnd.update(1, viewer.doc) # 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) ddth_c = controlModel.getDOFAccelerations() ype.flatten(ddth_des, ddth_des_flat) ype.flatten(dth, dth_flat) ype.flatten(ddth_c, ddth_c_flat) # jacobian ''' if stage == POWERFUL_BALANCING: #if stage != MOTION_TRACKING: footCenterL = controlModel.getBodyPositionGlobal(supL) footCenterR = controlModel.getBodyPositionGlobal(supR) else: footCenterL = controlModel.getBodyPositionGlobal(indexFootL[1]) footCenterR = controlModel.getBodyPositionGlobal(indexFootR[1]) ''' if footPartNum == 1: footCenterL = controlModel.getBodyPositionGlobal(supL) footCenterR = controlModel.getBodyPositionGlobal(supR) else: if stage == POWERFUL_BALANCING: footCenterL = controlModel.getBodyPositionGlobal(supL) footCenterR = controlModel.getBodyPositionGlobal(supR) else: footCenterL = ( controlModel.getBodyPositionGlobal(supL) + controlModel.getBodyPositionGlobal(indexFootL[1])) / 2.0 footCenterR = ( controlModel.getBodyPositionGlobal(supR) + controlModel.getBodyPositionGlobal(indexFootR[1])) / 2.0 refFootL = motionModel.getBodyPositionGlobal(supL) refFootR = motionModel.getBodyPositionGlobal(supR) footCenter = footCenterL + (footCenterR - footCenterL) / 2.0 footCenter[1] = 0. footCenter_ref = refFootL + (refFootR - refFootL) / 2.0 #footCenter_ref[1] = 0. positionFootL = [None] * footPartNum positionFootR = [None] * footPartNum for i in range(footPartNum): positionFootL[i] = controlModel.getBodyPositionGlobal( indexFootL[i]) positionFootR[i] = controlModel.getBodyPositionGlobal( indexFootR[i]) linkPositions = controlModel.getBodyPositionsGlobal() linkVelocities = controlModel.getBodyVelocitiesGlobal() linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal() linkInertias = controlModel.getBodyInertiasGlobal() jointPositions = controlModel.getJointPositionsGlobal() jointAxeses = controlModel.getDOFAxeses() CM = yrp.getCM(linkPositions, linkMasses, totalMass) dCM = yrp.getCM(linkVelocities, linkMasses, totalMass) CM_plane = copy.copy(CM) CM_plane[1] = 0. dCM_plane = copy.copy(dCM) dCM_plane[1] = 0. linkPositions_ref = motionModel.getBodyPositionsGlobal() CM_ref = yrp.getCM(linkPositions_ref, linkMasses, totalMass) CM_plane_ref = copy.copy(CM_ref) CM_plane_ref[1] = 0. P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM, linkInertias) dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses, linkVelocities, dCM, linkAngVelocities, linkInertias) yjc.computeJacobian2(Jsys, DOFs, jointPositions, jointAxeses, linkPositions, allLinkJointMasks) yjc.computeJacobianDerivative2(dJsys, DOFs, jointPositions, jointAxeses, linkAngVelocities, linkPositions, allLinkJointMasks) if g_initFlag == 0: softConstPoint = controlModel.getBodyPositionGlobal(constBody) softConstPoint[1] -= .3 g_initFlag = 1 yjc.computeJacobian2(jFootL[0], DOFs, jointPositions, jointAxeses, [positionFootL[0]], jointMasksFootL[0]) yjc.computeJacobianDerivative2(dJFootL[0], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootL[0]], jointMasksFootL[0], False) yjc.computeJacobian2(jFootR[0], DOFs, jointPositions, jointAxeses, [positionFootR[0]], jointMasksFootR[0]) yjc.computeJacobianDerivative2(dJFootR[0], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootR[0]], jointMasksFootR[0], False) yjc.computeAngJacobian2(jAngFootL[0], DOFs, jointPositions, jointAxeses, [positionFootL[0]], jointMasksFootL[0]) yjc.computeAngJacobianDerivative2(dJAngFootL[0], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootL[0]], jointMasksFootL[0], False) yjc.computeAngJacobian2(jAngFootR[0], DOFs, jointPositions, jointAxeses, [positionFootR[0]], jointMasksFootR[0]) yjc.computeAngJacobianDerivative2(dJAngFootR[0], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootR[0]], jointMasksFootR[0], False) bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds) CP = yrp.getCP(contactPositions, contactForces) for i in range(len(bodyIDsToCheck)): controlModel.SetBodyColor(bodyIDsToCheck[i], 0, 0, 0) contactFlagFootL = [0] * footPartNum contactFlagFootR = [0] * footPartNum for i in range(len(bodyIDs)): controlModel.SetBodyColor(bodyIDs[i], 255, 105, 105) index = controlModel.id2index(bodyIDs[i]) for j in range(len(indexFootL)): if index == indexFootL[j]: contactFlagFootL[j] = 1 if j != 0: yjc.computeJacobian2(jFootL[j], DOFs, jointPositions, jointAxeses, [positionFootL[j]], jointMasksFootL[j]) yjc.computeJacobianDerivative2( dJFootL[j], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootL[j]], jointMasksFootL[j], False) break for j in range(len(indexFootR)): if index == indexFootR[j]: contactFlagFootR[j] = 1 if j != 0: yjc.computeJacobian2(jFootR[j], DOFs, jointPositions, jointAxeses, [positionFootR[j]], jointMasksFootR[j]) yjc.computeJacobianDerivative2( dJFootR[j], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootR[j]], jointMasksFootR[j], False) break for j in range(len(indexFootL)): yjc.computeAngJacobian2(jAngFootL[j], DOFs, jointPositions, jointAxeses, [positionFootL[j]], jointMasksFootL[j]) yjc.computeAngJacobianDerivative2(dJAngFootL[j], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootL[j]], jointMasksFootL[j], False) yjc.computeAngJacobian2(jAngFootR[j], DOFs, jointPositions, jointAxeses, [positionFootR[j]], jointMasksFootR[j]) yjc.computeAngJacobianDerivative2(dJAngFootR[j], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootR[j]], jointMasksFootR[j], False) # if checkAll(contactFlagFootL, 0) == 1 and checkAll( contactFlagFootR, 0) == 1: footCenter = footCenter elif checkAll(contactFlagFootL, 0) == 1: footCenter = footCenterR elif checkAll(contactFlagFootR, 0) == 1: footCenter = footCenterL footCenter[1] = 0. desForeSupLAcc = [0, 0, 0] desForeSupRAcc = [0, 0, 0] totalNormalForce = [0, 0, 0] for i in range(len(contactForces)): totalNormalForce[0] += contactForces[i][0] totalNormalForce[1] += contactForces[i][1] totalNormalForce[2] += contactForces[i][2] # linear momentum CM_ref_plane = footCenter dL_des_plane = Kl * totalMass * (CM_ref_plane - CM_plane) - Dl * totalMass * dCM_plane # angular momentum CP_ref = footCenter timeStep = 30. if CP_old[0] == None or CP == None: dCP = None else: dCP = (CP - CP_old[0]) / (1 / timeStep) CP_old[0] = CP if CP != None and dCP != None: ddCP_des = Kh * (CP_ref - CP) - Dh * (dCP) CP_des = CP + dCP * (1 / timeStep) + .5 * ddCP_des * ( (1 / timeStep)**2) dH_des = np.cross( (CP_des - CM), (dL_des_plane + totalMass * mm.s2v(wcfg.gravity))) #dH_des = np.cross((CP_des - CM_plane), (dL_des_plane + totalMass*mm.s2v(wcfg.gravity))) else: dH_des = None # momentum matrix RS = np.dot(P, Jsys) R, S = np.vsplit(RS, 2) rs = np.dot((np.dot(dP, Jsys) + np.dot(P, dJsys)), dth_flat) r_bias, s_bias = np.hsplit(rs, 2) ############################## # soft point constraint P_des = softConstPoint P_cur = controlModel.getBodyPositionGlobal(constBody) dP_des = [0, 0, 0] dP_cur = controlModel.getBodyVelocityGlobal(constBody) ddP_des1 = Ksc * (P_des - P_cur) - Dsc * (dP_cur - dP_des) r = P_des - P_cur I = np.vstack(([1, 0, 0], [0, 1, 0], [0, 0, 1])) Z = np.hstack((I, mm.getCrossMatrixForm(-r))) yjc.computeJacobian2(Jconst, DOFs, jointPositions, jointAxeses, [softConstPoint], constJointMasks) JL, JA = np.vsplit(Jconst, 2) Q1 = np.dot(Z, Jconst) q1 = np.dot(JA, dth_flat) q2 = np.dot(mm.getCrossMatrixForm(q1), np.dot(mm.getCrossMatrixForm(q1), r)) yjc.computeJacobianDerivative2(dJconst, DOFs, jointPositions, jointAxeses, linkAngVelocities, [softConstPoint], constJointMasks, False) q_bias1 = np.dot(np.dot(Z, dJconst), dth_flat) + q2 ############################## flagContact = True if dH_des == None or np.any(np.isnan(dH_des)) == True: flagContact = False viewer.doc.showRenderer('rd_grf_des', False) viewer.motionViewWnd.update(1, viewer.doc) else: viewer.doc.showRenderer('rd_grf_des', True) viewer.motionViewWnd.update(1, viewer.doc) ''' 0 : initial 1 : contact 2 : fly 3 : landing ''' #MOTION = FORWARD_JUMP if mit.MOTION == mit.FORWARD_JUMP: frame_index = [136, 100] #frame_index = [100000, 100000] elif mit.MOTION == mit.TAEKWONDO: frame_index = [130, 100] #frame_index = [100000, 100000] else: frame_index = [1000000, 1000000] #MOTION = TAEKWONDO #frame_index = [135, 100] ''' if frame > 300 : if stage != DYNAMIC_BALANCING: print("#", frame,"-DYNAMIC_BALANCING") stage = DYNAMIC_BALANCING Kk = Kk*1 Dk = 2*(Kk**.5) ''' if frame > frame_index[0]: if stage != POWERFUL_BALANCING: print("#", frame, "-POWERFUL_BALANCING") stage = POWERFUL_BALANCING Kk = Kk * 2 Dk = 2 * (Kk**.5) elif frame > frame_index[1]: if stage != MOTION_TRACKING: print("#", frame, "-MOTION_TRACKING") stage = MOTION_TRACKING trackingW = w if stage == MOTION_TRACKING: trackingW = w2 Bt = Bt * 2 # optimization mot.addTrackingTerms(problem, totalDOF, Bt, trackingW, ddth_des_flat) mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1, q_bias1) if flagContact == True: if stage != MOTION_TRACKING + 10: mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias) mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias) a_sup_2 = [None] Jsup_2 = [None] dJsup_2 = [None] ############################## # Hard constraint if stage != MOTION_TRACKING: Kk2 = Kk * 2.0 else: Kk2 = Kk * 1.5 Dk2 = 2 * (Kk2**.5) ''' desLinearAccL, desPosL = getDesFootLinearAcc(motionModel, controlModel, supL, ModelOffset, CM_ref, CM, Kk2, Dk2) desLinearAccR, desPosR = getDesFootLinearAcc(motionModel, controlModel, supR, ModelOffset, CM_ref, CM, Kk2, Dk2) desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, supL, Kk2, Dk2) desAngularAccR = getDesFootAngularAcc(motionModel, controlModel, supR, Kk2, Dk2) ''' if stage != MOTION_TRACKING: idx = 0 #LEFT/RIGHT_TOES desLinearAccL, desPosL = getDesFootLinearAcc( motionModel, controlModel, indexFootL[idx], ModelOffset, CM_ref, CM, Kk2, Dk2) desLinearAccR, desPosR = getDesFootLinearAcc( motionModel, controlModel, indexFootR[idx], ModelOffset, CM_ref, CM, Kk2, Dk2) desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[idx], Kk2, Dk2) desAngularAccR = getDesFootAngularAcc(motionModel, controlModel, indexFootR[idx], Kk2, Dk2) a_sup_2 = np.hstack((np.hstack((desLinearAccL, desAngularAccL)), np.hstack((desLinearAccR, desAngularAccR)))) Jsup_2 = np.vstack((jFootL[idx], jFootR[idx])) dJsup_2 = np.vstack((dJFootL[idx], dJFootR[idx])) rd_DesPosL[0] = desPosL.copy() rd_DesPosR[0] = desPosR.copy() else: if footPartNum != 1: idx = 1 else: idx = 0 desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[idx], Kk2, Dk2) desAngularAccR = getDesFootAngularAcc(motionModel, controlModel, indexFootR[idx], Kk2, Dk2) a_sup_2 = np.hstack((desAngularAccL, desAngularAccR)) Jsup_2 = np.vstack((jAngFootL[idx], jAngFootR[idx])) dJsup_2 = np.vstack((dJAngFootL[idx], dJAngFootR[idx])) ############################## ############################## # Additional constraint if stage != MOTION_TRACKING: Kk2 = Kk * 1.5 Dk2 = 2 * (Kk2**.5) desForePosL = [0, 0, 0] desForePosR = [0, 0, 0] desRearPosL = [0, 0, 0] desRearPosR = [0, 0, 0] for i in range(1, footPartNum): if contactFlagFootL[i] == 1: desLinearAccL, desForePosL = getDesFootLinearAcc( motionModel, controlModel, indexFootL[i], ModelOffset, CM_ref, CM, Kk2, Dk2) desAngularAccL = getDesFootAngularAcc( motionModel, controlModel, indexFootL[i], Kk2, Dk2) a_sup_2 = np.hstack( (a_sup_2, np.hstack((desLinearAccL, desAngularAccL)))) Jsup_2 = np.vstack((Jsup_2, jFootL[i])) dJsup_2 = np.vstack((dJsup_2, dJFootL[i])) if contactFlagFootR[i] == 1: desLinearAccR, desForePosR = getDesFootLinearAcc( motionModel, controlModel, indexFootR[i], ModelOffset, CM_ref, CM, Kk2, Dk2) desAngularAccR = getDesFootAngularAcc( motionModel, controlModel, indexFootR[i], Kk2, Dk2) a_sup_2 = np.hstack( (a_sup_2, np.hstack((desLinearAccR, desAngularAccR)))) Jsup_2 = np.vstack((Jsup_2, jFootR[i])) dJsup_2 = np.vstack((dJsup_2, dJFootR[i])) rd_DesForePosL[0] = desForePosL rd_DesForePosR[0] = desForePosR rd_DesRearPosL[0] = desRearPosL rd_DesRearPosR[0] = desRearPosR ############################## mot.setConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat, a_sup_2) r = problem.solve() problem.clear() ype.nested(r['x'], ddth_sol) rootPos[0] = controlModel.getBodyPositionGlobal(selectedBody) localPos = [[0, 0, 0]] for i in range(stepsPerFrame): # apply penalty force bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds) vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces) extraForce[0] = viewer.GetForce() if (extraForce[0][0] != 0 or extraForce[0][1] != 0 or extraForce[0][2] != 0): forceApplyFrame += 1 #vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce) controlModel.applyBodyForceGlobal(selectedBody, extraForce[0]) applyedExtraForce[0] = extraForce[0] if forceApplyFrame * wcfg.timeStep > 0.1: viewer.ResetForce() forceApplyFrame = 0 controlModel.setDOFAccelerations(ddth_sol) controlModel.solveHybridDynamics() ''' extraForce[0] = viewer.GetForce() if (extraForce[0][0] != 0 or extraForce[0][1] != 0 or extraForce[0][2] != 0) : forceApplyFrame += 1 vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce) applyedExtraForce[0] = extraForce[0] if forceApplyFrame*wcfg.timeStep > 0.1: viewer.ResetForce() forceApplyFrame = 0 ''' vpWorld.step() # rendering rd_footCenter[0] = footCenter rd_CM[0] = CM.copy() rd_CM_plane[0] = CM_plane.copy() rd_footCenter_ref[0] = footCenter_ref rd_CM_plane_ref[0] = CM_ref.copy() rd_CM_ref[0] = CM_ref.copy() rd_CM_ref_vec[0] = (CM_ref - footCenter_ref) * 3. rd_CM_vec[0] = (CM - footCenter) * 3 #rd_CM_plane[0][1] = 0. if CP != None and dCP != None: rd_CP[0] = CP rd_CP_des[0] = CP_des rd_dL_des_plane[0] = dL_des_plane rd_dH_des[0] = dH_des rd_grf_des[0] = totalNormalForce - totalMass * mm.s2v( wcfg.gravity) #dL_des_plane - totalMass*mm.s2v(wcfg.gravity) rd_exf_des[0] = applyedExtraForce[0] rd_root_des[0] = rootPos[0] rd_CMP[0] = softConstPoint rd_soft_const_vec[0] = controlModel.getBodyPositionGlobal( constBody) - softConstPoint if (forceApplyFrame == 0): applyedExtraForce[0] = [0, 0, 0]
def simulateCallback(frame): motionModel.update(motion[frame]) # 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) ''' th2 = controlModel2.getDOFPositions() dth2 = controlModel2.getDOFVelocities() ddth_des2 = yct.getDesiredDOFAccelerations(th_r, th2, dth_r, dth2, ddth_r, Kt, Dt) ''' #ype.flatten(ddth_des, ddth_des_flat) #ype.flatten(dth, dth_flat) #Control #tracking control #print(Kt, Dt) #Tpd = yct.getDesiredDOFTorques(th_r, th, dth_r, dth, 100.0, 0.1)#0.65, 0.031) linkPositions = controlModel.getBodyPositionsGlobal() linkVelocities = controlModel.getBodyVelocitiesGlobal() linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal() linkInertias = controlModel.getBodyInertiasGlobal() jointPositions = controlModel.getJointPositionsGlobal() jointAxeses = controlModel.getDOFAxeses() CM = yrp.getCM(linkPositions, linkMasses, totalMass) dCM = yrp.getCM(linkVelocities, linkMasses, totalMass) footCenter1 = controlModel.getBodyPositionGlobal(sup) footCenter2 = controlModel.getBodyPositionGlobal(sup2) footCenter = (footCenter1 + footCenter2) / 2 ''' yjc.computeJacobian2(Jt, DOFs, jointPositions, jointAxeses, [CM], CMJointMask) pHatCom = CM - footCenter vCom = dCM ''' CM_plane = copy.copy(CM) CM_plane[1] = 0. dCM_plane = copy.copy(dCM) dCM_plane[1] = 0. CM_ref_plane = footCenter dL_des_plane = Kl * totalMass * (CM_ref_plane - CM_plane) - Dl * totalMass * dCM_plane dL_des_plane[1] = 0. CP_ref = footCenter bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds) CP = yrp.getCP(contactPositions, contactForces) if CP_old[0] == None or CP == None: dCP = None else: dCP = (CP - CP_old[0]) / (1 / 30.) CP_old[0] = CP if CP != None and dCP != None: ddCP_des = Kh * (CP_ref - CP) - Dh * (dCP) CP_des = CP + dCP * (1 / 30.) + .5 * ddCP_des * ((1 / 30.)**2) dH_des = np.cross( (CP_des - CM), (dL_des_plane - totalMass * mm.s2v(wcfg.gravity))) else: dH_des = None ################ ''' linkPositions = motionModel.getBodyPositionsGlobal() linkVelocities = motionModel.getBodyVelocitiesGlobal() linkAngVelocities = motionModel.getBodyAngVelocitiesGlobal() linkInertias = motionModel.getBodyInertiasGlobal() CM2 = yrp.getCM(linkPositions, linkMasses, totalMass) dCM2 = yrp.getCM(linkVelocities, linkMasses, totalMass) footCenter1 = motionModel.getBodyPositionGlobal(sup) footCenter2 = motionModel.getBodyPositionGlobal(sup2) footCenter = (footCenter1+footCenter2)/2 pHatComDes = CM2 - footCenter vComDes = dCM2 Wcp = -750 Wcv = -10 fCom = Wcp*(pHatComDes - pHatCom) + Wcv*(vComDes - vCom) ''' #print("VirtualForce", fCom) #fCom[0] = 0. #fCom[1] = 0. #fCom[2] = -20. #fCom = [0., 0., 100.] #print("VirtualForce", fCom) for i in range(stepsPerFrame): # apply penalty force bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds) vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces) controlModel.setDOFAccelerations(ddth_des) linkPositions = controlModel.getBodyPositionsGlobal() linkVelocities = controlModel.getBodyVelocitiesGlobal() linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal() linkInertias = controlModel.getBodyInertiasGlobal() jointPositions = controlModel.getJointPositionsGlobal() jointAxeses = controlModel.getDOFAxeses() CM = yrp.getCM(linkPositions, linkMasses, totalMass) dCM = yrp.getCM(linkVelocities, linkMasses, totalMass) footCenter1 = controlModel.getBodyPositionGlobal(sup) footCenter2 = controlModel.getBodyPositionGlobal(sup2) footCenter = (footCenter1 + footCenter2) / 2 CM_plane = copy.copy(CM) CM_plane[1] = 0. footCenter_plane = copy.copy(footCenter) footCenter_plane[1] = 0. yjc.computeJacobian2(Jt, DOFs, jointPositions, jointAxeses, [CM], CMJointMask) pHatCom = CM - footCenter vCom = dCM linkPositions2 = motionModel.getBodyPositionsGlobal() linkVelocities2 = motionModel.getBodyVelocitiesGlobal() linkAngVelocities2 = motionModel.getBodyAngVelocitiesGlobal() linkInertias2 = motionModel.getBodyInertiasGlobal() CM2 = yrp.getCM(linkPositions2, linkMasses, totalMass) dCM2 = yrp.getCM(linkVelocities2, linkMasses, totalMass) footCenter1 = motionModel.getBodyPositionGlobal(sup) footCenter2 = motionModel.getBodyPositionGlobal(sup2) footCenter = (footCenter1 + footCenter2) / 2 pHatComDes = CM2 - footCenter vComDes = dCM2 Wcp = 0 Wcv = 0 Wcm = 0 ''' 0 : initial 1 : contact 2 : fly 3 : landing ''' global stage if len(contactForces) == 0: if stage == 1: stage = 2 print("fly") else: if stage == 0: stage = 1 print("contact") elif stage == 2: stage = 3 print("landing") Wcp = -550 Wcv = -100 Wcm = 0 if stage == 1: Wcp = -550 Wcv = -100 Wcm = 0 #Wcp = -550 #Wcv = -100 elif stage == 3: #Wcp = -950 #Wcv = -300 Wcp = -950 Wcv = -300 Wcm = 0 # COM Position control fCom = Wcp * (pHatComDes - pHatCom) + Wcv * ( vComDes - vCom) + Wcm * (footCenter_plane - CM_plane) if len(contactForces) == 0: fCom[0] = 0. fCom[1] = -10. #-10 fCom[2] = 0. ''' fCom[0] = 10. fCom[1] = 0. fCom[2] = 250. ''' # Angular Momentum control L_ref = 0 L_con = 0 #R,i for i in range(1, controlModel.getBodyNum()): L_ref += ( linkMasses[i] * np.cross(linkPositions2[i] - [CM], linkVelocities2[i]) + linkInertias2[i] * linkAngVelocities2[i]) L_con += ( linkMasses[i] * np.cross(linkPositions[i] - [CM], linkVelocities[i]) + linkInertias[i] * linkAngVelocities[i]) for iJoint in range(1, 7): # from 'LeftUpLeg'(1) to 'RightFoot'(6) JpT1 = (Jt[0][6 + 3 * (iJoint - 1)], Jt[1][6 + 3 * (iJoint - 1)], Jt[2][6 + 3 * (iJoint - 1)]) JpT2 = (Jt[0][6 + 3 * (iJoint - 1) + 1], Jt[1][6 + 3 * (iJoint - 1) + 1], Jt[2][6 + 3 * (iJoint - 1) + 1]) JpT3 = (Jt[0][6 + 3 * (iJoint - 1) + 2], Jt[1][6 + 3 * (iJoint - 1) + 2], Jt[2][6 + 3 * (iJoint - 1) + 2]) Tfi = (np.dot(JpT1, fCom), np.dot(JpT2, fCom), np.dot(JpT3, fCom)) currentT = controlModel.getJointAngAccelerationLocal(iJoint) controlModel.setJointAngAccelerationLocal( iJoint, currentT + Tfi) currentT = controlModel.getJointAngAccelerationLocal(0) #print(currentT) #JpT = ( Jt[0][0], Jt[1][0], Jt[2][0]) #Tfi = JpT*fCom #controlModel.setJointAngAccelerationLocal(0, currentT+Tfi) ''' if (len(contactForces) != 0) : for iJoint in range(1, 7): # from 'LeftUpLeg'(1) to 'RightFoot'(6) JpT = ( Jt[0][6+3*(iJoint-1)], Jt[1][6+3*(iJoint-1)], Jt[2][6+3*(iJoint-1)]) Tfi = JpT*fCom currentT = controlModel.getJointAngAccelerationLocal(iJoint) controlModel.setJointAngAccelerationLocal(iJoint, currentT+Tfi) else: print("No Contact force!!") ''' controlModel.solveHybridDynamics() vpWorld.step() # rendering rd_CM[0] = CM rd_CM_plane[0] = CM.copy() rd_CM_plane[0][1] = 0. if CP != None and dCP != None: rd_CP[0] = CP rd_CP_des[0] = CP_des rd_dL_des_plane[0] = dL_des_plane rd_dH_des[0] = dH_des rd_grf_des[0] = dL_des_plane - totalMass * mm.s2v(wcfg.gravity) rd_vf[0] = fCom
def simulateCallback(frame): # seginfo segIndex = seg_index[0] curState = seginfo[segIndex]['state'] curInterval = yma.offsetInterval(acc_offset[0], seginfo[segIndex]['interval']) stanceLegs = seginfo[segIndex]['stanceHips'] swingLegs = seginfo[segIndex]['swingHips'] stanceFoots = seginfo[segIndex]['stanceFoots'] swingFoots = seginfo[segIndex]['swingFoots'] swingKnees = seginfo[segIndex]['swingKnees'] groundHeight = seginfo[segIndex]['ground_height'] # maxStfPushFrame = seginfo[segIndex]['max_stf_push_frame'] prev_frame = frame-1 if frame>0 else 0 # prev_frame = frame # information # dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(frame), bodyMasses, upperMass, uppers) # CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(frame), bodyMasses, upperMass, uppers) ## dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(frame), bodyMasses, totalMass) ## CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(frame), bodyMasses, totalMass) # stf_tar = motion_seg.getJointPositionGlobal(stanceFoots[0], frame) # CMr_tar = CM_tar - stf_tar dCM_tar = motion_seg.getJointVelocityGlobal(0, prev_frame) CM_tar = motion_seg.getJointPositionGlobal(0, prev_frame) # dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(prev_frame), bodyMasses, upperMass, uppers) # CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(prev_frame), bodyMasses, upperMass, uppers) # dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(prev_frame), bodyMasses, totalMass) # CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(prev_frame), bodyMasses, totalMass) stf_tar = motion_seg.getJointPositionGlobal(stanceFoots[0], prev_frame) CMr_tar = CM_tar - stf_tar dCM = avg_dCM[0] CM = controlModel.getJointPositionGlobal(0) # CM = yrp.getCM(controlModel.getJointPositionsGlobal(), bodyMasses, upperMass, uppers) # CM = yrp.getCM(controlModel.getJointPositionsGlobal(), bodyMasses, totalMass) CMreal = yrp.getCM(controlModel.getJointPositionsGlobal(), bodyMasses, totalMass) stf = controlModel.getJointPositionGlobal(stanceFoots[0]) CMr = CM - stf diff_dCM = mm.projectionOnPlane(dCM-dCM_tar, (1,0,0), (0,0,1)) diff_dCM_axis = np.cross((0,1,0), diff_dCM) rd_vec1[0] = diff_dCM; rd_vecori1[0] = CM_tar diff_CMr = mm.projectionOnPlane(CMr-CMr_tar, (1,0,0), (0,0,1)) # rd_vec1[0] = diff_CMr; rd_vecori1[0] = stf_tar diff_CMr_axis = np.cross((0,1,0), diff_CMr) direction = mm.normalize2(mm.projectionOnPlane(dCM_tar, (1,0,0), (0,0,1))) # direction = mm.normalize2(mm.projectionOnPlane(dCM, (1,0,0), (0,0,1))) directionAxis = np.cross((0,1,0), direction) diff_dCM_sag, diff_dCM_cor = mm.projectionOnVector2(diff_dCM, direction) # rd_vec1[0] = diff_dCM_sag; rd_vecori1[0] = CM_tar diff_dCM_sag_axis = np.cross((0,1,0), diff_dCM_sag) diff_dCM_cor_axis = np.cross((0,1,0), diff_dCM_cor) diff_CMr_sag, diff_CMr_cor = mm.projectionOnVector2(diff_CMr, direction) diff_CMr_sag_axis = np.cross((0,1,0), diff_CMr_sag) diff_CMr_cor_axis = np.cross((0,1,0), diff_CMr_cor) t = (frame-curInterval[0])/float(curInterval[1]-curInterval[0]) t_raw = t if t>1.: t=1. p_root = motion_stitch[frame].getJointPositionGlobal(0) R_root = motion_stitch[frame].getJointOrientationGlobal(0) motion_seg_orig.goToFrame(frame) motion_seg.goToFrame(frame) motion_stitch.goToFrame(frame) motion_debug1.append(motion_stitch[frame].copy()) motion_debug1.goToFrame(frame) motion_debug2.append(motion_stitch[frame].copy()) motion_debug2.goToFrame(frame) motion_debug3.append(motion_stitch[frame].copy()) motion_debug3.goToFrame(frame) # paper implementation M_tc.append(motion_stitch[prev_frame]) M_tc.goToFrame(frame) P_hat.append(M_tc[frame].copy()) P_hat.goToFrame(frame) p_temp = ym.JointPosture(skeleton) p_temp.rootPos = controlModel.getJointPositionGlobal(0) p_temp.setJointOrientationsLocal(controlModel.getJointOrientationsLocal()) P.append(p_temp) P.goToFrame(frame) # stance foot stabilize motion_stf_stabilize.append(motion_stitch[frame].copy()) motion_stf_stabilize.goToFrame(frame) if STANCE_FOOT_STABILIZE: for stanceFoot in stanceFoots: R_target_foot = motion_seg[frame].getJointOrientationGlobal(stanceFoot) R_current_foot = motion_stf_stabilize[frame].getJointOrientationGlobal(stanceFoot) motion_stf_stabilize[frame].setJointOrientationGlobal(stanceFoot, cm.slerp(R_current_foot, R_target_foot , stf_stabilize_func(t))) # R_target_foot = motion_seg[frame].getJointOrientationLocal(stanceFoot) # R_current_foot = motion_stf_stabilize[frame].getJointOrientationLocal(stanceFoot) # motion_stf_stabilize[frame].setJointOrientationLocal(stanceFoot, cm.slerp(R_current_foot, R_target_foot , stf_stabilize_func(t))) # match stance leg motion_match_stl.append(motion_stf_stabilize[frame].copy()) motion_match_stl.goToFrame(frame) if MATCH_STANCE_LEG: if curState!=yba.GaitState.STOP: for i in range(len(stanceLegs)): stanceLeg = stanceLegs[i] stanceFoot = stanceFoots[i] # # motion stance leg -> character stance leg as time goes R_motion = motion_match_stl[frame].getJointOrientationGlobal(stanceLeg) R_character = controlModel.getJointOrientationGlobal(stanceLeg) motion_match_stl[frame].setJointOrientationGlobal(stanceLeg, cm.slerp(R_motion, R_character, match_stl_func(t))) # t_y = match_stl_func_y(t) # t_xz = match_stl_func(t) # # R_motion = motion_match_stl[frame].getJointOrientationGlobal(stanceLeg) # R_character = controlModel.getJointOrientationGlobal(stanceLeg) # R = np.dot(R_character, R_motion.T) # R_y, R_xz = mm.projectRotation((0,1,0), R) # motion_match_stl[frame].mulJointOrientationGlobal(stanceLeg, mm.scaleSO3(R_xz, t_xz)) # motion_match_stl[frame].mulJointOrientationGlobal(stanceLeg, mm.scaleSO3(R_y, t_y)) # swing foot placement motion_swf_placement.append(motion_match_stl[frame].copy()) motion_swf_placement.goToFrame(frame) if SWING_FOOT_PLACEMENT: t_swing_foot_placement = swf_placement_func(t); if extended[0]: R_swp_sag = prev_R_swp[0][0] R_swp_cor = prev_R_swp[0][1] else: R_swp_sag = mm.I_SO3(); R_swp_cor = mm.I_SO3() R_swp_cor = np.dot(R_swp_cor, mm.exp(diff_dCM_cor_axis * K_swp_vel_cor * -t_swing_foot_placement)) if np.dot(direction, diff_CMr_sag) < 0: R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_dCM_sag_axis * K_swp_vel_sag * -t_swing_foot_placement)) R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_CMr_sag_axis * K_swp_pos_sag * -t_swing_foot_placement)) else: R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_dCM_sag_axis * K_swp_vel_sag_faster * -t_swing_foot_placement)) R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_CMr_sag_axis * K_swp_pos_sag_faster * -t_swing_foot_placement)) R_swp_cor = np.dot(R_swp_cor, mm.exp(diff_CMr_cor_axis * K_swp_pos_cor * -t_swing_foot_placement)) for i in range(len(swingLegs)): swingLeg = swingLegs[i] swingFoot = swingFoots[i] # save swing foot global orientation # R_swf = motion_swf_placement[frame].getJointOrientationGlobal(swingFoot) # rotate swing leg motion_swf_placement[frame].mulJointOrientationGlobal(swingLeg, R_swp_sag) motion_swf_placement[frame].mulJointOrientationGlobal(swingLeg, R_swp_cor) # restore swing foot global orientation # motion_swf_placement[frame].setJointOrientationGlobal(swingFoot, R_swf) prev_R_swp[0] = (R_swp_sag, R_swp_cor) # swing foot height motion_swf_height.append(motion_swf_placement[frame].copy()) motion_swf_height.goToFrame(frame) if SWING_FOOT_HEIGHT: for swingFoot in swingFoots: stanceFoot = stanceFoots[0] # save foot global orientation R_foot = motion_swf_height[frame].getJointOrientationGlobal(swingFoot) R_stance_foot = motion_swf_height[frame].getJointOrientationGlobal(stanceFoot) if OLD_SWING_HEIGHT: height_tar = motion_swf_height[frame].getJointPositionGlobal(swingFoot)[1] - motion_swf_height[frame].getJointPositionGlobal(stanceFoot)[1] else: height_tar = motion_swf_height[prev_frame].getJointPositionGlobal(swingFoot)[1] - groundHeight d_height_tar = motion_swf_height.getJointVelocityGlobal(swingFoot, prev_frame)[1] # height_tar += c_swf_mid_offset * swf_height_sine_func(t) # motion_debug1[frame] = motion_swf_height[frame].copy() # rotate motion_swf_height[frame].rotateByTarget(controlModel.getJointOrientationGlobal(0)) # motion_debug2[frame] = motion_swf_height[frame].copy() # motion_debug2[frame].translateByTarget(controlModel.getJointPositionGlobal(0)) if OLD_SWING_HEIGHT: height_cur = motion_swf_height[frame].getJointPositionGlobal(swingFoot)[1] - motion_swf_height[frame].getJointPositionGlobal(stanceFoot)[1] else: height_cur = controlModel.getJointPositionGlobal(swingFoot)[1] - halfFootHeight - c_swf_offset d_height_cur = controlModel.getJointVelocityGlobal(swingFoot)[1] if OLD_SWING_HEIGHT: offset_height = (height_tar - height_cur) * swf_height_func(t) * c5 else: offset_height = ((height_tar - height_cur) * c5 + (d_height_tar - d_height_cur) * c6) * swf_height_func(t) offset_sine = c_swf_mid_offset * swf_height_sine_func(t) # offset_sine = 0. offset = 0. offset += offset_height offset += offset_sine if offset > 0.: newPosition = motion_swf_height[frame].getJointPositionGlobal(swingFoot) newPosition[1] += offset aik.ik_analytic(motion_swf_height[frame], swingFoot, newPosition) else: if HIGHER_OFFSET: newPosition = motion_swf_height[frame].getJointPositionGlobal(stanceFoot) newPosition[1] -= offset aik.ik_analytic(motion_swf_height[frame], stanceFoot, newPosition) # return # motion_debug3[frame] = motion_swf_height[frame].copy() # motion_debug3[frame].translateByTarget(controlModel.getJointPositionGlobal(0)) motion_swf_height[frame].rotateByTarget(R_root) # restore foot global orientation motion_swf_height[frame].setJointOrientationGlobal(swingFoot, R_foot) motion_swf_height[frame].setJointOrientationGlobal(stanceFoot, R_stance_foot) if plot!=None: plot.addDataPoint('debug1', frame, offset_height) plot.addDataPoint('debug2', frame, height_tar - height_cur) # plot.addDataPoint('diff', frame, diff) # swing foot orientation motion_swf_orientation.append(motion_swf_height[frame].copy()) motion_swf_orientation.goToFrame(frame) if SWING_FOOT_ORIENTATION: swf_orientation_func = yfg.concatenate([yfg.zero, yfg.hermite2nd, yfg.one], [.25, .75]) for swingFoot in swingFoots: R_target_foot = motion_seg[curInterval[1]].getJointOrientationGlobal(swingFoot) R_current_foot = motion_swf_orientation[frame].getJointOrientationGlobal(swingFoot) motion_swf_orientation[frame].setJointOrientationGlobal(swingFoot, cm.slerp(R_current_foot, R_target_foot, swf_orientation_func(t))) # swf_stabilize_func = yfg.concatenate([yfg.hermite2nd, yfg.one], [c_taking_duration]) # push orientation # for swingFoot in swingFoots: # R_target_foot = motion_seg[frame].getJointOrientationGlobal(swingFoot) # R_current_foot = motion_swf_orientation[frame].getJointOrientationGlobal(swingFoot) # motion_swf_orientation[frame].setJointOrientationGlobal(swingFoot, cm.slerp(R_current_foot, R_target_foot , swf_stabilize_func(t))) # stance foot push motion_stf_push.append(motion_swf_orientation[frame].copy()) motion_stf_push.goToFrame(frame) if STANCE_FOOT_PUSH: for swingFoot in swingFoots: # max_t = (maxStfPushFrame)/float(curInterval[1]-curInterval[0]) # stf_push_func = yfg.concatenate([yfg.sine, yfg.zero], [max_t*2]) stf_push_func = yfg.concatenate([yfg.sine, yfg.zero], [c_taking_duration*2]) R_swp_sag = mm.I_SO3() # R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_dCM_sag_axis * K_stp_vel * -stf_push_func(t))) # if step_length_cur[0] < step_length_tar[0]: # ratio = step_length_cur[0] / step_length_tar[0] # R_max = maxmaxStfPushFrame # R_zero = R_swp_sag = np.dot(R_swp_sag, mm.exp((step_length_tar[0] - step_length_cur[0])*step_axis[0] * K_stp_pos * -stf_push_func(t))) motion_stf_push[frame].mulJointOrientationGlobal(swingFoot, R_swp_sag) # stance foot balancing motion_stf_balancing.append(motion_stf_push[frame].copy()) motion_stf_balancing.goToFrame(frame) if STANCE_FOOT_BALANCING: R_stb = mm.exp(diff_dCM_axis * K_stb_vel * stf_balancing_func(t)) R_stb = np.dot(R_stb, mm.exp(diff_CMr_axis * K_stb_pos * stf_balancing_func(t))) for stanceFoot in stanceFoots: if frame < 5: continue motion_stf_balancing[frame].mulJointOrientationGlobal(stanceFoot, R_stb) # control trajectory motion_control.append(motion_stf_balancing[frame].copy()) motion_control.goToFrame(frame) #======================================================================= # tracking with inverse dynamics #======================================================================= th_r = motion_control.getDOFPositions(frame) th = controlModel.getDOFPositions() dth_r = motion_control.getDOFVelocities(frame) dth = controlModel.getDOFVelocities() ddth_r = motion_control.getDOFAccelerations(frame) ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt) #======================================================================= # simulation #======================================================================= CP = mm.v3(0.,0.,0.) F = mm.v3(0.,0.,0.) avg_dCM[0] = mm.v3(0.,0.,0.) # external force rendering info del rd_forces[:]; del rd_force_points[:] for fi in forceInfos: if fi.startFrame <= frame and frame < fi.startFrame + fi.duration*(1/frameTime): rd_forces.append(fi.force) rd_force_points.append(controlModel.getBodyPositionGlobal(fi.targetBody) + -mm.normalize2(fi.force)*.2) for i in range(stepsPerFrame): bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds) vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces) # apply external force for fi in forceInfos: if fi.startFrame <= frame and frame < fi.startFrame + fi.duration*(1/frameTime): controlModel.applyBodyForceGlobal(fi.targetBody, fi.force) controlModel.setDOFAccelerations(ddth_des) controlModel.solveHybridDynamics() # # apply external force # for fi in forceInfos: # if fi.startFrame <= frame and frame < fi.startFrame + fi.duration*(1/frameTime): # controlModel.applyBodyForceGlobal(fi.targetBody, fi.force) vpWorld.step() # yvu.align2D(controlModel) if len(contactForces) > 0: CP += yrp.getCP(contactPositions, contactForces) F += sum(contactForces) avg_dCM[0] += controlModel.getJointVelocityGlobal(0) # avg_dCM[0] += yrp.getCM(controlModel.getJointVelocitiesGlobal(), bodyMasses, upperMass, uppers) # avg_dCM[0] += yrp.getCM(controlModel.getJointVelocitiesGlobal(), bodyMasses, totalMass) # if len(stanceFoots)>0: # avg_stf_v[0] += controlModel.getJointVelocityGlobal(stanceFoots[0]) # avg_stf_av[0] += controlModel.getJointAngVelocityGlobal(stanceFoots[0]) CP /= stepsPerFrame F /= stepsPerFrame avg_dCM[0] /= stepsPerFrame # if len(stanceFoots)>0: # avg_stf_v[0] /= stepsPerFrame # avg_stf_av[0] /= stepsPerFrame # rd_vec1[0] = avg_stf_av[0]; rd_vec1[0][0] = 0.; rd_vec1[0][2] = 0. # rd_vecori1[0]= controlModel.getJointPositionGlobal(stanceFoots[0]) #======================================================================= # segment editing #======================================================================= lastFrame = False if SEGMENT_EDITING: if curState==yba.GaitState.STOP: if frame == len(motion_seg)-1: lastFrame = True elif (curState==yba.GaitState.LSWING or curState==yba.GaitState.RSWING) and t>c_min_contact_time: swingID = lID if curState==yba.GaitState.LSWING else rID contact = False if swingID in bodyIDs: minContactVel = 1000. for i in range(len(bodyIDs)): if bodyIDs[i]==swingID: vel = controlModel.getBodyVelocityGlobal(swingID, contactPositionLocals[i]) vel[1] = 0 contactVel = mm.length(vel) if contactVel < minContactVel: minContactVel = contactVel if minContactVel < c_min_contact_vel: contact = True extended[0] = False if contact: # print frame, 'foot touch' lastFrame = True acc_offset[0] += frame - curInterval[1] elif frame == len(motion_seg)-1: print frame, 'extend frame', frame+1 preserveJoints = [] # preserveJoints = [lFoot, rFoot] # preserveJoints = [lFoot, rFoot, lKnee, rKnee] # preserveJoints = [lFoot, rFoot, lKnee, rKnee, lUpLeg, rUpLeg] stanceKnees = [rKnee] if curState==yba.GaitState.LSWING else [lKnee] preserveJoints = [stanceFoots[0], stanceKnees[0], stanceLegs[0]] diff = 3 motion_seg_orig.extend([motion_seg_orig[-1]]) motion_seg.extend(ymt.extendByIntegration_root(motion_seg, 1, diff)) motion_stitch.extend(ymt.extendByIntegration_constant(motion_stitch, 1, preserveJoints, diff)) # # extend for swing foot ground speed matching & swing foot height lower ## extendedPostures = ymt.extendByIntegration(motion_stitch, 1, preserveJoints, diff) ## extendedPostures = [motion_stitch[-1]] ## # extendFrameNum = frame - curInterval[1] + 1 # k = 1.-extendFrameNum/5. # if k<0.: k=0. # extendedPostures = ymt.extendByIntegrationAttenuation(motion_stitch, 1, preserveJoints, diff, k) # ## if len(swingFoots)>0 and np.inner(dCM_tar, dCM)>0.: ## print frame, 'speed matching' ## R_swf = motion_stitch[-1].getJointOrientationGlobal(swingFoots[0]) ## ## p_swf = motion_stitch[-1].getJointPositionGlobal(swingFoots[0]) ## v_swf = motion_stitch.getJointVelocityGlobal(swingFoots[0], frame-diff, frame) ## a_swf = motion_stitch.getJointAccelerationGlobal(swingFoots[0], frame-diff, frame) ## p_swf += v_swf * (frameTime) + a_swf * (frameTime)*(frameTime) ## aik.ik_analytic(extendedPostures[0], swingFoots[0], p_swf) ## ## extendedPostures[0].setJointOrientationGlobal(swingFoots[0], R_swf) # # motion_stitch.extend(extendedPostures) extended[0] = True else: if frame == len(motion_seg)-1: lastFrame = True if lastFrame: if segIndex < len(segments)-1: print '%d (%d): end of %dth seg (%s, %s)'%(frame, frame-curInterval[1],segIndex, yba.GaitState.text[curState], curInterval) if plot!=None: plot.addDataPoint('diff', frame, (frame-curInterval[1])*.01) if len(stanceFoots)>0 and len(swingFoots)>0: # step_cur = controlModel.getJointPositionGlobal(swingFoots[0]) - controlModel.getJointPositionGlobal(stanceFoots[0]) # step_tar = motion_seg[curInterval[1]].getJointPositionGlobal(swingFoots[0]) - motion_seg[curInterval[1]].getJointPositionGlobal(stanceFoots[0]) step_cur = controlModel.getJointPositionGlobal(0) - controlModel.getJointPositionGlobal(stanceFoots[0]) step_tar = motion_seg[curInterval[1]].getJointPositionGlobal(0) - motion_seg[curInterval[1]].getJointPositionGlobal(stanceFoots[0]) step_cur = mm.projectionOnPlane(step_cur, (1,0,0), (0,0,1)) step_tar = mm.projectionOnPlane(step_tar, (1,0,0), (0,0,1)) step_cur_sag, step_cur_cor = mm.projectionOnVector2(step_cur, direction) step_tar_sag, step_tar_cor = mm.projectionOnVector2(step_tar, direction) step_length_tar[0] = mm.length(step_tar_sag) if np.inner(step_tar_sag, step_cur_sag) > 0: step_length_cur[0] = mm.length(step_cur_sag) else: step_length_cur[0] = -mm.length(step_cur_sag) step_axis[0] = directionAxis # rd_vec1[0] = step_tar_sag # rd_vecori1[0] = motion_seg[curInterval[1]].getJointPositionGlobal(stanceFoots[0]) # rd_vec2[0] = step_cur_sag # rd_vecori2[0] = controlModel.getJointPositionGlobal(stanceFoots[0]) seg_index[0] += 1 curSeg = segments[seg_index[0]] stl_y_limit_num[0] = 0 stl_xz_limit_num[0] = 0 del motion_seg_orig[frame+1:] motion_seg_orig.extend(ymb.getAttachedNextMotion(curSeg, motion_seg_orig[-1], False, False)) del motion_seg[frame+1:] del motion_stitch[frame+1:] transitionLength = len(curSeg)-1 # motion_seg.extend(ymb.getAttachedNextMotion(curSeg, motion_seg[-1], False, False)) # motion_stitch.extend(ymb.getStitchedNextMotion(curSeg, motion_control[-1], transitionLength, stitch_func, True, False)) d = motion_seg[-1] - curSeg[0] d.rootPos[1] = 0. motion_seg.extend(ymb.getAttachedNextMotion(curSeg, d, True, False)) d = motion_control[-1] - curSeg[0] d.rootPos[1] = 0. motion_stitch.extend(ymb.getStitchedNextMotion(curSeg, d, transitionLength, stitch_func, True, False)) # motion_seg.extend(ymb.getAttachedNextMotion(curSeg, motion_seg[-1], False, True)) # motion_stitch.extend(ymb.getStitchedNextMotion(curSeg, motion_control[-1], transitionLength, stitch_func, True, True)) else: motion_seg_orig.append(motion_seg_orig[-1]) motion_seg.append(motion_seg[-1]) motion_stitch.append(motion_control[-1]) # rendering motionModel.update(motion_ori[frame]) # motionModel.update(motion_seg[frame]) rd_CP[0] = CP rd_CMP[0] = (CMreal[0] - (F[0]/F[1])*CMreal[1], 0, CMreal[2] - (F[2]/F[1])*CMreal[1]) if plot!=None: plot.addDataPoint('zero', frame, 0) plot.updatePoints()
def simulateCallback(self, frame): global ddth_des_flat global stepsPerFrame global wcfg global vpWorld # reload(tf) motionModel.update(motion[0]) self.frame = frame print("main:frame : ", frame) # motionModel.update(motion[0]) self.timeIndex = 0 self.setTimeStamp() # IK solver ''' solver.clear() solver.setInitPose(motion[0]) cVpBodyIds, cPositions, cPositionsLocal, cVelocities = vpWorld.getContactPoints(bodyIDsToCheck) if len(cVpBodyIds) > 1: solver.addConstraints(cVpBodyIds[1], cPositionsLocal[1], np.array((0., 0., 0.)), None, (False, True, False, False)) solver.addConstraints(cVpBodyIds[3], cPositionsLocal[3], np.array((0., 0., 0.)), None, (False, True, False, False)) solver.addConstraints(cVpBodyIds[5], cPositionsLocal[5], np.array((0., 0., 0.)), None, (False, True, False, False)) # solver.solve(controlModel, np.array((0., .15 + .05*math.sin(frame/10.), 0.))) ''' # constant setting # (Kt, damp, stepsPerFrame, simulSpeedInv) = viewer.objectInfoWnd.getVals() getVal = viewer.objectInfoWnd.getVal Kt = getVal('PD gain') damp = getVal('Joint Damping') stepsPerFrame = getVal('steps per frame') simulSpeedInv = getVal('1/simul speed') wcfg.timeStep = 1 / (30. * simulSpeedInv * stepsPerFrame) vpWorld.SetTimeStep(wcfg.timeStep) Dt = 2. * (Kt**.5)/20. # Dt = 0. controlModel.SetJointsDamping(damp) # controlModel.SetJointsDamping(1.) wLCP = math.pow(2., getVal('LCP weight')) wForce = math.pow(2., getVal('force weight')) wTorque = math.pow(2., getVal('tau weight')) # tracking th_r = motion.getDOFPositions(frame) th = controlModel.getDOFPositions() dth_r = motion.getDOFVelocities(frame) dth = controlModel.getDOFVelocities() ddth_r = motion.getDOFAccelerations(frame) weightMapTuple = config['weightMapTuple'] weightMapTuple = None ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt, weightMapTuple) ddth_c = controlModel.getDOFAccelerations() ype.flatten(ddth_des, ddth_des_flat) desForceFrameBegin = getVal('des force begin') desForceDuration = getVal('des force dur') * simulSpeedInv desForceFrame = [ desForceFrameBegin, desForceFrameBegin + desForceDuration] desForceRelFrame = float(frame - desForceFrame[0]) / desForceDuration desNormalForceMin = getVal('normal des force min') desNormalForceMax = getVal('normal des force max') desNormalForce = desNormalForceMin if desForceFrame[0] <= frame <= desForceFrame[1]: desNormalForce = desNormalForceMin * \ (1 - desForceRelFrame) + desNormalForceMax * desForceRelFrame totalForce = np.array([0., desNormalForce, 0., 0., 0., 0.]) # totalForce = np.array([-desNormalForce, 34.3, 0., 0., 0., 0.]) # totalForce = np.array([0., 34.3, desNormalForce, 0., 0., 0.]) # totalForce = np.array([50., 150.]) torques = None ddth_des_flat[0:6] = [0.] * 6 self.setTimeStamp() simulContactForces = np.zeros(3) cBodyIDs = None cPositions = None cPositionLocals = None cForces = None cBodyIDsControl = None cPositionsControl = None cPositionLocalsControl = None cForcesControl = None # if desForceFrame[0] <= frame <= desForceFrame[1]: if True: # totalForceImpulse = stepsPerFrame * totalForce cBodyIDsControl, cPositionsControl, cPositionLocalsControl, cForcesControl, torques \ = hls.calcLCPbasicControl( motion, vpWorld, controlModel, bodyIDsToCheck, 1., totalForce, [wLCP, wTorque, wForce], ddth_des_flat) # if cForces is not None: # print "control: ", sum(cForces) sumControlForce = np.array([0.]*6) if cForcesControl is not None: sumControlForce = np.hstack((sum(cForcesControl), np.array([0., 0., 0.]))) timeStamp = None torque_None = False if not (desForceFrame[0] <= frame <= desForceFrame[1]) or (torques is None): torque_None = True torques = ddth_des_flat elif np.linalg.norm(sumControlForce - totalForce) > np.linalg.norm(totalForce): print("control failed!") torque_None = True torques = ddth_des_flat else: torques *= 1. for i in range(int(stepsPerFrame)): if i % 5 == 0: cBodyIDs, cPositions, cPositionLocals, cForces, timeStamp \ = hls.calcLCPForces(motion, vpWorld, controlModel, bodyIDsToCheck, 1., torques, solver='qp') if i % 5 == 0 and len(cBodyIDs) > 0: # apply contact forces if False and not torque_None: vpWorld.applyPenaltyForce(cBodyIDs, cPositionLocals, cForcesControl) simulContactForces += sum(cForcesControl) else: vpWorld.applyPenaltyForce(cBodyIDs, cPositionLocals, cForces) simulContactForces += sum(cForces) # simulContactForces += sum(cForces) ype.nested(torques, torques_nested) controlModel.setDOFTorques(torques_nested[1:]) vpWorld.step() self.setTimeStamp() # rendering expected force del rd_cForcesControl[:] del rd_cPositionsControl[:] if cBodyIDsControl is not None: # print cBodyIDsControl for i in range(len(cBodyIDsControl)): # print expected force rd_cForcesControl.append(cForcesControl[i].copy() /50.) rd_cPositionsControl.append(cPositionsControl[i].copy()) # rendering sum of expected force del rd_ForceControl[:] del rd_Position[:] if cForcesControl is not None: # print expected force rd_ForceControl.append(sum(cForcesControl) /50.) rd_Position.append(np.array([0., 0., 0.1])) # graph expected force if cForcesControl is not None: sumForce = sum(cForcesControl) if sumForce[1] > 10000: sumForce[1] = 10000 viewer.cForceWnd.insertData('expForce', frame, sumForce[1]) else: viewer.cForceWnd.insertData('expForce', frame, 0.) # rendering calculated forces del rd_cForces[:] del rd_cPositions[:] for i in range(len(cBodyIDs)): # print calculated force rd_cForces.append(cForces[i].copy() / 50.) rd_cPositions.append(cPositions[i].copy()) # rendering joint position del rd_jointPos[:] for i in range(motion[0].skeleton.getJointNum()): rd_jointPos.append(motion[frame].getJointPositionGlobal(i)) # rendering desired force del rd_ForceDes[:] del rd_PositionDes[:] # rd_ForceDes.append(totalForce/50.) rd_ForceDes.append(totalForce[1] * np.array([0., 1., 0.]) / 50.) rd_PositionDes.append(np.array([0., 0., 0.])) # if self.cForces is not None: # rd_ForceDes.append(sum(self.cForces)[1]/50. * [0., 1., 0.]) # rd_PositionDes.append(np.array([0., 0., -0.1])) # graph calculated force if cForces is not None: sumForce = sum(cForces) # viewer.cForceWnd.insertData('realForce', frame, sumForce[1]) viewer.cForceWnd.insertData('realForce', frame, simulContactForces[1]/stepsPerFrame) else: viewer.cForceWnd.insertData('realForce', frame, 0.) # viewer.cForceWnd.insertData('realForce', frame, simulContactForces[1]/stepsPerFrame) # graph desired force if desForceFrame[0] <= frame <= desForceFrame[1]: viewer.cForceWnd.insertData('desForceMin', frame, totalForce[1]) # viewer.cForceWnd.insertData('desForceMin', frame, totalForce[1] * 1.0) # viewer.cForceWnd.insertData('desForceMax', frame, totalForce[1] * 1.1) else: viewer.cForceWnd.insertData('desForceMin', frame, 0.) viewer.cForceWnd.insertData('desForceMax', frame, 0.) self.setTimeStamp()
def setupQP(self, frame, motion, mcfg, model, world, config, timestep): motionModel = cvm.VpMotionModel(world, motion[frame], mcfg) # constants invdt = 1./timestep # dofs and flat data structure totalDOF = model.getTotalDOF() DOFs = model.getDOFs() self.totalDOF = totalDOF self.ddth_des_flat = ype.makeFlatList(totalDOF) self.ddth_r_flat = ype.makeFlatList(totalDOF) self.dth_flat = ype.makeFlatList(totalDOF) self.dth_r_flat = ype.makeFlatList(totalDOF) self.ddth_sol = ype.makeNestedList(DOFs) # momentum matrix linkMasses = model.getBodyMasses() totalMass = model.getTotalMass() TO = ymt.make_TO(linkMasses) dTO = ymt.make_dTO(len(linkMasses)) # optimization self.qp.clear() Vc_tmp = self.Vc_tmp # tracking w = self.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap']) th_r = motion.getDOFPositionsLocal(frame) th = model.getDOFPositionsLocal() dth_r = motion.getDOFVelocitiesLocal(frame) dth = model.getDOFVelocitiesLocal() ddth_r = motion.getDOFAccelerationsLocal(frame) ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, self.Kt, self.Dt) linkPositions = model.getBodyPositionsGlobal() linkVelocities = model.getBodyVelocitiesGlobal() linkAngVelocities = model.getBodyAngVelocitiesGlobal() linkInertias = model.getBodyInertiasGlobal() jointPositions = model.getJointPositionsGlobal() jointAxeses = model.getDOFAxesesLocal() #linkPositions_ref = motionModel.getBodyPositionsGlobal() #linkVelocities_ref = motionModel.getBodyVelocitiesGlobal() #linkAngVelocities_ref = motionModel.getBodyAngVelocitiesGlobal() #linkInertias_ref = motionModel.getBodyInertiasGlobal() #jointPositions_ref = motionModel.getJointPositionsGlobal() #jointAxeses_ref = motionModel.getDOFAxesesLocal() ype.flatten(ddth_des, self.ddth_des_flat) ype.flatten(dth, self.dth_flat) ype.flatten(dth_r, self.dth_r_flat) ype.flatten(ddth_r, self.ddth_r_flat) # get CoM 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) # jacobian Jsup = yjc.makeEmptyJacobian(DOFs, 1) dJsup = Jsup.copy() Jsys_old = None if self.Jsys != None: Jsys_old = self.Jsys.copy() if self.Jsys == None: self.Jsys = yjc.makeEmptyJacobian(DOFs, model.getBodyNum()) self.dJsys = self.Jsys.copy() allLinkJointMasks = yjc.getAllLinkJointMasks(motion[0].skeleton) yjc.computeJacobian2(self.Jsys, DOFs, jointPositions, jointAxeses, linkPositions, allLinkJointMasks) if Jsys_old ==None: self.dJsys = self.Jsys-self.Jsys else: self.dJsys = (self.Jsys - Jsys_old)*invdt #yjc.computeJacobianDerivative2(self.dJsys, DOFs, jointPositions, jointAxeses, linkAngVelocities, linkPositions, allLinkJointMasks) #CM_ref = yrp.getCM(linkPositions_ref, linkMasses, totalMass) #dCM_ref = yrp.getCM(linkVelocities_ref, linkMasses, totalMass) #CM_ref_plane = copy.copy(CM_ref); CM_ref_plane[1]=0. #dCM_ref_plane = copy.copy(dCM_ref); dCM_ref_plane[1]=0. #P_ref = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions_ref, CM_ref, linkInertias_ref) #dP_ref = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses, linkVelocities_ref, dCM_ref, linkAngVelocities_ref, linkInertias_ref) # get EoM totalActuator = totalDOF invM = np.zeros((totalActuator,totalDOF)) invMc = np.zeros(totalDOF) model.getInverseEquationOfMotion(invM, invMc) #print invMc # contact detection Ks = 1 Ds = 1 supsupR = motion[0].skeleton.getJointIndex('RightLeg') supsupL = motion[0].skeleton.getJointIndex('LeftLeg') supR = motion[0].skeleton.getJointIndex('RightFoot') supL = motion[0].skeleton.getJointIndex('LeftFoot') bodyIDsToCheck = range(world.getBodyNum()) #print bodyIDsToCheck #bodyIDsToCheck = [supsupR, supsupL] #bodyIDsToCheck = [supR, supL] mus = [.5]*len(bodyIDsToCheck) bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = world.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds) #bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = world.calcManyPenaltyForce(self.contactPerSide, bodyIDsToCheck, mus, Ks, Ds) #bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = world.calcOnePenaltyForce(bodyIDsToCheck, mus, Ks, Ds) #print bodyIDs, contactPositions footCenterL = model.getBodyPositionGlobal(supL) footCenterR = model.getBodyPositionGlobal(supR) footCenter = footCenterL.copy() footRefCenterL = motionModel.getBodyPositionGlobal(supL) footRefCenterR = motionModel.getBodyPositionGlobal(supR) #if supL in bodyIDs: #if supR in bodyIDs: #footCenter = footCenterL + (footCenterR-footCenterL)/2. #else: #footCenter = footCenterL.copy() #else: #if supR in bodyIDs: #footCenter = footCenterR.copy() #else: #footCenter = np.array((0,0,0)) contactL = 1 contactR = 1 if footRefCenterL[1] < 0.2: if footRefCenterR[1] < 0.2: footCenter = footCenterL + (footCenterR-footCenterL)/2. else: footCenter = footCenterL.copy() contactR = 0 else: contactL = 0 if footRefCenterR[1] < 0.2: footCenter = footCenterR.copy() else: footCenter = np.array((0,0,0)) contactR = 0 #print(contactR, contactL) footCenter[1] = 0. # linear momentum CM_ref = footCenter #CM_ref = #dL_des_plane = self.Kl*totalMass*(CM_ref - CM) + self.Dl*totalMass*(dCM_ref - dCM) dL_des_plane = self.Kl*totalMass*(CM_ref - CM) + self.Dl*totalMass*(-dCM) dL_des_plane[1] = 0. # angular momentum CP_ref = footCenter #bodyIDs, contactPositions, contactPositionLocals, contactForces = world.calcManyPenaltyForce(bodyIDsToCheck, mus, Ks, Ds) #CP = yrp.getCP(contactPositions, contactForces) CP = yrp.getSimpleCP(contactPositions) if self.CP_old==None or CP==None: dCP = None else: dCP = (CP - self.CP_old[0])/(1/30.) self.CP_old = CP if CP!=None and dCP!=None: ddCP_des = self.Kh*(CP_ref - CP) - self.Dh*(dCP) CP_des = CP + dCP*(1/30.) + .5*ddCP_des*((1/30.)**2) dH_des = np.cross((CP_des - CM), (dL_des_plane + totalMass*np.array((0,-9.8,0)))) #if contactChangeCount >0: # and contactChangeType == 'DtoS': ##dH_des *= (maxContactChangeCount - contactChangeCount)/(maxContactChangeCount*10) #dH_des *= (self.maxContactChangeCount - self.contactChangeCount)/(self.maxContactChangeCount) ##dH_des *= (contactChangeCount)/(maxContactChangeCount)*.9+.1 else: dH_des = None H = np.dot(P, np.dot(self.Jsys, self.dth_flat)) dH_des = -self.Kh*H[3:] # equality constraints JcTVc_append = np.zeros((totalDOF, 0)) VcTJc_list = [] VcTdJc_list = [] dVcTJc_list = [] ac_offset_list = [] totalContact = 4*len(bodyIDs) totalProblem = totalDOF+totalActuator+totalContact preSup = -1 for i in range(len(contactPositions)): sup = bodyIDs[i] supJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, sup)] if preSup != sup: bodyPos = linkPositions[sup] bodyVel = linkVelocities[sup] #yjc.computeJacobian2(Jsup, DOFs, jointPositions, jointAxeses, [bodyPos], supJointMasks) #yjc.computeJacobianDerivative2(dJsup, DOFs, jointPositions, jointAxeses, linkAngVelocities, [bodyPos], supJointMasks) Jsup = getPartJacobian(self.Jsys, sup) dJsup = getPartJacobian(self.dJsys, sup) R_dAd = np.hstack( (np.vstack( (np.eye(3), mm.getCrossMatrixForm(-bodyPos)) ), np.vstack( (np.zeros((3,3)), np.eye(3)) ) ) ) dR_dAd = np.hstack( (np.vstack( (np.eye(3), mm.getCrossMatrixForm(-bodyVel)) ), np.vstack( (np.zeros((3,3)), np.eye(3)) ) ) ) #R_dAd = np.hstack( (np.vstack( (np.eye(3), mm.getCrossMatrixForm(-contactPositions[i])) ), np.vstack( (np.zeros((3,3)), np.eye(3)) ) ) ) #dR_dAd = np.hstack( (np.vstack( (np.eye(3), mm.getCrossMatrixForm(-contactVelocities[i])) ), np.vstack( (np.zeros((3,3)), np.eye(3)) ) ) ) p = contactPositions[i] dotp = contactVelocities[i] VcT_tmp = np.zeros((4,6)) dVcT_tmp = VcT_tmp.copy() for ii in range(4): n = Vc_tmp[ii] pcn = np.cross(contactPositions[i], Vc_tmp[ii]) VcT_tmp[ii][:3] =n VcT_tmp[ii][3:] =pcn dotpcn = np.cross(contactVelocities[i], Vc_tmp[ii]) dVcT_tmp[ii][3:] = dotpcn Vc = np.dot(R_dAd, VcT_tmp.T) dVc = np.dot(R_dAd, dVcT_tmp.T) + np.dot(dR_dAd, VcT_tmp.T) JcTVc = np.dot( Jsup.T, Vc) JcTVc_append = np.hstack((JcTVc_append, JcTVc)) VcTJc_list.append( JcTVc.T ) VcTdJc_list.append( np.dot(Vc.T, dJsup) ) dVcTJc_list.append( np.dot(dVc.T, Jsup) ) #TODO: # when friction cones and velocity cones differ? #JcTVc = np.dot( Jsup.T, VcT.T) #JcTVc_append = np.hstack((JcTVc_append, JcTVc)) #VcTJc_list.append( JcTVc.T ) #VcTdJc_list.append( np.dot(VcT, dJsup) ) #dVcTJc_list.append( np.dot(dVcT, Jsup) ) penDepth = -0.05-contactPositions[i][1] if penDepth < 0.: penDepth = 0. #penDepth = 0. ac_offset = 1000.*penDepth*np.ones(4) ac_offset_list.append(ac_offset) preSup = sup extForce = np.zeros(totalActuator) if self.extDuration > 0: Jext = yjc.makeEmptyJacobian(DOFs, 1) extForcePos = model.getBodyPositionGlobal(self.extForceBody) extJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, self.extForceBody)] yjc.computeJacobian2(Jext, DOFs, jointPositions, jointAxeses, [extForcePos], extJointMasks) extForce = np.dot(Jext.T, self.extForce) self.extDuration -= timestep if self.extDuration < 0: self.extDuration = 0 self.addQPEqualityInverseEomConstraint(totalProblem, totalDOF, totalActuator, totalContact, invM, invMc, JcTVc_append, extForce) # inequality constraints if totalContact> 0: self.addQPInequalityContactForceConstraint(totalProblem, totalDOF, totalActuator, totalContact) self.addQPInequalityVelocityConstraint(totalProblem, totalDOF, totalActuator, totalContact, VcTJc_list, VcTdJc_list,dVcTJc_list, self.dth_flat, ac_offset_list, invdt) #self.addQPInequalityVelocityConstraint(totalProblem, totalDOF, totalActuator, totalContact, VcTJc_vel_list, VcTdJc_vel_list,dVcTJc_vel_list, self.dth_flat, ac_offset_list, 30.) torqueMax = 1000.*np.ones(totalActuator-6) torqueMin = -torqueMax self.addQPInequalityTorqueConstraint(totalProblem, totalDOF, totalActuator, totalContact, torqueMax, torqueMin) # objectives self.addQPTrackingTerms(totalProblem, 0, totalDOF, self.Bt, w, self.ddth_des_flat) self.addQPTorqueTerms(totalProblem, totalDOF, totalActuator, self.Btau, w) if totalContact > 0: self.addQPContactForceTerms(totalProblem, totalDOF+totalActuator, totalContact, self.Bcon) #if dH_des !=None: # allLinkJointMasks = yjc.getAllLinkJointMasks(motion[0].skeleton) # yjc.computeJacobian2(Jsys, DOFs, jointPositions, jointAxeses, linkPositions, allLinkJointMasks) # yjc.computeJacobianDerivative2(dJsys, DOFs, jointPositions, jointAxeses, linkAngVelocities, linkPositions, allLinkJointMasks) # self.addLinearAndAngularBalancigTerms(totalProblem, 0, totalDOF, self.Bl, self.Bh, P, self.Jsys, self.dth_flat, dP, self.dJsys, dL_des_plane, dH_des) # end effector #TODO: eeList = [supR, supL] #eeList = [] #if totalContact > 0: for ee in eeList: eeCenter = model.getBodyPositionGlobal(ee) eeJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, ee)] yjc.computeJacobian2(Jsup, DOFs, jointPositions, jointAxeses, [eeCenter], eeJointMasks) yjc.computeJacobianDerivative2(dJsup, DOFs, jointPositions, jointAxeses, linkAngVelocities, [eeCenter], eeJointMasks, False) ee_genvel_ref = np.dot(Jsup, self.dth_r_flat) ee_genacc_ref = np.dot(Jsup, self.ddth_r_flat) + np.dot(dJsup, self.dth_r_flat) ee_pos_ref = motionModel.getBodyPositionGlobal(ee) ee_pos = model.getBodyPositionGlobal(ee) ee_vel_ref = ee_genvel_ref[:3] ee_vel = model.getBodyVelocityGlobal(ee) ee_acc_ref = ee_genacc_ref[:3] ddp_des_pos = self.Ke*( (ee_pos_ref-th_r[0][0]) - (ee_pos-th[0][0]) ) ddp_des_pos += self.De*(ee_vel_ref - ee_vel) ddp_des_pos += ee_acc_ref eeOri = model.getBodyOrientationGlobal(ee) eeAngVel = model.getBodyAngVelocityGlobal(ee) ee_angvel_ref = ee_genvel_ref[3:] ee_angacc_ref = ee_genacc_ref[3:] a_ori_diff = mm.logSO3(mm.getSO3FromVectors(np.dot(eeOri, np.array([0,1,0])), np.array([0,1,0]))) ddp_des_ang = self.Ke*a_ori_diff + self.De*(-eeAngVel) #ddp_des_ang = self.Ke*a_ori_diff + self.De*(ee_angvel_ref-eeAngVel) #ddp_des_ang += ee_angacc_ref ddp_des = np.hstack( (ddp_des_pos, ddp_des_ang) ) #self.addEndEffectorTerms(totalProblem, 0, totalDOF, Jsup, dJsup, self.dth_flat, ddp_des, self.Be) self.addEqualityEndEffectorTerms(totalProblem, 0, totalDOF, Jsup, dJsup, self.dth_flat, ddp_des, self.Be) return contactPositions, CP, CM, footCenter, dL_des_plane, CM_ref
def simulateCallback(frame): global g_initFlag global forceShowFrame global forceApplyFrame global JsysPre global JsupPreL global JsupPreR global JsupPre global softConstPoint global stage motionModel.update(motion[frame]) Kt, Kk, Kl, Kh, Ksc, Bt, Bl, Bh, Bsc = viewer.GetParam() Dt = 2 * (Kt**.5) Dk = 2 * (Kk**.5) Dl = 2 * (Kl**.5) Dh = 2 * (Kh**.5) Dsc = 2 * (Ksc**.5) if Bsc == 0.0: viewer.doc.showRenderer('softConstraint', False) viewer.motionViewWnd.update(1, viewer.doc) else: viewer.doc.showRenderer('softConstraint', True) renderer1 = viewer.doc.getRenderer('softConstraint') renderer1.rc.setLineWidth(0.1 + Bsc * 3) viewer.motionViewWnd.update(1, viewer.doc) # 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) ddth_c = controlModel.getDOFAccelerations() ype.flatten(ddth_des, ddth_des_flat) ype.flatten(dth, dth_flat) ype.flatten(ddth_c, ddth_c_flat) # jacobian footCenterL = controlModel.getBodyPositionGlobal(supL) footCenterR = controlModel.getBodyPositionGlobal(supR) refFootL = motionModel.getBodyPositionGlobal(supL) refFootR = motionModel.getBodyPositionGlobal(supR) footCenter = footCenterL + (footCenterR - footCenterL) / 2.0 footCenter[1] = 0. footCenter_ref = refFootL + (refFootR - refFootL) / 2.0 #footCenter_ref[1] = 0. foreFootCenterL = controlModel.getBodyPositionGlobal(foreSupL) foreFootCenterR = controlModel.getBodyPositionGlobal(foreSupR) rearFootCenterL = controlModel.getBodyPositionGlobal(rearSupL) rearFootCenterR = controlModel.getBodyPositionGlobal(rearSupR) linkPositions = controlModel.getBodyPositionsGlobal() linkVelocities = controlModel.getBodyVelocitiesGlobal() linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal() linkInertias = controlModel.getBodyInertiasGlobal() jointPositions = controlModel.getJointPositionsGlobal() jointAxeses = controlModel.getDOFAxeses() CM = yrp.getCM(linkPositions, linkMasses, totalMass) dCM = yrp.getCM(linkVelocities, linkMasses, totalMass) CM_plane = copy.copy(CM) CM_plane[1] = 0. dCM_plane = copy.copy(dCM) dCM_plane[1] = 0. linkPositions_ref = motionModel.getBodyPositionsGlobal() CM_ref = yrp.getCM(linkPositions_ref, linkMasses, totalMass) CM_plane_ref = copy.copy(CM_ref) CM_plane_ref[1] = 0. P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM, linkInertias) dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses, linkVelocities, dCM, linkAngVelocities, linkInertias) yjc.computeJacobian2(Jsys, DOFs, jointPositions, jointAxeses, linkPositions, allLinkJointMasks) #dJsys = (Jsys - JsysPre)/(1/30.) #JsysPre = Jsys yjc.computeJacobianDerivative2(dJsys, DOFs, jointPositions, jointAxeses, linkAngVelocities, linkPositions, allLinkJointMasks) if g_initFlag == 0: softConstPoint = controlModel.getBodyPositionGlobal(constBody) softConstPoint[1] -= .3 g_initFlag = 1 yjc.computeJacobian2(JsupL, DOFs, jointPositions, jointAxeses, [footCenterL], supLJointMasks) #dJsupL = (JsupL - JsupPreL)/(1/30.) #JsupPreL = JsupL yjc.computeJacobianDerivative2(dJsupL, DOFs, jointPositions, jointAxeses, linkAngVelocities, [footCenterL], supLJointMasks, False) yjc.computeJacobian2(JsupR, DOFs, jointPositions, jointAxeses, [footCenterR], supRJointMasks) #dJsupR = (JsupR - JsupPreR)/(1/30.) #JsupPreR = JsupR yjc.computeJacobianDerivative2(dJsupR, DOFs, jointPositions, jointAxeses, linkAngVelocities, [footCenterR], supRJointMasks, False) bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds) CP = yrp.getCP(contactPositions, contactForces) for i in range(len(bodyIDsToCheck)): controlModel.SetBodyColor(bodyIDsToCheck[i], 0, 0, 0) flagForeSupLContact = 0 flagForeSupRContact = 0 flagRearSupLContact = 0 flagRearSupLContact = 0 for i in range(len(bodyIDs)): controlModel.SetBodyColor(bodyIDs[i], 255, 105, 105) index = controlModel.id2index(bodyIDs[i]) if index == foreSupL: flagForeSupLContact = 1 yjc.computeJacobian2(JforeSupL, DOFs, jointPositions, jointAxeses, [foreFootCenterL], foreSupLJointMasks) yjc.computeJacobianDerivative2(dJforeSupL, DOFs, jointPositions, jointAxeses, linkAngVelocities, [foreFootCenterL], foreSupLJointMasks, False) elif index == foreSupR: flagForeSupRContact = 1 yjc.computeJacobian2(JforeSupR, DOFs, jointPositions, jointAxeses, [foreFootCenterR], foreSupRJointMasks) yjc.computeJacobianDerivative2(dJforeSupR, DOFs, jointPositions, jointAxeses, linkAngVelocities, [foreFootCenterR], foreSupRJointMasks, False) elif index == rearSupL: flagRearSupLContact = 1 yjc.computeJacobian2(JrearSupL, DOFs, jointPositions, jointAxeses, [rearFootCenterL], rearSupLJointMasks) yjc.computeJacobianDerivative2(dJrearSupL, DOFs, jointPositions, jointAxeses, linkAngVelocities, [rearFootCenterL], rearSupLJointMasks, False) elif index == rearSupR: flagRearSupRContact = 1 yjc.computeJacobian2(JrearSupR, DOFs, jointPositions, jointAxeses, [rearFootCenterR], rearSupRJointMasks) yjc.computeJacobianDerivative2(dJrearSupR, DOFs, jointPositions, jointAxeses, linkAngVelocities, [rearFootCenterR], rearSupRJointMasks, False) # desForeSupLAcc = [0, 0, 0] desForeSupRAcc = [0, 0, 0] totalNormalForce = [0, 0, 0] for i in range(len(contactForces)): totalNormalForce[0] += contactForces[i][0] totalNormalForce[1] += contactForces[i][1] totalNormalForce[2] += contactForces[i][2] # linear momentum CM_ref_plane = footCenter dL_des_plane = Kl * totalMass * (CM_ref_plane - CM_plane) - Dl * totalMass * dCM_plane # angular momentum CP_ref = footCenter timeStep = 30. if CP_old[0] == None or CP == None: dCP = None else: dCP = (CP - CP_old[0]) / (1 / timeStep) CP_old[0] = CP if CP != None and dCP != None: ddCP_des = Kh * (CP_ref - CP) - Dh * (dCP) CP_des = CP + dCP * (1 / timeStep) + .5 * ddCP_des * ( (1 / timeStep)**2) dH_des = np.cross( (CP_des - CM), (dL_des_plane + totalMass * mm.s2v(wcfg.gravity))) #dH_des = np.cross((CP_des - CM_plane), (dL_des_plane + totalMass*mm.s2v(wcfg.gravity))) else: dH_des = None CMP = yrp.getCMP(contactForces, CM) r = [0, 0, 0] if CP != None and np.any(np.isnan(CMP)) != True: r = CP - CMP # momentum matrix RS = np.dot(P, Jsys) R, S = np.vsplit(RS, 2) rs = np.dot((np.dot(dP, Jsys) + np.dot(P, dJsys)), dth_flat) r_bias, s_bias = np.hsplit(rs, 2) ############################## # soft point constraint P_des = softConstPoint P_cur = controlModel.getBodyPositionGlobal(constBody) dP_des = [0, 0, 0] dP_cur = controlModel.getBodyVelocityGlobal(constBody) ddP_des1 = Ksc * (P_des - P_cur) - Dsc * (dP_cur - dP_des) r = P_des - P_cur I = np.vstack(([1, 0, 0], [0, 1, 0], [0, 0, 1])) Z = np.hstack((I, mm.getCrossMatrixForm(-r))) yjc.computeJacobian2(Jconst, DOFs, jointPositions, jointAxeses, [softConstPoint], constJointMasks) JL, JA = np.vsplit(Jconst, 2) Q1 = np.dot(Z, Jconst) q1 = np.dot(JA, dth_flat) q2 = np.dot(mm.getCrossMatrixForm(q1), np.dot(mm.getCrossMatrixForm(q1), r)) yjc.computeJacobianDerivative2(dJconst, DOFs, jointPositions, jointAxeses, linkAngVelocities, [softConstPoint], constJointMasks, False) q_bias1 = np.dot(np.dot(Z, dJconst), dth_flat) + q2 ############################## flagContact = True if dH_des == None or np.any(np.isnan(dH_des)) == True: flagContact = False viewer.doc.showRenderer('rd_grf_des', False) viewer.motionViewWnd.update(1, viewer.doc) else: viewer.doc.showRenderer('rd_grf_des', True) viewer.motionViewWnd.update(1, viewer.doc) ''' 0 : initial 1 : contact 2 : fly 3 : landing ''' #MOTION = FORWARD_JUMP if mit.MOTION == mit.FORWARD_JUMP: frame_index = [136, 100] elif mit.MOTION == mit.TAEKWONDO: frame_index = [130, 100] else: frame_index = [1000000, 1000000] #MOTION = TAEKWONDO #frame_index = [135, 100] if frame > frame_index[0]: stage = POWERFUL_BALANCING Kk = Kk * 2 Dk = 2 * (Kk**.5) elif frame > frame_index[1]: stage = MOTION_TRACKING trackingW = w if stage == MOTION_TRACKING: trackingW = w2 # optimization mot.addTrackingTerms(problem, totalDOF, Bt, trackingW, ddth_des_flat) mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1, q_bias1) if flagContact == True: if stage != MOTION_TRACKING: mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias) mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias) ############################## # Hard constraint desLinearAccL = getDesFootLinearAcc(motionModel, controlModel, supL, ModelOffset, CM_ref, CM, Kk, Dk) desLinearAccR = getDesFootLinearAcc(motionModel, controlModel, supR, ModelOffset, CM_ref, CM, Kk, Dk) desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, supL, Kk, Dk) desAngularAccR = getDesFootAngularAcc(motionModel, controlModel, supR, Kk, Dk) ''' desLinearAccL = [0,0,0] desAngularAccL = [0,0,0] desLinearAccR = [0,0,0] desAngularAccR = [0,0,0] refPos = motionModel.getBodyPositionGlobal(supL) curPos = controlModel.getBodyPositionGlobal(supL) refRootPos = motionModel.getBodyPositionGlobal(rootB) curRootPos = controlModel.getBodyPositionGlobal(rootB) refVecL = refPos - CM_ref if stage == MOTION_TRACKING: refPos = CM + refVecL refPos[1] += 0.05 refPos[0] -= 0.05 elif stage == POWERFUL_BALANCING: refPos = copy.copy(curPos) refPos[1] = 0 else: refPos[0] += ModelOffset[0] rd_root[0] = curRootPos refVel = motionModel.getBodyVelocityGlobal(supL) rd_footCenterL[0] = copy.copy(curPos) #rd_footCenterL[0][2] += 0.2 curVel = controlModel.getBodyVelocityGlobal(supL) #refAcc = (0,0,0) refAcc = motionModel.getBodyAccelerationGlobal(supL) if stage != MOTION_TRACKING: refPos[1] = 0 if refPos[1] < 0.0 : refPos[1] = 0.0 rd_DesPosL[0] = refPos desLinearAccL = yct.getDesiredAcceleration(refPos, curPos, refVel, curVel, refAcc, Kk, Dk) refPos = motionModel.getBodyPositionGlobal(supR) curPos = controlModel.getBodyPositionGlobal(supR) refVecR = refPos - CM_ref if stage == MOTION_TRACKING: refPos = CM + refVecR refPos[1] += 0.05 refPos[0] -= 0.05 elif stage == POWERFUL_BALANCING: refPos = copy.copy(curPos) refPos[1] = 0 else : refPos[0] += ModelOffset[0] refVel = motionModel.getBodyVelocityGlobal(supR) curVel = controlModel.getBodyVelocityGlobal(supR) refAcc = motionModel.getBodyAccelerationGlobal(supR) if stage != MOTION_TRACKING: refPos[1] = 0 if refPos[1] < 0.0 : refPos[1] = 0.0 rd_DesPosR[0] = refPos desLinearAccR = yct.getDesiredAcceleration(refPos, curPos, refVel, curVel, refAcc, Kk, Dk) curAng = [controlModel.getBodyOrientationGlobal(supL)] refAngVel = motionModel.getBodyAngVelocityGlobal(supL) curAngVel = controlModel.getBodyAngVelocityGlobal(supL) refAngAcc = (0,0,0) curAngY = np.dot(curAng, np.array([0,1,0])) refAngY = np.array([0,1,0]) if stage == DYNAMIC_BALANCING: refAng = [motionModel.getBodyOrientationGlobal(supL)] refAngY2 = np.dot(refAng, np.array([0,1,0])) refAngY = refAngY2[0] rd_footL_vec[0] = refAngY rd_footR_vec[0] = curAngY[0] aL = mm.logSO3(mm.getSO3FromVectors(curAngY[0], refAngY)) desAngularAccL = [Kk*aL + Dk*(refAngVel-curAngVel)] curAng = [controlModel.getBodyOrientationGlobal(supR)] refAngVel = motionModel.getBodyAngVelocityGlobal(supR) curAngVel = controlModel.getBodyAngVelocityGlobal(supR) refAngAcc = (0,0,0) curAngY = np.dot(curAng, np.array([0,1,0])) refAngY = np.array([0,1,0]) if stage == DYNAMIC_BALANCING: refAng = [motionModel.getBodyOrientationGlobal(supR)] refAngY2 = np.dot(refAng, np.array([0,1,0])) refAngY = refAngY2[0] aL = mm.logSO3(mm.getSO3FromVectors(curAngY[0], refAngY)) desAngularAccR = [Kk*aL + Dk*(refAngVel-curAngVel)] a_sup_2 = [desLinearAccL[0], desLinearAccL[1], desLinearAccL[2], desAngularAccL[0][0], desAngularAccL[0][1], desAngularAccL[0][2], desLinearAccR[0], desLinearAccR[1], desLinearAccR[2], desAngularAccR[0][0], desAngularAccR[0][1], desAngularAccR[0][2]] ''' a_sup_2 = np.hstack((np.hstack((desLinearAccL, desAngularAccL)), np.hstack((desLinearAccR, desAngularAccR)))) Jsup_2 = np.vstack((JsupL, JsupR)) dJsup_2 = np.vstack((dJsupL, dJsupR)) mot.setConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat, a_sup_2) ############################## ############################## # Additional constraint desLinearAccL = [0, 0, 0] desAngularAccL = [0, 0, 0] desLinearAccR = [0, 0, 0] desAngularAccR = [0, 0, 0] refPos = motionModel.getBodyPositionGlobal(supL) curPos = controlModel.getBodyPositionGlobal(supL) refVecL = refPos - CM_ref if stage == MOTION_TRACKING: refPos = CM + refVecL refPos[1] += 0.05 refPos[0] -= 0.05 elif stage == POWERFUL_BALANCING: refPos = copy.copy(curPos) refPos[1] = 0 else: refPos[0] += ModelOffset[0] refVel = motionModel.getBodyVelocityGlobal(supL) curVel = controlModel.getBodyVelocityGlobal(supL) #refAcc = (0,0,0) refAcc = motionModel.getBodyAccelerationGlobal(supL) if stage != MOTION_TRACKING: refPos[1] = 0 if refPos[1] < 0.0: refPos[1] = 0.0 desLinearAccL = yct.getDesiredAcceleration(refPos, curPos, refVel, curVel, refAcc, Kk, Dk) curAng = [controlModel.getBodyOrientationGlobal(supL)] refAngVel = motionModel.getBodyAngVelocityGlobal(supL) curAngVel = controlModel.getBodyAngVelocityGlobal(supL) refAngAcc = (0, 0, 0) curAngY = np.dot(curAng, np.array([0, 1, 0])) refAngY = np.array([0, 1, 0]) if stage == DYNAMIC_BALANCING: refAng = [motionModel.getBodyOrientationGlobal(supL)] refAngY2 = np.dot(refAng, np.array([0, 1, 0])) refAngY = refAngY2[0] rd_footL_vec[0] = refAngY rd_footR_vec[0] = curAngY[0] aL = mm.logSO3(mm.getSO3FromVectors(curAngY[0], refAngY)) desAngularAccL = [Kk * aL + Dk * (refAngVel - curAngVel)] ############################## r = problem.solve() problem.clear() ype.nested(r['x'], ddth_sol) rootPos[0] = controlModel.getBodyPositionGlobal(selectedBody) localPos = [[0, 0, 0]] for i in range(stepsPerFrame): # apply penalty force bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds) vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces) controlModel.setDOFAccelerations(ddth_sol) if flagForeSupLContact == 1: controlModel.setJointAngAccelerationLocal( foreSupL, (2.1, 0, 0)) if flagForeSupRContact == 1: controlModel.setJointAngAccelerationLocal( foreSupR, (2.1, 0, 0)) controlModel.solveHybridDynamics() extraForce[0] = viewer.GetForce() if (extraForce[0][0] != 0 or extraForce[0][1] != 0 or extraForce[0][2] != 0): forceApplyFrame += 1 vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce) applyedExtraForce[0] = extraForce[0] if forceApplyFrame * wcfg.timeStep > 0.1: viewer.ResetForce() forceApplyFrame = 0 vpWorld.step() # rendering rd_footCenter[0] = footCenter rd_CM[0] = CM.copy() rd_CM_plane[0] = CM_plane.copy() rd_footCenter_ref[0] = footCenter_ref rd_CM_plane_ref[0] = CM_ref.copy() rd_CM_ref[0] = CM_ref.copy() rd_CM_ref_vec[0] = (CM_ref - footCenter_ref) * 3. rd_CM_vec[0] = (CM - footCenter) * 3 #rd_CM_plane[0][1] = 0. if CP != None and dCP != None: rd_CP[0] = CP rd_CP_des[0] = CP_des rd_dL_des_plane[0] = dL_des_plane rd_dH_des[0] = dH_des rd_grf_des[0] = totalNormalForce - totalMass * mm.s2v( wcfg.gravity) #dL_des_plane - totalMass*mm.s2v(wcfg.gravity) rd_exf_des[0] = applyedExtraForce[0] rd_root_des[0] = rootPos[0] rd_CMP[0] = softConstPoint rd_soft_const_vec[0] = controlModel.getBodyPositionGlobal( constBody) - softConstPoint if (forceApplyFrame == 0): applyedExtraForce[0] = [0, 0, 0]
def simulateCallback(frame): global g_initFlag global forceShowFrame global forceApplyFrame global JsysPre global JsupPreL global JsupPreR global JsupPre global softConstPoint global stage global contactRendererName motionModel.update(motion[frame]) Kt, Kk, Kl, Kh, Ksc, Bt, Bl, Bh, Bsc = viewer.GetParam() Dt = 2*(Kt**.5) Dk = 2*(Kk**.5) Dl = 2*(Kl**.5) Dh = 2*(Kh**.5) Dsc = 2*(Ksc**.5) if Bsc == 0.0 : viewer.doc.showRenderer('softConstraint', False) viewer.motionViewWnd.update(1, viewer.doc) else: viewer.doc.showRenderer('softConstraint', True) renderer1 = viewer.doc.getRenderer('softConstraint') renderer1.rc.setLineWidth(0.1+Bsc*3) viewer.motionViewWnd.update(1, viewer.doc) # 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) ddth_c = controlModel.getDOFAccelerations() ype.flatten(ddth_des, ddth_des_flat) ype.flatten(dth, dth_flat) ype.flatten(ddth_c, ddth_c_flat) # jacobian refFootL = motionModel.getBodyPositionGlobal(supL) refFootR = motionModel.getBodyPositionGlobal(supR) positionFootL = [None]*footPartNum positionFootR = [None]*footPartNum for i in range(footPartNum): positionFootL[i] = controlModel.getBodyPositionGlobal(indexFootL[i]) positionFootR[i] = controlModel.getBodyPositionGlobal(indexFootR[i]) linkPositions = controlModel.getBodyPositionsGlobal() linkVelocities = controlModel.getBodyVelocitiesGlobal() linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal() linkInertias = controlModel.getBodyInertiasGlobal() jointPositions = controlModel.getJointPositionsGlobal() jointAxeses = controlModel.getDOFAxeses() CM = yrp.getCM(linkPositions, linkMasses, totalMass) dCM = yrp.getCM(linkVelocities, linkMasses, totalMass) CM_plane = copy.copy(CM); CM_plane[1]=0. dCM_plane = copy.copy(dCM); dCM_plane[1]=0. linkPositions_ref = motionModel.getBodyPositionsGlobal() CM_ref = yrp.getCM(linkPositions_ref, linkMasses, totalMass) CM_plane_ref = copy.copy(CM_ref) CM_plane_ref[1] = 0. P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM, linkInertias) dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses, linkVelocities, dCM, linkAngVelocities, linkInertias) yjc.computeJacobian2(Jsys, DOFs, jointPositions, jointAxeses, linkPositions, allLinkJointMasks) yjc.computeJacobianDerivative2(dJsys, DOFs, jointPositions, jointAxeses, linkAngVelocities, linkPositions, allLinkJointMasks) yjc.computeJacobian2(jFootL[0], DOFs, jointPositions, jointAxeses, [positionFootL[0]], jointMasksFootL[0]) yjc.computeJacobianDerivative2(dJFootL[0], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootL[0]], jointMasksFootL[0], False) yjc.computeJacobian2(jFootR[0], DOFs, jointPositions, jointAxeses, [positionFootR[0]], jointMasksFootR[0]) yjc.computeJacobianDerivative2(dJFootR[0], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootR[0]], jointMasksFootR[0], False) yjc.computeAngJacobian2(jAngFootL[0], DOFs, jointPositions, jointAxeses, [positionFootL[0]], jointMasksFootL[0]) yjc.computeAngJacobianDerivative2(dJAngFootL[0], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootL[0]], jointMasksFootL[0], False) yjc.computeAngJacobian2(jAngFootR[0], DOFs, jointPositions, jointAxeses, [positionFootR[0]], jointMasksFootR[0]) yjc.computeAngJacobianDerivative2(dJAngFootR[0], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootR[0]], jointMasksFootR[0], False) bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds) CP = yrp.getCP(contactPositions, contactForces) for i in range(len(bodyIDsToCheck)) : controlModel.SetBodyColor(bodyIDsToCheck[i], 0, 0, 0) ########################################## for i in range(len(rd_contactPositions)): rd_contactPositions[i] = [0,0,0] rd_contactForces[i] = [0,0,0] for i in range(len(contactPositions)): rd_contactPositions[i] = np.copy(contactPositions[i]) rd_contactForces[i] = np.copy(contactForces[i]) ''' if len(contactPositions) > 0: rd_contactPositions = np.copy(contactPositions) rd_contactForces = np.copy(contactForces) print("rd_contactPositions", rd_contactPositions) print("contactPositions", contactPositions) ''' ''' for i in range(len(contactRendererName)): viewer.doc.removeRenderer(contactRendererName[i]) del contactRendererName[:] for i in range(len(contactPositions)): contactRendererName.append(str(i)) #viewer.doc.addRenderer(str(i), yr.PointsRenderer([contactPositions[i]], (0,255,0))) viewer.doc.addRenderer(str(i), yr.ForcesRenderer([contactForces[i]], [contactPositions[i]], (0,255,0), .009, 0.009)) viewer.motionViewWnd.update(1, viewer.doc) ''' ########################################## contactFlagFootL = [0]*footPartNum contactFlagFootR = [0]*footPartNum partialDOFIndex = [22, 22] for i in range(len(bodyIDs)) : controlModel.SetBodyColor(bodyIDs[i], 255, 105, 105) index = controlModel.id2index(bodyIDs[i]) for j in range(len(indexFootL)): if index == indexFootL[j]: contactFlagFootL[j] = 1 if j != 0: yjc.computePartialJacobian2(jFootL[j], DOFs, jointPositions, jointAxeses, [positionFootL[j]], jointMasksFootL[j], partialDOFIndex) yjc.computePartialJacobianDerivative2(dJFootL[j], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootL[j]], jointMasksFootL[j], False, partialDOFIndex) break for j in range(len(indexFootR)): if index == indexFootR[j]: contactFlagFootR[j] = 1 if j != 0: yjc.computePartialJacobian2(jFootR[j], DOFs, jointPositions, jointAxeses, [positionFootR[j]], jointMasksFootR[j], partialDOFIndex) yjc.computePartialJacobianDerivative2(dJFootR[j], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootR[j]], jointMasksFootR[j], False, partialDOFIndex) break for j in range(len(indexFootL)): yjc.computeAngJacobian2(jAngFootL[j], DOFs, jointPositions, jointAxeses, [positionFootL[j]], jointMasksFootL[j]) yjc.computeAngJacobianDerivative2(dJAngFootL[j], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootL[j]], jointMasksFootL[j], False) yjc.computeAngJacobian2(jAngFootR[j], DOFs, jointPositions, jointAxeses, [positionFootR[j]], jointMasksFootR[j]) yjc.computeAngJacobianDerivative2(dJAngFootR[j], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootR[j]], jointMasksFootR[j], False) ''' if footPartNum == 1: footCenterL = controlModel.getBodyPositionGlobal(supL) footCenterR = controlModel.getBodyPositionGlobal(supR) else: if ((contactFlagFootL[3] == 1 or contactFlagFootL[4] == 1) and contactFlagFootL[0] == 0) or ((contactFlagFootR[3] == 1 or contactFlagFootR[4] == 1) and contactFlagFootR[0] == 0): r = 0.8 footCenterL = (controlModel.getBodyPositionGlobal(supL)*r + controlModel.getBodyPositionGlobal(indexFootL[1])*(1.0-r)) footCenterR = (controlModel.getBodyPositionGlobal(supR)*r + controlModel.getBodyPositionGlobal(indexFootR[1])*(1.0-r)) #footCenterL = controlModel.getBodyPositionGlobal(indexFootL[1]) #footCenterR = controlModel.getBodyPositionGlobal(indexFootR[1]) else : #footCenterL = (controlModel.getBodyPositionGlobal(supL) + controlModel.getBodyPositionGlobal(indexFootL[1]))/2.0 #footCenterR = (controlModel.getBodyPositionGlobal(supR) + controlModel.getBodyPositionGlobal(indexFootR[1]))/2.0 #footCenterL = controlModel.getBodyPositionGlobal(indexFootL[1]) #footCenterR = controlModel.getBodyPositionGlobal(indexFootR[1]) r = 0.8 footCenterL = (controlModel.getBodyPositionGlobal(indexFootL[1])*r + controlModel.getBodyPositionGlobal(indexFootL[3])*(1.0-r)) footCenterR = (controlModel.getBodyPositionGlobal(indexFootR[1])*r + controlModel.getBodyPositionGlobal(indexFootR[3])*(1.0-r)) ''' ''' if stage == POWERFUL_BALANCING: footCenterL = controlModel.getBodyPositionGlobal(indexFootL[1]) footCenterR = controlModel.getBodyPositionGlobal(indexFootR[1]) else: footCenterL = (controlModel.getBodyPositionGlobal(indexFootL[1]) + controlModel.getBodyPositionGlobal(indexFootL[3]) )/2.0 footCenterR = (controlModel.getBodyPositionGlobal(indexFootR[1]) + controlModel.getBodyPositionGlobal(indexFootR[3]))/2.0 ''' ''' p1 = controlModel.getBodyPositionGlobal(indexFootL[0]) p2 = controlModel.getBodyPositionGlobal(indexFootR[0]) p3 = controlModel.getBodyPositionGlobal(indexFootL[3]) p4 = controlModel.getBodyPositionGlobal(indexFootR[3]) print(frame, "supL", p1[1]) print(frame, "supR", p2[1]) print(frame, "calcL", p3[1]) print(frame, "calcR", p4[1]) ''' #footCenter = footCenterL + (footCenterR - footCenterL)/2.0 #footCenter[1] = 0. # ''' if checkAll(contactFlagFootL, 0) == 1 and checkAll(contactFlagFootR, 0) == 1: footCenter = footCenter elif checkAll(contactFlagFootL, 0) == 1 : footCenter = footCenterR elif checkAll(contactFlagFootR, 0) == 1 : footCenter = footCenterL ''' if footPartNum == 1: desFCL = (controlModel.getBodyPositionGlobal(supL)) desFCR = (controlModel.getBodyPositionGlobal(supR)) else : r = .4 desFCL = (controlModel.getBodyPositionGlobal(indexFootL[1])*r + controlModel.getBodyPositionGlobal(indexFootL[3])*(1.0-r))#controlModel.getBodyPositionGlobal(indexFootL[1]) desFCR = (controlModel.getBodyPositionGlobal(indexFootR[1])*r + controlModel.getBodyPositionGlobal(indexFootR[3])*(1.0-r))#controlModel.getBodyPositionGlobal(indexFootR[1]) desFC = desFCL + (desFCR - desFCL)/2.0 if checkAll(contactFlagFootL, 0) == 1 and checkAll(contactFlagFootR, 0) == 1: desFC = desFC elif checkAll(contactFlagFootL, 0) == 1 : desFC = desFCR elif checkAll(contactFlagFootR, 0) == 1 : desFC = desFCL #if stage == MOTION_TRACKING: # desFC = desFCL desFC[1] = 0 rd_footCenter_des[0] = desFC.copy() curRelCMVec = CM_plane - desFC vecRatio = mm.length(curRelCMVec)*0. #print(frame, vecRatio) footCenter = desFC - curRelCMVec*(vecRatio)#/10.0 footCenter_ref = refFootL + (refFootR - refFootL)/2.0 #footCenter_ref[1] = 0. footCenter[1] = 0. vecRatio = mm.length(curRelCMVec)*0. softConstPointOffset = -curRelCMVec*(vecRatio)#/10.0 #print(frame, vecRatio, softConstPointOffset) desForeSupLAcc = [0,0,0] desForeSupRAcc = [0,0,0] totalNormalForce = [0,0,0] for i in range(len(contactForces)): totalNormalForce[0] += contactForces[i][0] totalNormalForce[1] += contactForces[i][1] totalNormalForce[2] += contactForces[i][2] #print((totalMass*mm.s2v(wcfg.gravity))[1]) print("totalNormalForce=", totalNormalForce[1]) print("F_Diff=", (totalMass*mm.s2v(wcfg.gravity))[1]+totalNormalForce[1]) # linear momentum CM_ref_plane = footCenter dL_des_plane = Kl*totalMass*(CM_ref_plane - CM_plane) - Dl*totalMass*dCM_plane print("CM_Diff=",mm.length(CM_ref_plane - CM_plane)) # angular momentum CP_ref = footCenter timeStep = 30. if CP_old[0]==None or CP==None: dCP = None else: dCP = (CP - CP_old[0])/(1/timeStep) CP_old[0] = CP if CP!=None and dCP!=None: ddCP_des = Kh*(CP_ref - CP) - Dh*(dCP) CP_des = CP + dCP*(1/timeStep) + .5*ddCP_des*((1/timeStep)**2) #dH_des = np.cross((CP_des - CM), (dL_des_plane + totalMass*mm.s2v(wcfg.gravity))) dH_des = np.cross((CP_des - CM_plane), (dL_des_plane + totalMass*mm.s2v(wcfg.gravity))) print("CP_Diff=",mm.length(CP_des - CP)) else: dH_des = None # momentum matrix RS = np.dot(P, Jsys) R, S = np.vsplit(RS, 2) rs = np.dot((np.dot(dP, Jsys) + np.dot(P, dJsys)), dth_flat) r_bias, s_bias = np.hsplit(rs, 2) ############################## # soft point constraint ##################################################### P_cur = controlModel.getBodyPositionGlobal(constBody) constBodyVec = P_cur - footCenter softConstPoint = [footCenter[0]+softConstPointOffset[0], mm.length(constBodyVec), footCenter[2]+softConstPointOffset[2]] ##################################################### P_des = softConstPoint #P_cur = controlModel.getBodyPositionGlobal(constBody) dP_des = [0, 0, 0] dP_cur = controlModel.getBodyVelocityGlobal(constBody) ddP_des1 = Ksc*(-(P_des - P_cur)) - Dsc*(-(dP_cur - dP_des)) r = -(P_des - P_cur) I = np.vstack(([1,0,0],[0,1,0],[0,0,1])) Z = np.hstack((I, mm.getCrossMatrixForm(-r))) yjc.computeJacobian2(Jconst, DOFs, jointPositions, jointAxeses, [P_cur], constJointMasks) JL, JA = np.vsplit(Jconst, 2) Q1 = np.dot(Z, Jconst) q1 = np.dot(JA, dth_flat) q2 = np.dot(mm.getCrossMatrixForm(q1), np.dot(mm.getCrossMatrixForm(q1), r)) yjc.computeJacobianDerivative2(dJconst, DOFs, jointPositions, jointAxeses, linkAngVelocities, [P_cur], constJointMasks, False) q_bias1 = np.dot(np.dot(Z, dJconst), dth_flat) + q2 ############################## flagContact = True if dH_des==None or np.any(np.isnan(dH_des)) == True: flagContact = False viewer.doc.showRenderer('rd_grf_des', False) viewer.motionViewWnd.update(1, viewer.doc) else: viewer.doc.showRenderer('rd_grf_des', True) viewer.motionViewWnd.update(1, viewer.doc) ''' 0 : initial 1 : contact 2 : fly 3 : landing ''' #MOTION = FORWARD_JUMP if mit.MOTION == mit.FORWARD_JUMP : frame_index = [136, 100] #frame_index = [100000, 100000] elif mit.MOTION == mit.TAEKWONDO: frame_index = [130, 100] #frame_index = [100000, 100000] elif mit.MOTION == mit.TAEKWONDO2: frame_index = [130+40, 100] elif mit.MOTION == mit.WALK: frame_index = [10000, 60] else : frame_index = [1000000, 1000000] #MOTION = TAEKWONDO #frame_index = [135, 100] ''' if frame > 300 : if stage != DYNAMIC_BALANCING: print("#", frame,"-DYNAMIC_BALANCING") stage = DYNAMIC_BALANCING Kk = Kk*1 Dk = 2*(Kk**.5) ''' if frame > frame_index[0] : if stage != POWERFUL_BALANCING: print("#", frame,"-POWERFUL_BALANCING") stage = POWERFUL_BALANCING Kk = Kk*2 Dk = 2*(Kk**.5) elif frame > frame_index[1]: if stage != MOTION_TRACKING: print("#", frame,"-MOTION_TRACKING") stage = MOTION_TRACKING trackingW = w #if checkAll(contactFlagFootR, 0) != 1 : if stage == MOTION_TRACKING: trackingW = w2 #stage = POWERFUL_BALANCING Bt = Bt*2 # optimization mot.addTrackingTerms(problem, totalDOF, Bt, trackingW, ddth_des_flat) mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1, q_bias1) if flagContact == True: if stage != MOTION_TRACKING+10: mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias) mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias) a_sup_2 = [None] Jsup_2 = [None] dJsup_2 = [None] ############################## # Hard constraint if stage != MOTION_TRACKING: Kk2 = Kk * 2.0 else : Kk2 = Kk * 1.5 Dk2 = 2*(Kk2**.5) ''' desLinearAccL, desPosL = getDesFootLinearAcc(motionModel, controlModel, supL, ModelOffset, CM_ref, CM, Kk2, Dk2) desLinearAccR, desPosR = getDesFootLinearAcc(motionModel, controlModel, supR, ModelOffset, CM_ref, CM, Kk2, Dk2) desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, supL, Kk2, Dk2) desAngularAccR = getDesFootAngularAcc(motionModel, controlModel, supR, Kk2, Dk2) ''' if stage != MOTION_TRACKING: idx = 0 #LEFT/RIGHT_TOES if stage != MOTION_TRACKING: desLinearAccL, desPosL = getDesFootLinearAcc(motionModel, controlModel, indexFootL[idx], ModelOffset, CM_ref, CM, Kk2, Dk2, 0.14)#0.076) desLinearAccR, desPosR = getDesFootLinearAcc(motionModel, controlModel, indexFootR[idx], ModelOffset, CM_ref, CM, Kk2, Dk2, 0.14) desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[idx], Kk2, Dk2, [0,0,-1], [0,1,1.5]) desAngularAccR = getDesFootAngularAcc(motionModel, controlModel, indexFootR[idx], Kk2, Dk2, [0,0,-1], [0,1,1.5]) a_sup_2 = np.hstack(( np.hstack((desLinearAccL, desAngularAccL)), np.hstack((desLinearAccR, desAngularAccR)) )) Jsup_2 = np.vstack((jFootL[idx], jFootR[idx])) dJsup_2 = np.vstack((dJFootL[idx], dJFootR[idx])) else: desLinearAccL, desPosL = getDesFootLinearAcc(motionModel, controlModel, indexFootL[idx], ModelOffset, CM_ref, CM, Kk2, Dk2, 0.040) desLinearAccR, desPosR = getDesFootLinearAcc(motionModel, controlModel, indexFootR[idx], ModelOffset, CM_ref, CM, Kk2, Dk2, 0.040) desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[idx], Kk2, Dk2) a_sup_2 = np.hstack((desLinearAccL, desAngularAccL)) Jsup_2 = jFootL[idx] dJsup_2 = dJFootL[idx] rd_DesPosL[0] = desPosL.copy() rd_DesPosR[0] = desPosR.copy() else: if footPartNum != 5: idx = 0 desLinearAccL, desPosL = getDesFootLinearAcc(motionModel, controlModel, indexFootL[idx], ModelOffset, CM_ref, CM, Kk2, Dk2, 0.045) desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[idx], Kk2, Dk2) a_sup_2 = np.hstack(( desLinearAccL, desAngularAccL)) Jsup_2 = (jFootL[idx]) dJsup_2 = (dJFootL[idx]) ''' idx = 4 desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[idx], Kk2, Dk2) a_sup_2 = np.hstack(( a_sup_2, desAngularAccL)) Jsup_2 = np.vstack(( Jsup_2, jAngFootL[idx])) dJsup_2 = np.vstack(( dJsup_2, dJAngFootL[idx])) ''' ''' idx = 1 desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[idx], Kk2, Dk2) a_sup_2 = np.hstack(( a_sup_2, desAngularAccL)) Jsup_2 = np.vstack(( Jsup_2, jAngFootL[idx])) dJsup_2 = np.vstack(( dJsup_2, dJAngFootL[idx])) ''' else: idx = 0 desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[idx], Kk2, Dk2) desAngularAccR = getDesFootAngularAcc(motionModel, controlModel, indexFootR[idx], Kk2, Dk2) a_sup_2 = np.hstack(( desAngularAccL, desAngularAccR )) Jsup_2 = np.vstack((jAngFootL[idx], jAngFootR[idx])) dJsup_2 = np.vstack((dJAngFootL[idx], dJAngFootR[idx])) ############################## ############################## # Additional constraint if stage != MOTION_TRACKING+10: #Kk2 = Kk * 2.5 Kk2 = Kk * 2.5 Dk2 = 2*(Kk2**.5) desForePosL = [0,0,0] desForePosR = [0,0,0] desRearPosL = [0,0,0] desRearPosR = [0,0,0] for i in range(1, footPartNum) : if stage != MOTION_TRACKING: axis = [0,1,0] desAng = [0,1,0] desY = 0.04 if i == 1 or i == 2: desAng = [0,1,1.2] desY = 0.076 if contactFlagFootL[i] == 1: desLinearAccL, desForePosL = getDesFootLinearAcc(motionModel, controlModel, indexFootL[i], ModelOffset, CM_ref, CM, Kk2, Dk2, desY) desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[i], Kk2, Dk2, axis, desAng) a_sup_2 = np.hstack(( a_sup_2, np.hstack((desLinearAccL, desAngularAccL)) )) Jsup_2 = np.vstack(( Jsup_2, jFootL[i] )) dJsup_2 = np.vstack(( dJsup_2, dJFootL[i] )) if contactFlagFootR[i] == 1: desLinearAccR, desForePosR = getDesFootLinearAcc(motionModel, controlModel, indexFootR[i], ModelOffset, CM_ref, CM, Kk2, Dk2, desY) desAngularAccR = getDesFootAngularAcc(motionModel, controlModel, indexFootR[i], Kk2, Dk2, axis, desAng) a_sup_2 = np.hstack(( a_sup_2, np.hstack((desLinearAccR, desAngularAccR)) )) Jsup_2 = np.vstack(( Jsup_2, jFootR[i] )) dJsup_2 = np.vstack(( dJsup_2, dJFootR[i] )) else: if contactFlagFootL[i] == 1: desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[i], Kk2, Dk2) a_sup_2 = np.hstack(( a_sup_2, desAngularAccL )) Jsup_2 = np.vstack(( Jsup_2, jAngFootL[i] )) dJsup_2 = np.vstack(( dJsup_2, dJAngFootL[i] )) if contactFlagFootR[i] == 1: desAngularAccR = getDesFootAngularAcc(motionModel, controlModel, indexFootR[i], Kk2, Dk2) a_sup_2 = np.hstack(( a_sup_2, desAngularAccR )) Jsup_2 = np.vstack(( Jsup_2, jAngFootR[i] )) dJsup_2 = np.vstack(( dJsup_2, dJAngFootR[i] )) ''' for i in range(1, footPartNum) : if contactFlagFootL[i] == 1: desLinearAccL, desForePosL = getDesFootLinearAcc(motionModel, controlModel, indexFootL[i], ModelOffset, CM_ref, CM, Kk2, Dk2, 0.034) desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[i], Kk2, Dk2) a_sup_2 = np.hstack(( a_sup_2, np.hstack((desLinearAccL, desAngularAccL)) )) Jsup_2 = np.vstack(( Jsup_2, jFootL[i] )) dJsup_2 = np.vstack(( dJsup_2, dJFootL[i] )) if contactFlagFootR[i] == 1: desLinearAccR, desForePosR = getDesFootLinearAcc(motionModel, controlModel, indexFootR[i], ModelOffset, CM_ref, CM, Kk2, Dk2, 0.034) desAngularAccR = getDesFootAngularAcc(motionModel, controlModel, indexFootR[i], Kk2, Dk2) a_sup_2 = np.hstack(( a_sup_2, np.hstack((desLinearAccR, desAngularAccR)) )) Jsup_2 = np.vstack(( Jsup_2, jFootR[i] )) dJsup_2 = np.vstack(( dJsup_2, dJFootR[i] )) ''' rd_DesForePosL[0] = desForePosL rd_DesForePosR[0] = desForePosR rd_DesRearPosL[0] = desRearPosL rd_DesRearPosR[0] = desRearPosR ############################## mot.setConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat, a_sup_2) r = problem.solve() problem.clear() ype.nested(r['x'], ddth_sol) rootPos[0] = controlModel.getBodyPositionGlobal(selectedBody) localPos = [[0, 0, 0]] rd_Joint[0] = controlModel.getJointPositionGlobal(motion[0].skeleton.getJointIndex('LeftMetatarsal_1')) rd_Joint2[0] = controlModel.getJointPositionGlobal(motion[0].skeleton.getJointIndex('LeftMetatarsal_3')) rd_Joint3[0] = controlModel.getJointPositionGlobal(motion[0].skeleton.getJointIndex('LeftPhalange_1')) rd_Joint4[0] = controlModel.getJointPositionGlobal(motion[0].skeleton.getJointIndex('LeftPhalange_3')) for i in range(stepsPerFrame): # apply penalty force bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds) vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces) extraForce[0] = viewer.GetForce() if (extraForce[0][0] != 0 or extraForce[0][1] != 0 or extraForce[0][2] != 0) : forceApplyFrame += 1 #vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce) controlModel.applyBodyForceGlobal(selectedBody, extraForce[0]) applyedExtraForce[0] = extraForce[0] if forceApplyFrame*wcfg.timeStep > 0.1: viewer.ResetForce() forceApplyFrame = 0 controlModel.setDOFAccelerations(ddth_sol) controlModel.solveHybridDynamics() ''' extraForce[0] = viewer.GetForce() if (extraForce[0][0] != 0 or extraForce[0][1] != 0 or extraForce[0][2] != 0) : forceApplyFrame += 1 vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce) applyedExtraForce[0] = extraForce[0] if forceApplyFrame*wcfg.timeStep > 0.1: viewer.ResetForce() forceApplyFrame = 0 ''' vpWorld.step() # rendering rd_footCenter[0] = footCenter rd_CM[0] = CM.copy() rd_CM_plane[0] = CM_plane.copy() rd_footCenter_ref[0] = footCenter_ref rd_CM_plane_ref[0] = CM_ref.copy() rd_CM_ref[0] = CM_ref.copy() rd_CM_ref_vec[0] = (CM_ref - footCenter_ref)*3. rd_CM_vec[0] = (CM - footCenter)*3 #rd_CM_plane[0][1] = 0. if CP!=None and dCP!=None: rd_CP[0] = CP rd_CP_des[0] = CP_des rd_dL_des_plane[0] = dL_des_plane rd_dH_des[0] = dH_des rd_grf_des[0] = totalNormalForce# - totalMass*mm.s2v(wcfg.gravity)#dL_des_plane - totalMass*mm.s2v(wcfg.gravity) rd_exf_des[0] = applyedExtraForce[0] rd_root_des[0] = rootPos[0] rd_CMP[0] = softConstPoint rd_soft_const_vec[0] = controlModel.getBodyPositionGlobal(constBody)-softConstPoint #indexL = motion[0].skeleton.getJointIndex('Hips') #indexR = motion[0].skeleton.getJointIndex('Spine1') indexL = indexFootL[0] indexR = indexFootR[0] curAng = [controlModel.getBodyOrientationGlobal(indexL)] curAngY = np.dot(curAng, np.array([0,0,1])) rd_footL_vec[0] = np.copy(curAngY[0]) rd_footCenterL[0] = controlModel.getBodyPositionGlobal(indexL) curAng = [controlModel.getBodyOrientationGlobal(indexR)] curAngY = np.dot(curAng, np.array([0,0,1])) rd_footR_vec[0] = np.copy(curAngY[0]) rd_footCenterR[0] = controlModel.getBodyPositionGlobal(indexR) if (forceApplyFrame == 0) : applyedExtraForce[0] = [0, 0, 0]
def simulateCallback(frame): global g_initFlag global preFootCenterL, preFootCenterR global preFootOrientationL, preFootOrientationR global forceShowFrame global forceApplyFrame global JsysPre global JsupPreL global JsupPreR global JsupPre global softConstPoint global stage motionModel.update(motion[frame]) Kt, Kk, Kl, Kh, Ksc, Bt, Bl, Bh, Bsc = viewer.GetParam() if stage == 3: Bsc = 0 #Kl *= 1.5 Dt = 2*(Kt**.5) Dk = 2*(Kk**.5) Dl = 2*(Kl**.5) Dh = 2*(Kh**.5) Dsc = 2*(Ksc**.5) if Bsc == 0.0 : viewer.doc.showRenderer('softConstraint', False) viewer.motionViewWnd.update(1, viewer.doc) else: viewer.doc.showRenderer('softConstraint', True) renderer1 = viewer.doc.getRenderer('softConstraint') renderer1.rc.setLineWidth(0.1+Bsc*3) viewer.motionViewWnd.update(1, viewer.doc) # 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) ddth_c = controlModel.getDOFAccelerations() ype.flatten(ddth_des, ddth_des_flat) ype.flatten(dth, dth_flat) ype.flatten(ddth_c, ddth_c_flat) # jacobian footCenterL = controlModel.getBodyPositionGlobal(supL) footCenterR = controlModel.getBodyPositionGlobal(supR) refFootL = motionModel.getBodyPositionGlobal(supL) refFootR = motionModel.getBodyPositionGlobal(supR) footCenter = footCenterL + (footCenterR - footCenterL)/2.0 footCenter[1] = 0. footCenter_ref = refFootL + (refFootR - refFootL)/2.0 footCenter_ref[1] = 0. linkPositions = controlModel.getBodyPositionsGlobal() linkVelocities = controlModel.getBodyVelocitiesGlobal() linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal() linkInertias = controlModel.getBodyInertiasGlobal() jointPositions = controlModel.getJointPositionsGlobal() jointAxeses = controlModel.getDOFAxeses() CM = yrp.getCM(linkPositions, linkMasses, totalMass) dCM = yrp.getCM(linkVelocities, linkMasses, totalMass) CM_plane = copy.copy(CM); CM_plane[1]=0. dCM_plane = copy.copy(dCM); dCM_plane[1]=0. linkPositions_ref = motionModel.getBodyPositionsGlobal() CM_plane_ref = yrp.getCM(linkPositions_ref, linkMasses, totalMass) CM_plane_ref[1] = 0. P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM, linkInertias) dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses, linkVelocities, dCM, linkAngVelocities, linkInertias) yjc.computeJacobian2(Jsys, DOFs, jointPositions, jointAxeses, linkPositions, allLinkJointMasks) dJsys = (Jsys - JsysPre)/(1/30.) JsysPre = Jsys #yjc.computeJacobianDerivative2(dJsys, DOFs, jointPositions, jointAxeses, linkAngVelocities, linkPositions, allLinkJointMasks) if g_initFlag == 0: preFootCenterL = footCenterL preFootCenterR = footCenterR preFootCenterL[1] -= 0.02 preFootCenterR[1] -= 0.02 preFootOrientationL = controlModel.getBodyOrientationGlobal(supL) preFootOrientationR = controlModel.getBodyOrientationGlobal(supR) softConstPoint = controlModel.getBodyPositionGlobal(constBody) #softConstPoint[2] += 0.3 #softConstPoint[1] -= 1.1 #softConstPoint[0] += 0.1 softConstPoint[1] -= .3 #softConstPoint[0] -= .1 #softConstPoint[1] -= 1. #softConstPoint[0] -= .5 g_initFlag = 1 yjc.computeJacobian2(JsupL, DOFs, jointPositions, jointAxeses, [footCenterL], supLJointMasks) dJsupL = (JsupL - JsupPreL)/(1/30.) JsupPreL = JsupL #yjc.computeJacobianDerivative2(dJsupL, DOFs, jointPositions, jointAxeses, linkAngVelocities, [footCenterL], supLJointMasks, False) yjc.computeJacobian2(JsupR, DOFs, jointPositions, jointAxeses, [footCenterR], supRJointMasks) dJsupR = (JsupR - JsupPreR)/(1/30.) JsupPreR = JsupR #yjc.computeJacobianDerivative2(dJsupR, DOFs, jointPositions, jointAxeses, linkAngVelocities, [footCenterR], supRJointMasks, False) preFootCenter = preFootCenterL + (preFootCenterR - preFootCenterL)/2.0 preFootCenter[1] = 0 bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds) CP = yrp.getCP(contactPositions, contactForces) # linear momentum CM_ref_plane = footCenter #CM_ref_plane = preFootCenter dL_des_plane = Kl*totalMass*(CM_ref_plane - CM_plane) - Dl*totalMass*dCM_plane #print("dL_des_plane ", dL_des_plane ) #dL_des_plane[1] = 0. # angular momentum CP_ref = footCenter timeStep = 30. if CP_old[0]==None or CP==None: dCP = None else: dCP = (CP - CP_old[0])/(1/timeStep) CP_old[0] = CP if CP!=None and dCP!=None: ddCP_des = Kh*(CP_ref - CP) - Dh*(dCP) CP_des = CP + dCP*(1/timeStep) + .5*ddCP_des*((1/timeStep)**2) dH_des = np.cross((CP_des - CM), (dL_des_plane + totalMass*mm.s2v(wcfg.gravity))) #dH_des = np.cross((CP_des - CM_plane), (dL_des_plane + totalMass*mm.s2v(wcfg.gravity))) #dH_des = [0, 0, 0] else: dH_des = None CMP = yrp.getCMP(contactForces, CM) r = [0,0,0] if CP!= None and np.any(np.isnan(CMP))!=True : r = CP - CMP #print("r.l", mm.length(r)) #Bba = Bh*(mm.length(r)) Bba = Bh # momentum matrix RS = np.dot(P, Jsys) R, S = np.vsplit(RS, 2) rs = np.dot((np.dot(dP, Jsys) + np.dot(P, dJsys)), dth_flat) r_bias, s_bias = np.hsplit(rs, 2) ############################## # soft point constraint ''' cmDiff = footCenter - CM_plane print("cmDiff", cmDiff) if stage == 3: softConstPoint += ''' P_des = softConstPoint P_cur = controlModel.getBodyPositionGlobal(constBody) dP_des = [0, 0, 0] dP_cur = controlModel.getBodyVelocityGlobal(constBody) ddP_des1 = Ksc*(P_des - P_cur) - Dsc*(dP_cur - dP_des) r = P_des - P_cur I = np.vstack(([1,0,0],[0,1,0],[0,0,1])) Z = np.hstack((I, mm.getCrossMatrixForm(-r))) yjc.computeJacobian2(Jconst, DOFs, jointPositions, jointAxeses, [softConstPoint], constJointMasks) JL, JA = np.vsplit(Jconst, 2) Q1 = np.dot(Z, Jconst) q1 = np.dot(JA, dth_flat) q2 = np.dot(mm.getCrossMatrixForm(q1), np.dot(mm.getCrossMatrixForm(q1), r)) yjc.computeJacobianDerivative2(dJconst, DOFs, jointPositions, jointAxeses, linkAngVelocities, [softConstPoint], constJointMasks, False) q_bias1 = np.dot(np.dot(Z, dJconst), dth_flat) + q2 ''' P_des = preFootCenterR P_cur = controlModel.getBodyPositionGlobal(supR) P_cur[1] = 0 dP_des = [0, 0, 0] dP_cur = controlModel.getBodyVelocityGlobal(supR) ddP_des2 = Kp*(P_des - P_cur) - Dp*(dP_cur - dP_des) r = P_des - P_cur #print("r2", r) I = np.vstack(([1,0,0],[0,1,0],[0,0,1])) Z = np.hstack((I, mm.getCrossMatrixForm(-r))) JL, JA = np.vsplit(JsupR, 2) Q2 = np.dot(Z, JsupR) q1 = np.dot(JA, dth_flat) q2 = np.dot(mm.getCrossMatrixForm(q1), np.dot(mm.getCrossMatrixForm(q1), r)) q_bias2 = np.dot(np.dot(Z, dJsupR), dth_flat) + q2 ''' #print("Q1", Q1) ''' print("ddP_des1", ddP_des1) q_ddth1 = np.dot(Q1, ddth_c_flat) print("q_ddth1", q_ddth1) print("q_bias1", q_bias1) ddp1 = q_ddth1+q_bias1 print("ddp1", ddp1) print("diff1", ddP_des1-ddp1) ''' ''' print("ddP_des2", ddP_des2) q_ddth2 = np.dot(Q2, ddth_c_flat) print("q_ddth2", q_ddth2) print("q_bias2", q_bias2) ddp2 = q_ddth2+q_bias2 print("ddp2", ddp2) print("diff2", ddP_des2-ddp2) ''' ############################## ############################ # IK ''' P_des = preFootCenterL P_cur = controlModel.getJointPositionGlobal(supL) r = P_des - P_cur Q_des = preFootOrientationL Q_cur = controlModel.getJointOrientationGlobal(supL) rv = mm.logSO3(np.dot(Q_cur.transpose(), Q_des)) #print("rv", rv) des_v_sup = (r[0],r[1],r[2], rv[0], rv[1], rv[2]) A_large = np.dot(JsupL.T, JsupL) b_large = np.dot(JsupL.T, des_v_sup) des_d_th = npl.lstsq(A_large, b_large) ype.nested(des_d_th[0], d_th_IK_L) P_des2 = preFootCenterR P_cur2 = controlModel.getJointPositionGlobal(supR) r2 = P_des2 - P_cur2 Q_des2 = preFootOrientationR Q_cur2 = controlModel.getJointOrientationGlobal(supR) rv2 = mm.logSO3(np.dot(Q_cur2.transpose(), Q_des2)) #print("Q_des2", Q_des2) #print("Q_cur2", Q_cur2) #print("rv2", rv2) des_v_sup2 = (r2[0],r2[1],r2[2], rv2[0], rv2[1], rv[2]) A_large = np.dot(JsupR.T, JsupR) b_large = np.dot(JsupR.T, des_v_sup2) des_d_th = npl.lstsq(A_large, b_large) ype.nested(des_d_th[0], d_th_IK_R) for i in range(len(d_th_IK_L)): for j in range(len(d_th_IK_L[i])): d_th_IK[i][j] = d_th_IK_L[i][j] + d_th_IK_R[i][j] th_IK = yct.getIntegralDOF(th, d_th_IK, 1/timeStep) dd_th_IK = yct.getDesiredDOFAccelerations(th_IK, th, d_th_IK, dth, ddth_r, Kk, Dk) ype.flatten(d_th_IK, d_th_IK_flat) ype.flatten(dd_th_IK, dd_th_IK_flat) ''' ############################ flagContact = True if dH_des==None or np.any(np.isnan(dH_des)) == True: flagContact = False ''' 0 : initial 1 : contact 2 : fly 3 : landing ''' if flagContact == False : if stage == 1: stage = 2 print("fly") else: if stage == 0: stage = 1 print("contact") elif stage == 2: stage = 3 print("landing") if stage == 3: Bt = Bt*0.8 Bl = Bl*1 # optimization mot.addTrackingTerms(problem, totalDOF, Bt, w, ddth_des_flat) #mot.addTrackingTerms(problem, totalDOF, Bk, w_IK, dd_th_IK_flat) mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1, q_bias1) #mot.addSoftPointConstraintTerms(problem, totalDOF, Bp, ddP_des2, Q2, q_bias2) #mot.addConstraint(problem, totalDOF, JsupL, dJsupL, dth_flat, a_sup) #mot.addConstraint(problem, totalDOF, JsupR, dJsupR, dth_flat, a_sup2) desLinearAccL = [0,0,0] desAngularAccL = [0,0,0] desLinearAccR = [0,0,0] desAngularAccR = [0,0,0] refPos = motionModel.getBodyPositionGlobal(supL) refPos[0] += ModelOffset[0] refPos[1] = 0 refVel = motionModel.getBodyVelocityGlobal(supL) curPos = controlModel.getBodyPositionGlobal(supL) #curPos[1] = 0 curVel = controlModel.getBodyVelocityGlobal(supL) refAcc = (0,0,0) if stage == 3: refPos = curPos refPos[1] = 0 if curPos[1] < 0.0: curPos[1] = 0 else : curPos[1] = 0 rd_DesPosL[0] = refPos #(p_r, p, v_r, v, a_r, Kt, Dt) desLinearAccL = yct.getDesiredAcceleration(refPos, curPos, refVel, curVel, refAcc, Kk, Dk) #desLinearAccL[1] = 0 refPos = motionModel.getBodyPositionGlobal(supR) refPos[0] += ModelOffset[0] refPos[1] = 0 refVel = motionModel.getBodyVelocityGlobal(supR) curPos = controlModel.getBodyPositionGlobal(supR) #curPos[1] = 0 curVel = controlModel.getBodyVelocityGlobal(supR) if stage == 3: refPos = curPos refPos[1] = 0 if curPos[1] < 0.0: curPos[1] = 0 else : curPos[1] = 0 rd_DesPosR[0] = refPos desLinearAccR = yct.getDesiredAcceleration(refPos, curPos, refVel, curVel, refAcc, Kk, Dk) #desLinearAccR[1] = 0 #(th_r, th, dth_r, dth, ddth_r, Kt, Dt) refAng = [preFootOrientationL] curAng = [controlModel.getBodyOrientationGlobal(supL)] refAngVel = motionModel.getBodyAngVelocityGlobal(supL) curAngVel = controlModel.getBodyAngVelocityGlobal(supL) refAngAcc = (0,0,0) #desAngularAccL = yct.getDesiredAngAccelerations(refAng, curAng, refAngVel, curAngVel, refAngAcc, Kk, Dk) curAngY = np.dot(curAng, np.array([0,1,0])) aL = mm.logSO3(mm.getSO3FromVectors(curAngY[0], np.array([0,1,0]))) print("curAngYL=",curAngY, "aL=", aL) desAngularAccL = [Kk*aL + Dk*(refAngVel-curAngVel)] refAng = [preFootOrientationR] curAng = [controlModel.getBodyOrientationGlobal(supR)] refAngVel = motionModel.getBodyAngVelocityGlobal(supR) curAngVel = controlModel.getBodyAngVelocityGlobal(supR) refAngAcc = (0,0,0) #desAngularAccR = yct.getDesiredAngAccelerations(refAng, curAng, refAngVel, curAngVel, refAngAcc, Kk, Dk) curAngY = np.dot(curAng, np.array([0,1,0])) aL = mm.logSO3(mm.getSO3FromVectors(curAngY[0], np.array([0,1,0]))) desAngularAccR = [Kk*aL + Dk*(refAngVel-curAngVel)] print("curAngYR=",curAngY, "aL=", aL) a_sup_2 = [desLinearAccL[0], desLinearAccL[1], desLinearAccL[2], desAngularAccL[0][0], desAngularAccL[0][1], desAngularAccL[0][2], desLinearAccR[0], desLinearAccR[1], desLinearAccR[2], desAngularAccR[0][0], desAngularAccR[0][1], desAngularAccR[0][2]] if stage == 2 :#or stage == 3: refAccL = motionModel.getBodyAccelerationGlobal(supL) refAndAccL = motionModel.getBodyAngAccelerationGlobal(supL) refAccR = motionModel.getBodyAccelerationGlobal(supR) refAndAccR = motionModel.getBodyAngAccelerationGlobal(supR) a_sup_2 = [refAccL[0], refAccL[1], refAccL[2], refAndAccL[0], refAndAccL[1], refAndAccL[2], refAccR[0], refAccR[1], refAccR[2], refAndAccR[0], refAndAccR[1], refAndAccR[2]] ''' a_sup_2 = [0,0,0, desAngularAccL[0][0], desAngularAccL[0][1], desAngularAccL[0][2], 0,0,0, desAngularAccR[0][0], desAngularAccR[0][1], desAngularAccR[0][2]] ''' Jsup_2 = np.vstack((JsupL, JsupR)) dJsup_2 = np.vstack((dJsupL, dJsupR)) if flagContact == True: 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_2, dJsup_2, dth_flat, a_sup_2) #mot.setConstraint(problem, totalDOF, JsupR, dJsupR, dth_flat, a_sup2) #mot.setConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat, a_sup_2) #mot.addConstraint(problem, totalDOF, Jsup_2, dJsup_2, d_th_IK_flat, a_sup_2) ''' jZ = np.dot(dJsup_2.T, dJsup_2) lamda = 0.001 for i in range(len(jZ)): for j in range(len(jZ[0])): if i == j : jZ[i][j] += lamda jZInv = npl.pinv(jZ) jA = np.dot(Jsup_2, np.dot(jZInv, np.dot(dJsup_2.T, -Jsup_2))) mot.addConstraint2(problem, totalDOF, jA, a_sup_2) ''' r = problem.solve() problem.clear() ype.nested(r['x'], ddth_sol) rootPos[0] = controlModel.getBodyPositionGlobal(selectedBody) localPos = [[0, 0, 0]] for i in range(stepsPerFrame): # apply penalty force bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds) vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces) controlModel.setDOFAccelerations(ddth_sol) controlModel.solveHybridDynamics() extraForce[0] = viewer.GetForce() if (extraForce[0][0] != 0 or extraForce[0][1] != 0 or extraForce[0][2] != 0) : forceApplyFrame += 1 vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce) applyedExtraForce[0] = extraForce[0] if forceApplyFrame*wcfg.timeStep > 0.1: viewer.ResetForce() forceApplyFrame = 0 vpWorld.step() # rendering rd_footCenter[0] = footCenter rd_footCenterL[0] = preFootCenterL rd_footCenterR[0] = preFootCenterR rd_CM[0] = CM rd_CM_plane[0] = CM_plane.copy() rd_footCenter_ref[0] = footCenter_ref rd_CM_plane_ref[0] = CM_plane_ref.copy() #rd_CM_plane[0][1] = 0. if CP!=None and dCP!=None: rd_CP[0] = CP rd_CP_des[0] = CP_des rd_dL_des_plane[0] = dL_des_plane rd_dH_des[0] = dH_des rd_grf_des[0] = dL_des_plane - totalMass*mm.s2v(wcfg.gravity) rd_exf_des[0] = applyedExtraForce[0] #print("rd_exf_des", rd_exf_des[0]) rd_root_des[0] = rootPos[0] rd_CMP[0] = softConstPoint rd_soft_const_vec[0] = controlModel.getBodyPositionGlobal(constBody)-softConstPoint #if (applyedExtraForce[0][0] != 0 or applyedExtraForce[0][1] != 0 or applyedExtraForce[0][2] != 0) : if (forceApplyFrame == 0) : applyedExtraForce[0] = [0, 0, 0]
def simulateCallback(self, frame): global ddth_des_flat global stepsPerFrame global wcfg global vpWorld # reload(tf) self.frame = frame print "main:frame : ", frame # motionModel.update(motion[0]) self.timeIndex = 0 self.setTimeStamp() # constant setting # (Kt, damp, stepsPerFrame, simulSpeedInv) = viewer.objectInfoWnd.getVals() getVal = viewer.objectInfoWnd.getVal Kt = getVal('PD gain') damp = getVal('Joint Damping') stepsPerFrame = getVal('steps per frame') simulSpeedInv = getVal('1/simul speed') wcfg.timeStep = 1 / (30. * simulSpeedInv * stepsPerFrame) vpWorld.SetTimeStep(wcfg.timeStep) # Dt = 2. * (Kt**.5) Dt = 0. controlModel.SetJointsDamping(damp) wForce = math.pow(2., getVal('force weight')) wTorque = math.pow(2., getVal('tau weight')) # tracking th_r = motion.getDOFPositions(frame) th = controlModel.getDOFPositions() dth_r = motion.getDOFVelocities(frame) dth = controlModel.getDOFVelocities() ddth_r = motion.getDOFAccelerations(frame) # config['weightMapTuple']) ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt) ddth_c = controlModel.getDOFAccelerations() ype.flatten(ddth_des, ddth_des_flat) desForceFrameBegin = getVal('des force begin') desForceDuration = getVal('des force dur') * simulSpeedInv desForceFrame = [ desForceFrameBegin, desForceFrameBegin + desForceDuration ] desForceRelFrame = float(frame - desForceFrame[0]) / desForceDuration desNormalForceMin = getVal('normal des force min') desNormalForceMax = getVal('normal des force max') desNormalForce = desNormalForceMin if desForceFrame[0] <= frame <= desForceFrame[1]: desNormalForce = desNormalForceMin * \ (1 - desForceRelFrame) + desNormalForceMax * desForceRelFrame totalForce = np.array([0., desNormalForce, 0., 0., 0., 0.]) # totalForce = np.array([0., 720., 0., 0., 0., 0.]) # totalForce = np.array([50., 150.]) torques = None ddth_des_flat[0:6] = [0.] * 6 self.setTimeStamp() simulContactForces = np.zeros(3) torque_None = True for i in range(int(stepsPerFrame)): torques = None torque_None = True cBodyIDs = [] cPositions = [] cPositionLocals = [] cForces = [] cBodyIDsControl = [] cPositionsControl = [] cPositionLocalsControl = [] cForcesControl = [] if desForceFrame[0] <= frame <= desForceFrame[1]: if True: # totalForceImpulse = stepsPerFrame * totalForce cBodyIDs, cPositions, cPositionLocals, cForcesControl, torques \ = hls.calcLCPbasicControl( motion, vpWorld, controlModel, bodyIDsToCheck, 1., totalForce, wForce, wTorque, ddth_des_flat) # if cForces is not None: # print "control: ", sum(cForces) if torques is not None: # print torques[:6] torque_None = False # cForcesControl = cForces.copy() # cBodyIDsControl = cBodyIDs.copy() # cPositionsControl = cPositions.copy() # cPositionLocalsControl = cPositionLocals.copy() else: torques = ddth_des_flat cBodyIDs, cPositions, cPositionLocals, cForces, timeStamp \ = hls.calcLCPForces(motion, vpWorld, controlModel, bodyIDsToCheck, 1., torques, solver='qp') # if (not torque_None) and cForces is not None: # print "calcul: ", sum(cForces) if len(cBodyIDs) > 0: # apply contact forces if False and not torque_None: vpWorld.applyPenaltyForce(cBodyIDs, cPositionLocals, cForcesControl) simulContactForces += sum(cForcesControl) else: vpWorld.applyPenaltyForce(cBodyIDs, cPositionLocals, cForces) simulContactForces += sum(cForces) # simulContactForces += sum(cForces) ype.nested(torques, torques_nested) controlModel.setDOFTorques(torques_nested[1:]) vpWorld.step() self.setTimeStamp() # print ddth_des_flat # print torques print simulContactForces / stepsPerFrame self.cBodyIDs, self.cPositions, self.cPositionLocals, self.cForces, torques \ = hls.calcLCPbasicControl(motion, vpWorld, controlModel, bodyIDsToCheck, 1., totalForce, wForce, wTorque, ddth_des_flat) del rd_cForcesControl[:] del rd_cPositionsControl[:] for i in range(len(self.cBodyIDs)): # print expected force rd_cForcesControl.append(self.cForces[i].copy() / 50.) rd_cPositionsControl.append(self.cPositions[i].copy()) del rd_ForceControl[:] del rd_Position[:] if self.cForces is not None: # print expected force rd_ForceControl.append(sum(self.cForces) / 50.) rd_Position.append(np.array([0., 0., 0.1])) # graph if self.cForces is not None: sumForce = sum(self.cForces) viewer.cForceWnd.insertData('expForce', frame, sumForce[1]) else: viewer.cForceWnd.insertData('expForce', frame, 0.) self.cBodyIDs, self.cPositions, self.cPositionLocals, self.cForces, tmptmp \ = hls.calcLCPForces(motion, vpWorld, controlModel, bodyIDsToCheck, 1., torques, solver='qp') del rd_cForces[:] del rd_cPositions[:] for i in range(len(self.cBodyIDs)): # print calculated force rd_cForces.append(self.cForces[i].copy() / 50.) rd_cPositions.append(self.cPositions[i].copy()) del rd_jointPos[:] for i in range(motion[0].skeleton.getJointNum()): rd_jointPos.append(motion[frame].getJointPositionGlobal(i)) del rd_ForceDes[:] del rd_PositionDes[:] # rd_ForceDes.append(totalForce/50.) rd_ForceDes.append(totalForce[1] * np.array([0., 1., 0.]) / 50.) rd_PositionDes.append(np.array([0., 0., 0.])) # if self.cForces is not None: # rd_ForceDes.append(sum(self.cForces)[1]/50. * [0., 1., 0.]) # rd_PositionDes.append(np.array([0., 0., -0.1])) # graph if self.cForces is not None: sumForce = sum(self.cForces) # viewer.cForceWnd.insertData('realForce', frame, sumForce[1]) viewer.cForceWnd.insertData('realForce', frame, simulContactForces[1] / stepsPerFrame) else: viewer.cForceWnd.insertData('realForce', frame, 0.) viewer.cForceWnd.insertData('realForce', frame, simulContactForces[1] / stepsPerFrame) if desForceFrame[0] <= frame <= desForceFrame[1]: viewer.cForceWnd.insertData('desForceMin', frame, totalForce[1] * 1.) # viewer.cForceWnd.insertData('desForceMin', frame, totalForce[1] * .9) # viewer.cForceWnd.insertData('desForceMax', frame, totalForce[1] * 1.1) else: viewer.cForceWnd.insertData('desForceMin', frame, 0.) viewer.cForceWnd.insertData('desForceMax', frame, 0.) self.setTimeStamp()
def simulateCallback(frame): curTime = time.time() if frame % 30 == 1: pt[0] = time.time() global g_initFlag global forceShowFrame global forceApplyFrame global JsysPre global JsupPreL global JsupPreR global JsupPre global softConstPoint global stage global contactRendererName global desCOMOffset #motionModel.update(motion[0]) Kt, Kk, Kl, Kh, Ksc, Bt, Bl, Bh, B_CM, B_CMSd, B_Toe = viewer.GetParam( ) Dt = 2 * (Kt**.5) Dk = 2 * (Kk**.5) Dl = 2 * (Kl**.5) Dh = 2 * (Kh**.5) Dsc = 2 * (Ksc**.5) ''' if Bsc == 0.0 : viewer.doc.showRenderer('softConstraint', False) viewer.motionViewWnd.update(1, viewer.doc) else: viewer.doc.showRenderer('softConstraint', True) renderer1 = viewer.doc.getRenderer('softConstraint') renderer1.rc.setLineWidth(0.1+Bsc*3) viewer.motionViewWnd.update(1, viewer.doc) ''' pose = motion[0].copy() def solveIK(desComPos, desIdxs, desPos, desOri, cmW=10., posW=1., oriW=1.): numItr = 100 dt = .5 threshold = 0.1 for i in range(0, numItr): jPart_IK = [] print '----iter num', i IKModel.update(pose) th_r_IK = pose.getDOFPositions() jointPositions_IK = pose.getJointPositionsGlobal() jointAxeses_IK = pose.getDOFAxeses() linkPositions_IK = IKModel.getBodyPositionsGlobal() linkInertias_IK = IKModel.getBodyInertiasGlobal() CM_IK = yrp.getCM(linkPositions_IK, linkMasses, totalMass) print CM_IK P_IK = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions_IK, CM_IK, linkInertias_IK) yjc.computeJacobian2(Jsys_IK, DOFs, jointPositions_IK, jointAxeses_IK, linkPositions_IK, allLinkJointMasks) for j in range(0, len(desIdxs)): jPart_IK.append(Jsys_IK[6 * desIdxs[j]:6 * desIdxs[j] + 6]) J_IK, JAngCom_IK = np.vsplit(np.dot(P_IK, Jsys_IK), 2) dv_IK = cmW * (desComPos - CM_IK) for j in range(0, len(desIdxs)): J_IK = np.vstack((J_IK, jPart_IK[j])) pos_IK = IKModel.getBodyPositionGlobal(desIdxs[j]) dv_IK = np.append(dv_IK, posW * (desPos[j] - pos_IK)) ori_IK = IKModel.getBodyOrientationGlobal(desIdxs[j]) dv_IK = np.append(dv_IK, oriW * mm.logSO3(desOri[j] * ori_IK.T)) #print dv_IK[0:3] dth_IK_solve = npl.lstsq(J_IK, dv_IK) dth_IK_x = dth_IK_solve[0][:totalDOF] ype.nested(dth_IK_x, dth_IK) #print dth_IK[0][0:3] th_IK = yct.getIntegralDOF(th_r_IK, dth_IK, dt) pose.setDOFPositions(th_IK) if np.dot(dv_IK, dv_IK) < threshold: break linkPositions_ref = motionModel.getBodyPositionsGlobal() CM_ref = yrp.getCM(linkPositions_ref, linkMasses, totalMass) footCenterOffset = np.array([ viewer.objectInfoWnd.comOffsetX.value(), viewer.objectInfoWnd.comOffsetY.value(), viewer.objectInfoWnd.comOffsetZ.value() ]) #CM_IK_ref = footCenter + footCenterOffset CM_IK_ref = CM_ref + footCenterOffset #CM_IK_ref[1] = CM_ref[1] + footCenterOffset[1] motion[0].skeleton.getJointIndex(config['supLink']) #IKidxs = [indexFootL[0], indexFootR[0]] #IKdesPos = [motionModel.getBodyPositionGlobal(indexFootL[0]), motionModel.getBodyPositionGlobal(indexFootR[0])] #for i in range(0, 2): # #IKdesPos[i] += ModelOffset # IKdesPos[i][1] = 0.069 #IKori = [motionModel.getBodyOrientationGlobal(indexFootL[0]), motionModel.getBodyOrientationGlobal(indexFootR[0])] #IKdesOri = [None]*2 #for i in range(0, 2): # IKdesOri[i] = mm.I_SO3() IKidxs = config['Phalange'][0:1] + config['Phalange'][3:4] print IKidxs IKdesPos = [None] * len(IKidxs) IKdesOri = [None] * len(IKidxs) for i in range(0, len(IKidxs)): #print i IKdesPos[i] = motionModel.getBodyPositionGlobal(IKidxs[i]) IKdesPos[i][1] = 0.03 IKdesOri[i] = mm.I_SO3() print IKdesPos solveIK(CM_IK_ref, IKidxs, IKdesPos, IKdesOri) # tracking th_r_ori = motion.getDOFPositions(frame) th_r = copy.copy(th_r_ori) global leftHipTimer if viewer.objectInfoWnd.onLeftHip: leftHipTimer = 60 viewer.objectInfoWnd.onLeftHip = False if leftHipTimer > 0: viewer.objectInfoWnd.comOffsetX.value( 0.14 * np.sin(2 * 3.14 * leftHipTimer / 60.)) #viewer.objectInfoWnd.comOffsetZ.value(0.04*np.cos(2*3.14*leftHipTimer/90.)) #B_Hipd = viewer.objectInfoWnd.labelLeftHip.value() #newR1 = mm.exp(mm.v3(0.0,1.0,0.0), 3.14*0.5*B_Hipd/100.) #idx = motion[0].skeleton.getJointIndex('LeftUpLeg') #th_r[idx] = np.dot(th_r[idx], newR1) #idx = motion[0].skeleton.getJointIndex('RightUpLeg') #th_r[idx] = np.dot(th_r[idx], newR1) leftHipTimer -= 1 timeReport[0] += time.time() - curTime curTime = time.time() 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) ddth_c = controlModel.getDOFAccelerations() ype.flatten(ddth_des, ddth_des_flat) ype.flatten(dth, dth_flat) ype.flatten(ddth_c, ddth_c_flat) # jacobian refFootL = motionModel.getBodyPositionGlobal(supL) refFootR = motionModel.getBodyPositionGlobal(supR) positionFootL = [None] * footPartNum positionFootR = [None] * footPartNum for i in range(footPartNum): positionFootL[i] = controlModel.getBodyPositionGlobal( indexFootL[i]) positionFootR[i] = controlModel.getBodyPositionGlobal( indexFootR[i]) linkPositions = controlModel.getBodyPositionsGlobal() linkVelocities = controlModel.getBodyVelocitiesGlobal() linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal() linkInertias = controlModel.getBodyInertiasGlobal() jointPositions = controlModel.getJointPositionsGlobal() jointAxeses = controlModel.getDOFAxeses() CM = yrp.getCM(linkPositions, linkMasses, totalMass) dCM = yrp.getCM(linkVelocities, linkMasses, totalMass) CM_plane = copy.copy(CM) CM_plane[1] = 0. dCM_plane = copy.copy(dCM) dCM_plane[1] = 0. linkPositions_ref = motionModel.getBodyPositionsGlobal() linkVelocities_ref = motionModel.getBodyVelocitiesGlobal() linkAngVelocities_ref = motionModel.getBodyAngVelocitiesGlobal() linkInertias_ref = motionModel.getBodyInertiasGlobal() CM_ref = yrp.getCM(linkPositions_ref, linkMasses, totalMass) CM_plane_ref = copy.copy(CM_ref) CM_plane_ref[1] = 0. P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM, linkInertias) dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses, linkVelocities, dCM, linkAngVelocities, linkInertias) timeReport[1] += time.time() - curTime curTime = time.time() yjc.computeJacobian2(Jsys, DOFs, jointPositions, jointAxeses, linkPositions, allLinkJointMasks) timeReport[2] += time.time() - curTime curTime = time.time() # yjc.computeJacobianDerivative2(dJsys, DOFs, jointPositions, jointAxeses, linkAngVelocities, linkPositions, allLinkJointMasks) if frame > 0: dJsys = (Jsys - JsysPre) * 30. else: dJsys = (Jsys - Jsys) JsysPre = Jsys.copy() timeReport[3] += time.time() - curTime curTime = time.time() bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds) CP = yrp.getCP(contactPositions, contactForces) for i in range(len(bodyIDsToCheck)): controlModel.SetBodyColor(bodyIDsToCheck[i], 0, 0, 0, 255) contactFlagFootL = [0] * footPartNum contactFlagFootR = [0] * footPartNum partialDOFIndex = [22, 22] for i in range(len(bodyIDs)): controlModel.SetBodyColor(bodyIDs[i], 255, 105, 105, 200) index = controlModel.id2index(bodyIDs[i]) for j in range(len(indexFootL)): if index == indexFootL[j]: contactFlagFootL[j] = 1 for j in range(len(indexFootR)): if index == indexFootR[j]: contactFlagFootR[j] = 1 for j in range(0, footPartNum): jAngFootR[j] = Jsys[6 * indexFootR[j]:6 * indexFootR[j] + 6][3:] #.copy() jAngFootL[j] = Jsys[6 * indexFootL[j]:6 * indexFootL[j] + 6][3:] #.copy() dJAngFootR[j] = dJsys[6 * indexFootR[j]:6 * indexFootR[j] + 6][3:] #.copy() dJAngFootL[j] = dJsys[6 * indexFootL[j]:6 * indexFootL[j] + 6][3:] #.copy() jFootR[j] = Jsys[6 * indexFootR[j]:6 * indexFootR[j] + 6] #.copy() jFootL[j] = Jsys[6 * indexFootL[j]:6 * indexFootL[j] + 6] #.copy() dJFootR[j] = dJsys[6 * indexFootR[j]:6 * indexFootR[j] + 6] #.copy() dJFootL[j] = dJsys[6 * indexFootL[j]:6 * indexFootL[j] + 6] #.copy() if footPartNum == 1: desFCL = (controlModel.getBodyPositionGlobal(supL)) desFCR = (controlModel.getBodyPositionGlobal(supR)) else: r = .5 + desCOMOffset desFCL = (controlModel.getBodyPositionGlobal(indexFootL[0]) * r + controlModel.getBodyPositionGlobal(indexFootL[1]) * (1.0 - r) ) #controlModel.getBodyPositionGlobal(indexFootL[1]) desFCR = (controlModel.getBodyPositionGlobal(indexFootR[0]) * r + controlModel.getBodyPositionGlobal(indexFootR[1]) * (1.0 - r) ) #controlModel.getBodyPositionGlobal(indexFootR[1]) desFC = desFCL + (desFCR - desFCL) / 2.0 desFC[1] = 0 rd_footCenter_des[0] = desFC.copy() curRelCMVec = CM_plane - desFC vecRatio = mm.length(curRelCMVec) * 0. #print(frame, vecRatio) footCenter = desFC - curRelCMVec * (vecRatio) #/10.0 footCenter = ( getBodyGlobalPos(controlModel, motion, 'LeftCalcaneus_1') + getBodyGlobalPos(controlModel, motion, 'LeftPhalange_1') + getBodyGlobalPos(controlModel, motion, 'RightCalcaneus_1') + getBodyGlobalPos(controlModel, motion, 'RightPhalange_1')) / 4. #footCenter = (getBodyGlobalPos(controlModel, motion, 'LeftCalcaneus_1') + getBodyGlobalPos(controlModel, motion, 'LeftTalus_1') + getBodyGlobalPos(controlModel, motion, 'RightCalcaneus_1') + getBodyGlobalPos(controlModel, motion, 'RightTalus_1'))/4. footCenter_ref = refFootL + (refFootR - refFootL) / 2.0 #footCenter_ref[1] = 0. footCenter[1] = 0. footCenterOffset = np.array([ viewer.objectInfoWnd.comOffsetX.value(), 0, viewer.objectInfoWnd.comOffsetZ.value() ]) #footCenter += footCenterOffset vecRatio = mm.length(curRelCMVec) * 0. softConstPointOffset = -curRelCMVec * (vecRatio) #/10.0 #print(frame, vecRatio, softConstPointOffset) desForeSupLAcc = [0, 0, 0] desForeSupRAcc = [0, 0, 0] totalNormalForce = [0, 0, 0] for i in range(len(contactForces)): totalNormalForce[0] += contactForces[i][0] totalNormalForce[1] += contactForces[i][1] totalNormalForce[2] += contactForces[i][2] #print((totalMass*mm.s2v(wcfg.gravity))[1]) footCenterOffset = np.array([ viewer.objectInfoWnd.comOffsetX.value(), viewer.objectInfoWnd.comOffsetY.value(), viewer.objectInfoWnd.comOffsetZ.value() ]) ###################### # optimization terms ###################### # linear momentum CM_ref_plane = footCenter + footCenterOffset dL_des_plane = Kl * totalMass * (CM_ref_plane - CM_plane) - Dl * totalMass * dCM_plane dL_des_plane[1] = Kl * totalMass * (CM_ref[1] + footCenterOffset[1] - CM[1]) - Dl * totalMass * dCM[1] # angular momentum CP_ref = footCenter + footCenterOffset timeStep = 30. if CP_old[0] == None or CP == None: dCP = None else: dCP = (CP - CP_old[0]) / (1 / timeStep) CP_old[0] = CP if CP != None and dCP != None: ddCP_des = Kh * (CP_ref - CP) - Dh * (dCP) CP_des = CP + dCP * (1 / timeStep) + .5 * ddCP_des * ( (1 / timeStep)**2) #dH_des = np.cross((CP_des - CM), (dL_des_plane + totalMass*mm.s2v(wcfg.gravity))) dH_des = np.cross( (CP_des - CM_plane), (dL_des_plane + totalMass * mm.s2v(wcfg.gravity))) else: dH_des = None # momentum matrix RS = np.dot(P, Jsys) R, S = np.vsplit(RS, 2) rs = np.dot((np.dot(dP, Jsys) + np.dot(P, dJsys)), dth_flat) r_bias, s_bias = np.hsplit(rs, 2) flagContact = True if dH_des == None or np.any(np.isnan(dH_des)) == True: flagContact = False #viewer.doc.showRenderer('rd_grf_des', False) #viewer.motionViewWnd.update(1, viewer.doc) #else: #viewer.doc.showRenderer('rd_grf_des', True) #viewer.motionViewWnd.update(1, viewer.doc) ''' 0 : initial 1 : contact 2 : fly 3 : landing ''' trackingW = w #if checkAll(contactFlagFootR, 0) != 1 : if 0: #stage == MOTION_TRACKING: trackingW = w2 #stage = POWERFUL_BALANCING Bt = Bt * 2 ####################### # optimization ####################### mot.addTrackingTerms(problem, totalDOF, Bt, trackingW, ddth_des_flat) #if flagContact == True: # mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias) # mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias) a_sup_2 = None Jsup_2 = None dJsup_2 = None ############################## #if Jsup_2 != None: # mot.addConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat, a_sup_2) timeReport[4] += time.time() - curTime curTime = time.time() r = problem.solve() problem.clear() ype.nested(r['x'], ddth_sol) rootPos[0] = controlModel.getBodyPositionGlobal(selectedBody) localPos = [[0, 0, 0]] timeReport[5] += time.time() - curTime curTime = time.time() for i in range(stepsPerFrame): # apply penalty force bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds) vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces) extraForce[0] = viewer.GetForce() if (extraForce[0][0] != 0 or extraForce[0][1] != 0 or extraForce[0][2] != 0): forceApplyFrame += 1 #vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce) controlModel.applyBodyForceGlobal(selectedBody, extraForce[0]) applyedExtraForce[0] = extraForce[0] if forceApplyFrame * wcfg.timeStep > 0.1: viewer.ResetForce() forceApplyFrame = 0 controlModel.setDOFAccelerations(ddth_sol) controlModel.solveHybridDynamics() vpWorld.step() if frame % 30 == 0: print 'elapsed time for 30 frames:', time.time() - pt[0] # rendering rd_footCenter[0] = footCenter rd_CM[0] = CM.copy() rd_CM_plane[0] = CM_plane.copy() rd_footCenter_ref[0] = footCenter_ref rd_CM_plane_ref[0] = CM_ref.copy() rd_CM_ref[0] = CM_ref.copy() rd_CM_ref_vec[0] = (CM_ref - footCenter_ref) * 3. rd_CM_vec[0] = (CM - CM_plane) rd_CM_des[0] = CM_ref_plane.copy() rd_CM_des[0][1] = .01 #rd_CM_plane[0][1] = 0. if CP != None and dCP != None: rd_CP[0] = CP rd_CP_des[0] = CP_des rd_dL_des_plane[0] = dL_des_plane rd_dH_des[0] = dH_des rd_grf_des[ 0] = totalNormalForce # - totalMass*mm.s2v(wcfg.gravity)#dL_des_plane - totalMass*mm.s2v(wcfg.gravity) rd_exf_des[0] = applyedExtraForce[0] rd_root_des[0] = rootPos[0] rd_CMP[0] = softConstPoint rd_soft_const_vec[0] = controlModel.getBodyPositionGlobal( constBody) - softConstPoint #indexL = motion[0].skeleton.getJointIndex('Hips') #indexR = motion[0].skeleton.getJointIndex('Spine1') indexL = indexFootL[0] indexR = indexFootR[0] curAng = [controlModel.getBodyOrientationGlobal(indexL)] curAngY = np.dot(curAng, np.array([0, 0, 1])) rd_footL_vec[0] = np.copy(curAngY[0]) rd_footCenterL[0] = controlModel.getBodyPositionGlobal(indexL) curAng = [controlModel.getBodyOrientationGlobal(indexR)] curAngY = np.dot(curAng, np.array([0, 0, 1])) rd_footR_vec[0] = np.copy(curAngY[0]) rd_footCenterR[0] = controlModel.getBodyPositionGlobal(indexR) if (forceApplyFrame == 0): applyedExtraForce[0] = [0, 0, 0] timeReport[6] += time.time() - curTime