def getDesiredDOFAccelerationsExtended(th_r, th, dth, Kt, Dt): ddth_des = [None] * len(th_r) # type: list[np.ndarray] p_r0 = th_r[0][0] p0 = th[0][0] v0 = dth[0][0:3] th_r0 = th_r[0][1] th0 = th[0][1] dth0 = dth[0][3:6] a_des0 = Kt[0] * (p_r0 - p0) + Dt[0] * (-v0) ddth_des0 = Kt[0] * (mm.logSO3(np.dot(th0.transpose(), th_r0))) + Dt[0] * (-dth0) ddth_des[0] = np.concatenate((a_des0, ddth_des0)) for i in range(1, len(th_r)): if th[i].shape[0] == 3: ddth_des[i] = Kt[i + 1] * (mm.logSO3( np.dot(th[i].transpose(), th_r[i]))) + Dt[i + 1] * (-dth[i]) elif th[i].shape[0] > 0: ddth_des[i] = Kt[i + 1] * (mm.logSO3( np.dot(th[i].transpose(), th_r[i]))) + Dt[i + 1] * (-dth[i]) else: ddth_des[i] = np.zeros(0) return ddth_des
def compute(self, positions_hat, weightMap=None): skel = self.skel # invM = inv(skel.M + self.Kd * self.h) # p = -self.Kp.dot(skel.q + skel.dq * self.h - qhat) # d = -self.Kd.dot(skel.dq) # qddot = invM.dot(-skel.c + p + d + skel.constraint_forces()) # tau = p + d - self.Kd.dot(qddot) * self.h ''' # Check the balance COP = skel.body('h_heel_left').to_world([0.05, 0, 0]) offset = skel.C[0] - COP[0] # Adjust the target pose k1 = 200.0 if 0.0 < offset and offset < 0.1 else 2000.0 k2 = 100.0 kd = 10.0 if 0.0 < offset and offset < 0.1 else 100.0 q_delta1 = np.array([-k1, -k2, -k1, -k2]) * offset q_delta2 = np.ones(4) * kd * (self.preoffset - offset) tau[np.array([17, 25, 19, 26])] += (q_delta1 + q_delta2) self.preoffset = offset #''' # Make sure the first six are zero th_r = positions_hat ddth_des = [None] * len(th_r) th = self.model.getDOFPositions() dth = self.model.getDOFVelocities() p_r0 = th_r[0][0] p0 = th[0][0] v0 = dth[0][:3] th_r0 = th_r[0][1] th0 = th[0][1] dth0 = dth[0][3:] kt = self.Kp[0, 0] dt = self.Kd[0, 0] if weightMap is not None: kt = Kt * weightMap[0] dt = Dt * (weightMap[0]**.5) # dt = 0. a_des0 = kt * (p_r0 - p0) - dt * v0 ddth_des0 = kt * (mm.logSO3(np.dot(th0.transpose(), th_r0))) - dt * dth0 ddth_des[0] = np.concatenate((a_des0, ddth_des0)) for i in range(1, len(th_r)): if weightMap is not None: kt = Kt * weightMap[i] dt = Dt * (weightMap[i]**.5) # dt = 0. ddth_des[i] = kt * (mm.logSO3(np.dot(th[i].transpose(), th_r[i]))) - dt * dth[i] # ddth_des[i] = kt*(mm.logSO3(np.dot(th[i].transpose(), th_r[i]))) + dt*( - dth[i]) #+ ddth_r[i] return np.concatenate(ddth_des)
def __init__(self, body, des, weight): """ :type body: pydart.BodyNode :param des: :param weight: """ super().__init__() self.body = body self.des = mm.seq2Vec3(mm.logSO3(des)) self.weight = weight self.cur = mm.seq2Vec3(mm.logSO3(self.body.world_transform()[:3, :3]))
def getDesFootAngularAcc(refModel, controlModel, footIndex, Kk, Dk, axis=[0, 1, 0], desAng=[0, 1, 0]): desAngularAcc = [0, 0, 0] curAng = [controlModel.getBodyOrientationGlobal(footIndex)] refAngVel = refModel.getBodyAngVelocityGlobal(footIndex) curAngVel = controlModel.getBodyAngVelocityGlobal(footIndex) refAngAcc = (0, 0, 0) curAngY = np.dot(curAng, np.array(axis)) refAngY = np.array(desAng) if stage == MOTION_TRACKING + 10: refAng = [refModel.getBodyOrientationGlobal(footIndex)] refAngY2 = np.dot(refAng, np.array([0, 1, 0])) refAngY = refAngY2[0] aL = mm.logSO3(mm.getSO3FromVectors(curAngY[0], refAngY)) desAngularAcc = Kk * aL + Dk * (refAngVel - curAngVel) return desAngularAcc
def blendSegmentSmooth(motionSegment0, motionSegment1, attachPosition=True, attachOrientation=True): motionSegment1 = motionSegment1.copy() if attachPosition: p_offset = motionSegment0[0].rootPos - motionSegment1[0].rootPos motionSegment1.translateByOffset(p_offset) if attachOrientation: R_offset = np.dot(motionSegment0[0].localRs[0], motionSegment1[0].localRs[0].T) R_offset = mm.exp( mm.projectionOnVector(mm.logSO3(R_offset), mm.v3(0, 1, 0))) # # project on y axis motionSegment1.rotateTrajectory(R_offset) newMotion = ym.JointMotion([None] * (int( (len(motionSegment0) + len(motionSegment1)) / 2.))) # newMotion = ym.JointMotion( [None]*(int( t*len(motionSegment0) + (1-t)*len(motionSegment1)) ) ) df0 = float(len(newMotion)) / len(motionSegment0) df1 = float(len(newMotion)) / len(motionSegment1) for frame in range(len(newMotion)): normalizedFrame = float(frame) / (len(newMotion) - 1) normalizedFrame2 = yfg.H1(normalizedFrame) normalizedFrame2 += df0 * yfg.H2(normalizedFrame) normalizedFrame2 += df1 * yfg.H3(normalizedFrame) posture0_at_normalizedFrame = motionSegment0.getPostureAt( normalizedFrame2 * (len(motionSegment0) - 1)) posture1_at_normalizedFrame = motionSegment1.getPostureAt( normalizedFrame2 * (len(motionSegment1) - 1)) newMotion[frame] = posture0_at_normalizedFrame.blendPosture( posture1_at_normalizedFrame, normalizedFrame2) return newMotion
def getDesiredAngAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt): ddth_des = [None] * len(th_r) for i in range(len(th_r)): ddth_des[i] = Kt * (mm.logSO3(np.dot(th[i].transpose(), th_r[i])) ) + Dt * (dth_r[i] - dth[i]) + ddth_r[i] return ddth_des
def getDesiredDOFAccelerations_flat(th_r, th, dth_r, dth, ddth_r, Kt, Dt, joint_dof_info, weightMap=None): ddth_des_flat = np.zeros_like(th) # type: list[np.ndarray] kt = Kt dt = Dt for i in range(len(joint_dof_info)): dof_start_index, dof = joint_dof_info[i] _th_r = th_r[dof_start_index:dof_start_index + dof] _th = th[dof_start_index:dof_start_index + dof] _dth = dth[dof_start_index:dof_start_index + dof] if weightMap is not None: kt = Kt * weightMap[i] dt = Dt * (weightMap[i]**.5) # dt = 0. if dof == 0: continue if dof == 6: ddth_des_flat[dof_start_index + 0:dof_start_index + 3] = kt * (_th_r[:3] - _th[:3]) + dt * ( -_dth[:3]) #+ ddth_r[i] ddth_des_flat[dof_start_index + 3:dof_start_index + 6] = kt * ( mm.logSO3(np.dot(mm.exp(_th[3:]).T, mm.exp( _th_r[3:])))) + dt * (-_dth[3:]) #+ ddth_r[i] if dof == 3: ddth_des_flat[dof_start_index + 0:dof_start_index + 3] = kt * (mm.logSO3( np.dot(mm.exp(_th).T, mm.exp(_th_r)))) + dt * ( -_dth) #+ ddth_r[i] else: ddth_des_flat[dof_start_index + 0:dof_start_index + dof] = kt * (_th_r - _th) + dt * (-_dth ) #+ ddth_r[i] return ddth_des_flat
def getBlendedNextMotion2(nextMotionA, nextMotionB, prevEndPosture, t=None, attachPosition=True, attachOrientation=True): dA = prevEndPosture - nextMotionA[0] dB = prevEndPosture - nextMotionB[0] newNextMotionA = nextMotionA.copy() newNextMotionB = nextMotionB.copy() if attachPosition: p_offset_A = dA.rootPos p_offset_B = dB.rootPos # d.disableTranslation() newNextMotionA.translateByOffset(p_offset_A) newNextMotionB.translateByOffset(p_offset_B) if attachOrientation: R_offset_A = dA.getJointOrientationLocal(0) R_offset_A = mm.exp( mm.projectionOnVector(mm.logSO3(R_offset_A), mm.v3(0, 1, 0))) # # project on y axis R_offset_B = dA.getJointOrientationLocal(0) R_offset_B = mm.exp( mm.projectionOnVector(mm.logSO3(R_offset_B), mm.v3(0, 1, 0))) # # project on y axis # d.disableRotations([0]) newNextMotionA.rotateTrajectory(R_offset_A) newNextMotionB.rotateTrajectory(R_offset_B) if t == None: blendedNextMotion = blendSegmentSmooth(newNextMotionA, newNextMotionB) else: blendedNextMotion = blendSegmentFixed(newNextMotionA, newNextMotionB, t) # del blendedNextMotion[0] return blendedNextMotion
def reward(self): body_e = list(map(self.skel.body, self.idx_e)) p_e = np.asarray([body.world_transform()[:3, 3] for body in body_e]).flatten() p_e_ori = [body.world_transform()[:3, :3] for body in body_e] # p_e_ori_diff = np.asarray([mm.logSO3(np.dot(p_e_ori[idx].T, self.prev_ref_p_e_ori_hat[idx])) for idx in range(len(p_e_ori))]).flatten() q_diff = np.asarray( self.skel.position_differences(self.prev_ref_q, self.skel.q)) dq_diff = np.asarray( self.skel.velocity_differences(self.prev_ref_dq, self.skel.dq)) rewards = list() # q reward # rewards.append(exp_reward_term(self.w_p, self.exp_p, # np.concatenate(list(q_diff[get_joint_dof_range(self.skel.joint(joint_name))] for joint_name in self.reward_joint)))) rewards.append(exp_reward_term(self.w_p, self.exp_p, q_diff)) # dq reward # rewards.append(exp_reward_term(self.w_v, self.exp_v, # np.concatenate(list(dq_diff[get_joint_dof_range(self.skel.joint(joint_name))] for joint_name in self.reward_joint)))) rewards.append(exp_reward_term(self.w_v, self.exp_v, dq_diff)) # end effector reward rewards.append( exp_reward_term(self.w_e, self.exp_e, p_e - self.prev_ref_p_e_hat)) # end effector orientation reward # rewards.append(exp_reward_term(self.w_e_ori, self.exp_e_ori, p_e_ori_diff)) # com reward rewards.append( exp_reward_term(self.w_c, self.exp_c, self.skel.com() - self.prev_ref_com)) # com_vel reward # rewards.append(exp_reward_term(self.w_c_v, self.exp_c_v, self.skel.com_velocity() - self.prev_ref_com_vel)) # rewards.append(exp_reward_term(self.w_c_v, self.exp_c_v, self.skel.com_spatial_velocity() - self.prev_ref_com_spatial_vel)) # torso reward torso_ori = self.skel.body('Spine').world_transform()[:3, :3] torso_ori_diff = np.asarray( mm.logSO3(np.dot(torso_ori.T, self.prev_ref_torso_ori))) rewards.append(exp_reward_term(self.w_t, self.exp_t, torso_ori_diff)) # ankle y position reward # ankle_joint_y_pos = np.asarray([joint.get_world_frame_after_transform()[1, 3] for joint in [self.skel.joint(joint_name) for joint_name in self.ankle_joint_names]]).flatten() # rewards.append(exp_reward_term(self.w_y_ankle, self.exp_y_ankle, ankle_joint_y_pos - self.prev_ref_ankle_joint_y_pos)) return sum(rewards) / (len(self.ref_motion) + self.ref_motion.fps)
def calcDeltaq(self): deltaq = np.zeros(self.skel.q.shape) if self.Rs is not None: p_r0 = self.Rs[0][0] p0 = self.skel.q[3:6] th_r0 = self.Rs[0][1] th0 = mm.exp(self.skel.q[:3]) deltaq[:6] = np.hstack((mm.logSO3(np.dot(th0.transpose(), th_r0)), p_r0 - p0)) # TODO: # apply variety dofs dofOffset = 6 for i in range(1, len(self.skel.joints)): # for i in range(1, len(self.Rs)): joint = self.skel.joints[i] if joint.num_dofs() == 3: deltaq[dofOffset:dofOffset + 3] = mm.logSO3( np.dot(joint.get_local_transform()[:3, :3].transpose(), self.Rs[i])) elif joint.num_dofs() == 2: targetAngle1 = math.atan2(-self.Rs[i][1, 2], self.Rs[i][2, 2]) targetAngle2 = math.atan2(-self.Rs[i][0, 1], self.Rs[i][0, 0]) deltaq[dofOffset:dofOffset + 2] = np.array( [targetAngle1, targetAngle2]) elif joint.num_dofs() == 1: deltaq[dofOffset] = math.atan2(self.Rs[i][2, 1], self.Rs[i][1, 1]) dofOffset += joint.num_dofs() # a_des0 = kt*(p_r0 - p0) + dt*(- v0) #+ a_r0 # ddth_des0 = kt*(mm.logSO3(np.dot(th0.transpose(), th_r0))) + dt*(- dth0) #+ ddth_r0 return deltaq
def renderVpBody(self, body_idx): # glPushMatrix() # _t = self._model.getBodyPositionGlobal(body_idx) # glTranslatef(_t[0], _t[1], _t[2]) # print(body_idx, self._model.index2name(body_idx), self._model.getBodyShape(body_idx)) # print(self._model.index2name(body_idx), self._model.getBodyGeomsType(body_idx), self._model.getBodyGeomsSize(body_idx)) # print(self._model.index2name(body_idx), self._model.getBodyGeomsGlobalFrame(body_idx)) geom_types = self._model.getBodyGeomsType(body_idx) print('renderVpBody: ', geom_types) geom_sizes = self._model.getBodyGeomsSize(body_idx) print('renderVpBody: ', geom_sizes) geom_frames = self._model.getBodyGeomsGlobalFrame(body_idx) print('renderVpBody: ', geom_frames) for i in range(self._model.getBodyGeomNum(body_idx)): glPushMatrix() geom_type, geom_sizes, _T = geom_types[i], geom_sizes[ i], geom_frames[i] _t = _T[:3, 3].flatten() _r = mm.logSO3(_T[:3, :3]) # print(_t) glTranslatef(_t[0], _t[1], _t[2]) # glMultMatrixd(_T) # pGeom->GetShape(&type, data) if geom_type == 'B' or geom_type == 'M': data = .5 * geom_sizes # data[0] *= SCALAR_1_2 # data[1] *= SCALAR_1_2 # data[2] *= SCALAR_1_2 _draw_box(data) elif geom_type == 'C': data = geom_sizes[i] # data.append(pGeom.GetRadius()) # data.append(pGeom.GetHeight()) data[1] -= 2. * data[0] _draw_capsule(data[0], data[1]) elif geom_type == 'S': data = geom_sizes[i] _draw_sphere(data[0]) glPopMatrix()
def reward(self): p_e = np.asarray([self.skel.getBodyPositionGlobal(body_idx) for body_idx in self.idx_e]).flatten() q = self.skel.getDOFPositions() dq = self.skel.getDOFVelocities() q_diff = np.asarray([mm.logSO3(np.dot(self.prev_ref_q[i].T, q[i])) for i in range(1, len(q))]).flatten() dq_diff = np.asarray([self.prev_ref_dq[i] - dq[i] for i in range(1, len(dq))]).flatten() q_reward = exp_reward_term(self.w_p, self.exp_p, np.concatenate(list(q_diff[self.skel.getJointDOFInternalIndexes(self.skel.name2index(joint_name))] for joint_name in self.reward_joint))) dq_reward = exp_reward_term(self.w_v, self.exp_v, np.concatenate(list(dq_diff[self.skel.getJointDOFInternalIndexes(self.skel.name2index(joint_name))] for joint_name in self.reward_joint))) ee_reward = exp_reward_term(self.w_e, self.exp_e, p_e - self.prev_ref_p_e_hat) com_reward = exp_reward_term(self.w_c, self.exp_c, self.skel.getCOM() - self.prev_ref_com) reward = q_reward + dq_reward + ee_reward + com_reward return reward
def solve(self, desComPos, cmW=1., posW=1., oriW=1.): """ :type desComPos: np.array :type cmW: float :type posW: float :type oriW: float :return: """ numItr = 1000 dt = .05 threshold = 0.00001 totalDOF = self.model.getTotalDOF() for i in range(numItr): Jc_IK = self.getConstJacobian(self.model) dv_IK = cmW * (desComPos - self.model.getCOM()) for j in range(len(self.bodyIdx)): ori_IK = self.model.getBodyOrientationGlobal(self.bodyIdx[j]) pos_IK = self.model.getBody(self.bodyIdx[j]).to_world( self.localPos[j]) dv_IK_tmp = posW * (self.desPos[j] - pos_IK) # type: np.array for k in range(3): if self.desPosMask[j][k]: dv_IK = np.append(dv_IK, dv_IK_tmp[k]) if self.desPosMask[j][3]: dv_IK = np.append( dv_IK, oriW * mm.logSO3(np.dot(self.desOri[j], ori_IK.T))) dth_IK_solve = npl.lstsq(Jc_IK, dv_IK) dth_IK_x = dth_IK_solve[0][:totalDOF] self.model.skeleton.q += dt * dth_IK_x # ype.nested(dth_IK_x, dth_IK) # th_IK = yct.getIntegralDOF(th_r_IK, dth_IK, dt) # self.pose.setDOFPositions(th_IK) if np.dot(dv_IK, dv_IK) < threshold: break
def get_deep_state(self): R_pelvis = self.getBody(0).world_transform()[:3, :3] com = self.getCOM() state = [ self.getBody(i).to_world() - com for i in range(self.body_num) ] R = [ mm.logSO3( np.dot(R_pelvis.T, self.getBody(i).world_transform()[:3, :3])) / math.pi for i in range(self.body_num) ] v = [ self.getBody(i).world_linear_velocity() for i in range(self.body_num) ] w = [ self.getBody(i).world_angular_velocity() for i in range(self.body_num) ] state.extend(R) state.extend(v) state.extend(w) return np.asarray(state).flatten()
def drawSO3(SO3, origin=numpy.array([0,0,0]), color=(0,255,0), lineWidth=1.0, name=''): glEnable(GL_LINE_STIPPLE) glLineStipple(2, 0xFAFA) drawVector(mmMath.logSO3(SO3), origin, color, lineWidth) glDisable(GL_LINE_STIPPLE)
def update(self): self.cur = mm.seq2Vec3(mm.logSO3(self.body.world_transform()[:3, :3]))
def simulateCallback(frame): print(frame) # print() # print(dartModel.getJointVelocityGlobal(0)) # print(dartModel.getDOFVelocities()[0]) # print(dartModel.get_dq()[:6]) dartMotionModel.update(motion[frame]) global g_initFlag global forceShowTime global preFootCenter global maxContactChangeCount global contactChangeCount global contact global contactChangeType # print('contactstate:', contact, contactChangeCount) Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals(['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt']) # Dt = 2.*(Kt**.5) Dt = Kt/100. Dl = (Kl**.5) Dh = (Kh**.5) dt_sup = 2.*(kt_sup**.5) # Dt = .2*(Kt**.5) # Dl = .2*(Kl**.5) # Dh = .2*(Kh**.5) # dt_sup = .2*(kt_sup**.5) pdcontroller.setKpKd(Kt, Dt) footHeight = dartModel.getBody(supL).shapenodes[0].shape.size()[1]/2. doubleTosingleOffset = 0.15 singleTodoubleOffset = 0.30 #doubleTosingleOffset = 0.09 doubleTosingleVelOffset = 0.0 com_offset_x, com_offset_y, com_offset_z = getParamVals(['com X offset', 'com Y offset', 'com Z offset']) footOffset = np.array((com_offset_x, com_offset_y, com_offset_z)) des_com = dartMotionModel.getCOM() + footOffset footCenterL = dartMotionModel.getBodyPositionGlobal(supL) footCenterR = dartMotionModel.getBodyPositionGlobal(supR) footBodyOriL = dartMotionModel.getBodyOrientationGlobal(supL) footBodyOriR = dartMotionModel.getBodyOrientationGlobal(supR) torso_pos = dartMotionModel.getBodyPositionGlobal(4) torso_ori = dartMotionModel.getBodyOrientationGlobal(4) # ik_solver.setInitPose(motion[frame]) # ik_solver.addConstraints(supL, np.zeros(3), footCenterL, footBodyOriL, (True, True, True, True)) # ik_solver.addConstraints(supR, np.zeros(3), footCenterR, footBodyOriR, (True, True, True, True)) # ik_solver.addConstraints(4, np.zeros(3), torso_pos, torso_ori, (False, False, False, True)) # ik_solver.solve(des_com) # ik_solver.clear() # tracking # th_r = motion.getDOFPositions(frame) th_r = dartMotionModel.getDOFPositions() th = dartModel.getDOFPositions() th_r_flat = dartMotionModel.get_q() # dth_r = motion.getDOFVelocities(frame) # dth = dartModel.getDOFVelocities() # ddth_r = motion.getDOFAccelerations(frame) # ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt) dth_flat = dartModel.get_dq() # dth_flat = np.concatenate(dth) # ddth_des_flat = pdcontroller.compute(dartMotionModel.get_q()) # ddth_des_flat = pdcontroller.compute(th_r) ddth_des_flat = pdcontroller.compute_flat(th_r_flat) # ype.flatten(ddth_des, ddth_des_flat) # ype.flatten(dth, dth_flat) ################################################# # jacobian ################################################# footOriL = dartModel.getJointOrientationGlobal(supL) footOriR = dartModel.getJointOrientationGlobal(supR) footCenterL = dartModel.getBodyPositionGlobal(supL) footCenterR = dartModel.getBodyPositionGlobal(supR) footBodyOriL = dartModel.getBodyOrientationGlobal(supL) footBodyOriR = dartModel.getBodyOrientationGlobal(supR) footBodyVelL = dartModel.getBodyVelocityGlobal(supL) footBodyVelR = dartModel.getBodyVelocityGlobal(supR) footBodyAngVelL = dartModel.getBodyAngVelocityGlobal(supL) footBodyAngVelR = dartModel.getBodyAngVelocityGlobal(supR) refFootL = dartMotionModel.getBodyPositionGlobal(supL) refFootR = dartMotionModel.getBodyPositionGlobal(supR) # refFootAngVelL = motion.getJointAngVelocityGlobal(supL, frame) # refFootAngVelR = motion.getJointAngVelocityGlobal(supR, frame) refFootAngVelL = np.zeros(3) refFootAngVelR = np.zeros(3) refFootJointVelR = motion.getJointVelocityGlobal(supR, frame) refFootJointAngVelR = motion.getJointAngVelocityGlobal(supR, frame) refFootJointR = motion.getJointPositionGlobal(supR, frame) # refFootVelR = refFootJointVelR + np.cross(refFootJointAngVelR, (refFootR-refFootJointR)) refFootVelR = np.zeros(3) refFootJointVelL = motion.getJointVelocityGlobal(supL, frame) refFootJointAngVelL = motion.getJointAngVelocityGlobal(supL, frame) refFootJointL = motion.getJointPositionGlobal(supL, frame) # refFootVelL = refFootJointVelL + np.cross(refFootJointAngVelL, (refFootL-refFootJointL)) refFootVelL = np.zeros(3) contactR = 1 contactL = 1 if refFootVelR[1] < 0 and refFootVelR[1]*frame_step_size + refFootR[1] > singleTodoubleOffset: contactR = 0 if refFootVelL[1] < 0 and refFootVelL[1]*frame_step_size + refFootL[1] > singleTodoubleOffset: contactL = 0 if refFootVelR[1] > 0 and refFootVelR[1]*frame_step_size + refFootR[1] > doubleTosingleOffset: contactR = 0 if refFootVelL[1] > 0 and refFootVelL[1]*frame_step_size + refFootL[1] > doubleTosingleOffset: contactL = 0 # contactR = 0 # contMotionOffset = th[0][0] - th_r[0][0] contMotionOffset = dartModel.getBodyPositionGlobal(0) - dartMotionModel.getBodyPositionGlobal(0) linkPositions = dartModel.getBodyPositionsGlobal() linkVelocities = dartModel.getBodyVelocitiesGlobal() linkAngVelocities = dartModel.getBodyAngVelocitiesGlobal() linkInertias = dartModel.getBodyInertiasGlobal() CM = dartModel.skeleton.com() dCM = dartModel.skeleton.com_velocity() CM_plane = copy.copy(CM) CM_plane[1]=0. dCM_plane = copy.copy(dCM) dCM_plane[1]=0. P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM, linkInertias) dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses, linkVelocities, dCM, linkAngVelocities, linkInertias) #calculate contact state #if g_initFlag == 1 and contact == 1 and refFootR[1] < doubleTosingleOffset and footCenterR[1] < 0.08: if g_initFlag == 1: #contact state # 0: flying 1: right only 2: left only 3: double #if contact == 2 and refFootR[1] < doubleTosingleOffset: if contact == 2 and contactR==1: contact = 3 maxContactChangeCount+=30 contactChangeCount += maxContactChangeCount contactChangeType = 'StoD' #elif contact == 3 and refFootL[1] < doubleTosingleOffset: elif contact == 1 and contactL==1: contact = 3 maxContactChangeCount+=30 contactChangeCount += maxContactChangeCount contactChangeType = 'StoD' #elif contact == 3 and refFootR[1] > doubleTosingleOffset: elif contact == 3 and contactR == 0: contact = 2 contactChangeCount += maxContactChangeCount contactChangeType = 'DtoS' #elif contact == 3 and refFootL[1] > doubleTosingleOffset: elif contact == 3 and contactL == 0: contact = 1 contactChangeCount += maxContactChangeCount contactChangeType = 'DtoS' else: contact = 0 #if refFootR[1] < doubleTosingleOffset: if contactR == 1: contact +=1 #if refFootL[1] < doubleTosingleOffset: if contactL == 1: contact +=2 #initialization if g_initFlag == 0: softConstPoint = footCenterR.copy() footCenter = footCenterL + (footCenterR - footCenterL)/2.0 footCenter[1] = 0. preFootCenter = footCenter.copy() #footToBodyFootRotL = np.dot(np.transpose(footOriL), footBodyOriL) #footToBodyFootRotR = np.dot(np.transpose(footOriR), footBodyOriR) # if refFootR[1] < doubleTosingleOffset: # contact +=1 # if refFootL[1] < doubleTosingleOffset: # contact +=2 if refFootR[1] < footHeight: contact +=1 if refFootL[1] < footHeight: contact +=2 g_initFlag = 1 #calculate jacobian body_num = dartModel.getBodyNum() Jsys = np.zeros((6*body_num, totalDOF)) dJsys = np.zeros((6*body_num, totalDOF)) for i in range(dartModel.getBodyNum()): # body_i_jacobian = dartModel.getBody(i).world_jacobian()[range(-3, 3), :] # body_i_jacobian_deriv = dartModel.getBody(i).world_jacobian_classic_deriv()[range(-3, 3), :] # Jsys[6*i:6*i+6, :] = body_i_jacobian # dJsys[6*i:6*i+6, :] = body_i_jacobian_deriv Jsys[6*i:6*i+6, :] = dartModel.getBody(i).world_jacobian()[range(-3, 3), :] dJsys[6*i:6*i+6, :] = dartModel.getBody(i).world_jacobian_classic_deriv()[range(-3, 3), :] # dJsys = (Jsys - Jpre[0])/frame_step_size # Jpre[0] = Jsys.copy() JsupL = dartModel.getBody(supL).world_jacobian()[range(-3, 3), :] dJsupL = dartModel.getBody(supL).world_jacobian_classic_deriv()[range(-3, 3), :] # dJsupL = np.zeros_like(JsupL) # dJsupL = (JsupL - Jpre[1])/frame_step_size # Jpre[1] = JsupL.copy() JsupR = dartModel.getBody(supR).world_jacobian()[range(-3, 3), :] dJsupR = dartModel.getBody(supR).world_jacobian_classic_deriv()[range(-3, 3), :] # dJsupR = np.zeros_like(JsupR) # dJsupR = (JsupR - Jpre[2])/frame_step_size # Jpre[2] = JsupR.copy() #calculate footCenter footCenter = .5 * (footCenterL + footCenterR) + footOffset #if refFootR[1] >doubleTosingleOffset: #if refFootR[1] > doubleTosingleOffset or footCenterR[1] > 0.08: #if contact == 1 or footCenterR[1] > 0.08: #if contact == 2 or footCenterR[1] > doubleTosingleOffset/2: if contact == 2: footCenter = footCenterL.copy() + footOffset #elif contact == 1 or footCenterL[1] > doubleTosingleOffset/2: if contact == 1: footCenter = footCenterR.copy() + footOffset footCenter[1] = 0. if contactChangeCount > 0 and contactChangeType == 'StoD': #change footcenter gradually footCenter = preFootCenter + (maxContactChangeCount - contactChangeCount)*(footCenter-preFootCenter)/maxContactChangeCount preFootCenter = footCenter.copy() # linear momentum #TODO: # We should consider dCM_ref, shouldn't we? # add getBodyPositionGlobal and getBodyPositionsGlobal in csVpModel! # todo that, set joint velocities to vpModel CM_ref_plane = footCenter dL_des_plane = Kl*totalMass*(CM_ref_plane - CM_plane) - Dl*totalMass*dCM_plane dL_des_plane[1] = 0. # CM_ref = footCenter.copy() # CM_ref[1] = dartMotionModel.getCOM()[1] # CM_ref += np.array((0., com_offset_y, 0.)) # dL_des_plane = Kl*totalMass*(CM_ref - CM) - Dl*totalMass*dCM # angular momentum CP_ref = footCenter bodyIDs, contactPositions, contactPositionLocals, contactForces = [], [], [], [] if DART_CONTACT_ON: bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.get_dart_contact_info() else: bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds) #bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds) CP = yrp.getCP(contactPositions, contactForces) if CP_old[0] is None or CP is None: dCP = None else: dCP = (CP - CP_old[0])/frame_step_size CP_old[0] = CP CP_des[0] = None # if CP_des[0] is None: # CP_des[0] = footCenter if CP is not None and dCP is not None: ddCP_des = Kh*(CP_ref - CP) - Dh*(dCP) CP_des[0] = CP + dCP * frame_step_size + .5 * ddCP_des*(frame_step_size**2) # dCP_des[0] += ddCP_des * frame_step_size # CP_des[0] += dCP_des[0] * frame_step_size + .5 * ddCP_des*(frame_step_size ** 2) dH_des = np.cross(CP_des[0] - CM, dL_des_plane + totalMass*mm.s2v(wcfg.gravity)) if contactChangeCount > 0:# and contactChangeType == 'DtoS': #dH_des *= (maxContactChangeCount - contactChangeCount)/(maxContactChangeCount*10) dH_des *= (maxContactChangeCount - contactChangeCount)/(maxContactChangeCount) #dH_des *= (contactChangeCount)/(maxContactChangeCount)*.9+.1 else: dH_des = None # H = np.dot(P, np.dot(Jsys, dth_flat)) # dH_des = -Kh* H[3:] # soft point constraint #softConstPoint = refFootR.copy() ##softConstPoint[0] += 0.2 #Ksc = 50 #Dsc = 2*(Ksc**.5) #Bsc = 1. #P_des = softConstPoint #P_cur = controlModel.getBodyPositionGlobal(constBody) #dP_des = [0, 0, 0] #dP_cur = controlModel.getBodyVelocityGlobal(constBody) #ddP_des1 = Ksc*(P_des - P_cur) + Dsc*(dP_des - dP_cur) #r = P_des - P_cur #I = np.vstack(([1,0,0],[0,1,0],[0,0,1])) #Z = np.hstack((I, mm.getCrossMatrixForm(-r))) #yjc.computeJacobian2(Jconst, DOFs, jointPositions, jointAxeses, [softConstPoint], constJointMasks) #dJconst = (Jconst - Jconst)/(1/30.) #JconstPre = Jconst.copy() ##yjc.computeJacobianDerivative2(dJconst, DOFs, jointPositions, jointAxeses, linkAngVelocities, [softConstPoint], constJointMasks, False) #JL, JA = np.vsplit(Jconst, 2) #Q1 = np.dot(Z, Jconst) #q1 = np.dot(JA, dth_flat) #q2 = np.dot(mm.getCrossMatrixForm(q1), np.dot(mm.getCrossMatrixForm(q1), r)) #q_bias1 = np.dot(np.dot(Z, dJconst), dth_flat) + q2 #set up equality constraint a_oriL = mm.logSO3(mm.getSO3FromVectors(np.dot(footBodyOriL, np.array([0,1,0])), np.array([0,1,0]))) a_oriR = mm.logSO3(mm.getSO3FromVectors(np.dot(footBodyOriR, np.array([0,1,0])), np.array([0,1,0]))) footErrorL = refFootL.copy() footErrorL[1] = dartModel.getBody(supL).shapenodes[0].shape.size()[1]/2. footErrorL += -footCenterL + contMotionOffset footErrorR = refFootR.copy() footErrorR[1] = dartModel.getBody(supR).shapenodes[0].shape.size()[1]/2. footErrorR += -footCenterR + contMotionOffset a_supL = np.append(kt_sup*footErrorL + dt_sup*(refFootVelL - footBodyVelL), kt_sup*a_oriL+dt_sup*(refFootAngVelL-footBodyAngVelL)) a_supR = np.append(kt_sup*footErrorR + dt_sup*(refFootVelR - footBodyVelR), kt_sup*a_oriR+dt_sup*(refFootAngVelR-footBodyAngVelR)) if contactChangeCount > 0 and contactChangeType == 'DtoS': a_supL = np.append(kt_sup*(refFootL - footCenterL + contMotionOffset) + dt_sup*(refFootVelL - footBodyVelL), 4*kt_sup*a_oriL+2*dt_sup*(refFootAngVelL-footBodyAngVelL)) a_supR = np.append(kt_sup*(refFootR - footCenterR + contMotionOffset) + dt_sup*(refFootVelR - footBodyVelR), 4*kt_sup*a_oriR+2*dt_sup*(refFootAngVelR-footBodyAngVelR)) elif contactChangeCount > 0 and contactChangeType == 'StoD': linkt = (13.*contactChangeCount)/(maxContactChangeCount)+1. lindt = 2*(linkt**.5) angkt = (13.*contactChangeCount)/(maxContactChangeCount)+1. angdt = 2*(angkt**.5) a_supL = np.append(linkt*kt_sup*(refFootL - footCenterL + contMotionOffset) + lindt*dt_sup*(refFootVelL - footBodyVelL), angkt*kt_sup*a_oriL+angdt*dt_sup*(refFootAngVelL-footBodyAngVelL)) a_supR = np.append(linkt*kt_sup*(refFootR - footCenterR + contMotionOffset) + lindt*dt_sup*(refFootVelR - footBodyVelR), angkt*kt_sup*a_oriR+angdt*dt_sup*(refFootAngVelR-footBodyAngVelR)) # momentum matrix RS = np.dot(P, Jsys) R, S = np.vsplit(RS, 2) rs = np.dot((np.dot(dP, Jsys) + np.dot(P, dJsys)), dth_flat) r_bias, s_bias = np.hsplit(rs, 2) ####################################################### # optimization ####################################################### #if contact == 2 and footCenterR[1] > doubleTosingleOffset/2: if contact == 2: config['weightMap']['RightUpLeg'] = .8 config['weightMap']['RightLeg'] = .8 config['weightMap']['RightFoot'] = .8 else: config['weightMap']['RightUpLeg'] = .1 config['weightMap']['RightLeg'] = .25 config['weightMap']['RightFoot'] = .2 #if contact == 1 and footCenterL[1] > doubleTosingleOffset/2: if contact == 1: config['weightMap']['LeftUpLeg'] = .8 config['weightMap']['LeftLeg'] = .8 config['weightMap']['LeftFoot'] = .8 else: config['weightMap']['LeftUpLeg'] = .1 config['weightMap']['LeftLeg'] = .25 config['weightMap']['LeftFoot'] = .2 w = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap']) #if contact == 2: #mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1, q_bias1) mot.addTrackingTerms(problem, totalDOF, Bt, w, ddth_des_flat) mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias) if dH_des is not None: mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias) #mot.setConstraint(problem, totalDOF, Jsup, dJsup, dth_flat, a_sup) #mot.addConstraint(problem, totalDOF, Jsup, dJsup, dth_flat, a_sup) #if contact & 1 and contactChangeCount == 0: if contact & 1: #if refFootR[1] < doubleTosingleOffset: mot.addConstraint(problem, totalDOF, JsupR, dJsupR, dth_flat, a_supR) if contact & 2: #if refFootL[1] < doubleTosingleOffset: mot.addConstraint(problem, totalDOF, JsupL, dJsupL, dth_flat, a_supL) if contactChangeCount >0: contactChangeCount -= 1 if contactChangeCount == 0: maxContactChangeCount = 30 contactChangeType = 0 r = problem.solve() problem.clear() # ype.nested(r['x'], ddth_sol) ddth_sol = np.asarray(r['x']) # ddth_sol[:6] = np.zeros(6) rootPos[0] = dartModel.getBodyPositionGlobal(selectedBody) localPos = [[0, 0, 0]] for i in range(stepsPerFrame): # apply penalty force if not DART_CONTACT_ON: bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds) dartModel.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces) #bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds) # dartModel.skeleton.set_accelerations(ddth_sol) # dartModel.skeleton.set_accelerations(ddth_des_flat) # dartModel.skeleton.set_forces(np.zeros(totalDOF)) # ddth_des_flat[:6] = np.zeros(6) th_r_flat = dartMotionModel.get_q() ddth_des_flat = pdcontroller.compute_flat(th_r_flat) dartModel.skeleton.set_forces(ddth_des_flat) if forceShowTime > viewer.objectInfoWnd.labelForceDur.value(): forceShowTime = 0 viewer_ResetForceState() forceforce = np.array([viewer.objectInfoWnd.labelForceX.value(), viewer.objectInfoWnd.labelForceY.value(), viewer.objectInfoWnd.labelForceZ.value()]) extraForce[0] = getParamVal('Fm') * mm.normalize2(forceforce) if viewer_GetForceState(): forceShowTime += wcfg.timeStep dartModel.applyPenaltyForce(selectedBodyId, localPos, extraForce) dartModel.step() if DART_CONTACT_ON: bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.get_dart_contact_info() else: bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds) if viewer.reset: viewer.reset = False dartModel.reset() # print(dartModel.getCOM()) # rendering rightFootVectorX[0] = np.dot(footOriL, np.array([.1, 0, 0])) rightFootVectorY[0] = np.dot(footOriL, np.array([0, .1, 0])) rightFootVectorZ[0] = np.dot(footOriL, np.array([0, 0,.1])) rightFootPos[0] = footCenterL rightVectorX[0] = np.dot(footBodyOriL, np.array([.1,0,0])) rightVectorY[0] = np.dot(footBodyOriL, np.array([0,.1,0])) rightVectorZ[0] = np.dot(footBodyOriL, np.array([0,0,.1])) rightPos[0] = footCenterL + np.array([.1,0,0]) rd_footCenter[0] = footCenter rd_footCenterL[0] = footCenterL rd_footCenterR[0] = footCenterR rd_CM[0] = CM rd_CM_plane[0] = CM.copy() rd_CM_plane[0][1] = 0. if CP is not None and dCP is not None: rd_CP[0] = CP rd_CP_des[0] = CP_des[0] rd_dL_des_plane[0] = [dL_des_plane[0]/100, dL_des_plane[1]/100, dL_des_plane[2]/100] rd_dH_des[0] = dH_des rd_grf_des[0] = dL_des_plane - totalMass*mm.s2v(wcfg.gravity) rd_root_des[0] = rootPos[0] del rd_CF[:] del rd_CF_pos[:] for i in range(len(contactPositions)): rd_CF.append( contactForces[i]/100) rd_CF_pos.append(contactPositions[i].copy()) if viewer_GetForceState(): rd_exfen_des[0] = [extraForce[0][0]/100, extraForce[0][1]/100, extraForce[0][2]/100] rd_exf_des[0] = [0,0,0] else: rd_exf_des[0] = [extraForce[0][0]/100, extraForce[0][1]/100, extraForce[0][2]/100] rd_exfen_des[0] = [0,0,0] extraForcePos[0] = dartModel.getBodyPositionGlobal(selectedBody)
def preprocess(SEGMENT_FOOT=False): # dir = './icmotion_test/' # paths = glob.glob(dir+'*.temp') dir = './ppmotion/' # paths = glob.glob(dir+'*.bvh') paths = None if SEGMENT_FOOT: paths = glob.glob(dir+'segfoot_wd2_WalkForwardNormal00.bvh') paths = glob.glob(dir+'segfoot_wd2_WalkForwardNormal00_REPEATED.bvh') else: paths = glob.glob(dir+'wd2_WalkForwardNormal00.bvh') paths = glob.glob(dir+'wd2_WalkForwardNormal00_REPEATED.bvh') # paths = glob.glob(dir+'wd2_WalkForwardVFast00.bvh') ''' paths = glob.glob(dir+'wd2_WalkSameSame01.bvh') paths = glob.glob(dir+'wd2_u-turn_1.bvh') paths = glob.glob(dir+'wd2_cross_walk*.bvh') paths = glob.glob(dir+'*_REPEATED.bvh') paths = glob.glob(dir+'wd2_pick_walk_1.bvh') paths = glob.glob(dir+'wd2_WalkSameSame01_REPEATED_FOOT.bvh') paths = glob.glob(dir+'wd2_WalkForwardSlow01_REPEATED_FOOT.bvh') paths = glob.glob(dir+'wd2_WalkForwardNormal01_REPEATED_FOOT.bvh') paths = glob.glob(dir+'wd2_WalkSoldier00_REPEATED_FOOT.bvh') ''' hRef = .1; vRef = .4 ## hRef = .1; vRef = .2 # dir = './ppmotion_long/' # paths = glob.glob(dir+'wd2_WalkBackward00_REPEATED.bvh') # hRef = 10.; vRef = .4 # dir = './rawmotion_slope/' # paths = glob.glob(dir+'*.bvh') ## paths = [dir+'woddy2_walk_normal_to_slope.bvh'] # hRef = 10000.; vRef = .4*100 # # dir = './ppmotion_slope/' # paths = glob.glob(dir+'*.bvh') # hRef = 10000.; vRef = .2 jumpThreshold = 15; jumpBias = 1. stopThreshold = 15; stopBias = 0. for path in paths: motion_ori = yf.readBvhFile(path) # informations skeleton = motion_ori[0].skeleton lFoot = skeleton.getJointIndex('LeftFoot'); rFoot = skeleton.getJointIndex('RightFoot') lHip = skeleton.getJointIndex('LeftUpLeg'); rHip = skeleton.getJointIndex('RightUpLeg') lKnee = skeleton.getJointIndex('LeftLeg'); rKnee = skeleton.getJointIndex('RightLeg') lFoot = skeleton.getJointIndex('LeftFoot'); rFoot = skeleton.getJointIndex('RightFoot') mcfgfile = open(dir + 'mcfg', 'rb') mcfg = pickle.load(mcfgfile) mcfgfile.close() wcfg = ypc.WorldConfig() vpWorld = cvw.VpWorld(wcfg) motionModel = cvm.VpMotionModel(vpWorld, motion_ori[0], mcfg) # bodyMasses = getBodyMasses() bodyMasses = motionModel.getBodyMasses() uppers = [skeleton.getJointIndex(name) for name in ['Hips', 'Spine', 'Spine1', 'LeftArm', 'LeftForeArm', 'RightArm', 'RightForeArm']] upperMass = sum([bodyMasses[i] for i in uppers]) lc = yma.getElementContactStates(motion_ori, 'LeftFoot', hRef, vRef) rc = yma.getElementContactStates(motion_ori, 'RightFoot', hRef, vRef) # intervals, states = yba.getBipedGaitIntervals(lc, rc, jumpThreshold, jumpBias, stopThreshold, stopBias) intervals, states = yba.getBipedGaitIntervals2(lc, rc, jumpThreshold, jumpBias, stopThreshold, stopBias) seginfos = [{} for i in range(len(intervals))] for i in range(len(intervals)): start = intervals[i][0]; end = intervals[i][1] seginfos[i]['interval'] = intervals[i] seginfos[i]['state'] = states[i] # print yba.GaitState.text[states[i]], intervals[i] stanceHips = []; swingHips = []; stanceFoots = []; swingFoots = []; swingKnees = [] if states[i]==yba.GaitState.LSWING: stanceHips = [rHip]; stanceFoots = [rFoot]; swingHips = [lHip]; swingFoots = [lFoot]; swingKnees = [lKnee] elif states[i]==yba.GaitState.RSWING: stanceHips = [lHip]; stanceFoots = [lFoot]; swingHips = [rHip]; swingFoots = [rFoot]; swingKnees = [rKnee] elif states[i]==yba.GaitState.STOP: stanceHips = [rHip, lHip]; stanceFoots = [rFoot, lFoot] elif states[i]==yba.GaitState.JUMP: swingHips = [rHip, lHip]; swingFoots = [rFoot, lFoot] seginfos[i]['stanceHips'] = stanceHips seginfos[i]['swingHips'] = swingHips seginfos[i]['stanceFoots'] = stanceFoots seginfos[i]['swingFoots'] = swingFoots seginfos[i]['swingKnees'] = swingKnees if start<end: if SEGMENT_FOOT: # segmented foot seginfos[i]['ground_height'] = min([posture_seg.getJointPositionGlobal(foot)[1] - 0.05 for foot in [lFoot, rFoot] for posture_seg in motion_ori[start+1:end+1]]) else: # box foot seginfos[i]['ground_height'] = min([posture_seg.getJointPositionGlobal(foot)[1] for foot in [lFoot, rFoot] for posture_seg in motion_ori[start+1:end+1]]) seginfos[i]['max_stf_push_frame'] = None if len(swingFoots) > 0: pushes = [] for frame in range(start, int((start+end)//2) + 1): dCM_tar = yrp.getCM(motion_ori.getJointVelocitiesGlobal(frame), bodyMasses, None, uppers) direction = mm.normalize2(mm.projectionOnPlane(dCM_tar, (1,0,0), (0,0,1))) directionAxis = np.cross((0,1,0), direction) pushes.append(mm.componentOnVector(mm.logSO3(motion_ori[frame].getJointOrientationFromParentGlobal(swingFoots[0])), directionAxis)) seginfos[i]['max_stf_push_frame'] = pushes.index(max(pushes)) # write .seg inputName = os.path.basename(path) root = os.path.splitext(inputName)[0] outputName = root+'.seg' outputFile = open(dir+outputName, 'wb') pickle.dump(seginfos, outputFile) outputFile.close() print(outputName, 'done') pprint.pprint(seginfos) print('FINISHED')
def step_model(self): contacts, points, angles, orientations, root_orientation = self.controller.step(self.get_target()) # pairs = [[0,11,3,4], # [0,8,10,2], # [0,13,6,7], # [0,9,12,5], # [0,1]] pairs = [[0,18,11,3,4], [0,14,8,10,2], [0,19,13,6,7], [0,14,9,12,5], [0,14,17,1]] self.lines = [] for pair in pairs: for i in range(len(pair)-1): self.lines.append([points[pair[i]], points[pair[i+1]]]) # print(len(orientations)) for i in range(len(angles)): self.all_angles[i].append(angles[i]) for j in range(len(self.model.joints)): if j == 0: joint = self.model.joints[j] # type: pydart.FreeJoint joint_idx = joint_list.index(joint.name) hip_angles = mm.logSO3(np.dot(root_orientation, orientations[joint_idx])) # hip_angles = mm.logSO3(root_orientation) joint.set_position(np.array([hip_angles[0], hip_angles[1], hip_angles[2], points[0][0], points[0][1], points[0][2]])) continue joint = self.model.joints[j] # type: pydart.BallJoint joint_idx = joint_list.index(joint.name) joint.set_position(angles[joint_idx*3:joint_idx*3+3]) self.ik.clean_constraints() self.ik.add_joint_pos_const('LeftForeArm', np.asarray(points[10])) self.ik.add_joint_pos_const('LeftHand', np.asarray(points[2])) self.ik.add_joint_pos_const('LeftLeg', np.asarray(points[11])) self.ik.add_joint_pos_const('LeftFoot', np.asarray(points[3])) if contacts[0] > 0.8 and False: body_transform = self.model.body('LeftFoot').transform()[:3, :3] angle = math.acos(body_transform[1, 1]) body_ori = np.dot(body_transform, mm.rotX(-angle)) self.ik.add_orientation_const('LeftFoot', body_ori) self.ik.add_joint_pos_const('RightForeArm', np.asarray(points[12])) self.ik.add_joint_pos_const('RightHand', np.asarray(points[5])) self.ik.add_joint_pos_const('RightLeg', np.asarray(points[13])) self.ik.add_joint_pos_const('RightFoot', np.asarray(points[6])) self.ik.solve() foot_joint_ori = mm.exp(self.model.joint('LeftFoot').position()) self.model.joint('LeftFoot').set_position(mm.logSO3(np.dot(foot_joint_ori, np.dot(mm.rotX(-.6), mm.rotZ(.4))))) foot_joint_ori = mm.exp(self.model.joint('RightFoot').position()) self.model.joint('RightFoot').set_position(mm.logSO3(np.dot(foot_joint_ori, np.dot(mm.rotX(-.6), mm.rotZ(-.4))))) left_foot = self.model.body('LeftFoot') if (left_foot.to_world([0.05, -0.045, 0.1125])[1] < 0. or left_foot.to_world([-0.05, -0.045, 0.1125])[1] < 0.) \ and (left_foot.to_world([0.05, -0.045, -0.1125])[1] < 0. or left_foot.to_world([-0.05, -0.045, -0.1125])[1] < 0.): left_toe_pos1 = left_foot.to_world([0.05, -0.045, +0.1125]) left_toe_pos1[1] = 0. left_toe_pos2 = left_foot.to_world([-0.05, -0.045, +0.1125]) left_toe_pos2[1] = 0. left_heel_pos1 = left_foot.to_world([0.05, -0.045, -0.1125]) left_heel_pos1[1] = 0. left_heel_pos2 = left_foot.to_world([-0.05, -0.045, -0.1125]) left_heel_pos2[1] = 0. self.ik.clean_constraints() self.ik.add_position_const('LeftFoot', left_toe_pos1, np.array([0.05, -0.045, +0.1125])) self.ik.add_position_const('LeftFoot', left_toe_pos2, np.array([-0.05, -0.045, +0.1125])) self.ik.add_position_const('LeftFoot', left_heel_pos1, np.array([0.05, -0.045, -0.1125])) self.ik.add_position_const('LeftFoot', left_heel_pos2, np.array([-0.05, -0.045, -0.1125])) self.ik.solve() right_foot = self.model.body('RightFoot') if (right_foot.to_world([0.05, -0.045, 0.1125])[1] < 0. or right_foot.to_world([-0.05, -0.045, 0.1125])[1] < 0.) \ and (right_foot.to_world([0.05, -0.045, -0.1125])[1] < 0. or right_foot.to_world([-0.05, -0.045, -0.1125])[1] < 0.): right_toe_pos1 = right_foot.to_world([0.05, -0.045, +0.1125]) right_toe_pos1[1] = 0. right_toe_pos2 = right_foot.to_world([-0.05, -0.045, +0.1125]) right_toe_pos2[1] = 0. right_heel_pos1 = right_foot.to_world([0.05, -0.045, -0.1125]) right_heel_pos1[1] = 0. right_heel_pos2 = right_foot.to_world([-0.05, -0.045, -0.1125]) right_heel_pos2[1] = 0. self.ik.clean_constraints() self.ik.add_position_const('RightFoot', right_toe_pos1, np.array([0.05, -0.045, +0.1125])) self.ik.add_position_const('RightFoot', right_toe_pos2, np.array([-0.05, -0.045, +0.1125])) self.ik.add_position_const('RightFoot', right_heel_pos1, np.array([0.05, -0.045, -0.1125])) self.ik.add_position_const('RightFoot', right_heel_pos2, np.array([-0.05, -0.045, -0.1125])) self.ik.solve()
def simulateCallback(frame): motionModel.update(motion[frame]) global g_initFlag global forceShowTime global JsysPre global JsupPreL global JsupPreR global JsupPre global JconstPre global preFootCenter global maxContactChangeCount global contactChangeCount global contact global contactChangeType # Kt, Kl, Kh, Bl, Bh, kt_sup = viewer.GetParam() Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals( ['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt']) Dt = 2 * (Kt**.5) Dl = 2 * (Kl**.5) Dh = 2 * (Kh**.5) dt_sup = 2 * (kt_sup**.5) doubleTosingleOffset = 0.15 singleTodoubleOffset = 0.30 # doubleTosingleOffset = 0.09 doubleTosingleVelOffset = 0.0 # tracking th_r = motion.getDOFPositions(frame) th = controlModel.getDOFPositions() dth_r = motion.getDOFVelocities(frame) dth = controlModel.getDOFVelocities() ddth_r = motion.getDOFAccelerations(frame) ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt) ype.flatten(ddth_des, ddth_des_flat) ype.flatten(dth, dth_flat) ################################################# # jacobian ################################################# # caution!! body orientation and joint orientation of foot are totally different!! footOriL = controlModel.getJointOrientationGlobal(supL) footOriR = controlModel.getJointOrientationGlobal(supR) # desire footCenter[1] = 0.041135 # desire footCenter[1] = 0.0197 footCenterL = controlModel.getBodyPositionGlobal(supL) footCenterR = controlModel.getBodyPositionGlobal(supR) footBodyOriL = controlModel.getBodyOrientationGlobal(supL) footBodyOriR = controlModel.getBodyOrientationGlobal(supR) footBodyVelL = controlModel.getBodyVelocityGlobal(supL) footBodyVelR = controlModel.getBodyVelocityGlobal(supR) footBodyAngVelL = controlModel.getBodyAngVelocityGlobal(supL) footBodyAngVelR = controlModel.getBodyAngVelocityGlobal(supR) refFootL = motionModel.getBodyPositionGlobal(supL) refFootR = motionModel.getBodyPositionGlobal(supR) refFootVelL = motionModel.getBodyVelocityGlobal(supL) refFootVelR = motionModel.getBodyVelocityGlobal(supR) refFootAngVelL = motionModel.getBodyAngVelocityGlobal(supL) refFootAngVelR = motionModel.getBodyAngVelocityGlobal(supR) refFootJointVelR = motion.getJointVelocityGlobal(supR, frame) refFootJointAngVelR = motion.getJointAngVelocityGlobal(supR, frame) refFootJointR = motion.getJointPositionGlobal(supR, frame) refFootVelR = refFootJointVelR + np.cross(refFootJointAngVelR, (refFootR - refFootJointR)) refFootJointVelL = motion.getJointVelocityGlobal(supL, frame) refFootJointAngVelL = motion.getJointAngVelocityGlobal(supL, frame) refFootJointL = motion.getJointPositionGlobal(supL, frame) refFootVelL = refFootJointVelL + np.cross(refFootJointAngVelL, (refFootL - refFootJointL)) contactR = 1 contactL = 1 if refFootVelR[1] < 0 and refFootVelR[1] / 30. + refFootR[ 1] > singleTodoubleOffset: contactR = 0 if refFootVelL[1] < 0 and refFootVelL[1] / 30. + refFootL[ 1] > singleTodoubleOffset: contactL = 0 if refFootVelR[1] > 0 and refFootVelR[1] / 30. + refFootR[ 1] > doubleTosingleOffset: contactR = 0 if refFootVelL[1] > 0 and refFootVelL[1] / 30. + refFootL[ 1] > doubleTosingleOffset: contactL = 0 # if 32 < frame < 147: # contactR = 0 contMotionOffset = th[0][0] - th_r[0][0] linkPositions = controlModel.getBodyPositionsGlobal() linkVelocities = controlModel.getBodyVelocitiesGlobal() linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal() linkInertias = controlModel.getBodyInertiasGlobal() jointPositions = controlModel.getJointPositionsGlobal() jointAxeses = controlModel.getDOFAxeses() CM = yrp.getCM(linkPositions, linkMasses, totalMass) dCM = yrp.getCM(linkVelocities, linkMasses, totalMass) CM_plane = copy.copy(CM) CM_plane[1] = 0. dCM_plane = copy.copy(dCM) dCM_plane[1] = 0. P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM, linkInertias) dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses, linkVelocities, dCM, linkAngVelocities, linkInertias) # calculate jacobian Jsys, dJsys = controlModel.computeCom_J_dJdq() JsupL = Jsys[6 * supL:6 * supL + 6, :] dJsupL = dJsys[6 * supL:6 * supL + 6] JsupR = Jsys[6 * supR:6 * supR + 6, :] dJsupR = dJsys[6 * supR:6 * supR + 6] # calculate contact state # if g_initFlag == 1 and contact == 1 and refFootR[1] < doubleTosingleOffset and footCenterR[1] < 0.08: if g_initFlag == 1: # contact state # 0: flying 1: right only 2: left only 3: double # if contact == 2 and refFootR[1] < doubleTosingleOffset: if contact == 2 and contactR == 1: contact = 3 maxContactChangeCount += 30 contactChangeCount += maxContactChangeCount contactChangeType = 'StoD' # elif contact == 3 and refFootL[1] < doubleTosingleOffset: elif contact == 1 and contactL == 1: contact = 3 maxContactChangeCount += 30 contactChangeCount += maxContactChangeCount contactChangeType = 'StoD' # elif contact == 3 and refFootR[1] > doubleTosingleOffset: elif contact == 3 and contactR == 0: contact = 2 contactChangeCount += maxContactChangeCount contactChangeType = 'DtoS' # elif contact == 3 and refFootL[1] > doubleTosingleOffset: elif contact == 3 and contactL == 0: contact = 1 contactChangeCount += maxContactChangeCount contactChangeType = 'DtoS' else: contact = 0 # if refFootR[1] < doubleTosingleOffset: if contactR == 1: contact += 1 # if refFootL[1] < doubleTosingleOffset: if contactL == 1: contact += 2 # initialization if g_initFlag == 0: JsysPre = Jsys.copy() JsupPreL = JsupL.copy() JsupPreR = JsupR.copy() JconstPre = Jconst.copy() softConstPoint = footCenterR.copy() # yjc.computeJacobian2(JsysPre, DOFs, jointPositions, jointAxeses, linkPositions, allLinkJointMasks) # yjc.computeJacobian2(JsupPreL, DOFs, jointPositions, jointAxeses, [footCenterL], supLJointMasks) # yjc.computeJacobian2(JsupPreR, DOFs, jointPositions, jointAxeses, [footCenterR], supRJointMasks) # yjc.computeJacobian2(JconstPre, DOFs, jointPositions, jointAxeses, [softConstPoint], constJointMasks) footCenter = footCenterL + (footCenterR - footCenterL) / 2.0 footCenter[1] = 0. preFootCenter = footCenter.copy() # footToBodyFootRotL = np.dot(np.transpose(footOriL), footBodyOriL) # footToBodyFootRotR = np.dot(np.transpose(footOriR), footBodyOriR) if refFootR[1] < doubleTosingleOffset: contact += 1 if refFootL[1] < doubleTosingleOffset: contact += 2 g_initFlag = 1 # calculate footCenter footCenter = footCenterL + (footCenterR - footCenterL) / 2.0 # if refFootR[1] >doubleTosingleOffset: # if refFootR[1] > doubleTosingleOffset or footCenterR[1] > 0.08: # if contact == 1 or footCenterR[1] > 0.08: # if contact == 2 or footCenterR[1] > doubleTosingleOffset/2: if contact == 2: footCenter = footCenterL.copy() # elif contact == 1 or footCenterL[1] > doubleTosingleOffset/2: if contact == 1: footCenter = footCenterR.copy() footCenter[1] = 0. if contactChangeCount > 0 and contactChangeType == 'StoD': # change footcenter gradually footCenter = preFootCenter + ( maxContactChangeCount - contactChangeCount) * ( footCenter - preFootCenter) / maxContactChangeCount preFootCenter = footCenter.copy() # linear momentum # TODO: # We should consider dCM_ref, shouldn't we? # add getBodyPositionGlobal and getBodyPositionsGlobal in csVpModel! # todo that, set joint velocities to vpModel CM_ref_plane = footCenter dL_des_plane = Kl * totalMass * (CM_ref_plane - CM_plane) - Dl * totalMass * dCM_plane # dL_des_plane[1] = 0. # angular momentum CP_ref = footCenter bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds) # bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds) CP = yrp.getCP(contactPositions, contactForces) if CP_old[0] is None or CP is None: dCP = None else: dCP = (CP - CP_old[0]) / (1 / 30.) CP_old[0] = CP if CP is not None and dCP is not None: ddCP_des = Kh * (CP_ref - CP) - Dh * (dCP) CP_des = CP + dCP * (1 / 30.) + .5 * ddCP_des * ((1 / 30.)**2) dH_des = np.cross( (CP_des - CM), (dL_des_plane + totalMass * mm.s2v(wcfg.gravity))) if contactChangeCount > 0: # and contactChangeType == 'DtoS': # dH_des *= (maxContactChangeCount - contactChangeCount)/(maxContactChangeCount*10) dH_des *= (maxContactChangeCount - contactChangeCount) / (maxContactChangeCount) # dH_des *= (contactChangeCount)/(maxContactChangeCount)*.9+.1 else: dH_des = None # H = np.dot(P, np.dot(Jsys, dth_flat)) # dH_des = -Kh* H[3:] # soft point constraint #softConstPoint = refFootR.copy() ##softConstPoint[0] += 0.2 #Ksc = 50 #Dsc = 2*(Ksc**.5) #Bsc = 1. #P_des = softConstPoint #P_cur = controlModel.getBodyPositionGlobal(constBody) #dP_des = [0, 0, 0] #dP_cur = controlModel.getBodyVelocityGlobal(constBody) #ddP_des1 = Ksc*(P_des - P_cur) + Dsc*(dP_des - dP_cur) #r = P_des - P_cur #I = np.vstack(([1,0,0],[0,1,0],[0,0,1])) #Z = np.hstack((I, mm.getCrossMatrixForm(-r))) #yjc.computeJacobian2(Jconst, DOFs, jointPositions, jointAxeses, [softConstPoint], constJointMasks) #dJconst = (Jconst - Jconst)/(1/30.) #JconstPre = Jconst.copy() ##yjc.computeJacobianDerivative2(dJconst, DOFs, jointPositions, jointAxeses, linkAngVelocities, [softConstPoint], constJointMasks, False) #JL, JA = np.vsplit(Jconst, 2) #Q1 = np.dot(Z, Jconst) #q1 = np.dot(JA, dth_flat) #q2 = np.dot(mm.getCrossMatrixForm(q1), np.dot(mm.getCrossMatrixForm(q1), r)) #q_bias1 = np.dot(np.dot(Z, dJconst), dth_flat) + q2 #set up equality constraint a_oriL = mm.logSO3( mm.getSO3FromVectors(np.dot(footBodyOriL, np.array([0, 1, 0])), np.array([0, 1, 0]))) a_oriR = mm.logSO3( mm.getSO3FromVectors(np.dot(footBodyOriR, np.array([0, 1, 0])), np.array([0, 1, 0]))) #if contact == 3 and contactChangeCount < maxContactChangeCount/4 and contactChangeCount >=1: #kt_sup = 30 #viewer.objectInfoWnd.labelSupKt.value(kt_sup) #viewer.objectInfoWnd.sliderSupKt.value(initSupKt*10) # a_supL = np.append(kt_sup*(refFootL - footCenterL + contMotionOffset) + dt_sup*(refFootVelL - footBodyVelL), kt_sup*a_oriL+dt_sup*(refFootAngVelL-footBodyAngVelL)) # a_supR = np.append(kt_sup*(refFootR - footCenterR + contMotionOffset) + dt_sup*(refFootVelR - footBodyVelR), kt_sup*a_oriR+dt_sup*(refFootAngVelR-footBodyAngVelR)) a_supL = np.append( kt_sup * (refFootL - footCenterL + contMotionOffset) - dt_sup * footBodyVelL, kt_sup * a_oriL - dt_sup * footBodyAngVelL) a_supR = np.append( kt_sup * (refFootR - footCenterR + contMotionOffset) - dt_sup * footBodyVelR, kt_sup * a_oriR - dt_sup * footBodyAngVelR) if contactChangeCount > 0 and contactChangeType == 'DtoS': #refFootR += (footCenter-CM_plane)/2. #refFootR[1] = 0 #pre contact value are needed #if contact == 2: ##refFootR[0] += 0.2 ##refFootR[2] -= 0.05 #offsetDropR = (footCenter-CM_plane)/2. #refFootR += offsetDropR #refFootR[1] = 0. ##refFootR[2] = footCenterR[2] - contMotionOffset[2] ##refFootR[0] = footCenterR[0] - contMotionOffset[0] #refFootL[0] += 0.05 #refFootL[2] -= 0.05 #elif contact == 1: #offsetDropL = (footCenter-CM_plane)/2. #refFootL += offsetDropL #refFootL[1] = 0. #a_supL = np.append(kt_sup*(refFootL - footCenterL + contMotionOffset) + dt_sup*(refFootVelL - footBodyVelL), kt_sup*a_oriL+dt_sup*(refFootAngVelL-footBodyAngVelL)) #a_supR = np.append(kt_sup*(refFootR - footCenterR + contMotionOffset) + dt_sup*(refFootVelR - footBodyVelR), kt_sup*a_oriR+dt_sup*(refFootAngVelR-footBodyAngVelR)) #a_supL = np.append(kt_sup*(refFootL - footCenterL + contMotionOffset) + dt_sup*(refFootVelL - footBodyVelL), 16*kt_sup*a_oriL+4*dt_sup*(refFootAngVelL-footBodyAngVelL)) #a_supR = np.append(kt_sup*(refFootR - footCenterR + contMotionOffset) + dt_sup*(refFootVelR - footBodyVelR), 16*kt_sup*a_oriR+4*dt_sup*(refFootAngVelR-footBodyAngVelR)) a_supL = np.append( kt_sup * (refFootL - footCenterL + contMotionOffset) + dt_sup * (refFootVelL - footBodyVelL), 4 * kt_sup * a_oriL + 2 * dt_sup * (refFootAngVelL - footBodyAngVelL)) a_supR = np.append( kt_sup * (refFootR - footCenterR + contMotionOffset) + dt_sup * (refFootVelR - footBodyVelR), 4 * kt_sup * a_oriR + 2 * dt_sup * (refFootAngVelR - footBodyAngVelR)) elif contactChangeCount > 0 and contactChangeType == 'StoD': #refFootR[0] +=0.05 #refFootR[2] +=0.05 linkt = (13. * contactChangeCount) / (maxContactChangeCount) + 1. lindt = 2 * (linkt**.5) angkt = (13. * contactChangeCount) / (maxContactChangeCount) + 1. angdt = 2 * (angkt**.5) #a_supL = np.append(4*kt_sup*(refFootL - footCenterL + contMotionOffset) + 2*dt_sup*(refFootVelL - footBodyVelL), 16*kt_sup*a_oriL+4*dt_sup*(refFootAngVelL-footBodyAngVelL)) #a_supR = np.append(4*kt_sup*(refFootR - footCenterR + contMotionOffset) + 2*dt_sup*(refFootVelR - footBodyVelR), 16*kt_sup*a_oriR+4*dt_sup*(refFootAngVelR-footBodyAngVelR)) a_supL = np.append( linkt * kt_sup * (refFootL - footCenterL + contMotionOffset) + lindt * dt_sup * (refFootVelL - footBodyVelL), angkt * kt_sup * a_oriL + angdt * dt_sup * (refFootAngVelL - footBodyAngVelL)) a_supR = np.append( linkt * kt_sup * (refFootR - footCenterR + contMotionOffset) + lindt * dt_sup * (refFootVelR - footBodyVelR), angkt * kt_sup * a_oriR + angdt * dt_sup * (refFootAngVelR - footBodyAngVelR)) #a_supL = np.append(16*kt_sup*(refFootL - footCenterL + contMotionOffset) + 4*dt_sup*(refFootVelL - footBodyVelL), 16*kt_sup*a_oriL+4*dt_sup*(refFootAngVelL-footBodyAngVelL)) #a_supR = np.append(16*kt_sup*(refFootR - footCenterR + contMotionOffset) + 4*dt_sup*(refFootVelR - footBodyVelR), 16*kt_sup*a_oriR+4*dt_sup*(refFootAngVelR-footBodyAngVelR)) #a_supL = np.append(4*kt_sup*(refFootL - footCenterL + contMotionOffset) + 2*dt_sup*(refFootVelL - footBodyVelL), 32*kt_sup*a_oriL+5.6*dt_sup*(refFootAngVelL-footBodyAngVelL)) #a_supR = np.append(4*kt_sup*(refFootR - footCenterR + contMotionOffset) + 2*dt_sup*(refFootVelR - footBodyVelR), 32*kt_sup*a_oriR+5.6*dt_sup*(refFootAngVelR-footBodyAngVelR)) #a_supL[1] = kt_sup*(refFootL[1] - footCenterL[1] + contMotionOffset[1]) + dt_sup*(refFootVelL[1] - footBodyVelL[1]) #a_supR[1] = kt_sup*(refFootR[1] - footCenterR[1] + contMotionOffset[1]) + dt_sup*(refFootVelR[1] - footBodyVelR[1]) ##if contact == 2: #if refFootR[1] <doubleTosingleOffset : #Jsup = np.vstack((JsupL, JsupR)) #dJsup = np.vstack((dJsupL, dJsupR)) #a_sup = np.append(a_supL, a_supR) #else: #Jsup = JsupL.copy() #dJsup = dJsupL.copy() #a_sup = a_supL.copy() # momentum matrix RS = np.dot(P, Jsys) R, S = np.vsplit(RS, 2) # rs = np.dot((np.dot(dP, Jsys) + np.dot(P, dJsys)), dth_flat) rs = np.dot(dP, np.dot(Jsys, dth_flat)) + np.dot(P, dJsys) r_bias, s_bias = np.hsplit(rs, 2) ####################################################### # optimization ####################################################### #if contact == 2 and footCenterR[1] > doubleTosingleOffset/2: if contact == 2: config['weightMap']['RightUpLeg'] = .8 config['weightMap']['RightLeg'] = .8 config['weightMap']['RightFoot'] = .8 else: config['weightMap']['RightUpLeg'] = .1 config['weightMap']['RightLeg'] = .25 config['weightMap']['RightFoot'] = .2 #if contact == 1 and footCenterL[1] > doubleTosingleOffset/2: if contact == 1: config['weightMap']['LeftUpLeg'] = .8 config['weightMap']['LeftLeg'] = .8 config['weightMap']['LeftFoot'] = .8 else: config['weightMap']['LeftUpLeg'] = .1 config['weightMap']['LeftLeg'] = .25 config['weightMap']['LeftFoot'] = .2 w = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap']) #if contact == 2: #mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1, q_bias1) mot.addTrackingTerms(problem, totalDOF, Bt, w, ddth_des_flat) if dH_des is not None: mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias) mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias) #if contact & 1 and contactChangeCount == 0: if contact & 1: #if refFootR[1] < doubleTosingleOffset: mot.addConstraint2(problem, totalDOF, JsupR, dJsupR, dth_flat, a_supR) if contact & 2: #if refFootL[1] < doubleTosingleOffset: mot.addConstraint2(problem, totalDOF, JsupL, dJsupL, dth_flat, a_supL) if contactChangeCount > 0: contactChangeCount -= 1 if contactChangeCount == 0: maxContactChangeCount = 30 contactChangeType = 0 r = problem.solve() problem.clear() ype.nested(r['x'], ddth_sol) rootPos[0] = controlModel.getBodyPositionGlobal(selectedBody) localPos = [[0, 0, 0]] for i in range(stepsPerFrame): # apply penalty force bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds) # print(contactForces) #bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds) vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces) controlModel.setDOFAccelerations(ddth_sol) controlModel.solveHybridDynamics() if forceShowTime > viewer.objectInfoWnd.labelForceDur.value(): forceShowTime = 0 viewer_ResetForceState() forceforce = np.array([ viewer.objectInfoWnd.labelForceX.value(), viewer.objectInfoWnd.labelForceY.value(), viewer.objectInfoWnd.labelForceZ.value() ]) extraForce[0] = getParamVal('Fm') * mm.normalize2(forceforce) # extraForce[0] = viewer.objectInfoWnd.labelFm.value() * mm.normalize2(forceforce) if viewer_GetForceState(): forceShowTime += wcfg.timeStep vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce) vpWorld.step() # rendering rightFootVectorX[0] = np.dot(footOriL, np.array([.1, 0, 0])) rightFootVectorY[0] = np.dot(footOriL, np.array([0, .1, 0])) rightFootVectorZ[0] = np.dot(footOriL, np.array([0, 0, .1])) rightFootPos[0] = footCenterL rightVectorX[0] = np.dot(footBodyOriL, np.array([.1, 0, 0])) rightVectorY[0] = np.dot(footBodyOriL, np.array([0, .1, 0])) rightVectorZ[0] = np.dot(footBodyOriL, np.array([0, 0, .1])) rightPos[0] = footCenterL + np.array([.1, 0, 0]) rd_footCenter[0] = footCenter rd_footCenterL[0] = footCenterL rd_footCenterR[0] = footCenterR rd_CM[0] = CM rd_CM_plane[0] = CM.copy() rd_CM_plane[0][1] = 0. if CP is not None and dCP is not None: rd_CP[0] = CP rd_CP_des[0] = CP_des rd_dL_des_plane[0] = [ dL_des_plane[0] / 100, dL_des_plane[1] / 100, dL_des_plane[2] / 100 ] rd_dH_des[0] = dH_des rd_grf_des[0] = dL_des_plane - totalMass * mm.s2v(wcfg.gravity) rd_root_des[0] = rootPos[0] del rd_CF[:] del rd_CF_pos[:] for i in range(len(contactPositions)): rd_CF.append(contactForces[i] / 400) rd_CF_pos.append(contactPositions[i].copy()) if viewer_GetForceState(): rd_exfen_des[0] = [ extraForce[0][0] / 100, extraForce[0][1] / 100, extraForce[0][2] / 100 ] rd_exf_des[0] = [0, 0, 0] else: rd_exf_des[0] = [ extraForce[0][0] / 100, extraForce[0][1] / 100, extraForce[0][2] / 100 ] rd_exfen_des[0] = [0, 0, 0] extraForcePos[0] = controlModel.getBodyPositionGlobal(selectedBody)
def simulateCallback(frame): global g_initFlag global forceShowTime global preFootCenter global maxContactChangeCount global contactChangeCount global contact global contactChangeType Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals( ['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt']) Dt = 2. * (Kt**.5) Dl = 2. * (Kl**.5) Dh = 2. * (Kh**.5) dt_sup = 2. * (kt_sup**.5) # Dt = .2*(Kt**.5) # Dl = .2*(Kl**.5) # Dh = .2*(Kh**.5) # dt_sup = .2*(kt_sup**.5) pdcontroller.setKpKd(Kt, Dt) doubleTosingleOffset = 0.15 singleTodoubleOffset = 0.30 #doubleTosingleOffset = 0.09 doubleTosingleVelOffset = 0.0 # tracking # th_r = motion.getDOFPositions(frame) # th = dartModel.getDOFPositions() # dth_r = motion.getDOFVelocities(frame) # dth = dartModel.getDOFVelocities() # ddth_r = motion.getDOFAccelerations(frame) # ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt) dth_flat = dartModel.get_dq() # ype.flatten(ddth_des, ddth_des_flat) # ype.flatten(dth, dth_flat) ################################################# # jacobian ################################################# #caution!! body orientation and joint orientation of foot are totally different!! footOriL = dartModel.getJointOrientationGlobal(supL) footOriR = dartModel.getJointOrientationGlobal(supR) #desire footCenter[1] = 0.041135 #desire footCenter[1] = 0.0197 footCenterL = dartModel.getBodyPositionGlobal(supL) footCenterR = dartModel.getBodyPositionGlobal(supR) footBodyOriL = dartModel.getBodyOrientationGlobal(supL) footBodyOriR = dartModel.getBodyOrientationGlobal(supR) footBodyVelL = dartModel.getBodyVelocityGlobal(supL) footBodyVelR = dartModel.getBodyVelocityGlobal(supR) footBodyAngVelL = dartModel.getBodyAngVelocityGlobal(supL) footBodyAngVelR = dartModel.getBodyAngVelocityGlobal(supR) refFootL = dartMotionModel.getBodyPositionGlobal(supL) refFootR = dartMotionModel.getBodyPositionGlobal(supR) refFootAngVelL = motion.getJointAngVelocityGlobal(supL, frame) refFootAngVelR = motion.getJointAngVelocityGlobal(supR, frame) refFootJointVelR = motion.getJointVelocityGlobal(supR, frame) refFootJointAngVelR = motion.getJointAngVelocityGlobal(supR, frame) refFootJointR = motion.getJointPositionGlobal(supR, frame) refFootVelR = refFootJointVelR + np.cross(refFootJointAngVelR, (refFootR - refFootJointR)) refFootJointVelL = motion.getJointVelocityGlobal(supL, frame) refFootJointAngVelL = motion.getJointAngVelocityGlobal(supL, frame) refFootJointL = motion.getJointPositionGlobal(supL, frame) refFootVelL = refFootJointVelL + np.cross(refFootJointAngVelL, (refFootL - refFootJointL)) contactR = 1 contactL = 1 if refFootVelR[1] < 0 and refFootVelR[1] * frame_step_size + refFootR[ 1] > singleTodoubleOffset: contactR = 0 if refFootVelL[1] < 0 and refFootVelL[1] * frame_step_size + refFootL[ 1] > singleTodoubleOffset: contactL = 0 if refFootVelR[1] > 0 and refFootVelR[1] * frame_step_size + refFootR[ 1] > doubleTosingleOffset: contactR = 0 if refFootVelL[1] > 0 and refFootVelL[1] * frame_step_size + refFootL[ 1] > doubleTosingleOffset: contactL = 0 # contactR = 0 # contMotionOffset = th[0][0] - th_r[0][0] contMotionOffset = dartModel.getBodyPositionGlobal( 0) - dartMotionModel.getBodyPositionGlobal(0) linkPositions = dartModel.getBodyPositionsGlobal() linkVelocities = dartModel.getBodyVelocitiesGlobal() linkAngVelocities = dartModel.getBodyAngVelocitiesGlobal() linkInertias = dartModel.getBodyInertiasGlobal() CM = dartModel.skeleton.com() dCM = dartModel.skeleton.com_velocity() CM_plane = copy.copy(CM) CM_plane[1] = 0. dCM_plane = copy.copy(dCM) dCM_plane[1] = 0. P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM, linkInertias) dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses, linkVelocities, dCM, linkAngVelocities, linkInertias) #calculate contact state #if g_initFlag == 1 and contact == 1 and refFootR[1] < doubleTosingleOffset and footCenterR[1] < 0.08: if g_initFlag == 1: #contact state # 0: flying 1: right only 2: left only 3: double #if contact == 2 and refFootR[1] < doubleTosingleOffset: if contact == 2 and contactR == 1: contact = 3 maxContactChangeCount += 30 contactChangeCount += maxContactChangeCount contactChangeType = 'StoD' #elif contact == 3 and refFootL[1] < doubleTosingleOffset: elif contact == 1 and contactL == 1: contact = 3 maxContactChangeCount += 30 contactChangeCount += maxContactChangeCount contactChangeType = 'StoD' #elif contact == 3 and refFootR[1] > doubleTosingleOffset: elif contact == 3 and contactR == 0: contact = 2 contactChangeCount += maxContactChangeCount contactChangeType = 'DtoS' #elif contact == 3 and refFootL[1] > doubleTosingleOffset: elif contact == 3 and contactL == 0: contact = 1 contactChangeCount += maxContactChangeCount contactChangeType = 'DtoS' else: contact = 0 #if refFootR[1] < doubleTosingleOffset: if contactR == 1: contact += 1 #if refFootL[1] < doubleTosingleOffset: if contactL == 1: contact += 2 #initialization if g_initFlag == 0: softConstPoint = footCenterR.copy() footCenter = footCenterL + (footCenterR - footCenterL) / 2.0 footCenter[1] = 0. preFootCenter = footCenter.copy() #footToBodyFootRotL = np.dot(np.transpose(footOriL), footBodyOriL) #footToBodyFootRotR = np.dot(np.transpose(footOriR), footBodyOriR) if refFootR[1] < doubleTosingleOffset: contact += 1 if refFootL[1] < doubleTosingleOffset: contact += 2 g_initFlag = 1 #calculate jacobian body_num = dartModel.getBodyNum() Jsys = np.zeros((6 * body_num, totalDOF)) dJsys = np.zeros((6 * body_num, totalDOF)) for i in range(dartModel.getBodyNum()): body_i_jacobian = dartModel.getBody(i).world_jacobian()[ range(-3, 3), :] body_i_jacobian_deriv = dartModel.getBody( i).world_jacobian_classic_deriv()[range(-3, 3), :] Jsys[6 * i:6 * i + 6, :] = body_i_jacobian dJsys[6 * i:6 * i + 6, :] = body_i_jacobian_deriv JsupL = dartModel.getBody(supL).world_jacobian()[range(-3, 3), :] dJsupL = dartModel.getBody(supL).world_jacobian_classic_deriv()[ range(-3, 3), :] JsupR = dartModel.getBody(supR).world_jacobian()[range(-3, 3), :] dJsupR = dartModel.getBody(supR).world_jacobian_classic_deriv()[ range(-3, 3), :] dartMotionModel.update(motion[frame]) # ddth_des_flat = pdcontroller.compute(dartMotionModel.get_q()) ddth_des_flat = pdcontroller.compute(motion.getDOFPositions(frame)) #calculate footCenter footCenter = .5 * (footCenterL + footCenterR) #if refFootR[1] >doubleTosingleOffset: #if refFootR[1] > doubleTosingleOffset or footCenterR[1] > 0.08: #if contact == 1 or footCenterR[1] > 0.08: #if contact == 2 or footCenterR[1] > doubleTosingleOffset/2: if contact == 2: footCenter = footCenterL.copy() #elif contact == 1 or footCenterL[1] > doubleTosingleOffset/2: if contact == 1: footCenter = footCenterR.copy() footCenter[1] = 0. if contactChangeCount > 0 and contactChangeType == 'StoD': #change footcenter gradually footCenter = preFootCenter + ( maxContactChangeCount - contactChangeCount) * ( footCenter - preFootCenter) / maxContactChangeCount preFootCenter = footCenter.copy() # foot adjustment foot_angle_weight = 1. foot_dCM_weight = 5. foot_center_diff = CM_plane + dCM_plane * frame_step_size * foot_dCM_weight - footCenter foot_center_diff_norm = np.linalg.norm(foot_center_diff) foot_left_height = dartModel.getJointPositionGlobal(foot_left_idx)[1] foot_right_height = dartModel.getJointPositionGlobal(foot_left_idx)[1] foot_left_angle = foot_angle_weight * math.atan2( foot_center_diff_norm, foot_left_height) foot_right_angle = foot_angle_weight * math.atan2( foot_center_diff_norm, foot_right_height) foot_axis = np.cross(np.array((0., 1., 0.)), foot_center_diff) foot_left_R = mm.exp(foot_axis, foot_left_angle) foot_right_R = mm.exp(foot_axis, foot_right_angle) # motion[frame].mulJointOrientationGlobal(foot_left_idx, foot_left_R) # motion[frame].mulJointOrientationGlobal(foot_right_idx, foot_right_R) # hfi.footAdjust(motion[frame], footIdDic, SEGMENT_FOOT_MAG, SEGMENT_FOOT_RAD, 0.) # linear momentum #TODO: # We should consider dCM_ref, shouldn't we? # add getBodyPositionGlobal and getBodyPositionsGlobal in csVpModel! # to do that, set joint velocities to vpModel CM_ref_plane = footCenter dL_des_plane = Kl * totalMass * (CM_ref_plane - CM_plane) - Dl * totalMass * dCM_plane dL_des_plane[1] = 0. # angular momentum CP_ref = footCenter bodyIDs, contactPositions, contactPositionLocals, contactForces = [], [], [], [] if DART_CONTACT_ON: bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.get_dart_contact_info( ) else: bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds) #bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds) CP = yrp.getCP(contactPositions, contactForces) if CP_old[0] is None or CP is None: dCP = None else: dCP = (CP - CP_old[0]) / frame_step_size CP_old[0] = CP # CP_des = None if CP_des[0] is None: CP_des[0] = footCenter if CP is not None and dCP is not None: ddCP_des = Kh * (CP_ref - CP) - Dh * dCP CP_des[0] = CP + dCP * frame_step_size + .5 * ddCP_des * ( frame_step_size**2) # dCP_des[0] += ddCP_des * frame_step_size # CP_des[0] += dCP_des[0] * frame_step_size + .5 * ddCP_des*(frame_step_size ** 2) dH_des = np.cross( CP_des[0] - CM, (dL_des_plane + totalMass * mm.s2v(wcfg.gravity))) if contactChangeCount > 0: # and contactChangeType == 'DtoS': #dH_des *= (maxContactChangeCount - contactChangeCount)/(maxContactChangeCount*10) dH_des *= (maxContactChangeCount - contactChangeCount) / maxContactChangeCount #dH_des *= (contactChangeCount)/(maxContactChangeCount)*.9+.1 else: dH_des = None # H = np.dot(P, np.dot(Jsys, dth_flat)) # dH_des = -Kh* H[3:] # soft point constraint #softConstPoint = refFootR.copy() ##softConstPoint[0] += 0.2 #Ksc = 50 #Dsc = 2*(Ksc**.5) #Bsc = 1. #P_des = softConstPoint #P_cur = controlModel.getBodyPositionGlobal(constBody) #dP_des = [0, 0, 0] #dP_cur = controlModel.getBodyVelocityGlobal(constBody) #ddP_des1 = Ksc*(P_des - P_cur) + Dsc*(dP_des - dP_cur) #r = P_des - P_cur #I = np.vstack(([1,0,0],[0,1,0],[0,0,1])) #Z = np.hstack((I, mm.getCrossMatrixForm(-r))) #yjc.computeJacobian2(Jconst, DOFs, jointPositions, jointAxeses, [softConstPoint], constJointMasks) #dJconst = (Jconst - Jconst)/(1/30.) #JconstPre = Jconst.copy() ##yjc.computeJacobianDerivative2(dJconst, DOFs, jointPositions, jointAxeses, linkAngVelocities, [softConstPoint], constJointMasks, False) #JL, JA = np.vsplit(Jconst, 2) #Q1 = np.dot(Z, Jconst) #q1 = np.dot(JA, dth_flat) #q2 = np.dot(mm.getCrossMatrixForm(q1), np.dot(mm.getCrossMatrixForm(q1), r)) #q_bias1 = np.dot(np.dot(Z, dJconst), dth_flat) + q2 #set up equality constraint # a_oriL = mm.logSO3(mm.getSO3FromVectors(np.dot(footBodyOriL, np.array([0,1,0])), np.array([0,1,0]))) # a_oriR = mm.logSO3(mm.getSO3FromVectors(np.dot(footBodyOriR, np.array([0,1,0])), np.array([0,1,0]))) left_foot_up_vec, right_foot_up_vec = hfi.get_foot_up_vector( motion[frame], footIdDic, None) a_oriL = mm.logSO3( mm.getSO3FromVectors(left_foot_up_vec, np.array([0, 1, 0]))) a_oriR = mm.logSO3( mm.getSO3FromVectors(right_foot_up_vec, np.array([0, 1, 0]))) #if contact == 3 and contactChangeCount < maxContactChangeCount/4 and contactChangeCount >=1: #kt_sup = 30 #viewer.objectInfoWnd.labelSupKt.value(kt_sup) #viewer.objectInfoWnd.sliderSupKt.value(initSupKt*10) # a_supL = np.append(kt_sup*(refFootL - footCenterL + contMotionOffset) + dt_sup*(refFootVelL - footBodyVelL), kt_sup*a_oriL+dt_sup*(refFootAngVelL-footBodyAngVelL)) # a_supR = np.append(kt_sup*(refFootR - footCenterR + contMotionOffset) + dt_sup*(refFootVelR - footBodyVelR), kt_sup*a_oriR+dt_sup*(refFootAngVelR-footBodyAngVelR)) a_supL = np.append( kt_sup * (refFootL - footCenterL + contMotionOffset) + dt_sup * (refFootVelL - footBodyVelL), kt_sup * a_oriL + dt_sup * (refFootAngVelL - footBodyAngVelL)) a_supR = np.append( kt_sup * (refFootR - footCenterR + contMotionOffset) + dt_sup * (refFootVelR - footBodyVelR), kt_sup * a_oriR + dt_sup * (refFootAngVelR - footBodyAngVelR)) # a_supL[3:] = 0. # a_supR[3:] = 0. if contactChangeCount > 0 and contactChangeType == 'DtoS': #refFootR += (footCenter-CM_plane)/2. #refFootR[1] = 0 #pre contact value are needed #if contact == 2: ##refFootR[0] += 0.2 ##refFootR[2] -= 0.05 #offsetDropR = (footCenter-CM_plane)/2. #refFootR += offsetDropR #refFootR[1] = 0. ##refFootR[2] = footCenterR[2] - contMotionOffset[2] ##refFootR[0] = footCenterR[0] - contMotionOffset[0] #refFootL[0] += 0.05 #refFootL[2] -= 0.05 #elif contact == 1: #offsetDropL = (footCenter-CM_plane)/2. #refFootL += offsetDropL #refFootL[1] = 0. #a_supL = np.append(kt_sup*(refFootL - footCenterL + contMotionOffset) + dt_sup*(refFootVelL - footBodyVelL), kt_sup*a_oriL+dt_sup*(refFootAngVelL-footBodyAngVelL)) #a_supR = np.append(kt_sup*(refFootR - footCenterR + contMotionOffset) + dt_sup*(refFootVelR - footBodyVelR), kt_sup*a_oriR+dt_sup*(refFootAngVelR-footBodyAngVelR)) #a_supL = np.append(kt_sup*(refFootL - footCenterL + contMotionOffset) + dt_sup*(refFootVelL - footBodyVelL), 16*kt_sup*a_oriL+4*dt_sup*(refFootAngVelL-footBodyAngVelL)) #a_supR = np.append(kt_sup*(refFootR - footCenterR + contMotionOffset) + dt_sup*(refFootVelR - footBodyVelR), 16*kt_sup*a_oriR+4*dt_sup*(refFootAngVelR-footBodyAngVelR)) a_supL = np.append( kt_sup * (refFootL - footCenterL + contMotionOffset) + dt_sup * (refFootVelL - footBodyVelL), 4 * kt_sup * a_oriL + 2 * dt_sup * (refFootAngVelL - footBodyAngVelL)) a_supR = np.append( kt_sup * (refFootR - footCenterR + contMotionOffset) + dt_sup * (refFootVelR - footBodyVelR), 4 * kt_sup * a_oriR + 2 * dt_sup * (refFootAngVelR - footBodyAngVelR)) elif contactChangeCount > 0 and contactChangeType == 'StoD': #refFootR[0] +=0.05 #refFootR[2] +=0.05 linkt = (13. * contactChangeCount) / maxContactChangeCount + 1. lindt = 2 * (linkt**.5) angkt = (13. * contactChangeCount) / maxContactChangeCount + 1. angdt = 2 * (angkt**.5) #a_supL = np.append(4*kt_sup*(refFootL - footCenterL + contMotionOffset) + 2*dt_sup*(refFootVelL - footBodyVelL), 16*kt_sup*a_oriL+4*dt_sup*(refFootAngVelL-footBodyAngVelL)) #a_supR = np.append(4*kt_sup*(refFootR - footCenterR + contMotionOffset) + 2*dt_sup*(refFootVelR - footBodyVelR), 16*kt_sup*a_oriR+4*dt_sup*(refFootAngVelR-footBodyAngVelR)) a_supL = np.append( linkt * kt_sup * (refFootL - footCenterL + contMotionOffset) + lindt * dt_sup * (refFootVelL - footBodyVelL), angkt * kt_sup * a_oriL + angdt * dt_sup * (refFootAngVelL - footBodyAngVelL)) a_supR = np.append( linkt * kt_sup * (refFootR - footCenterR + contMotionOffset) + lindt * dt_sup * (refFootVelR - footBodyVelR), angkt * kt_sup * a_oriR + angdt * dt_sup * (refFootAngVelR - footBodyAngVelR)) #a_supL = np.append(16*kt_sup*(refFootL - footCenterL + contMotionOffset) + 4*dt_sup*(refFootVelL - footBodyVelL), 16*kt_sup*a_oriL+4*dt_sup*(refFootAngVelL-footBodyAngVelL)) #a_supR = np.append(16*kt_sup*(refFootR - footCenterR + contMotionOffset) + 4*dt_sup*(refFootVelR - footBodyVelR), 16*kt_sup*a_oriR+4*dt_sup*(refFootAngVelR-footBodyAngVelR)) #a_supL = np.append(4*kt_sup*(refFootL - footCenterL + contMotionOffset) + 2*dt_sup*(refFootVelL - footBodyVelL), 32*kt_sup*a_oriL+5.6*dt_sup*(refFootAngVelL-footBodyAngVelL)) #a_supR = np.append(4*kt_sup*(refFootR - footCenterR + contMotionOffset) + 2*dt_sup*(refFootVelR - footBodyVelR), 32*kt_sup*a_oriR+5.6*dt_sup*(refFootAngVelR-footBodyAngVelR)) #a_supL[1] = kt_sup*(refFootL[1] - footCenterL[1] + contMotionOffset[1]) + dt_sup*(refFootVelL[1] - footBodyVelL[1]) #a_supR[1] = kt_sup*(refFootR[1] - footCenterR[1] + contMotionOffset[1]) + dt_sup*(refFootVelR[1] - footBodyVelR[1]) ##if contact == 2: #if refFootR[1] <doubleTosingleOffset : #Jsup = np.vstack((JsupL, JsupR)) #dJsup = np.vstack((dJsupL, dJsupR)) #a_sup = np.append(a_supL, a_supR) #else: #Jsup = JsupL.copy() #dJsup = dJsupL.copy() #a_sup = a_supL.copy() # momentum matrix RS = np.dot(P, Jsys) R, S = np.vsplit(RS, 2) rs = np.dot((np.dot(dP, Jsys) + np.dot(P, dJsys)), dth_flat) r_bias, s_bias = np.hsplit(rs, 2) ####################################################### # optimization ####################################################### #if contact == 2 and footCenterR[1] > doubleTosingleOffset/2: if contact == 2: config['weightMap']['RightUpLeg'] = .8 config['weightMap']['RightLeg'] = .8 config['weightMap']['RightFoot'] = .8 else: config['weightMap']['RightUpLeg'] = .1 config['weightMap']['RightLeg'] = .25 config['weightMap']['RightFoot'] = .2 #if contact == 1 and footCenterL[1] > doubleTosingleOffset/2: if contact == 1: config['weightMap']['LeftUpLeg'] = .8 config['weightMap']['LeftLeg'] = .8 config['weightMap']['LeftFoot'] = .8 else: config['weightMap']['LeftUpLeg'] = .1 config['weightMap']['LeftLeg'] = .25 config['weightMap']['LeftFoot'] = .2 w = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap']) #if contact == 2: #mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1, q_bias1) mot.addTrackingTerms(problem, totalDOF, Bt, w, ddth_des_flat) if dH_des is not None: mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias) mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias) #mot.setConstraint(problem, totalDOF, Jsup, dJsup, dth_flat, a_sup) #mot.addConstraint(problem, totalDOF, Jsup, dJsup, dth_flat, a_sup) #if contact & 1 and contactChangeCount == 0: if contact & 1: #if refFootR[1] < doubleTosingleOffset: mot.addConstraint(problem, totalDOF, JsupR, dJsupR, dth_flat, a_supR) if contact & 2: #if refFootL[1] < doubleTosingleOffset: mot.addConstraint(problem, totalDOF, JsupL, dJsupL, dth_flat, a_supL) if contactChangeCount > 0: contactChangeCount -= 1 if contactChangeCount == 0: maxContactChangeCount = 30 contactChangeType = 0 r = problem.solve() problem.clear() # ype.nested(r['x'], ddth_sol) ddth_sol = np.asarray(r['x']) # remove foot seg effect ddth_sol[foot_dofs] = ddth_des_flat[foot_dofs] # ddth_sol[:] = ddth_des_flat[:] rootPos[0] = dartModel.getBodyPositionGlobal(selectedBody) localPos = [[0, 0, 0]] for i in range(stepsPerFrame): # apply penalty force if not DART_CONTACT_ON: bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds) dartModel.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces) #bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds) dartModel.skeleton.set_accelerations(ddth_sol) if forceShowTime > viewer.objectInfoWnd.labelForceDur.value(): forceShowTime = 0 viewer_ResetForceState() forceforce = np.array([ viewer.objectInfoWnd.labelForceX.value(), viewer.objectInfoWnd.labelForceY.value(), viewer.objectInfoWnd.labelForceZ.value() ]) extraForce[0] = getParamVal('Fm') * mm.normalize2(forceforce) if viewer_GetForceState(): forceShowTime += wcfg.timeStep dartModel.applyPenaltyForce(selectedBodyId, localPos, extraForce) dartModel.step() if DART_CONTACT_ON: bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.get_dart_contact_info( ) else: bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds) # rendering rightFootVectorX[0] = np.dot(footOriL, np.array([.1, 0, 0])) rightFootVectorY[0] = np.dot(footOriL, np.array([0, .1, 0])) rightFootVectorZ[0] = np.dot(footOriL, np.array([0, 0, .1])) rightFootPos[0] = footCenterL rightVectorX[0] = np.dot(footBodyOriL, np.array([.1, 0, 0])) rightVectorY[0] = np.dot(footBodyOriL, np.array([0, .1, 0])) rightVectorZ[0] = np.dot(footBodyOriL, np.array([0, 0, .1])) rightPos[0] = footCenterL + np.array([.1, 0, 0]) rd_footCenter[0] = footCenter rd_footCenterL[0] = footCenterL rd_footCenterR[0] = footCenterR rd_CM[0] = CM rd_CM_plane[0] = CM.copy() rd_CM_plane[0][1] = 0. if CP is not None and dCP is not None: rd_CP[0] = CP rd_CP_des[0] = CP_des[0] rd_dL_des_plane[0] = [ dL_des_plane[0] / 100, dL_des_plane[1] / 100, dL_des_plane[2] / 100 ] rd_dH_des[0] = dH_des rd_grf_des[0] = dL_des_plane - totalMass * mm.s2v(wcfg.gravity) rd_root_des[0] = rootPos[0] del rd_CF[:] del rd_CF_pos[:] for i in range(len(contactPositions)): rd_CF.append(contactForces[i] / 100) rd_CF_pos.append(contactPositions[i].copy()) if viewer_GetForceState(): rd_exfen_des[0] = [ extraForce[0][0] / 100, extraForce[0][1] / 100, extraForce[0][2] / 100 ] rd_exf_des[0] = [0, 0, 0] else: rd_exf_des[0] = [ extraForce[0][0] / 100, extraForce[0][1] / 100, extraForce[0][2] / 100 ] rd_exfen_des[0] = [0, 0, 0] extraForcePos[0] = dartModel.getBodyPositionGlobal(selectedBody)
def SE32vlogSO3(T): v = np.append(T[0:3, 3].flatten(), mm.logSO3(T[0:3, 0:3])) return v
def get_rnn_ref_pose_step(self, reset=False): if not reset: self.prev_ref_q = self.ref_skel.positions() self.prev_ref_dq = self.ref_skel.velocities() self.prev_ref_p_e_hat = np.asarray([ body.world_transform()[:3, 3] for body in self.ref_body_e ]).flatten() self.prev_ref_com = self.ref_skel.com() p = self.goal_in_world_frame target = Pose2d( [p[0] / self.RNN_MOTION_SCALE, -p[2] / self.RNN_MOTION_SCALE]) target = self.rnn.pose.relativePose(target) target = target.p t_len = v_len(target) if t_len > 80: ratio = 80 / t_len target[0] *= ratio target[1] *= ratio contacts, points, angles, orientations, root_orientation = self.rnn.step( target) for j in range(len(self.ref_skel.joints)): if j == 0: joint = self.ref_skel.joints[j] # type: pydart.FreeJoint joint_idx = self.rnn_joint_list.index(joint.name) hip_angles = mm.logSO3( np.dot(root_orientation, orientations[joint_idx])) # hip_angles = mm.logSO3(root_orientation) joint.set_position( np.array([ hip_angles[0], hip_angles[1], hip_angles[2], points[0][0], points[0][1], points[0][2] ])) continue joint = self.ref_skel.joints[j] # type: pydart.BallJoint joint_idx = self.rnn_joint_list.index(joint.name) joint.set_position(angles[joint_idx * 3:joint_idx * 3 + 3]) self.ik.clean_constraints() self.ik.add_joint_pos_const('LeftForeArm', np.asarray(points[10])) self.ik.add_joint_pos_const('LeftHand', np.asarray(points[2])) self.ik.add_joint_pos_const('LeftLeg', np.asarray(points[11])) self.ik.add_joint_pos_const('LeftFoot', np.asarray(points[3])) if contacts[0] > 0.8 and False: body_transform = self.ref_skel.body('LeftFoot').transform()[:3, :3] angle = acos(body_transform[1, 1]) body_ori = np.dot(body_transform, mm.rotX(-angle)) self.ik.add_orientation_const('LeftFoot', body_ori) self.ik.add_joint_pos_const('RightForeArm', np.asarray(points[12])) self.ik.add_joint_pos_const('RightHand', np.asarray(points[5])) self.ik.add_joint_pos_const('RightLeg', np.asarray(points[13])) self.ik.add_joint_pos_const('RightFoot', np.asarray(points[6])) self.ik.solve() foot_joint_ori = mm.exp(self.ref_skel.joint('LeftFoot').position()) self.ref_skel.joint('LeftFoot').set_position( mm.logSO3(np.dot(foot_joint_ori, np.dot(mm.rotX(-.6), mm.rotZ(.4))))) foot_joint_ori = mm.exp(self.ref_skel.joint('RightFoot').position()) self.ref_skel.joint('RightFoot').set_position( mm.logSO3( np.dot(foot_joint_ori, np.dot(mm.rotX(-.6), mm.rotZ(-.4))))) if not self.first: dq = 30. * self.ref_skel.position_differences( self.ref_skel.positions(), self.prev_ref_q) self.ref_skel.set_velocities(dq) if reset: self.prev_ref_q = self.ref_skel.positions() self.prev_ref_dq = self.ref_skel.velocities() self.prev_ref_p_e_hat = np.asarray([ body.world_transform()[:3, 3] for body in self.ref_body_e ]).flatten() self.prev_ref_com = self.ref_skel.com()
def get_rnn_ref_pose_step(self): p = self.goal_in_world_frame target = Pose2d( [p[0] / self.RNN_MOTION_SCALE, -p[2] / self.RNN_MOTION_SCALE]) target = self.rnn.pose.relativePose(target) target = target.p t_len = v_len(target) if t_len > 80: ratio = 80 / t_len target[0] *= ratio target[1] *= ratio contacts, points, angles, orientations, root_orientation = self.rnn.step( target) for j in range(len(self.ik_skel.joints)): if j == 0: joint = self.ik_skel.joints[j] # type: pydart.FreeJoint joint_idx = self.rnn_joint_list.index(joint.name) hip_angles = mm.logSO3( np.dot(root_orientation, orientations[joint_idx])) # hip_angles = mm.logSO3(root_orientation) joint.set_position( np.array([ hip_angles[0], hip_angles[1], hip_angles[2], points[0][0], points[0][1], points[0][2] ])) continue joint = self.ik_skel.joints[j] # type: pydart.BallJoint joint_idx = self.rnn_joint_list.index(joint.name) joint.set_position(angles[joint_idx * 3:joint_idx * 3 + 3]) self.ik.clean_constraints() self.ik.add_joint_pos_const('Hips', np.asarray(points[0])) self.ik.add_joint_pos_const('LeftForeArm', np.asarray(points[10])) self.ik.add_joint_pos_const('LeftHand', np.asarray(points[2])) self.ik.add_joint_pos_const('LeftLeg', np.asarray(points[11])) self.ik.add_joint_pos_const('LeftFoot', np.asarray(points[3])) if contacts[0] > 0.8 and False: body_transform = self.ik_skel.body('LeftFoot').transform()[:3, :3] angle = acos(body_transform[1, 1]) body_ori = np.dot(body_transform, mm.rotX(-angle)) self.ik.add_orientation_const('LeftFoot', body_ori) self.ik.add_joint_pos_const('RightForeArm', np.asarray(points[12])) self.ik.add_joint_pos_const('RightHand', np.asarray(points[5])) self.ik.add_joint_pos_const('RightLeg', np.asarray(points[13])) self.ik.add_joint_pos_const('RightFoot', np.asarray(points[6])) self.ik.add_joint_pos_const('Neck1', np.asarray(points[17])) self.ik.solve() foot_joint_ori = mm.exp(self.ik_skel.joint('LeftFoot').position()) self.ik_skel.joint('LeftFoot').set_position( mm.logSO3(np.dot(foot_joint_ori, np.dot(mm.rotX(-.6), mm.rotZ(.4))))) foot_joint_ori = mm.exp(self.ik_skel.joint('RightFoot').position()) self.ik_skel.joint('RightFoot').set_position( mm.logSO3( np.dot(foot_joint_ori, np.dot(mm.rotX(-.6), mm.rotZ(-.4))))) left_foot = self.ik_skel.body('LeftFoot') if (left_foot.to_world([0.05, -0.045, 0.1125])[1] < 0. or left_foot.to_world([-0.05, -0.045, 0.1125])[1] < 0.) \ and (left_foot.to_world([0.05, -0.045, -0.1125])[1] < 0. or left_foot.to_world([-0.05, -0.045, -0.1125])[1] < 0.): left_toe_pos1 = left_foot.to_world([0.05, -0.045, +0.1125]) left_toe_pos1[1] = 0. left_toe_pos2 = left_foot.to_world([-0.05, -0.045, +0.1125]) left_toe_pos2[1] = 0. left_heel_pos1 = left_foot.to_world([0.05, -0.045, -0.1125]) left_heel_pos1[1] = 0. left_heel_pos2 = left_foot.to_world([-0.05, -0.045, -0.1125]) left_heel_pos2[1] = 0. self.ik.clean_constraints() self.ik.add_position_const('LeftFoot', left_toe_pos1, np.array([0.05, -0.045, +0.1125])) self.ik.add_position_const('LeftFoot', left_toe_pos2, np.array([-0.05, -0.045, +0.1125])) self.ik.add_position_const('LeftFoot', left_heel_pos1, np.array([0.05, -0.045, -0.1125])) self.ik.add_position_const('LeftFoot', left_heel_pos2, np.array([-0.05, -0.045, -0.1125])) self.ik.solve() right_foot = self.ik_skel.body('RightFoot') if (right_foot.to_world([0.05, -0.045, 0.1125])[1] < 0. or right_foot.to_world([-0.05, -0.045, 0.1125])[1] < 0.) \ and (right_foot.to_world([0.05, -0.045, -0.1125])[1] < 0. or right_foot.to_world([-0.05, -0.045, -0.1125])[1] < 0.): right_toe_pos1 = right_foot.to_world([0.05, -0.045, +0.1125]) right_toe_pos1[1] = 0. right_toe_pos2 = right_foot.to_world([-0.05, -0.045, +0.1125]) right_toe_pos2[1] = 0. right_heel_pos1 = right_foot.to_world([0.05, -0.045, -0.1125]) right_heel_pos1[1] = 0. right_heel_pos2 = right_foot.to_world([-0.05, -0.045, -0.1125]) right_heel_pos2[1] = 0. self.ik.clean_constraints() self.ik.add_position_const('RightFoot', right_toe_pos1, np.array([0.05, -0.045, +0.1125])) self.ik.add_position_const('RightFoot', right_toe_pos2, np.array([-0.05, -0.045, +0.1125])) self.ik.add_position_const('RightFoot', right_heel_pos1, np.array([0.05, -0.045, -0.1125])) self.ik.add_position_const('RightFoot', right_heel_pos2, np.array([-0.05, -0.045, -0.1125])) self.ik.solve()
def simulateCallback(frame): # print(frame) # print(motion[frame].getJointOrientationLocal(footIdDic['RightFoot_foot_0_1_0'])) if True: if frame == 0: foot_viewer.check_all_seg() if motionFile == 'wd2_tiptoe_zygote.bvh': setParamVal('com Y offset', -0.01) elif frame == start_frame: foot_viewer.check_tilt_left_all() # elif frame == start_frame+10: setParamVal('left tilt angle', -0.2) setParamVal('right tilt angle', 0.2) elif frame == start_frame + 90: setParamVal('left tilt angle', 0.) setParamVal('right tilt angle', 0.) foot_viewer.check_all_seg() # hfi.footAdjust(motion[frame], idDic, SEGMENT_FOOT_MAG=.03, SEGMENT_FOOT_RAD=.015, baseHeight=0.02) if abs(getParamVal('tiptoe angle')) > 0.001: tiptoe_angle = getParamVal('tiptoe angle') motion[frame].mulJointOrientationLocal(idDic['LeftFoot_foot_0_0_0'], mm.exp(mm.unitX(), -math.pi * tiptoe_angle)) motion[frame].mulJointOrientationLocal(idDic['LeftFoot_foot_0_1_0'], mm.exp(mm.unitX(), -math.pi * tiptoe_angle)) motion[frame].mulJointOrientationLocal(idDic['RightFoot_foot_0_0_0'], mm.exp(mm.unitX(), -math.pi * tiptoe_angle)) motion[frame].mulJointOrientationLocal(idDic['RightFoot_foot_0_1_0'], mm.exp(mm.unitX(), -math.pi * tiptoe_angle)) motion[frame].mulJointOrientationLocal(idDic['LeftFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle * 0.95)) motion[frame].mulJointOrientationLocal(idDic['RightFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle * 0.95)) if getParamVal('left tilt angle') > 0.001: left_tilt_angle = getParamVal('left tilt angle') if motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1') is not None: motion[frame].mulJointOrientationLocal(idDic['LeftFoot_foot_0_1'], mm.exp(mm.unitZ(), -math.pi * left_tilt_angle)) else: motion[frame].mulJointOrientationLocal(idDic['LeftFoot_foot_0_1_0'], mm.exp(mm.unitZ(), -math.pi * left_tilt_angle)) motion[frame].mulJointOrientationLocal(idDic['LeftFoot_foot_1_0'], mm.exp(mm.unitZ(), -math.pi * left_tilt_angle*0.8)) motion[frame].mulJointOrientationLocal(idDic['LeftFoot'], mm.exp(mm.unitZ(), math.pi * left_tilt_angle*0.8)) elif getParamVal('left tilt angle') < -0.001: left_tilt_angle = getParamVal('left tilt angle') motion[frame].mulJointOrientationLocal(idDic['LeftFoot_foot_0_0'], mm.exp(mm.unitZ(), -math.pi * left_tilt_angle)) if motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1') is not None: motion[frame].mulJointOrientationLocal(idDic['LeftFoot_foot_0_1'], mm.exp(mm.unitZ(), math.pi * left_tilt_angle)) # else: # motion[frame].mulJointOrientationLocal(idDic['LeftFoot_foot_0_1_0'], mm.exp(mm.unitZ(), math.pi * left_tilt_angle)) motion[frame].mulJointOrientationLocal(idDic['LeftFoot_foot_1_0'], mm.exp(mm.unitZ(), -math.pi * left_tilt_angle*0.8)) motion[frame].mulJointOrientationLocal(idDic['LeftFoot'], mm.exp(mm.unitZ(), math.pi * left_tilt_angle*0.8)) if getParamVal('right tilt angle') > 0.001: right_tilt_angle = getParamVal('right tilt angle') if motion[0].skeleton.getJointIndex('RightFoot_foot_0_1') is not None: motion[frame].mulJointOrientationLocal(idDic['RightFoot_foot_0_1'], mm.exp(mm.unitZ(), math.pi * right_tilt_angle)) else: motion[frame].mulJointOrientationLocal(idDic['RightFoot_foot_0_1_0'], mm.exp(mm.unitZ(), math.pi * right_tilt_angle)) motion[frame].mulJointOrientationLocal(idDic['RightFoot_foot_1_0'], mm.exp(mm.unitZ(), math.pi * right_tilt_angle*0.8)) motion[frame].mulJointOrientationLocal(idDic['RightFoot'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle*0.8)) elif getParamVal('right tilt angle') < -0.001: right_tilt_angle = getParamVal('right tilt angle') motion[frame].mulJointOrientationLocal(idDic['RightFoot_foot_0_0'], mm.exp(mm.unitZ(), math.pi * right_tilt_angle)) if motion[0].skeleton.getJointIndex('RightFoot_foot_0_1') is not None: motion[frame].mulJointOrientationLocal(idDic['RightFoot_foot_0_1'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle)) # else: # motion[frame].mulJointOrientationLocal(idDic['RightFoot_foot_0_1_0'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle)) motion[frame].mulJointOrientationLocal(idDic['RightFoot_foot_1_0'], mm.exp(mm.unitZ(), math.pi * right_tilt_angle*0.8)) motion[frame].mulJointOrientationLocal(idDic['RightFoot'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle*0.8)) motionModel.update(motion[frame]) motionModel.translateByOffset(np.array([getParamVal('com X offset'), getParamVal('com Y offset'), getParamVal('com Z offset')])) controlModel_ik.set_q(controlModel.get_q()) global g_initFlag global forceShowTime global JsysPre global JsupPreL global JsupPreR global JconstPre global preFootCenter global maxContactChangeCount global contactChangeCount global contact global contactChangeType Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals(['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt']) Dt = 2*(Kt**.5) Dl = 2*(Kl**.5) Dh = 2*(Kh**.5) dt_sup = 2*(kt_sup**.5) # tracking th_r = motion.getDOFPositions(frame) th = controlModel.getDOFPositions() dth_r = motion.getDOFVelocities(frame) dth = controlModel.getDOFVelocities() ddth_r = motion.getDOFAccelerations(frame) ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt) # ype.flatten(fix_dofs(DOFs, ddth_des, mcfg, joint_names), ddth_des_flat) # ype.flatten(fix_dofs(DOFs, dth, mcfg, joint_names), dth_flat) ype.flatten(ddth_des, ddth_des_flat) ype.flatten(dth, dth_flat) ################################################# # jacobian ################################################# contact_des_ids = list() # desired contact segments if foot_viewer.check_om_l.value(): contact_des_ids.append(motion[0].skeleton.getJointIndex('LeftFoot_foot_0_0')) if foot_viewer.check_op_l.value(): contact_des_ids.append(motion[0].skeleton.getJointIndex('LeftFoot_foot_0_0_0')) if foot_viewer.check_im_l is not None and foot_viewer.check_im_l.value(): contact_des_ids.append(motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1')) if foot_viewer.check_ip_l.value(): contact_des_ids.append(motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1_0')) if foot_viewer.check_h_l.value(): contact_des_ids.append(motion[0].skeleton.getJointIndex('LeftFoot_foot_1_0')) if foot_viewer.check_om_r.value(): contact_des_ids.append(motion[0].skeleton.getJointIndex('RightFoot_foot_0_0')) if foot_viewer.check_op_r.value(): contact_des_ids.append(motion[0].skeleton.getJointIndex('RightFoot_foot_0_0_0')) if foot_viewer.check_im_r is not None and foot_viewer.check_im_r.value(): contact_des_ids.append(motion[0].skeleton.getJointIndex('RightFoot_foot_0_1')) if foot_viewer.check_ip_r.value(): contact_des_ids.append(motion[0].skeleton.getJointIndex('RightFoot_foot_0_1_0')) if foot_viewer.check_h_r.value(): contact_des_ids.append(motion[0].skeleton.getJointIndex('RightFoot_foot_1_0')) contact_ids = list() # temp idx for balancing contact_ids.extend(contact_des_ids) contact_joint_ori = list(map(controlModel.getJointOrientationGlobal, contact_ids)) contact_joint_pos = list(map(controlModel.getJointPositionGlobal, contact_ids)) contact_body_ori = list(map(controlModel.getBodyOrientationGlobal, contact_ids)) contact_body_pos = list(map(controlModel.getBodyPositionGlobal, contact_ids)) contact_body_vel = list(map(controlModel.getBodyVelocityGlobal, contact_ids)) contact_body_angvel = list(map(controlModel.getBodyAngVelocityGlobal, contact_ids)) ref_joint_ori = list(map(motion[frame].getJointOrientationGlobal, contact_ids)) ref_joint_pos = list(map(motion[frame].getJointPositionGlobal, contact_ids)) ref_joint_vel = [motion.getJointVelocityGlobal(joint_idx, frame) for joint_idx in contact_ids] ref_joint_angvel = [motion.getJointAngVelocityGlobal(joint_idx, frame) for joint_idx in contact_ids] ref_body_ori = list(map(motionModel.getBodyOrientationGlobal, contact_ids)) ref_body_pos = list(map(motionModel.getBodyPositionGlobal, contact_ids)) # ref_body_vel = list(map(controlModel.getBodyVelocityGlobal, contact_ids)) ref_body_angvel = [motion.getJointAngVelocityGlobal(joint_idx, frame) for joint_idx in contact_ids] ref_body_vel = [ref_joint_vel[i] + np.cross(ref_joint_angvel[i], ref_body_pos[i] - ref_joint_pos[i]) for i in range(len(ref_joint_vel))] is_contact = [1] * len(contact_ids) contact_right = len(set(contact_des_ids).intersection(rIDlist)) > 0 contact_left = len(set(contact_des_ids).intersection(lIDlist)) > 0 contMotionOffset = th[0][0] - th_r[0][0] linkPositions = controlModel.getBodyPositionsGlobal() linkVelocities = controlModel.getBodyVelocitiesGlobal() linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal() linkInertias = controlModel.getBodyInertiasGlobal() CM = yrp.getCM(linkPositions, linkMasses, totalMass) dCM = yrp.getCM(linkVelocities, linkMasses, totalMass) CM_plane = copy.copy(CM) CM_plane[1] = 0. dCM_plane = copy.copy(dCM) dCM_plane[1] = 0. P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM, linkInertias) dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses, linkVelocities, dCM, linkAngVelocities, linkInertias) # calculate jacobian Jsys, dJsys = controlModel.computeCom_J_dJdq() J_contacts = [] # type: list[np.ndarray] dJ_contacts = [] # type: list[np.ndarray] for contact_id in contact_ids: J_contacts.append(Jsys[6*contact_id:6*contact_id + 6, :]) dJ_contacts.append(dJsys[6*contact_id:6*contact_id + 6]) # calculate footCenter footCenter = sum(contact_body_pos) / len(contact_body_pos) if len(contact_body_pos) > 0 \ else .5 * (controlModel.getBodyPositionGlobal(supL) + controlModel.getBodyPositionGlobal(supR)) footCenter[1] = 0. # if len(contact_body_pos) > 2: # hull = ConvexHull(contact_body_pos) footCenter_ref = sum(ref_body_pos) / len(ref_body_pos) if len(ref_body_pos) > 0 \ else .5 * (motionModel.getBodyPositionGlobal(supL) + motionModel.getBodyPositionGlobal(supR)) footCenter_ref = footCenter_ref + contMotionOffset # if len(ref_body_pos) > 2: # hull = ConvexHull(ref_body_pos) footCenter_ref[1] = 0. # footCenter[0] = footCenter[0] + getParamVal('com X offset') # footCenter[1] = footCenter[0] + getParamVal('com Y offset') # footCenter[2] = footCenter[2] + getParamVal('com Z offset') # initialization if g_initFlag == 0: preFootCenter[0] = footCenter.copy() g_initFlag = 1 # if contactChangeCount == 0 and np.linalg.norm(footCenter - preFootCenter[0]) > 0.01: # contactChangeCount += 30 if contactChangeCount > 0: # change footcenter gradually footCenter = preFootCenter[0] + (maxContactChangeCount - contactChangeCount)*(footCenter-preFootCenter[0])/maxContactChangeCount else: preFootCenter[0] = footCenter.copy() # footCenter = np.array([1.54049, 0., -0.29345]) # linear momentum # TODO: # We should consider dCM_ref, shouldn't we? # add getBodyPositionGlobal and getBodyPositionsGlobal in csVpModel! # to do that, set joint velocities to vpModel CM_ref_plane = footCenter # CM_ref_plane = footCenter_ref CM_ref = footCenter + np.array([getParamVal('com X offset'), motionModel.getCOM()[1] + getParamVal('com Y offset'), getParamVal('com Z offset')]) dL_des_plane = Kl * totalMass * (CM_ref - CM) - Dl * totalMass * dCM # dL_des_plane = Kl * totalMass * (CM_ref_plane - CM_plane) - Dl * totalMass * dCM_plane # dL_des_plane[1] = 0. # print('dCM_plane : ', np.linalg.norm(dCM_plane)) # angular momentum CP_ref = footCenter # CP_ref = footCenter_ref bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds) CP = yrp.getCP(contactPositions, contactForces) if CP_old[0] is None or CP is None: dCP = None else: dCP = (CP - CP_old[0])/(1/30.) CP_old[0] = CP if CP is not None and dCP is not None: ddCP_des = Kh*(CP_ref - CP) - Dh * dCP dCP_des = dCP + ddCP_des * (1/30.) CP_des = CP + dCP_des * (1/30.) # CP_des = footCenter CP_des = CP + dCP*(1/30.) + .5*ddCP_des*((1/30.)**2) dH_des = np.cross((CP_des - CM), (dL_des_plane + totalMass * mm.s2v(wcfg.gravity))) if contactChangeCount > 0: # and contactChangeType == 'DtoS': dH_des *= (maxContactChangeCount - contactChangeCount)/maxContactChangeCount else: dH_des = None # convex hull contact_pos_2d = np.asarray([np.array([contactPosition[0], contactPosition[2]]) for contactPosition in contactPositions]) p = np.array([CM_plane[0], CM_plane[2]]) # hull = None # type: Delaunay # if contact_pos_2d.shape[0] > 0: # hull = Delaunay(contact_pos_2d) # print(hull.find_simplex(p) >= 0) # set up equality constraint # TODO: # logSO3 is just q'', not acceleration. # To make a_oris acceleration, q'' -> a will be needed # body_ddqs = list(map(mm.logSO3, [mm.getSO3FromVectors(np.dot(body_ori, mm.unitY()), mm.unitY()) for body_ori in contact_body_ori])) # body_ddqs = list(map(mm.logSO3, [np.dot(contact_body_ori[i].T, np.dot(ref_body_ori[i], mm.getSO3FromVectors(np.dot(ref_body_ori[i], mm.unitY()), mm.unitY()))) for i in range(len(contact_body_ori))])) body_ddqs = list(map(mm.logSO3, [np.dot(contact_body_ori[i].T, np.dot(ref_body_ori[i], mm.getSO3FromVectors(np.dot(ref_body_ori[i], up_vec_in_each_link[contact_ids[i]]), mm.unitY()))) for i in range(len(contact_body_ori))])) body_qs = list(map(mm.logSO3, contact_body_ori)) body_angs = [np.dot(contact_body_ori[i], contact_body_angvel[i]) for i in range(len(contact_body_ori))] body_dqs = [mm.vel2qd(body_angs[i], body_qs[i]) for i in range(len(body_angs))] a_oris = [np.dot(contact_body_ori[i], mm.qdd2accel(body_ddqs[i], body_dqs[i], body_qs[i])) for i in range(len(contact_body_ori))] a_oris = list(map(mm.logSO3, [np.dot(np.dot(ref_body_ori[i], mm.getSO3FromVectors(np.dot(ref_body_ori[i], up_vec_in_each_link[contact_ids[i]]), mm.unitY())), contact_body_ori[i].T) for i in range(len(contact_body_ori))])) # body_ddq = body_ddqs[0] # body_ori = contact_body_ori[0] # body_ang = np.dot(body_ori.T, contact_body_angvel[0]) # # body_q = mm.logSO3(body_ori) # body_dq = mm.vel2qd(body_ang, body_q) # a_ori = np.dot(body_ori, mm.qdd2accel(body_ddq, body_dq, body_q)) KT_SUP = np.diag([kt_sup/10., kt_sup, kt_sup/10.]) # a_oris = list(map(mm.logSO3, [mm.getSO3FromVectors(np.dot(body_ori, mm.unitY()), mm.unitY()) for body_ori in contact_body_ori])) # a_sups = [np.append(kt_sup*(ref_body_pos[i] - contact_body_pos[i] + contMotionOffset) + dt_sup*(ref_body_vel[i] - contact_body_vel[i]), # kt_sup*a_oris[i]+dt_sup*(ref_body_angvel[i]-contact_body_angvel[i])) for i in range(len(a_oris))] # a_sups = [np.append(kt_sup*(ref_body_pos[i] - contact_body_pos[i] + contMotionOffset) - dt_sup * contact_body_vel[i], # kt_sup*a_oris[i] - dt_sup * contact_body_angvel[i]) for i in range(len(a_oris))] a_sups = [np.append(np.dot(KT_SUP, (ref_body_pos[i] - contact_body_pos[i] + contMotionOffset)) - dt_sup * contact_body_vel[i], kt_sup*a_oris[i] - dt_sup * contact_body_angvel[i]) for i in range(len(a_oris))] # set up soft constraint ee_idx = motion[frame].skeleton.getJointIndex('Spine') ee_ori = controlModel.getBodyOrientationGlobal(ee_idx) ee_ang = controlModel.getBodyAngVelocityGlobal(ee_idx) J_ee = Jsys[6*ee_idx+3:6*ee_idx+6, :] dJ_ee = dJsys[6*ee_idx+3:6*ee_idx+6] a_ee = 20. * mm.logSO3(mm.getSO3FromVectors(np.dot(ee_ori, mm.unitZ()), mm.unitY())) - 1. * ee_ang # momentum matrix RS = np.dot(P, Jsys) R, S = np.vsplit(RS, 2) # rs = np.dot((np.dot(dP, Jsys) + np.dot(P, dJsys)), dth_flat) rs = np.dot(dP, np.dot(Jsys, dth_flat)) + np.dot(P, dJsys) r_bias, s_bias = np.hsplit(rs, 2) ####################################################### # optimization ####################################################### # if contact == 2 and footCenterR[1] > doubleTosingleOffset/2: if contact_left and not contact_right: config['weightMap']['RightUpLeg'] = .8 config['weightMap']['RightLeg'] = .8 config['weightMap']['RightFoot'] = .8 else: config['weightMap']['RightUpLeg'] = .1 config['weightMap']['RightLeg'] = .25 config['weightMap']['RightFoot'] = .2 # if contact == 1 and footCenterL[1] > doubleTosingleOffset/2: if contact_right and not contact_left: config['weightMap']['LeftUpLeg'] = .8 config['weightMap']['LeftLeg'] = .8 config['weightMap']['LeftFoot'] = .8 else: config['weightMap']['LeftUpLeg'] = .1 config['weightMap']['LeftLeg'] = .25 config['weightMap']['LeftFoot'] = .2 w = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap']) mot.addTrackingTerms(problem, totalDOF, Bt, w, ddth_des_flat) if dH_des is not None: mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias) mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias) # mot.addEndEffectorTerms(problem, totalDOF, 1., J_ee, dJ_ee, dth, a_ee) if True: for c_idx in range(len(contact_ids)): mot.addConstraint2(problem, totalDOF, J_contacts[c_idx], dJ_contacts[c_idx], dth_flat, a_sups[c_idx]) if contactChangeCount > 0: contactChangeCount = contactChangeCount - 1 if contactChangeCount == 0: maxContactChangeCount = 30 contactChangeType = 0 r = problem.solve() problem.clear() ddth_sol_flat = np.asarray(r['x']) # ddth_sol_flat[foot_seg_dofs] = np.array(ddth_des_flat)[foot_seg_dofs] ype.nested(ddth_sol_flat, ddth_sol) rootPos[0] = controlModel.getBodyPositionGlobal(selectedBody) localPos = [[0, 0, 0]] for i in range(stepsPerFrame): # apply penalty force bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds) # bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds) vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces) controlModel.setDOFAccelerations(ddth_sol) # controlModel.setDOFAccelerations(ddth_des) # controlModel.set_ddq(ddth_sol_flat) # controlModel.set_ddq(ddth_des_flat) controlModel.solveHybridDynamics() if forceShowTime > viewer.objectInfoWnd.labelForceDur.value(): forceShowTime = 0 viewer_ResetForceState() forceforce = np.array([viewer.objectInfoWnd.labelForceX.value(), viewer.objectInfoWnd.labelForceY.value(), viewer.objectInfoWnd.labelForceZ.value()]) extraForce[0] = getParamVal('Fm') * mm.normalize2(forceforce) if viewer_GetForceState(): forceShowTime += wcfg.timeStep vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce) vpWorld.step() controlModel_ik.set_q(controlModel.get_q()) # rendering bodyIDs, geomIDs, positionLocalsForGeom = vpWorld.getContactInfoForcePlate(bodyIDsToCheck) for foot_seg_id in footIdlist: control_model_renderer.body_colors[foot_seg_id] = (255, 240, 255) control_model_renderer.geom_colors[foot_seg_id] = [(255, 240, 255)] * controlModel.getBodyGeomNum(foot_seg_id) for i in range(len(geomIDs)): control_model_renderer.geom_colors[bodyIDs[i]][geomIDs[i]] = (255, 0, 0) # for foot_seg_id in footIdlist: # control_model_renderer.body_colors[foot_seg_id] = (255, 240, 255) # # for contact_id in contact_ids: # control_model_renderer.body_colors[contact_id] = (255, 0, 0) rd_footCenter[0] = footCenter rd_footCenter_ref[0] = footCenter_ref rd_CM[0] = CM rd_CM_plane[0] = CM.copy() rd_CM_plane[0][1] = 0. if CP is not None and dCP is not None: rd_CP[0] = CP rd_CP_des[0] = CP_des rd_dL_des_plane[0] = [dL_des_plane[0]/100, dL_des_plane[1]/100, dL_des_plane[2]/100] rd_dH_des[0] = dH_des rd_grf_des[0] = dL_des_plane - totalMass * mm.s2v(wcfg.gravity) del rd_foot_ori[:] del rd_foot_pos[:] # for seg_foot_id in footIdlist: # rd_foot_ori.append(controlModel.getJointOrientationGlobal(seg_foot_id)) # rd_foot_pos.append(controlModel.getJointPositionGlobal(seg_foot_id)) rd_foot_ori.append(controlModel.getJointOrientationGlobal(supL)) rd_foot_ori.append(controlModel.getJointOrientationGlobal(supR)) rd_foot_pos.append(controlModel.getJointPositionGlobal(supL)) rd_foot_pos.append(controlModel.getJointPositionGlobal(supR)) rd_root_des[0] = rootPos[0] rd_root_ori[0] = controlModel.getBodyOrientationGlobal(0) rd_root_pos[0] = controlModel.getBodyPositionGlobal(0) del rd_CF[:] del rd_CF_pos[:] for i in range(len(contactPositions)): rd_CF.append(contactForces[i]/400) rd_CF_pos.append(contactPositions[i].copy()) if viewer_GetForceState(): rd_exfen_des[0] = [extraForce[0][0]/100, extraForce[0][1]/100, extraForce[0][2]/100] rd_exf_des[0] = [0, 0, 0] else: rd_exf_des[0] = [extraForce[0][0]/100, extraForce[0][1]/100, extraForce[0][2]/100] rd_exfen_des[0] = [0, 0, 0] extraForcePos[0] = controlModel.getBodyPositionGlobal(selectedBody) # render contact_ids # render skeleton if SKELETON_ON: Ts = dict() Ts['pelvis'] = controlModel.getJointTransform(idDic['Hips']) Ts['thigh_R'] = controlModel.getJointTransform(idDic['RightUpLeg']) Ts['shin_R'] = controlModel.getJointTransform(idDic['RightLeg']) Ts['foot_R'] = controlModel.getJointTransform(idDic['RightFoot']) Ts['foot_heel_R'] = controlModel.getJointTransform(idDic['RightFoot']) Ts['heel_R'] = np.eye(4) Ts['outside_metatarsal_R'] = controlModel.getJointTransform(idDic['RightFoot_foot_0_0']) Ts['outside_phalanges_R'] = controlModel.getJointTransform(idDic['RightFoot_foot_0_0_0']) # Ts['inside_metatarsal_R'] = controlModel.getJointTransform(idDic['RightFoot_foot_0_1']) Ts['inside_metatarsal_R'] = np.eye(4) Ts['inside_phalanges_R'] = controlModel.getJointTransform(idDic['RightFoot_foot_0_1_0']) Ts['spine_ribs'] = controlModel.getJointTransform(idDic['Spine']) Ts['head'] = controlModel.getJointTransform(idDic['Spine1']) Ts['upper_limb_R'] = controlModel.getJointTransform(idDic['RightArm']) Ts['lower_limb_R'] = controlModel.getJointTransform(idDic['RightForeArm']) Ts['thigh_L'] = controlModel.getJointTransform(idDic['LeftUpLeg']) Ts['shin_L'] = controlModel.getJointTransform(idDic['LeftLeg']) Ts['foot_L'] = controlModel.getJointTransform(idDic['LeftFoot']) Ts['foot_heel_L'] = controlModel.getJointTransform(idDic['LeftFoot']) Ts['heel_L'] = np.eye(4) Ts['outside_metatarsal_L'] = controlModel.getJointTransform(idDic['LeftFoot_foot_0_0']) Ts['outside_phalanges_L'] = controlModel.getJointTransform(idDic['LeftFoot_foot_0_0_0']) # Ts['inside_metatarsal_L'] = controlModel.getJointTransform(idDic['LeftFoot_foot_0_1']) Ts['inside_metatarsal_L'] = np.eye(4) Ts['inside_phalanges_L'] = controlModel.getJointTransform(idDic['LeftFoot_foot_0_1_0']) Ts['upper_limb_L'] = controlModel.getJointTransform(idDic['LeftArm']) Ts['lower_limb_L'] = controlModel.getJointTransform(idDic['LeftForeArm']) skeleton_renderer.appendFrameState(Ts)
def getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt, weightMap=None): ddth_des = [None] * len(th_r) # type: list[np.ndarray] # p_r0 = mm.T2p(th_r[0]) # p0 = mm.T2p(th[0]) # v_r0 = dth_r[0][0:3] # v0 = dth[0][0:3] # a_r0 = ddth_r[0][0:3] # th_r0 = mm.T2R(th_r[0]) # th0 = mm.T2R(th[0]) # dth_r0 = dth_r[0][3:6] # dth0 = dth[0][3:6] # ddth_r0 = ddth_r[0][3:6] p_r0 = th_r[0][0] p0 = th[0][0] # v_r0 = dth_r[0][0:3] v0 = dth[0][0:3] # a_r0 = ddth_r[0][0:3] th_r0 = th_r[0][1] th0 = th[0][1] # dth_r0 = dth_r[0][3:6] dth0 = dth[0][3:6] # ddth_r0 = ddth_r[0][3:6] kt = Kt dt = Dt if weightMap is not None: kt = Kt * weightMap[0] dt = Dt * (weightMap[0]**.5) # dt = 0. # a_des0 = kt*(p_r0 - p0) + dt*(v_r0 - v0) + a_r0 # ddth_des0 = kt*(mm.logSO3(np.dot(th0.transpose(), th_r0))) + dt*(dth_r0 - dth0) + ddth_r0 a_des0 = kt * (p_r0 - p0) + dt * (-v0) #+ a_r0 ddth_des0 = kt * (mm.logSO3(np.dot(th0.transpose(), th_r0))) + dt * ( -dth0) #+ ddth_r0 ddth_des[0] = np.concatenate((a_des0, ddth_des0)) for i in range(1, len(th_r)): if weightMap is not None: kt = Kt * weightMap[i] dt = Dt * (weightMap[i]**.5) # dt = 0. # ddth_des[i] = kt*(mm.logSO3(np.dot(th[i].transpose(), th_r[i]))) + dt*(dth_r[i] - dth[i]) #+ ddth_r[i] if th[i].shape[0] == 3: ddth_des[i] = kt * (mm.logSO3(np.dot( th[i].transpose(), th_r[i]))) + dt * (-dth[i]) #+ ddth_r[i] elif th[i].shape[0] > 0: ddth_des[i] = kt * (mm.logSO3(np.dot( th[i].transpose(), th_r[i]))) + dt * (-dth[i]) #+ ddth_r[i] else: ddth_des[i] = np.zeros(0) return ddth_des
def simulateCallback(frame): # print() # print(dartModel.getJointVelocityGlobal(0)) # print(dartModel.getDOFVelocities()[0]) # print(dartModel.get_dq()[:6]) # dartMotionModel.update(motion[frame]) global g_initFlag global forceShowTime global preFootCenter global maxContactChangeCount global contactChangeCount global contact global contactChangeType # print('contactstate:', contact, contactChangeCount) Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals( ['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt']) Dt = 2. * (Kt**.5) Dl = (Kl**.5) Dh = (Kh**.5) dt_sup = 2. * (kt_sup**.5) # Dt = .2*(Kt**.5) # Dl = .2*(Kl**.5) # Dh = .2*(Kh**.5) # dt_sup = .2*(kt_sup**.5) pdcontroller.setKpKd(Kt, Dt) footHeight = dartModel.getBody(supL).shapenodes[0].shape.size()[1] / 2. doubleTosingleOffset = 0.15 singleTodoubleOffset = 0.30 com_offset_x, com_offset_y, com_offset_z = getParamVals( ['com X offset', 'com Y offset', 'com Z offset']) footOffset = np.array((com_offset_x, com_offset_y, com_offset_z)) # tracking # th_r = motion.getDOFPositions(frame) th_r = dartMotionModel.getDOFPositions() th = dartModel.getDOFPositions() th_r_flat = dartMotionModel.get_q() # dth_r = motion.getDOFVelocities(frame) # dth = dartModel.getDOFVelocities() # ddth_r = motion.getDOFAccelerations(frame) # ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt) dth_flat = dartModel.get_dq() # dth_flat = np.concatenate(dth) # ddth_des_flat = pdcontroller.compute(dartMotionModel.get_q()) # ddth_des_flat = pdcontroller.compute(th_r) ddth_des_flat = pdcontroller.compute_flat(th_r_flat) # ype.flatten(ddth_des, ddth_des_flat) # ype.flatten(dth, dth_flat) print(dartModel.skeleton.get_spd_tau(th_r_flat, Kt, Dt)) ################################################# # jacobian ################################################# footOriL = dartModel.getJointOrientationGlobal(supL) footOriR = dartModel.getJointOrientationGlobal(supR) footCenterL = dartModel.getBodyPositionGlobal(supL) footCenterR = dartModel.getBodyPositionGlobal(supR) footBodyOriL = dartModel.getBodyOrientationGlobal(supL) footBodyOriR = dartModel.getBodyOrientationGlobal(supR) footBodyVelL = dartModel.getBodyVelocityGlobal(supL) footBodyVelR = dartModel.getBodyVelocityGlobal(supR) footBodyAngVelL = dartModel.getBodyAngVelocityGlobal(supL) footBodyAngVelR = dartModel.getBodyAngVelocityGlobal(supR) refFootL = dartMotionModel.getBodyPositionGlobal(supL) refFootR = dartMotionModel.getBodyPositionGlobal(supR) # refFootAngVelL = motion.getJointAngVelocityGlobal(supL, frame) # refFootAngVelR = motion.getJointAngVelocityGlobal(supR, frame) refFootAngVelL = np.zeros(3) refFootAngVelR = np.zeros(3) refFootVelR = np.zeros(3) refFootVelL = np.zeros(3) contactR = 1 contactL = 1 if refFootVelR[1] < 0 and refFootVelR[1] * frame_step_size + refFootR[ 1] > singleTodoubleOffset: contactR = 0 if refFootVelL[1] < 0 and refFootVelL[1] * frame_step_size + refFootL[ 1] > singleTodoubleOffset: contactL = 0 if refFootVelR[1] > 0 and refFootVelR[1] * frame_step_size + refFootR[ 1] > doubleTosingleOffset: contactR = 0 if refFootVelL[1] > 0 and refFootVelL[1] * frame_step_size + refFootL[ 1] > doubleTosingleOffset: contactL = 0 # contactR = 0 # contMotionOffset = th[0][0] - th_r[0][0] # contMotionOffset = dartModel.getBodyPositionGlobal(0) - dartMotionModel.getBodyPositionGlobal(0) contMotionOffset = controlToMotionOffset linkPositions = dartModel.getBodyPositionsGlobal() linkVelocities = dartModel.getBodyVelocitiesGlobal() linkAngVelocities = dartModel.getBodyAngVelocitiesGlobal() linkInertias = dartModel.getBodyInertiasGlobal() CM = dartModel.skeleton.com() dCM = dartModel.skeleton.com_velocity() CM_plane = copy.copy(CM) CM_plane[1] = 0. dCM_plane = copy.copy(dCM) dCM_plane[1] = 0. P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM, linkInertias) dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses, linkVelocities, dCM, linkAngVelocities, linkInertias) #calculate contact state #if g_initFlag == 1 and contact == 1 and refFootR[1] < doubleTosingleOffset and footCenterR[1] < 0.08: if g_initFlag == 1: #contact state # 0: flying 1: right only 2: left only 3: double #if contact == 2 and refFootR[1] < doubleTosingleOffset: if contact == 2 and contactR == 1: contact = 3 maxContactChangeCount += 30 contactChangeCount += maxContactChangeCount contactChangeType = 'StoD' #elif contact == 3 and refFootL[1] < doubleTosingleOffset: elif contact == 1 and contactL == 1: contact = 3 maxContactChangeCount += 30 contactChangeCount += maxContactChangeCount contactChangeType = 'StoD' #elif contact == 3 and refFootR[1] > doubleTosingleOffset: elif contact == 3 and contactR == 0: contact = 2 contactChangeCount += maxContactChangeCount contactChangeType = 'DtoS' #elif contact == 3 and refFootL[1] > doubleTosingleOffset: elif contact == 3 and contactL == 0: contact = 1 contactChangeCount += maxContactChangeCount contactChangeType = 'DtoS' else: contact = 0 #if refFootR[1] < doubleTosingleOffset: if contactR == 1: contact += 1 #if refFootL[1] < doubleTosingleOffset: if contactL == 1: contact += 2 #initialization if g_initFlag == 0: softConstPoint = footCenterR.copy() footCenter = footCenterL + (footCenterR - footCenterL) / 2.0 footCenter[1] = 0. preFootCenter = footCenter.copy() #footToBodyFootRotL = np.dot(np.transpose(footOriL), footBodyOriL) #footToBodyFootRotR = np.dot(np.transpose(footOriR), footBodyOriR) # if refFootR[1] < doubleTosingleOffset: # contact +=1 # if refFootL[1] < doubleTosingleOffset: # contact +=2 if refFootR[1] < footHeight: contact += 1 if refFootL[1] < footHeight: contact += 2 g_initFlag = 1 contact = 2 # contact = 1 + 2 # calculate jacobian body_num = dartModel.getBodyNum() Jsys = np.zeros((6 * body_num, totalDOF)) dJsys = np.zeros((6 * body_num, totalDOF)) for i in range(dartModel.getBodyNum()): Jsys[6 * i:6 * i + 6, :] = dartModel.getBody(i).world_jacobian()[range(-3, 3), :] dJsys[6 * i:6 * i + 6, :] = dartModel.getBody( i).world_jacobian_classic_deriv()[range(-3, 3), :] JsupL = dartModel.getBody(supL).world_jacobian()[range(-3, 3), :] dJsupL = dartModel.getBody(supL).world_jacobian_classic_deriv()[ range(-3, 3), :] JsupR = dartModel.getBody(supR).world_jacobian()[range(-3, 3), :] dJsupR = dartModel.getBody(supR).world_jacobian_classic_deriv()[ range(-3, 3), :] # calculate footCenter footCenter = .5 * (footCenterL + footCenterR) + footOffset if contact == 2: footCenter = footCenterL.copy() + footOffset if contact == 1: footCenter = footCenterR.copy() + footOffset footCenter[1] = 0. footCenter[0] += 0.02 preFootCenter = footCenter.copy() # linear momentum # CM_ref_plane = footCenter.copy() # CM_ref_plane += np.array([0., 0.9, 0.]) # dL_des_plane = Kl*totalMass*(CM_ref_plane - CM_plane) - Dl*totalMass*dCM_plane # dL_des_plane[1] = 0. kl = np.diagflat([Kl * 5., Kl, Kl * 5.]) dl = np.diagflat([2.2 * Dl, Dl, 2.2 * Dl]) CM_ref = footCenter.copy() CM_ref[1] = dartMotionModel.getCOM()[1] - 0.1 # CM_ref += np.array((0., com_offset_y, 0.)) # dL_des_plane = Kl*totalMass*(CM_ref - CM) - Dl*totalMass*dCM dL_des_plane = kl.dot(totalMass * (CM_ref - CM)) - dl.dot(totalMass * dCM) # angular momentum CP_ref = footCenter CP = yrp.getCP(contactPositions, contactForces) if CP_old[0] is None or CP is None: dCP = None else: dCP = (CP - CP_old[0]) / frame_step_size CP_old[0] = CP CP_des[0] = None # if CP_des[0] is None: # CP_des[0] = footCenter if CP is not None and dCP is not None: ddCP_des = Kh * (CP_ref - CP) - Dh * (dCP) CP_des[0] = CP + dCP * frame_step_size + .5 * ddCP_des * ( frame_step_size**2) dH_des = np.cross( CP_des[0] - CM, dL_des_plane - totalMass * mm.s2v(dartModel.world.gravity())) # dH_des = np.cross(footCenter - CM, dL_des_plane - totalMass*mm.s2v(dartModel.world.gravity())) # H = np.dot(P, np.dot(Jsys, dth_flat)) # dH_des = -Kh * H[3:] else: dH_des = None # set up equality constraint a_oriL = mm.logSO3( mm.getSO3FromVectors(np.dot(footBodyOriL, np.array([0, 1, 0])), np.array([0, 1, 0]))) a_oriR = mm.logSO3( mm.getSO3FromVectors(np.dot(footBodyOriR, np.array([0, 1, 0])), np.array([0, 1, 0]))) footErrorL = refFootL.copy() footErrorL[1] = dartModel.getBody( supL).shapenodes[0].shape.size()[1] / 2. footErrorL += -footCenterL + contMotionOffset footErrorR = refFootR.copy() footErrorR[1] = dartModel.getBody( supR).shapenodes[0].shape.size()[1] / 2. footErrorR += -footCenterR + contMotionOffset a_supL = np.append( kt_sup * footErrorL + dt_sup * (refFootVelL - footBodyVelL), kt_sup * a_oriL + dt_sup * (refFootAngVelL - footBodyAngVelL)) a_supR = np.append( kt_sup * footErrorR + dt_sup * (refFootVelR - footBodyVelR), kt_sup * a_oriR + dt_sup * (refFootAngVelR - footBodyAngVelR)) # momentum matrix RS = np.dot(P, Jsys) R, S = np.vsplit(RS, 2) rs = np.dot((np.dot(dP, Jsys) + np.dot(P, dJsys)), dth_flat) r_bias, s_bias = np.hsplit(rs, 2) ####################################################### # optimization ####################################################### if LEG_FLEXIBLE: if contact == 2: config['weightMap']['j_thigh_right'] = .8 config['weightMap']['j_shin_right'] = .8 config['weightMap']['j_heel_right'] = .8 else: config['weightMap']['j_thigh_right'] = .1 config['weightMap']['j_shin_right'] = .25 config['weightMap']['j_heel_right'] = .2 if contact == 1: config['weightMap']['j_thigh_left'] = .8 config['weightMap']['j_shin_left'] = .8 config['weightMap']['j_heel_left'] = .8 else: config['weightMap']['j_thigh_left'] = .1 config['weightMap']['j_shin_left'] = .25 config['weightMap']['j_heel_left'] = .2 w = mot.getTrackingWeightDart(DOFs, dartModel.skeleton, config['weightMap']) mot.addTrackingTerms(problem, totalDOF, Bt, w, ddth_des_flat) mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias) if dH_des is not None: mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias) if contact & 1: mot.addConstraint(problem, totalDOF, JsupR, dJsupR, dth_flat, a_supR) if contact & 2: mot.addConstraint(problem, totalDOF, JsupL, dJsupL, dth_flat, a_supL) if contactChangeCount > 0: contactChangeCount -= 1 if contactChangeCount == 0: maxContactChangeCount = 30 contactChangeType = 0 r = problem.solve() problem.clear() # ype.nested(r['x'], ddth_sol) ddth_sol = np.asarray(r['x']) # ddth_sol[:6] = np.zeros(6) if dH_des is None: ddth_sol = ddth_des_flat rootPos[0] = dartModel.getBodyPositionGlobal(selectedBody) localPos = [[0, 0, 0]] inv_h = 1. / time_step _bodyIDs, _contactPositions, _contactPositionLocals, _contactForces = [], [], [], [] for iii in range(stepsPerFrame): _ddq, _tau, _bodyIDs, _contactPositions, _contactPositionLocals, _contactForces = hqp.calc_QP( dartModel.skeleton, ddth_sol, inv_h) # _ddq, _tau, _bodyIDs, _contactPositions, _contactPositionLocals, _contactForces = hqp.calc_QP(dartModel.skeleton, ddth_des_flat, inv_h) # print(frame, i, tau) dartModel.applyPenaltyForce(_bodyIDs, _contactPositionLocals, _contactForces) dartModel.skeleton.set_forces(_tau) if forceShowTime > viewer.objectInfoWnd.labelForceDur.value(): forceShowTime = 0 viewer_ResetForceState() forceforce = np.array([ viewer.objectInfoWnd.labelForceX.value(), viewer.objectInfoWnd.labelForceY.value(), viewer.objectInfoWnd.labelForceZ.value() ]) extraForce[0] = getParamVal('Fm') * mm.normalize2(forceforce) if viewer_GetForceState(): forceShowTime += time_step dartModel.applyPenaltyForce(selectedBodyId, localPos, extraForce) dartModel.step() del bodyIDs[:] del contactPositions[:] del contactPositions[:] del contactPositionLocals[:] del contactForces[:] bodyIDs.extend(_bodyIDs) contactPositions.extend(_contactPositions) contactPositionLocals.extend(_contactPositionLocals) contactForces.extend(_contactForces) # rendering rightFootVectorX[0] = np.dot(footOriL, np.array([.1, 0, 0])) rightFootVectorY[0] = np.dot(footOriL, np.array([0, .1, 0])) rightFootVectorZ[0] = np.dot(footOriL, np.array([0, 0, .1])) rightFootPos[0] = footCenterL rightVectorX[0] = np.dot(footBodyOriL, np.array([.1, 0, 0])) rightVectorY[0] = np.dot(footBodyOriL, np.array([0, .1, 0])) rightVectorZ[0] = np.dot(footBodyOriL, np.array([0, 0, .1])) rightPos[0] = footCenterL + np.array([.1, 0, 0]) rd_footCenter[0] = footCenter rd_footCenterL[0] = footCenterL rd_footCenterR[0] = footCenterR rd_CM[0] = CM rd_CM_plane[0] = CM.copy() rd_CM_plane[0][1] = 0. if CP is not None and dCP is not None: rd_CP[0] = CP rd_CP_des[0] = CP_des[0] rd_dL_des_plane[0] = [ dL_des_plane[0] / 100, dL_des_plane[1] / 100, dL_des_plane[2] / 100 ] rd_dH_des[0] = dH_des rd_grf_des[0] = dL_des_plane - totalMass * mm.s2v( dartModel.world.gravity()) rd_root_des[0] = rootPos[0] del rd_CF[:] del rd_CF_pos[:] for i in range(len(contactPositions)): rd_CF.append(contactForces[i] / 100) rd_CF_pos.append(contactPositions[i].copy()) if viewer_GetForceState(): rd_exfen_des[0] = [ extraForce[0][0] / 100, extraForce[0][1] / 100, extraForce[0][2] / 100 ] rd_exf_des[0] = [0, 0, 0] else: rd_exf_des[0] = [ extraForce[0][0] / 100, extraForce[0][1] / 100, extraForce[0][2] / 100 ] rd_exfen_des[0] = [0, 0, 0] extraForcePos[0] = dartModel.getBodyPositionGlobal(selectedBody)
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 main(): # np.set_printoptions(precision=4, linewidth=200) np.set_printoptions(precision=5, threshold=np.inf, suppress=True, linewidth=3000) motionFile = 'wd2_tiptoe_zygote.bvh' # motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped(motionFile, SEGMENT_FOOT_RAD=0.008) motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped(motionFile, SEGMENT_FOOT_MAG=0.01, SEGMENT_FOOT_RAD=0.008) # motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped() # motion, mcfg, wcfg, stepsPerFrame, config = mit.create_jump_biped() vpWorld = cvw.VpWorld(wcfg) vpWorld.SetGlobalDamping(0.999) motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg) controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg) # controlModel_shadow_for_ik = cvm.VpControlModel(vpWorld, motion[0], mcfg) vpWorld.initialize() controlModel.initializeHybridDynamics() # controlToMotionOffset = (1.5, -0.02, 0) controlToMotionOffset = (1.5, 0., 0) controlModel.translateByOffset(controlToMotionOffset) # controlModel_shadow_for_ik.set_q(controlModel.get_q()) # controlModel_shadow_for_ik.computeJacobian(0, np.array([0., 0., 0.])) wcfg_ik = copy.deepcopy(wcfg) vpWorld_ik = cvw.VpWorld(wcfg_ik) controlModel_ik = cvm.VpControlModel(vpWorld_ik, motion[0], mcfg) vpWorld_ik.initialize() controlModel_ik.set_q(np.zeros_like(controlModel.get_q())) print('<?xml version="1.0" ?>') print('<skel version="1.0">') print('<world name="world 1">') print(' <physics>') print(' <time_step>0.001</time_step>') print(' <gravity>0 -9.81 0</gravity>') print(' <collision_detector>fcl_mesh</collision_detector>') print(' </physics>') print(' <skeleton name="grount skeleton">') print(' <mobile>false</mobile>') print(' <body name="ground">') print(' <transformation>0 -0.025 0 0 0 0</transformation>') print(' <visualization_shape name=" - visual - 0">') print(' <transformation>0.0 0.0 0.0 0.0 0.0 0.0 </transformation>') print(' <geometry>') print(' <box>') print(' <size>10000.0 0.05 10000.0</size>') print(' </box>') print(' </geometry>') print(' </visualization_shape>') print(' <collision_shape name=" - collision - 0">') print(' <transformation>0.0 0.0 0.0 0.0 0.0 0.0 </transformation>') print(' <geometry>') print(' <box>') print(' <size>100.0 0.05 100.0</size>') print(' </box>') print(' </geometry>') print(' </collision_shape>') print(' </body>') print(' <joint name="joint 1" type="free">') print(' <parent>world</parent>') print(' <child>ground</child>') print(' </joint>') print(' </skeleton>') print(' <skeleton name="dartModel">') for i in range(controlModel.getJointNum()): body_name = controlModel_ik.index2name(i) print(' <body name="'+body_name+'">') print(' <transformation>'+str(np.concatenate((controlModel_ik.getBodyPositionGlobal(i), mm.logSO3(controlModel_ik.getBodyOrientationGlobal(i)))))[1:-1]+'</transformation>') print(' <inertia>') print(' <mass>' + str(controlModel_ik.getBodyMass(i))+'</mass>') print(' <offset>'+str(controlModel_ik.getBodyLocalCom(i))[1:-1]+'</offset>') print(' </inertia>') geom_types = controlModel_ik.getBodyGeomsType(i) geom_sizes = controlModel_ik.getBodyGeomsSize(i) geom_local_frames = controlModel_ik.getBodyGeomsLocalFrame(i) geom_count = 0 for j in range(len(geom_types)): if geom_types[j] == 'B': print(' <visualization_shape name="'+body_name+' - visual shape '+str(geom_count)+'">') print(' <transformation>'+str(np.concatenate((geom_local_frames[j][:3, 3], mm.logSO3(geom_local_frames[j][:3, :3]))))[1:-1]+'</transformation>') print(' <geometry>') print(' <box>') print(' <size>'+str(geom_sizes[j])[1:-1]+'</size>') print(' </box>') print(' </geometry>') print(' </visualization_shape>') geom_count += 1 elif geom_types[j] in ('C', 'D', 'E', 'F'): print(' <visualization_shape name="'+body_name+' - visual shape '+str(geom_count)+'">') print(' <transformation>'+str(np.concatenate((geom_local_frames[j][:3, 3], mm.logSO3(geom_local_frames[j][:3, :3]))))[1:-1]+'</transformation>') print(' <geometry>') print(' <capsule>') print(' <radius>'+str(geom_sizes[j][0])+'</radius>') print(' <height>'+str(geom_sizes[j][1]-2.*geom_sizes[j][0])+'</height>') print(' </capsule>') print(' </geometry>') print(' </visualization_shape>') geom_count += 1 geom_count = 0 for j in range(len(geom_types)): if geom_types[j] == 'B': print(' <collision_shape name="'+body_name+' - collision shape '+str(geom_count)+'">') print(' <transformation>'+str(np.concatenate((geom_local_frames[j][:3, 3], mm.logSO3(geom_local_frames[j][:3, :3]))))[1:-1]+'</transformation>') print(' <geometry>') print(' <box>') print(' <size>'+str(geom_sizes[j])[1:-1]+'</size>') print(' </box>') print(' </geometry>') print(' </collision_shape>') geom_count += 1 if geom_types[j] in ('C', 'D'): print(' <collision_shape name="'+body_name+' - collision shape '+str(geom_count)+'">') print(' <transformation>'+str(np.concatenate((np.dot(geom_local_frames[j], mm.TransVToSE3((geom_sizes[j][1]/2.-geom_sizes[j][0])*mm.unitZ()))[:3, 3], mm.logSO3(geom_local_frames[j][:3, :3]))))[1:-1]+'</transformation>') print(' <geometry>') print(' <sphere>') print(' <radius>'+str(geom_sizes[j][0])+'</radius>') print(' </sphere>') print(' </geometry>') print(' </collision_shape>') geom_count += 1 if geom_types[j] in ('C', 'F'): print(' <collision_shape name="'+body_name+' - collision shape '+str(geom_count)+'">') print(' <transformation>'+str(np.concatenate((np.dot(geom_local_frames[j], mm.TransVToSE3(-(geom_sizes[j][1]/2.-geom_sizes[j][0])*mm.unitZ()))[:3, 3], mm.logSO3(geom_local_frames[j][:3, :3]))))[1:-1]+'</transformation>') print(' <geometry>') print(' <sphere>') print(' <radius>'+str(geom_sizes[j][0])+'</radius>') print(' </sphere>') print(' </geometry>') print(' </collision_shape>') geom_count += 1 print(' </body>') for i in range(controlModel.getJointNum()): name = controlModel_ik.index2name(i) inv_bone_R, inv_bone_p = controlModel_ik.getInvBoneT(i) if i == 0: print(' <joint name="j_'+name+'" type="free">') print(' <transformation>'+str(np.concatenate((inv_bone_p, mm.logSO3(inv_bone_R))))[1:-1]+'</transformation>') print(' <parent>world</parent>') print(' <child>'+name+'</child>') print(' </joint>') else: parent_name = controlModel_ik.index2name(controlModel_ik.getParentIndex(i)) print(' <joint name="j_'+name+'" type="ball">') print(' <transformation>'+str(np.concatenate((inv_bone_p, mm.logSO3(inv_bone_R))))[1:-1]+'</transformation>') print(' <parent>'+parent_name+'</parent>') print(' <child>'+name+'</child>') print(' </joint>') print(' </skeleton>') print('</world>') print('</skel>')